06.2｜两连杆机械臂：从质心速度推到计算力矩与稳定性

完整建立平面两转动关节的动能、势能、M/C/g，解释耦合、重力、摩擦、执行器，再推导计算力矩与 Lyapunov 稳定性，区分理想模型和实际约束。

1. 先画清角度，重力符号才不会错

考虑竖直平面中的两个转动关节。世界 $x$ 水平向右、 $y$ 竖直向上，重力加速度为 $[0,-g_0]^T$ 。第一杆角 $q_1$ 从水平正轴逆时针量，第二杆角 $q_2$ 相对第一杆量。因此第二杆的绝对角是 $q_1+q_2$ 。

参数	意义	单位
$l_1,c_1,c_2$	第一杆长度、第一/第二杆质心到相应关节的距离	m
$m_1,m_2$	两杆质量	kg
$I_1,I_2$	各杆绕自身质心、垂直运动平面的转动惯量	kg·m²
$q_i,\dot q_i,\ddot q_i$	关节角、角速度、角加速度	rad、rad/s、rad/s²
$\tau_i$	作用在关节上的驱动力矩	N·m

本课刚体、平面、固定基座；不含柔性和齿隙。杆长不是质心距离，不能直接把 $c_i$ 当 $l_i$ ；也不能把绕关节惯量和绕质心惯量重复相加。

2. 为什么动力学要先做一遍运动学

两个质心的位置是

r_1=\begin{bmatrix}c_1\cos q_1\\c_1\sin q_1\end{bmatrix},\quad r_2=\begin{bmatrix}l_1\cos q_1+c_2\cos(q_1+q_2)\\l_1\sin q_1+c_2\sin(q_1+q_2)\end{bmatrix}.

对时间求导，用 $d\cos q/dt=-\sin q\dot q$ 。第一杆速度平方为 $\|\dot r_1\|^2=c_1^2\dot q_1^2$ 。第二杆两段速度相加，平方展开中的交叉项由两段切向方向夹角 $q_2$ 给出：

\|\dot r_2\|^2=l_1^2\dot q_1^2+c_2^2(\dot q_1+\dot q_2)^2 +2l_1c_2\cos q_2\,\dot q_1(\dot q_1+\dot q_2).

这个交叉项就是关节耦合的来源之一。若忽略它，就等于假装第二杆质心的两个运动分量互不影响。

3. 把动能整理成惯性矩阵

每杆动能等于质心平动能加绕质心转动能：

T=\frac12m_1\|\dot r_1\|^2+\frac12I_1\dot q_1^2+ \frac12m_2\|\dot r_2\|^2+\frac12I_2(\dot q_1+\dot q_2)^2.

定义三个常量以缩短代数：

a=I_1+I_2+m_1c_1^2+m_2(l_1^2+c_2^2),\quad \beta=m_2l_1c_2,\quad d=I_2+m_2c_2^2.

三者单位都是 kg·m²。把同类项归并成 $T=\tfrac12\dot q^TM(q)\dot q$ ，便读出

M(q)=\begin{bmatrix} a+2\beta\cos q_2 & d+\beta\cos q_2\\ d+\beta\cos q_2 & d \end{bmatrix}.

对称的两个非对角元并不是巧合：二次型中的交叉速度项要求一致的系数。 $M_{12}$ 表明第二关节加速度会影响第一关节所需力矩。 $M$ 随 $q_2$ 变化，意味着机械臂伸直与折叠时的“惯性负担”不同。

4. 势能决定重力补偿，不需要凭直觉猜

取基座高度为零，重力势能为

U=m_1g_0c_1\sin q_1+m_2g_0[l_1\sin q_1+c_2\sin(q_1+q_2)].

因为角度从水平轴量，势能含正弦，对角度求偏导后重力项含余弦：

g(q)=\begin{bmatrix} (m_1c_1+m_2l_1)g_0\cos q_1+m_2c_2g_0\cos(q_1+q_2)\\ m_2c_2g_0\cos(q_1+q_2) \end{bmatrix}.

方程左侧的 $g$ 是为了抵消重力所需的广义力，不是实际重力本身的符号。当两杆水平伸向右边时，两个 $g_i$ 都为正，电机要施加逆时针力矩防止下落。两杆竖直时相应力臂为零，这是一个立即可做的符号检查。

5. 速度乘积项从哪来

以第一关节为例：

\frac{\partial T}{\partial\dot q_1}=M_{11}\dot q_1+M_{12}\dot q_2.

对时间求导不仅要微分速度，还要微分随 $q_2$ 变化的 $M$ ：

\frac{d}{dt}\frac{\partial T}{\partial\dot q_1} =M_{11}\ddot q_1+M_{12}\ddot q_2 -2\beta\sin q_2\dot q_1\dot q_2-\beta\sin q_2\dot q_2^2.

这里 $T$ 不显含 $q_1$ ，所以 $\partial T/\partial q_1=0$ 。第二关节需要再减去 $\partial T/\partial q_2$ 。令 $h=\beta\sin q_2$ ，把两个容易漏掉的混合项分别写出：

\begin{aligned} \frac{d}{dt}\frac{\partial T}{\partial\dot q_2} &=M_{12}\ddot q_1+M_{22}\ddot q_2-h\dot q_1\dot q_2,\\ \frac{\partial T}{\partial q_2}&=-h(\dot q_1^2+\dot q_1\dot q_2). \end{aligned}

两者相减时， $-h\dot q_1\dot q_2$ 与 $+h\dot q_1\dot q_2$ 抵消，速度项只剩 $+h\dot q_1^2$ 。连同第一关节，整理得

c(q,\dot q)=\begin{bmatrix} -\beta\sin q_2(2\dot q_1\dot q_2+\dot q_2^2)\\ \beta\sin q_2\dot q_1^2 \end{bmatrix}.

于是完整方程是 $M\ddot q+c+g=\tau$ 。 $c$ 用 N·m；它不含加速度，但含速度乘积，来源是运动坐标与连杆耦合。

把 $c=C\dot q$ 写成矩阵时，一种方便的选择为

C=\begin{bmatrix} -h\dot q_2&-h(\dot q_1+\dot q_2)\\ h\dot q_1&0 \end{bmatrix},\qquad h=\beta\sin q_2.

$C$ 的表达不唯一，物理上确定的是 $C\dot q$ 。 为能量分析，常选满足 $\dot M-2C$ 反对称的形式。本例

\dot M-2C=\begin{bmatrix}0&h(2\dot q_1+\dot q_2)\\-h(2\dot q_1+\dot q_2)&0\end{bmatrix}.

任意实向量 $z$ 满足 $z^T(\dot M-2C)z=0$ 。这说明某些速度耦合只在自由度间交换能量，而不是凭空产生能量。

6. 把电机与摩擦接进来

简单关节摩擦可建成 $f_i=b_i\dot q_i+\tau_{c,i}\tanh(\dot q_i/\epsilon)$ 。 $b_i$ 是黏性摩擦系数， $\tau_{c,i}$ 是近似库仑摩擦力矩， $\epsilon$ 是小速度尺度。双曲正切是数值平滑近似，并不能真实表示所有静摩擦滞回。

若输入是电机电压，还需要

L_e\dot i=V-R_e i-k_e\omega_m,\qquad \tau_j\approx\eta N k_t i.

$i$ 为电流 A， $V$ 为电压 V， $L_e$ 为电感 H， $R_e$ 为电阻 Ω， $k_e\omega_m$ 是反电动势， $k_ti$ 是电机力矩， $N=\omega_m/\dot q$ 是减速比， $\eta$ 是传动效率。不同能流方向的效率可能不同。

若驱动器内电流环足够快，可把关节力矩当近似输入；否则应将电流增广为状态。不能在控制律里请求无限带宽、无限力矩，再用“理论稳定”解释实机震荡。

7. 为什么会想到计算力矩控制

目标是跟踪 $q_d(t)$ 。定义误差 $e=q-q_d$ ， $\dot e=\dot q-\dot q_d$ 。先设计一个容易稳定的虚拟加速度

a_c=\ddot q_d-K_d\dot e-K_pe.

$K_p$ 的单位为 $\mathrm{s^{-2}}$ ， $K_d$ 为 $\mathrm{s^{-1}}$ ，每项都是角加速度。然后用模型把“我希望的加速度”转换成实际力矩：

\tau=\hat M(q)a_c+\hat c(q,\dot q)+\hat g(q)+\hat f(\dot q).

为什么使用当前 $q,\dot q$ 计算补偿？因为真实系统此时的重力、惯性、摩擦由当前状态决定，而不是由期望状态决定。参考加速度则来自轨迹规划器。

若模型精确、测量准确、力矩能实现，代回动力学得到 $\ddot q=a_c$ ，因此

\ddot e+K_d\dot e+K_pe=0.

非线性耦合系统被变成每轴可设计的二阶误差系统。取对角正定增益，每轴特征方程是 $s^2+k_ds+k_p=0$ ；选 $k_p=\omega_n^2$ 、 $k_d=2\zeta\omega_n$ ，即可由自然频率和阻尼比设计响应。

Lyapunov 证明不是写一个负号就结束

对误差系统取

V_e=\frac12\dot e^T\dot e+\frac12e^TK_pe.

它不是实际机械能，而是误差的正定度量；量纲可用统一尺度归一化。对时间求导并代入误差方程：

\dot V_e=\dot e^T\ddot e+\dot e^TK_pe=-\dot e^TK_d\dot e\le0.

仅得到半负定，因为瞬间 $\dot e=0$ 时导数为零。还需检查不变集：若一直 $\dot e=0$ ，则 $\ddot e=-K_pe=0$ ，正定 $K_p$ 迫使 $e=0$ 。由不变集论证得到理想误差系统渐近收敛；等价地也可由线性特征根证明指数稳定。不能把这份证明直接套到有饱和、延迟、错误模型的系统。

8. 更稳健的起点：PD 加重力补偿

只保持固定构型时，可先用

\tau=g(q)-K_{p\tau}e-K_{d\tau}\dot q,\qquad e=q-q_d.

这里 $K_{p\tau},K_{d\tau}$ 为对称正定的力矩反馈增益，单位分别为 N·m/rad、N·m·s/rad，不能直接照搬上一节虚拟加速度增益的数值。设重力模型准确、摩擦耗散、目标固定，取实际动能加误差弹性势能

V=\frac12\dot q^TM(q)\dot q+\frac12e^TK_{p\tau}e.

利用上面的反对称性质，可推得 $\dot V=-\dot q^TK_{d\tau}\dot q-\dot q^Tf(\dot q)\le0$ 。它不需要精确抵消所有惯性和速度项，因而是理解“利用物理结构设计控制”的好入口。角度多圈与关节限位需明确，不能把欧氏误差的全局证明等同于所有姿态参数空间上的全局稳定。

模型不准时，计算力矩误差会表现为剩余广义力 $\Delta$ 。若能证明其有界，可研究输入到状态稳定性或最终有界，而不是继续声称零误差。积分、扰动观测器、自适应控制都应从这份剩余误差的结构出发设计。

9. 数值练习与 C++ 核心

设 $m_1=m_2=1$ kg， $l_1=1$ m， $c_1=c_2=0.5$ m， $I_1=I_2=1/12$ kg·m²。在 $q_1=q_2=0$ 、速度为零时，先计算 $M$ 和 $g$ ，再求实现 $[1,0]^T$ rad/s² 所需力矩。

展开完整计算与可编译测试

有 $a=5/3$ 、 $\beta=1/2$ 、 $d=1/3$ ，所以

M=\begin{bmatrix}8/3&5/6\\5/6&1/3\end{bmatrix},\qquad g=[19.62,4.905]^T\ \mathrm{N\,m}.

因此需求力矩为 $[22.2866667,5.7383333]^T$ N·m。第二关节期望加速度为零仍需额外力矩，因为它需要抵消第一关节加速对它的耦合作用。

#include <array>
#include <cmath>
#include <cassert>
using V2=std::array<double,2>;
struct Arm { double m11,m12,m22; V2 c,g; };
Arm model(double q1,double q2,double v1,double v2) {
    const double a=5.0/3, beta=0.5,d=1.0/3,h=beta*std::sin(q2);
    return {a+2*beta*std::cos(q2),d+beta*std::cos(q2),d,
      {-h*(2*v1*v2+v2*v2),h*v1*v1},
      {1.5*9.81*std::cos(q1)+0.5*9.81*std::cos(q1+q2),
       0.5*9.81*std::cos(q1+q2)}};
}
V2 inverse(const Arm& a,V2 dd) {
    return {a.m11*dd[0]+a.m12*dd[1]+a.c[0]+a.g[0],
            a.m12*dd[0]+a.m22*dd[1]+a.c[1]+a.g[1]};
}
V2 forward(const Arm& a,V2 tau) {
    const double d=a.m11*a.m22-a.m12*a.m12;
    assert(d>0);
    const double b0=tau[0]-a.c[0]-a.g[0],b1=tau[1]-a.c[1]-a.g[1];
    return {(a.m22*b0-a.m12*b1)/d,(-a.m12*b0+a.m11*b1)/d};
}
int main() {
    Arm a=model(0,0,0,0); auto t=inverse(a,{1,0});
    assert(std::abs(t[0]-(19.62+8.0/3))<1e-12);
    assert(std::abs(t[1]-(4.905+5.0/6))<1e-12);
    for(double q2: {-2.0,-0.4,0.0,1.2,3.0}) {
        auto m=model(0.7,q2,0.3,-0.6);
        assert(m.m11>0 && m.m11*m.m22>m.m12*m.m12);
        auto r=forward(m,inverse(m,{0.2,-0.8}));
        assert(std::abs(r[0]-0.2)<1e-10 && std::abs(r[1]+0.8)<1e-10);
    }
}

上述二阶显式解只为手算小例子，规模增大应使用稳定线性求解器。继续练习可做PID 在线题，再把输出从归一化指令改成带单位的力矩，并分析饱和后的积分处理。

来源与边界

两连杆参数与数例为本课自选，逐项推导不复制教材段落。动力学结构对照 Modern Robotics 8.1，计算力矩的模型补偿结构对照作者官方 11.4 课程。进一步做扰动估计可接扰动观测器，但不能省略力矩实现、测量同步和可观测性检查。