43、四轮人形二阶全向轨迹级联控制

四轮人形二阶全向轨迹级联控制

1. 引言

轮式移动机器人在众多应用中广泛使用，尤其在平坦且结构化的地面上表现出色。与双足步行机器人相比，它们速度更快、到达位置更高效，且在机械能需求方面通常更有效。在许多机器人应用中，特别是在杂乱环境中，全向滚动运动能力使机器人无需明确的偏航控制就能轻松改变身体姿势，向任何方向移动。

如今，全向轮作为移动机器人的运动方式，因其能向任何方向移动，尤其适用于在结构化的狭窄空间中行驶，在众多应用中需求极高。全向轮与传统轮子相比，运动学约束更少，能让机器人拥有更广泛的移动范围，并且具有更高的机动性和全向性。

全向轮机器人在现代有诸多应用场景：
- 个人辅助 ：如个人助理机器人可作为步行辅助工具，为行走不便的人提供引导和动态支持。
- 社交辅助 ：有用于社交互动的人机助理机器人。
- 医疗保健 ：带有麦克纳姆轮的机器人系统可为轮椅提供全向运动。
- 工业领域 ：麦克纳姆轮平台可作为全向车辆式机械臂在工业工作空间中移动；还可用于室内物料自动运输、仓库作业等。
- 体育训练 ：全向机器人平台可作为体育训练技术，在球拍运动中提供辅助和训练。
- 家庭环境 ：全向轮移动机器人可作为家庭助理。

本文主要介绍了一种人形机器人的轨迹跟踪模型，该机器人有两个带有三个关节的上肢，固定在安装有四个异步滚动驱动麦克纳姆轮的平台上。主要贡献是开发了一种三级联运动学轨迹跟踪控制器，每个级联由从机器人运动学模型推导的不同阶导数组成。同时，基于轮子编码器和惯性测量单元，采用确定性方法开发了不同的观测器来辅助控制。

2. 相关工作

• 位置误差校准 ：对全向轮机器人的位置误差和校准方法的研究具有重要意义。一些工作通过数值估计和确定性度量误差建模两种主要方法来校准全向轮移动机器人的里程计位置误差。
• 运动控制 ：不同的研究者提出了多种全向轮机器人的运动控制方法，如基于参考的控制、时变比例积分微分控制、基于观测器和高阶滑模的控制等。部分工作结合软计算技术和传统控制方法进行跟踪控制，以应对干扰和容错问题。

与以往工作不同的是，本文提出了一种基于模型的递归控制方法，其特点是实现内部多级联，结合多个高阶输入。通过连续逼近参考模型来减少数值误差，在控制结构和观测器模型方面与大多数相关工作有本质区别。

3. 机器人运动模型

本文描述了所提出机器人结构在仿真模型层面的基本设计部分，包括机载机械臂和四个麦克纳姆轮的全向移动结构的运动学模型。

机器人的四个麦克纳姆轮对称地径向排列在底盘下方，每个轮子可独立双向旋转。机载机械臂有三个旋转关节：肩部（θ0）、肘部（θ1）和腕部（θ2），均在俯仰方向转动，机器人的方向假设为机械臂的偏航运动（θt）。

3.1 机械臂运动方程

• 位置方程 ：在二维平面（z = 0）中，机械臂在矢状面（俯仰）的位置方程为：
• (x_a = l_1 \cdot \cos(\theta_0) + l_2 \cdot \cos(\theta_0 + \theta_1) + l_3 \cdot \cos(\theta_0 + \theta_1 + \theta_2))
• (y_a = l_1 \cdot \sin(\theta_0) + l_2 \cdot \sin(\theta_0 + \theta_1) + l_3 \cdot \sin(\theta_0 + \theta_1 + \theta_2))
  其中，关节的函数形式定义如下：
• 假设齿轮角度和齿数分别为(\phi_i)和(n_j)，令(\phi_0)为驱动关节，则(\theta_0 = \phi_0)。
• 令(\phi_6)为驱动齿轮，将旋转传递给齿轮(\phi_8)（(\phi_8 = \frac{n_6}{n_8} \cdot \phi_6)），则(\theta_1 = \phi_8)。
• 令(\phi_1)将运动传递给(\phi_5)（(\phi_5 = \frac{n_1}{n_5} \cdot \phi_1)），则(\theta_2 = \phi_5)。

由此可得机械臂的运动学定律：
(\begin{bmatrix}x_{a_{t + 1}} - x_{a_t}\y_{a_{t + 1}} - y_{a_t}\end{bmatrix} = \begin{bmatrix}-l_1c_0 - \frac{n_6}{n_8}(l_1c_0 + l_2c_{01}) - \frac{n_1}{n_5}(l_1c_0 + l_2c_{01} + l_3c_{012})\l_1s_0 \frac{n_6}{n_8}(l_1s_0 + l_2s_{01}) \frac{n_1}{n_5}(l_1s_0 + l_2s_{01} + l_3s_{012})\end{bmatrix} \begin{bmatrix}\theta_0 - \phi_0\\theta_1 - \phi_8\\theta_2 - \phi_5\end{bmatrix})

当(\lim_{\phi_i \to \theta_i}(x, y)_{t + 1} - (x, y)_t = 0)时，该定律成立，其中((x, y))为笛卡尔位置，((\theta_i - \phi_i))为瞬时关节误差。

• 速度和加速度方程 ：对位置方程求一阶导数可得笛卡尔速度：
  (\begin{bmatrix}\dot{x} a\\dot{y}_a\end{bmatrix} = l_1 \begin{bmatrix}-s_0\c_0\end{bmatrix} \dot{\theta}_0 + l_2 \begin{bmatrix}-s {01}\c_{01}\end{bmatrix} \sum_{i = 0}^{1} \dot{\theta} i + l_3 \begin{bmatrix}-s {012}\c_{012}\end{bmatrix} \sum_{i = 1}^{2} \dot{\theta}_i)

二阶导数描述了机械臂的笛卡尔加速度：
(\begin{bmatrix}\ddot{x}_a\\ddot{y}_a\end{bmatrix} = J_t \cdot \begin{bmatrix}\ddot{\theta}_0\\ddot{\theta}_1\\ddot{\theta}_2\end{bmatrix} + \dot{J}_t \cdot \begin{bmatrix}\dot{\theta}_0\\dot{\theta}_1\\dot{\theta}_2\end{bmatrix})
其中，(J_t)是一个非静态的(2×3)矩阵。

3.2 全向运动模型

设(u_t)为机器人笛卡尔形式的状态向量，(u = (x, y)^{\top})，则正向运动学方程为：
(\dot{u} = rK \cdot \dot{\Phi})
其中，(K)是包含几何参数的静态运动学控制矩阵，(r)是轮子半径，(\Phi_t = (\phi_1, \phi_2, \phi_3, \phi_4)^{\top})是四个轮子的角速度向量。

反向运动学方程为：
(\dot{\Phi} = \frac{1}{r} \cdot K^+ \cdot \dot{u} = \frac{1}{r} \cdot K^{\top}(K \cdot K^{\top})^{-1} \cdot \dot{u})

根据几何关系，笛卡尔速度(\dot{x})和(\dot{y})可由轮子的切向速度(V_k)表示：
(\dot{x} = V_1 \cdot \cos(\alpha_1 - \frac{\pi}{2}) + V_2 \cdot \cos(\alpha_2 - \frac{\pi}{2}) + V_3 \cdot \cos(\alpha_3 - \frac{\pi}{2}) + V_4 \cdot \cos(\alpha_4 - \frac{\pi}{2}))
(\dot{y} = V_1 \cdot \sin(\alpha_1 - \frac{\pi}{2}) + V_2 \cdot \sin(\alpha_2 - \frac{\pi}{2}) + V_3 \cdot \sin(\alpha_3 - \frac{\pi}{2}) + V_4 \cdot \sin(\alpha_4 - \frac{\pi}{2}))

其中，(V_k = r \cdot \dot{\phi}_k)，静态非方阵运动学控制矩阵(K)为：
(K = \begin{bmatrix}\cos(\alpha_1 - \frac{\pi}{2}) & \cos(\alpha_2 - \frac{\pi}{2}) & \cos(\alpha_3 - \frac{\pi}{2}) & \cos(\alpha_4 - \frac{\pi}{2})\\sin(\alpha_1 - \frac{\pi}{2}) & \sin(\alpha_2 - \frac{\pi}{2}) & \sin(\alpha_3 - \frac{\pi}{2}) & \sin(\alpha_4 - \frac{\pi}{2})\end{bmatrix})

速度全向模型可表示为：
(\begin{bmatrix}\dot{x}\\dot{y}\end{bmatrix} = r \cdot K \cdot \begin{bmatrix}\dot{\phi}_1\\dot{\phi}_2\\dot{\phi}_3\\dot{\phi}_4\end{bmatrix})

同理，可推导出二阶和三阶运动学模型：
二阶运动学模型：
(\begin{bmatrix}\ddot{x}\\ddot{y}\end{bmatrix} = r \cdot \begin{bmatrix}\cos(\alpha_1 - \frac{\pi}{2}) & \cos(\alpha_2 - \frac{\pi}{2}) & \cos(\alpha_3 - \frac{\pi}{2}) & \cos(\alpha_4 - \frac{\pi}{2})\\sin(\alpha_1 - \frac{\pi}{2}) & \sin(\alpha_2 - \frac{\pi}{2}) & \sin(\alpha_3 - \frac{\pi}{2}) & \sin(\alpha_4 - \frac{\pi}{2})\end{bmatrix} \cdot \begin{bmatrix}\ddot{\phi}_1\\ddot{\phi}_2\\ddot{\phi}_3\\ddot{\phi}_4\end{bmatrix})

以下是机器人运动模型的主要方程总结：
|方程类型|方程表达式|
| ---- | ---- |
|机械臂位置方程|(x_a = l_1 \cdot \cos(\theta_0) + l_2 \cdot \cos(\theta_0 + \theta_1) + l_3 \cdot \cos(\theta_0 + \theta_1 + \theta_2)) (y_a = l_1 \cdot \sin(\theta_0) + l_2 \cdot \sin(\theta_0 + \theta_1) + l_3 \cdot \sin(\theta_0 + \theta_1 + \theta_2))|
|机械臂运动学定律|(\begin{bmatrix}x_{a_{t + 1}} - x_{a_t}\y_{a_{t + 1}} - y_{a_t}\end{bmatrix} = \begin{bmatrix}-l_1c_0 - \frac{n_6}{n_8}(l_1c_0 + l_2c_{01}) - \frac{n_1}{n_5}(l_1c_0 + l_2c_{01} + l_3c_{012})\l_1s_0 \frac{n_6}{n_8}(l_1s_0 + l_2s_{01}) \frac{n_1}{n_5}(l_1s_0 + l_2s_{01} + l_3s_{012})\end{bmatrix} \begin{bmatrix}\theta_0 - \phi_0\\theta_1 - \phi_8\\theta_2 - \phi_5\end{bmatrix})|
|机械臂速度方程|(\begin{bmatrix}\dot{x} a\\dot{y}_a\end{bmatrix} = l_1 \begin{bmatrix}-s_0\c_0\end{bmatrix} \dot{\theta}_0 + l_2 \begin{bmatrix}-s {01}\c_{01}\end{bmatrix} \sum_{i = 0}^{1} \dot{\theta} i + l_3 \begin{bmatrix}-s {012}\c_{012}\end{bmatrix} \sum_{i = 1}^{2} \dot{\theta}_i)|
|机械臂加速度方程|(\begin{bmatrix}\ddot{x}_a\\ddot{y}_a\end{bmatrix} = J_t \cdot \begin{bmatrix}\ddot{\theta}_0\\ddot{\theta}_1\\ddot{\theta}_2\end{bmatrix} + \dot{J}_t \cdot \begin{bmatrix}\dot{\theta}_0\\dot{\theta}_1\\dot{\theta}_2\end{bmatrix})|
|全向正向运动学方程|(\dot{u} = rK \cdot \dot{\Phi})|
|全向反向运动学方程|(\dot{\Phi} = \frac{1}{r} \cdot K^+ \cdot \dot{u} = \frac{1}{r} \cdot K^{\top}(K \cdot K^{\top})^{-1} \cdot \dot{u})|
|笛卡尔速度方程|(\dot{x} = V_1 \cdot \cos(\alpha_1 - \frac{\pi}{2}) + V_2 \cdot \cos(\alpha_2 - \frac{\pi}{2}) + V_3 \cdot \cos(\alpha_3 - \frac{\pi}{2}) + V_4 \cdot \cos(\alpha_4 - \frac{\pi}{2})) (\dot{y} = V_1 \cdot \sin(\alpha_1 - \frac{\pi}{2}) + V_2 \cdot \sin(\alpha_2 - \frac{\pi}{2}) + V_3 \cdot \sin(\alpha_3 - \frac{\pi}{2}) + V_4 \cdot \sin(\alpha_4 - \frac{\pi}{2}))|
|速度全向模型|(\begin{bmatrix}\dot{x}\\dot{y}\end{bmatrix} = r \cdot K \cdot \begin{bmatrix}\dot{\phi}_1\\dot{\phi}_2\\dot{\phi}_3\\dot{\phi}_4\end{bmatrix})|
|二阶运动学模型|(\begin{bmatrix}\ddot{x}\\ddot{y}\end{bmatrix} = r \cdot \begin{bmatrix}\cos(\alpha_1 - \frac{\pi}{2}) & \cos(\alpha_2 - \frac{\pi}{2}) & \cos(\alpha_3 - \frac{\pi}{2}) & \cos(\alpha_4 - \frac{\pi}{2})\\sin(\alpha_1 - \frac{\pi}{2}) & \sin(\alpha_2 - \frac{\pi}{2}) & \sin(\alpha_3 - \frac{\pi}{2}) & \sin(\alpha_4 - \frac{\pi}{2})\end{bmatrix} \cdot \begin{bmatrix}\ddot{\phi}_1\\ddot{\phi}_2\\ddot{\phi}_3\\ddot{\phi}_4\end{bmatrix})|

机器人运动模型的流程如下：

graph TD;
    A[定义机器人状态和参数] --> B[推导机械臂位置方程];
    B --> C[得出机械臂运动学定律];
    C --> D[计算机械臂速度和加速度方程];
    A --> E[建立全向运动学模型];
    E --> F[推导正向和反向运动学方程];
    F --> G[计算笛卡尔速度和全向速度模型];
    G --> H[推导二阶运动学模型];

4. 传感模型与观测器

为了实现有效的控制，需要建立传感模型和观测器作为在线反馈元素。基于轮子编码器和惯性测量单元，利用多传感器输入来构建观测器。

• 轮子编码器 ：轮子编码器可以测量轮子的旋转角度和速度，通过对轮子旋转信息的采集和处理，可以得到机器人的位置和速度信息。
• 惯性测量单元（IMU） ：IMU 可以测量机器人的加速度和角速度，提供机器人的姿态和运动状态信息。

将轮子编码器和 IMU 的数据进行融合，可以更准确地估计机器人的位置、速度和姿态。观测器的设计目的是根据传感器的测量值，实时估计机器人的状态，为控制器提供准确的反馈信息。

5. 三级联控制器

本文提出的三级联控制器是核心部分，每个级联由从机器人运动学模型推导的不同阶导数组成，分别为距离级联、速度级联和加速度级联。

• 距离级联 ：主要负责控制机器人的位置误差，通过比较机器人的实际位置和期望位置，计算位置误差，并根据误差调整机器人的运动。
• 速度级联 ：在距离级联的基础上，进一步控制机器人的速度，确保机器人以合适的速度向目标位置移动。通过比较机器人的实际速度和期望速度，计算速度误差，并进行调整。
• 加速度级联 ：对机器人的加速度进行控制，提高机器人运动的稳定性和响应速度。根据速度误差的变化率，计算加速度误差并进行补偿。

三级联控制器的工作流程如下：

graph TD;
    A[设定目标位置、速度和加速度] --> B[距离级联：计算位置误差];
    B --> C[速度级联：根据位置误差计算速度参考值];
    C --> D[速度级联：计算速度误差];
    D --> E[加速度级联：根据速度误差计算加速度参考值];
    E --> F[加速度级联：计算加速度误差];
    F --> G[输出控制信号调整机器人运动];
    G --> H[更新机器人实际位置、速度和加速度];
    H --> B;

三级联控制器的优势在于通过三个递归反馈级联，能够同时减少位置、速度和加速度的误差，提高了控制的鲁棒性。与传统的 PID 控制器不同，该方法将每个导数作为独立的递归控制周期，在较低阶和更快采样的控制循环中发挥作用。

6. 总结

本文介绍了一种四轮人形二阶全向轨迹级联控制方法，主要内容总结如下：
1. 机器人应用背景 ：轮式移动机器人尤其是全向轮机器人在多个领域有广泛应用，如个人辅助、医疗保健、工业等。
2. 相关工作对比 ：与以往的全向轮机器人控制方法相比，本文提出的基于模型的递归控制方法在控制结构和观测器模型上有本质区别。
3. 机器人运动模型 ：建立了机械臂和全向移动结构的运动学模型，包括位置、速度和加速度方程。
4. 传感模型与观测器 ：基于轮子编码器和惯性测量单元构建观测器，为控制器提供准确的反馈信息。
5. 三级联控制器 ：开发了三级联轨迹跟踪控制器，通过距离、速度和加速度级联，同时减少位置、速度和加速度误差，提高了控制的鲁棒性。

通过数值模拟验证了所提出的模型和方法的有效性和可行性，该方法具有良好的性能、重路由灵活性和机动性。未来，可以进一步研究如何将该方法应用于实际机器人系统中，提高机器人在复杂环境下的运动控制能力。

以下是本文涉及的关键技术点总结：
|技术点|描述|
| ---- | ---- |
|全向轮机器人|具有全向滚动运动能力，运动学约束少，机动性高|
|三级联控制器|由距离、速度和加速度级联组成，同时减少多方面误差|
|运动学模型|包括机械臂和全向移动结构的位置、速度和加速度方程|
|观测器|基于轮子编码器和惯性测量单元，提供准确反馈信息|