30、机器人系统控制：从执行器模型到反步控制

机器人系统控制：从执行器模型到反步控制

1. 广义质量矩阵与执行器基础

在机器人系统控制中，广义质量矩阵 (M(q)) 起着关键作用。这里的广义质量矩阵是在各连杆添加电机组件后得到的新质量矩阵。若电枢惯性或电机电感可忽略，还能推导出这些方程的特殊形式。

机器人系统主要通过旋转关节、棱柱关节或它们的叠加来构建。直流电机能将电能转化为旋转运动，可直接用于驱动机器人系统中的旋转关节。而驱动棱柱关节常用的执行器有：
- 液压缸
- 气动活塞
- 机电直线电机

液压缸和气动执行器适用于需要大负载和大行程的应用，如挖掘机、推土机等土方机械就使用液压缸。机电直线电机则用于需要快速响应和便携性的应用，是机器人系统驱动的常见组件。

机电直线电机是一种将传统电机与机械子系统相结合的执行器，能把电机的旋转运动转化为平移运动。机械子系统可以是螺杆机构或齿轮等。其工作原理如下：

graph LR
    A[电机] --> B[旋转丝杠]
    B --> C[带动线性平台沿导轨平移]

电机使丝杠旋转，丝杠带动线性平台沿导轨平移，线性平台中的驱动螺母在沿丝杠移动时，被驱动外壳的导轨限制旋转。

2. 机电直线电机的运动方程推导

2.1 示例 6.13

当电机固定在惯性系中，且有外力 (f) 作用于线性平台时，推导机电直线电机的运动方程。
- 系统动能 (T) 可表示为：
(T = \frac{1}{2}m_T \dot{x}^2 + \frac{1}{2}J \dot{\theta}^2)
其中，(m_T) 是线性平台的质量，(J) 是电枢和驱动轴的转动惯量，(x) 是线性平台的位移，(\theta) 是电枢的旋转角度。
- 位移 (x) 和旋转角度 (\theta) 满足运动学方程：
(x = c\theta)
其中，(c) 是丝杠的螺距常数，单位为 (m rad^{-1})。
- 外力 (f) 作用于线性平台和扭矩 (\tau) 作用于电枢所做的虚功为：
(\delta W = (f \mathbf{x} 0) \cdot \delta \mathbf{r}_p + \tau \delta \theta = (f \mathbf{x}_0) \cdot \delta x \mathbf{x}_0 + \tau \delta \theta = (cf + \tau) \delta \theta = Q {\theta} \delta \theta)
- 根据拉格朗日方程可得：
(\frac{d}{dt}(\frac{\partial T}{\partial \dot{\theta}}) - \frac{\partial T}{\partial \theta} + \frac{\partial V}{\partial \theta} = Q_{\theta})
((J + m_T c^2) \ddot{\theta} = cf + \tau)
- 直线电机的电气和机械领域的组合方程为：
((J + m_{\tau} c^2) \ddot{\theta} = cf + k_t i)
(L \frac{di}{dt} + R i + k_b \dot{\theta} = e)
其中，(L) 是电枢电感，(R) 是电枢电阻，(k_b) 是反电动势常数，(k_t) 是电机扭矩常数，(e) 是输入电压。机械子系统方程用电机位移表示为：
((m_T + \frac{J}{c^2}) \ddot{x} = f + \frac{1}{c} k_t i)

2.2 示例 6.14

假设球形机器人机械手的连杆 3 由机电直线电机驱动，且电枢和驱动轴组件的惯性矩阵可忽略，连杆可建模为集中在质心的质量。当考虑执行器动力学时，求完整的控制方程。
- 线性执行器安装在机械臂中，电机外壳和定子固定在连杆 2 上，线性平台刚性连接到连杆 3（伸缩臂）。此时，连杆 2 和连杆 3 的质量和惯性矩阵会因执行器的加入而改变：
- 连杆 2：
(m_2 = m_{2,link} + \Delta m_2)
(I_2 = I_{2,link} + \Delta I_2)
- 连杆 3：
(m_3 = m_{3,link} + \Delta m_3)
(I_3 = I_{3,link} + \Delta I_2)
- 连杆 3 质心的距离 (d_{q,r}(t)) 与执行器位移 (x(t)) 的关系为：
(d_{q,r}(t) = x(t) + z_{c3})
其中，(z_{c3}) 是常数。
- 电枢相对于电机外壳的旋转 (\theta_a) 与线性执行器位移 (x) 的关系为 (x = c\theta_a)，电枢和驱动轴的动能 (T_a) 为：
(T_a = \frac{1}{2}m_a v_{0,ca} \cdot v_{0,ca} + \frac{1}{2} \boldsymbol{\omega} {0,a} \cdot I_a \boldsymbol{\omega} {0,a})
其中，(v_{0,ca}) 是电枢和驱动轴组件质心的速度，(\boldsymbol{\omega} {0,a}) 是电枢的角速度：
(\boldsymbol{\omega} {0,a} = \boldsymbol{\omega} {0,2} + \dot{\theta}_a \mathbf{z}_3 = \boldsymbol{\omega} {0,2} + \frac{1}{c} \dot{x} \mathbf{z} 3)
- 机器人的动能 (T) 为：
(T = T_1 + T_2 + T_3 + T_a)
(= \frac{1}{2}m_1 v {0,1} \cdot v_{0,1} + \frac{1}{2} \boldsymbol{\omega} {0,1} \cdot I_1 \boldsymbol{\omega} {0,1} + \frac{1}{2}m_2 v_{0,2} \cdot v_{0,2} + \frac{1}{2} \boldsymbol{\omega} {0,2} \cdot I_2 \boldsymbol{\omega} {0,2} + \frac{1}{2}m_3 v_{0,3} \cdot v_{0,3} + \frac{1}{2} \boldsymbol{\omega} {0,3} \cdot I_3 \boldsymbol{\omega} {0,3} + \frac{1}{2}m_a v_{0,ca} \cdot v_{0,ca} + \frac{1}{2} \boldsymbol{\omega} {0,a} \cdot I_a \boldsymbol{\omega} {0,3})
假设电枢质量和惯性可忽略，且连杆 1 质心不平移，动能最终形式为：
(T = \frac{1}{2}m_2 v_{0,2} \cdot v_{0,2} + \frac{1}{2}m_3 v_{0,3} \cdot v_{0,3})
- 势能 (V) 为：
(V = m_3 g(d_{o,p} - d_{q,r} \cos \theta_2))
- 电磁扭矩 (\tau_3) 所做的虚功为：
(\delta W = \tau_3 \delta \theta_a = \tau_3 \frac{1}{c} \delta x = \frac{1}{c} \tau_3 \delta d_{q,r})
- 电枢电路的基尔霍夫电压定律为：
(L_3 \frac{di_3}{dt} + R_3 i_3 + k_{b3} \dot{\theta} a = e_3)
(\tau_3 = k {t3} i_3)
- 最终的控制方程组为：
(M(q) \ddot{q} = n(q, \dot{q}) + \begin{cases} \tau_1 \ \tau_2 \ \frac{1}{c} k_{t3} i_3 \end{cases})
(L_3 \frac{di_3}{dt} + R_3 i_3 + \frac{1}{c} k_{b3} \dot{d}_{q,r} = e_3)
其中，广义质量矩阵 (M) 和非线性右侧 (n) 的形式与之前问题推导的类似，但系数 (m_2) 和 (m_3) 因线性执行器质量的加入而改变。

若将示例 6.11 和 6.12 中关节 1 和 2 执行器的方程与线性执行器的方程组合，可得到包含执行器动力学的机器人系统方程：
(M(q(t)) \ddot{q}(t) = n(q(t), \dot{q}(t)) + \begin{bmatrix} K_{b1} & 0 & 0 \ 0 & K_{t2} & 0 \ 0 & 0 & \frac{1}{c} K_{t3} \end{bmatrix} \begin{cases} i_1 \ i_2 \ i_3 \end{cases})
(\begin{bmatrix} L_1 & 0 & 0 \ 0 & L_2 & 0 \ 0 & 0 & L_3 \end{bmatrix} \begin{cases} \frac{di_1}{dt} \ \frac{di_2}{dt} \ \frac{di_3}{dt} \end{cases} + \begin{bmatrix} R_1 & 0 & 0 \ 0 & R_2 & 0 \ 0 & 0 & R_3 \end{bmatrix} \begin{cases} i_1 \ i_2 \ i_3 \end{cases} + \begin{bmatrix} K_{b1} & 0 & 0 \ 0 & K_{b2} & 0 \ 0 & 0 & \frac{1}{c} K_{b3} \end{bmatrix} \begin{cases} \dot{\theta} 1 \ \dot{\theta}_2 \ \dot{d} {qr} \end{cases} = \begin{cases} e_1 \ e_2 \ e_3 \end{cases})
这里的 (M) 和 (n) 相较于无执行器时的标称值，考虑了两个旋转执行器和一个线性执行器的质量和惯性。

3. 反步控制与执行器动力学

在机器人系统控制中，通常会推导具有 (\boldsymbol{\tau}(t) := \boldsymbol{\tau}(q(t), \dot{q}(t))) 结构的反馈控制器，其形式为：
(M(q(t)) \ddot{q}(t) = n(q(t), \dot{q}(t)) + \boldsymbol{\tau})

受控系统的稳定性和收敛性基于对特定李雅普诺夫函数的分析，该函数与上述控制方程和特定反馈输入 (\boldsymbol{\tau} := \boldsymbol{\tau}(q(t), \dot{q}(t))) 相匹配。但在实际中，执行扭矩或力通常并非直接控制，更常见的是通过电压或电流来驱动电机，进而产生作用于机器人的力或力矩。

包含直流电机或机电直线电机执行器动力学的一般模型由一组耦合的机械和电气子系统组成：
(M(q(t)) \ddot{q}(t) = n(q(t), \dot{q}(t)) + K_t i(t))
(L \frac{di}{dt}(t) + R i(t) + K_b \dot{q}(t) = e(t))
其中，(L) 是 (N×N) 对角电感矩阵，(R) 是 (N×N) 对角电阻矩阵，(K_b) 是 (N×N) 对角反电动势常数矩阵，(K_t) 是 (N×N) 对角扭矩常数矩阵。这些方程也可改写为一阶形式：
(\dot{x}_1(t) = f(x_1(t)) + G(x_1(t)) x_2(t))
(\dot{x}_2(t) = h(x_1(t), x_2(t), u(t)))

假设能直接在上述一阶方程中施加理想反馈律 (x_2(t) := k(x_1(t)))，并且当将此理想反馈代入一阶方程时，(x_1) 运动的稳定性可由满足以下条件的李雅普诺夫函数保证：
(\mathcal{V}_1 := \mathcal{V}_1(x_1) > 0)，对于所有 (x_1 \neq 0)
(\dot{\mathcal{V}}_1 := \frac{\partial V}{\partial x_1} \cdot (f(x_1) + G(x_1) k(x_1)) < 0)，对于所有 (x_1 \neq 0)

然而，当考虑耦合的一阶方程时，实际施加的是输入 (u(t))，无法保证期望的控制律 (x_2(t) \equiv k(x_1(t))) 在每个时刻都成立。为此，定义新状态 (z)：
(z(t) := x_2(t) - k(x_1(t)))
它衡量了耦合系统的演化与理想控制律的接近程度。引入新状态 (z) 后，一阶形式的控制方程可表示为：
(\dot{x}_1(t) = f(x_1) + G(x_1(t)) k(x_1(t)) + G(x_1) z(t))
(\dot{z}(t) = h(t) - \dot{k}(t) := v(t))

基于控制律 (k(x_1)) 对应满足上述条件的李雅普诺夫函数 (\mathcal{V}_1) 这一假设，可以为耦合方程设计反馈控制器。选择李雅普诺夫函数：
(\mathcal{V}_2 \begin{pmatrix} x_1 \ z \end{pmatrix} := \mathcal{V}_1(x_1) + \frac{1}{2} z^T z)

沿着耦合方程的轨迹计算李雅普诺夫函数 (\mathcal{V}_2) 的导数：
(\dot{\mathcal{V}}_2 = \frac{\partial \mathcal{V}_1}{\partial x_1} \cdot {(f(x_1) + G(x_1) k(x_1)) + G(x_1) z} + z^T \dot{z})
(= \frac{\partial \mathcal{V}_1}{\partial x_1} \cdot {f(x_1) + G(x_1) k(x_1)} + z^T {G(x_1)^T \frac{\partial \mathcal{V}_1}{\partial x_1}^T + v})

若选择 (v) 为：
(v := - \kappa z - G(x_1)^T \frac{\partial \mathcal{V}_1}{\partial x_1}^T)
则有：
(\mathcal{V}_2 \begin{pmatrix} x_1 \ z \end{pmatrix} > 0)，对于所有 (\begin{pmatrix} x_1 \ z \end{pmatrix} \neq 0)
(\dot{\mathcal{V}}_2 \begin{pmatrix} x_1 \ z \end{pmatrix} < 0)，对于所有 (\begin{pmatrix} x_1 \ z \end{pmatrix} \neq 0)

这表明耦合动力学在原点处的平衡点是渐近稳定的。以下通过示例展示反步控制的应用。

3.1 示例 6.15

使用基于理想动态逆的李雅普诺夫函数和反馈控制器，为示例 6.14 中的机器人系统设计设定点控制器。
- 定义状态 (x_1)：
(x_1 := \begin{cases} e \ \dot{e} \end{cases})，其中 (e := q - q_d)
- 定义“理想反馈律” (k(x_1))：
(k(x_1) := K_t^{-1} {M(-G_1(\dot{q} - \dot{q}_d) - G_0(q - q_d)) - n})
(= K_t^{-1} {M(-G_1 \dot{e} - G_0 e) - n})
- 定义状态 (z)：
(z := i - k(x_1))，用于衡量状态 (i) 对理想动态逆控制律的跟踪程度。此时，机械运动方程可写为：
(\dot{x}_1(t) := \begin{bmatrix} 0 & I \ -G_0 & -G_1 \end{bmatrix} x_1(t) + \begin{bmatrix} 0 \ M^{-1} K_t \end{bmatrix} z(t))
(= A x_1(t) + B z(t))
- 定义与状态 (x_1) 相关的李雅普诺夫函数 (\mathcal{V}_1(x_1))：
(\mathcal{V}_1(x_1) := \frac{1}{2} x_1^T P x_1)，其中 (P) 是对称正定的李雅普诺夫方程 (PA + A^T P = -Q) 的解，(Q) 是对称正定矩阵。由于 (A) 是 Hurwitz 矩阵，这样的矩阵 (P) 一定存在。当 (z(t) \equiv 0) 或 (i(t) \equiv k(x_1(t))) 时，(\mathcal{V}_1) 可验证为关于 (x_1) 的李雅普诺夫函数。
- 电气领域方程用新状态表示为：
(\dot{z} = \frac{di}{dt} - \dot{k})
(= L^{-1}(-R i - K_b \dot{q} + e) - \dot{k})
- 展开反馈的导数 (\dot{k})：
(\dot{k} = K_t^{-1} M \begin{bmatrix} -G_0 & -G_1 \end{bmatrix} \dot{x}_1 - K_t^{-1} \frac{\partial n}{\partial x_1} \dot{x}_1)
(= \left{K_t^{-1} M \begin{bmatrix} -G_0 & -G_1 \end{bmatrix} - K_t^{-1} \frac{\partial n}{\partial x_1} \right} {A x_1 + B z})
- 电气子系统控制方程可写为：
(\dot{z} = L^{-1}(-R i - K_b \dot{q} + e) - G_2(A x_1 + B z))
其中 (G_2 = \left{K_t^{-1} M \begin{bmatrix} -G_0 & -G_1 \end{bmatrix} - K_t^{-1} \frac{\partial n}{\partial x_1} \right})

根据前面推导，选择 (v = - \frac{1}{2} k z - G^T \frac{\partial \mathcal{V}_1}{\partial x_1}^T)，则有：
(- \frac{1}{2} k z - G^T \frac{\partial \mathcal{V}_1}{\partial x_1}^T = L^{-1}(R i - K_b \dot{q} + e) - G_2(A x + B z))

当执行器的输入电压 (e) 规定为：
(e = L \left(- \frac{1}{2} k z - B^T P x_1 + G_2[A x_1 + B z] \right) - R i + K_b \dot{q})
时，控制输入电压 (e) 能产生期望的渐近行为。此时，向量 (v) 变为：
(v = - \frac{1}{2} k z - B^T P x_1)

沿着耦合的机械 - 电气系统轨迹计算李雅普诺夫函数 (\mathcal{V}_2 := \mathcal{V}_1 + \frac{1}{2} z^T z) 的导数：
(\dot{\mathcal{V}}_2 := \frac{1}{2}[(A x_1 + B z)^T P x_1 + x_1^T P^T(A x_1 + B z)] + z^T \dot{z})
(= \frac{1}{2} \left[x_1^T (A^T P + P A) x_1 + z^T B^T P x_1 + z^T \left(- \frac{1}{2} k z - B^T P x_1 \right) \right])
(= - \frac{1}{2} x_1^T Q x_1 - \frac{1}{2} k z^T z)

这表明对于 (x := \begin{pmatrix} x_1^T & z^T \end{pmatrix}^T \neq \mathbf{0}^T)，有 (\mathcal{V}_2 > 0) 且 (\dot{\mathcal{V}}_2 < 0)，因此耦合动力学的原点是渐近稳定的。

4. 相关问题

以下是一些关于机器人系统控制的问题，涉及重力补偿和 PD 设定点控制：
|问题编号|机器人类型|自由度|广义坐标|广义力|系统参数|
| ---- | ---- | ---- | ---- | ---- | ---- |
|6.1|二自由度 PUMA 模型|(2)|(\begin{cases} \theta_1(t) \ \theta_2(t) \end{cases})|(\begin{cases} \tau_1(t) \ \tau_2(t) \end{cases})|(m_1 = m_2 = 2 kg)，(L_1 = 0.25 m)|
|6.2|三自由度 PUMA 模型|(3)|(\begin{cases} \theta_1(t) \ \theta_2(t) \ \theta_3(t) \end{cases})|(\begin{cases} \tau_1(t) \ \tau_2(t) \ \tau_3(t) \end{cases})|(W = 0.5 m)，(D = 0.5 m)，(L_1 = 1 m)，(L_2 = 1 m)，(m_3 = 20 kg)|
|6.3|二自由度 PUMA 模型|(2)|(\begin{cases} \theta_1(t) \ \theta_2(t) \end{cases})|(\begin{cases} \tau_1(t) \ \tau_2(t) \end{cases})|(m_1 = m_2 = 25 kg)，(I_{11,2} = 20 kg m^2)，(I_{22,2} = 25 kg m^2)，(I_{33,2} = 10 kg m^2)，(D = 0.25 m)，(L_1 = 0.5 m)|
|6.4|三自由度笛卡尔机器人|(3)|(\begin{cases} x(t) \ y(t) \ z(t) \end{cases})|(\begin{cases} f_x \ f_y \ f_z \end{cases})|(m_1 = 20 kg)，(m_2 = 10 kg)，(m_3 = 5 kg)|
|6.5|三自由度球形手腕|(3)|(\begin{cases} \psi(t) \ \theta(t) \ \phi(t) \end{cases})|(\begin{cases} \tau_1 \ \tau_2 \ \tau_3 \end{cases})|(m_1 = 5 kg)，(m_2 = 5 kg)，(m_3 = 2 kg)，(I_{33,1} = 5 kg m^2)，(I_{33,2} = 4 kg m^2)，(I_{33} = 2 kg m^2)，(I_{11,3} = 3 kg m^2)，(I_{22,2} = 1 kg m^2)，(z_{p,c2} = 0.1 m)，(z_{qc3} = 0.1 m)|
|6.6|三自由度 SCARA 机器人|(3)|(\begin{cases} \theta_1(t) \ \theta_2(t) \ d(t) \end{cases})|(\begin{cases} \tau_1 \ \tau_2 \ f \end{cases})|(m_1 = 10 kg)，(m_2 = 10 kg)，(m_3 = 2 kg)，(L_1 = 0.25 m)，(D_1 = 0.2 m)，(L_2 = 0.2 m)，(D_2 = 0.1 m)|
|6.7|三自由度机器人|(3)|(\begin{cases} \theta_1(t) \ \theta_2(t) \ d(t) \end{cases})|(\begin{cases} \tau_1 \ \tau_2 \ f \end{cases})|(m_1 = 5 kg)，(m_2 = 4 kg)，(m_3 = 2 kg)，(L_1 = 0.1 m)，(L_2 = 0.2 m)，(I_{33,1} = 2 kg m^2)，(I_{33,2} = 3 kg m^2)，(I_{33,3} = 4 kg m^2)|
|6.8|三自由度机器人|(3)|(\begin{cases} \theta(t) \ H(t) \ L(t) \end{cases})|(\begin{cases} \tau(t) \ f_1(t) \ f_2(t) \end{cases})|(m_1 = 10 kg)，(m_2 = 10 kg)，(m_3 = 3 kg)，(L_A = 0.2 m)，(H_1 = 0.3 m)，(D_2 = 0.2 m)，(H_3 = 0.1 m)，(D_3 = 0.2 m)|
|6.9|三自由度机器人|(3)|(\begin{cases} \theta(t) \ H(t) \ L(t) \end{cases})|(\begin{cases} m(t) \ f_1(t) \ f_2(t) \end{cases})|(m_1 = 10 kg)，(m_2 = 10 kg)，(m_3 = 3 kg)，(I_{33,1} = 2 kg m^2)，(I_{33,3} = 10 kg m^2)，(L_A = 0.2 m)，(H_1 = 0.3 m)，(D_2 = 0.2 m)，(H_3 = 0.1 m)，(D_3 = 0.2 m)|

对于这些问题，要求根据相关定理推导使用 PD 反馈和重力补偿的控制器以实现设定点控制，并编写程序模拟控制器性能，绘制不同初始条件、目标状态和反馈增益下的状态轨迹、设定点误差和控制输入随时间的变化曲线。

解决这些问题的步骤如下：
1. 根据机器人的类型和自由度，确定广义坐标和广义力。
2. 结合给定的系统参数，推导使用 PD 反馈和重力补偿的控制器。
3. 编写程序对控制器性能进行模拟。
4. 绘制状态轨迹、设定点误差和控制输入随时间的变化曲线，分析不同初始条件、目标状态和反馈增益的影响。

graph LR
    A[确定机器人信息] --> B[推导控制器]
    B --> C[编写模拟程序]
    C --> D[绘制曲线并分析]

通过以上步骤，可以深入理解机器人系统控制中执行器模型、反步控制等方法的应用，为实际机器人系统的设计和控制提供理论支持和实践经验。