四足机器人MPC控制与浮动基座动力学建模

——用 60 个公式进行完整推导

目录

01    MPC:用模型预测,用优化决定控制

预测时域与控制时域,分别表示什么?

优化器怎样选择这一段控制?

为什么每次只执行第一项?

02    从牛顿定律到浮动基座动力学

再看浮动基座:机身和关节一起运动

从每个材料块的牛顿方程开始

单摆的位置、速度与加速度

将单摆的牛顿方程投影到摆角方向

用虚位移表达同一次受力投影

把材料块的位置写成机身与关节的函数

把机身转动也写成速度系数

把三组速度系数合成雅克比

展开雅克比的导数,分清加速度项与速度项

把各材料块的牛顿方程投影到同一组方向

直接从牛顿惯性项收集出M和C

把重力换算到整机坐标

由关节电机的虚功得到选择矩阵

加入接触力,得到完整的速度形式

先看清位移向量,再写材料块速度

把一根杆内的材料块合并,得到式(57)

先明确三段位移与两组速度系数

展开两种点速度,推导关节列的关系

代回关节列关系,把两种M写成相同矩阵

退回单摆,核对全部系数

03    一条被冠军验证过的工程链路


这篇文章围绕两个问题展开:

一是 MPC 如何用模型预测、用优化决定控制?

二是被用来预测的模型:四足机器人的浮动基座动力学,是如何从牛顿定律一步步推出来?

内容整理自深蓝学院《四足机器人:从动力学建模到强化学习》课程学员笔记。

01    MPC:用模型预测,用优化决定控制

MPC 在每个采样时刻,利用当前估计状态和系统模型,计算未来一段控制输入,使预测输出尽量跟随参考,并满足约束。

控制器只执行这段输入的第一项,再根据新的反馈重新计算。

预测时域与控制时域,分别表示什么?

预测时域 p:向前预测多少步输出;

控制时域 m:允许独立调整多少步控制输入,通常 m ≤ p。

采用常见的设置时 ,m 步之后的输入保持不变,但输出仍预测到第 p 步。每步间隔为采样周期 T s 。

参考轨迹是希望达到的输出;预测输出则由模型根据待求控制序列算出。优化器调整控制序列,进而改变预测输出。

图中橙色首项用于当前执行,其余部分用于评估未来。

优化器怎样选择这一段控制?

$\min_{\Delta U} J = w_y \sum_{i=1}^{p} \|\hat{y}(k+i|k)-r(k+i)\|^2 + w_{\Delta u}\sum_{i=0}^{m-1} \|\Delta u(k+i|k)\|^2$

ΔU 表示待求的控制增量序列。第一项衡量预测输出 ŷ 与参考 r 的偏差;第二项衡量相邻控制输入的变化 Δu。

两个权重分别决定对跟踪精度与控制平滑性的重视程度。

优化还必须遵守模型关系,以及输入、输入变化率和输出的边界。

这一基本形式在采用线性模型和线性约束时,可由二次规划(QP)求解;不需要预先枚举所有动作序列。

为什么每次只执行第一项?

经过一个采样周期后,实际输出可能因扰动和模型误差偏离预测。此时更新状态估计,将预测时域向前移动一步,再计算新的最优序列。

这种“预测—优化—执行首项—反馈更新”的重复过程,就是滚动时域控制。

MPC 位于反馈控制器内部,动力学模型用于预测施加控制后的运动。后文推导的浮动基座动力学,为建立机器人的预测模型提供基础。

02    从牛顿定律到浮动基座动力学

先看单摆:一个摆角的运动方程

无质量杆带摆球绕固定轴转动,转轴无摩擦;

惯性力矩+重力项=电机力矩:

$ml^2\ddot{\theta} + mgl\sin\theta = \tau$                        (1)

  • $m,\ l,\ g$  m为球质量(kg),l为杆长(m);g为重力加速度大小,约9.81 m/s²。
  • $\theta,\ \ddot{\theta}$  θ为摆角,从竖直向下起算,向右为正;两点表示角加速度。
  • $\tau$   电机沿摆角增加方向施加的力矩;本式各项均为N·m。

再看浮动基座:机身和关节一起运动

机身没有固定在地面,平移、转动与关节转动统一写成:

$M(q)\dot{v} + C(q,v)v + G(q) = S^\top \tau + \sum_{i} J_{c,i}^\top f_c^{(i)} $              (2)

  • $q,\ v,\ \dot{v}$  q记录机身位置、姿态和关节角;v由平移速度、机身角速度和关节速度组成;一点 表示对时间求导。
  • $M(q),\ C(q,v)$  M是质量矩阵;C乘以整机速度v后,给出科氏、离心等速度相关项。
  • $G(q)$  按整机坐标表示、放在左侧的重力项,对应课件的g(q)。它是向量,与单摆式中的加速度g不同。
  • $T,\ S^\mathrm{T}$ τ在这里是各关节电机力矩的向量;S是关节坐标选择矩阵,S转置后把力矩填入整机对应位置。
  • $f_c^{(i)},\ J_{c,i}$  f是地面对第i个接触点的力;J是位置雅克比,将整机坐标变化换成该点位移。J转置乘f,把接触力换算到整机坐标。
  • $i,\ \sum_i,\ T$  i编号各接触点;求和号将各点作用相加;上标T表示矩阵转置。

从每个材料块的牛顿方程开始

$m_a \ddot{r}_a = F_a. $             (3)

下标$a$编号材料块;$m_a$是质量,$r_a$是位置,$\ddot{r}_a$是加速度,$F_a$是合力。

后文对刚体各材料块求和,保留平动和转动惯性。

单摆的位置、速度与加速度

将支点作为原点,在摆动平面内采用水平 、竖直 坐标。摆球的位置由摆角唯一确定:

$r(\theta) = \begin{bmatrix} l\sin\theta \\ -l\cos\theta \end{bmatrix}. $            (4)

对时间求一次导数,使用复合函数求导法则。

把位置对角度的导数记为$J(\theta)$,它把角速度换算为摆球的线速度:

$J(\theta)=\frac{\partial r}{\partial \theta}=\begin{bmatrix} l\cos\theta \\ l\sin\theta \end{bmatrix},\quad \dot{r}=J\dot{\theta}.$       (5)

速度不仅取决于摆角变化的快慢,还取决于当前位置。

因此再次求导时,$J$与$\dot{\theta}$两部分都需要求导:

$\ddot{r}=J\ddot{\theta}+\dot{J}\dot{\theta}. $       (6)

$\dot{J}=\begin{bmatrix} -l\sin\theta \\ l\cos\theta \end{bmatrix}\dot{\theta},\quad \ddot{r}=\begin{bmatrix} l\cos\theta \\ l\sin\theta \end{bmatrix}\ddot{\theta}+\begin{bmatrix} -l\sin\theta \\ l\cos\theta \end{bmatrix}\dot{\theta}^2. $       (7)

定义沿杆向外的单位向量$e_r$和沿角度增加方向的切向单位向量$e_t$:

$e_r=\begin{bmatrix} \sin\theta \\ -\cos\theta \end{bmatrix},\quad e_t=\begin{bmatrix} \cos\theta \\ \sin\theta \end{bmatrix},\quad e_r^\top e_t=0.$      (8)

$J = l e_t,\quad \ddot{r}=l e_t\ddot{\theta}-l e_r\dot{\theta}^2. $         (9)

第一项沿切线,来自角速度大小的变化;第二项沿杆指向支点,来自速度方向的变化。

$e_r,e_t$分别是径向、切向单位向量,彼此垂直。

将单摆的牛顿方程投影到摆角方向

沿杆的被动轴向力记为$F_T = -T e_r$,重力向下。电机力矩在杆端等效为切向力$\tau/l$,因为“切向力乘杆长”等于力矩。

摆球的牛顿方程是:

$m\ddot{r}=F^{\text{grav}}+F_T+\frac{\tau}{l}e_t,\quad F^{\text{grav}}=\begin{bmatrix}0\\-mg\end{bmatrix}. $          (10)

先只看切线方向。轴向力没有切向分量,切向加速度为$l\ddot{\theta}$,所以:

$\frac{\tau}{l}-mg\sin\theta=ml\ddot{\theta}. $     (11)

这行两边都是N。两边同乘$l$,再把重力项移到左边,所有项就都变为$\text{N}\cdot\text{m}$:

$ml^2\ddot{\theta}+mgl\sin\theta=\tau. $      (12)

将这个投影写成多连杆也能使用的形式。

式(12)已经给出单摆的动力学方程。

接下来要推广到整机的多个坐标,因此把“取切向分量、乘杆长”写成统一形式,便于对各连杆的材料块求和。求和时,理想被动关节连接力的虚功会成对相消,便不必逐一求这些内部未知力。

这就是引入 “虚位移” 与 “虚功” 的目的。

固定当前时刻,想象摆角作一个任意微小变化$\delta\theta$,杆长保持不变。摆球位置的一阶变化记为$\delta r$,称为虚位移。它不要求真实发生,也不指定经过多长时间。

下面先从位置关系求出它。

用虚位移表达同一次受力投影

先对摆球的两个位置分量求一阶变化。微小转角引起的位置变化,等于相应变化率乘这个转角:

$x = l\sin\theta \implies \delta x = l\cos\theta\,\delta\theta,$
$z = -l\cos\theta \implies \delta z = l\sin\theta\,\delta\theta. $    (13)

把这两个变化率排成一列,记为位置雅可比$J$,便得到:

$\delta r = \begin{bmatrix}\delta x \\ \delta z\end{bmatrix} = \underbrace{\begin{bmatrix}l\cos\theta \\ l\sin\theta\end{bmatrix}}_{J(\theta)=\partial r/\partial\theta} \delta\theta = l e_t \delta\theta. $       (14)

$J$与前面速度式中的相同。因为$J = l e_t$,$J^\top$作用在力上,就相当于先取切向分量、再乘杆长。


任一力$\boldsymbol{F}$对这次假想位移的功记为$\delta W = \boldsymbol{F}^\top \delta r$,称为虚功。把牛顿方程移到同一侧,再与$\delta r$点乘,得到:

$\left(\boldsymbol{F}^{\text{grav}} + \boldsymbol{F}_T + \frac{\tau}{l}\boldsymbol{e}_t - m\ddot{\boldsymbol{r}}\right)^\top \delta r = 0. $   (15)

代入$\delta r = l e_t \delta\theta$。杆力沿杆,与虚位移垂直,虚功为0;余下电机、重力和惯性三项:

$\left(\tau - mgl\sin\theta - ml^2\ddot{\theta}\right)\delta\theta = 0. $    (16)

$\delta\theta$可以任取,所以括号必须为0,恰好回到式(12)。零来自牛顿定律,实际力与惯性项始终一起投影。

下一步把一个摆角换成整机坐标:先求各材料块的位置与雅克比,再用同一组坐标变化投影并求和。

后面的浮动基座推导就沿这条路展开。

把材料块的位置写成机身与关节的函数

机身、腿都由材料组成。

把它们分成小质量块,对每块使用牛顿定律。腿上的材料块相对机身的位置,随关节角改变。

第$a$块质量记为$m_a$,机身坐标下的位置记为$\rho_a(\theta)$。

$p_B$是机身原点的世界位置;$R_B$把机身坐标分量转到世界系。材料块的世界位置因此是:

$r_a = p_B + R_B\rho_a(\theta). $  (17)

对时间求导,两份相乘的因子都要求导:

$\dot{r}_a = \dot{p}_B + \dot{R}_B\rho_a + R_B\dot{\rho}_a. $    (18)

相对位置通过12个关节角随时间变化。先逐项求导,再把各个系数排成一张表:

$\dot{\rho}_a = \frac{\partial \rho_a}{\partial \theta_1}\dot{\theta}_1 + \dots + \frac{\partial \rho_a}{\partial \theta_{12}}\dot{\theta}_{12}. $      (19)

$K_a = \begin{bmatrix} \frac{\partial \rho_a}{\partial \theta_1} & \dots & \frac{\partial \rho_a}{\partial \theta_{12}} \end{bmatrix},\quad \dot{\rho}_a = K_a\dot{\theta}. $     (20)

$K$有3行、12列;每列是某一个关节角前面的三维系数。不带动该材料块的关节,对应列为0。

把机身转动也写成速度系数

位置求导后,关节部分已经写成K乘关节速度。现在处理机身转动产生的那一项。

机身角速度记为$\omega_B$,用世界坐标表示。只考虑机身转动时,点的线速度就是“角速度叉乘相对位置”:

$\dot{R}_B\rho_a = \omega_B \times (R_B\rho_a). $     (21)

把世界系中的相对位置记为$s_a = R_B\rho_a$。将刚才的关节速度关系一起代回位置导数:

$\dot{r}_a = \dot{p}_B + \omega_B \times s_a + R_B K_a \dot{\theta}. $     (22)

三项依次来自机身平移、机身转动、关节运动。$s$始终是从机身原点指向材料块的世界系向量。

$q = (p_B,R_B,\theta),\quad v = \begin{bmatrix}\dot{p}_B \\ \omega_B \\ \dot{\theta}\end{bmatrix} \in \mathbb{R}^{18}. $      (23)

$q$记录机身位置、姿态和关节角。$v$的中间三项直接使用机身角速度。后面的$\delta q$表示对应18个运动方向的微小变化,其中机身转动用世界系微小转角表示。

把三组速度系数合成雅克比

先把叉乘写成矩阵乘法。下面的矩阵只由s的三个分量组成:

$[\boldsymbol{s}]_\times = \begin{bmatrix} 0 & -s_z & s_y \\ s_z & 0 & -s_x \\ -s_y & s_x & 0 \end{bmatrix},\quad [\boldsymbol{s}]_\times \boldsymbol{w} = \boldsymbol{s} \times \boldsymbol{w}. $        (24)

交换叉乘的顺序要变号,所以机身转动项的系数是负的叉乘矩阵:

$\omega_B \times s_a = -s_a \times \omega_B = -[s_a]_\times \omega_B.$       (25)

$I_3$是三维单位矩阵。把平移、转动、关节速度前面的系数,按$v$的相同顺序排在一行:

$\dot{r}_a = \underbrace{\begin{bmatrix}I_3 & -[s_a]_\times & R_B K_a\end{bmatrix}}_{J_a(q)} \begin{bmatrix}\dot{p}_B \\ \omega_B \\ \dot{\theta}\end{bmatrix} = J_a v. $        (26)

这就是雅可比:把整机18维速度换算为该材料块的3维速度。它来自刚才三个速度项的系数合并。
再求一次时间导数,两份因子都要求导:

$\ddot{r}_a = \frac{d}{dt}\left(J_a v\right) = J_a \dot{v} + \dot{J}_a v. $        (27)

展开雅克比的导数,分清加速度项与速度项

J也随运动变化。单位矩阵导数为0;另外两组系数逐项求导:

$\dot{J}_a = \begin{bmatrix} \boldsymbol{0} & -[\dot{\boldsymbol{s}}_a]_\times & \dot{R}_B K_a + R_B \dot{K}_a \end{bmatrix}. $    (28)

先展开$s$的导数,再展开$K$的各个元素的导数:

$\dot{s}_a = \dot{R}_B\rho_a + R_B\dot{\rho}_a = \omega_B \times s_a + R_B K_a \dot{\theta}. $      (29)

$(\dot{K}_a)_{ij} = \sum_{k=1}^{12}\frac{\partial (K_a)_{ij}}{\partial \theta_k}\dot{\theta}_k = \sum_{k=1}^{12}\frac{\partial^2 (\rho_a)_i}{\partial \theta_j \partial \theta_k}\dot{\theta}_k. $    (30)

将它们代入,再用刚才的$v$去乘,就得到:

$\begin{aligned} \dot{J}_a v &= \omega_B \times (\omega_B \times s_a) \\ &\quad + 2\omega_B \times (R_B K_a \dot{\theta}) + R_B \dot{K}_a \dot{\theta}. \end{aligned} $          (31)

系数2来自两次乘积求导中相同的一项。上式只含当前速度,不含待求加速度;加速度都在前一项$J_a \dot{v}$里。

相关阅读:聊聊雅可比矩阵,如何统一机器人控制的两条主线?

把各材料块的牛顿方程投影到同一组方向

每块都满足牛顿定律。将已经求出的加速度直接代入:

$m_a \ddot{r}_a = F_a \implies m_a J_a \dot{v} + m_a \dot{J}_a v = F_a. $     (32)

为了把每块的三维方程统一起来,固定当前时刻,给机身和关节一组微小变化。机身微小转角也用世界系表示:

$\delta q = \begin{bmatrix}\delta p_B \\ \delta \varphi_B \\ \delta \theta\end{bmatrix},\quad \delta r_a = J_a \delta q. $     (33)

像单摆一样,两边与虚位移点乘,再对全部材料块求和:

$\sum_a (m_a \ddot{r}_a)^\top \delta r_a = \sum_a F_a^\top \delta r_a. $     (34)

$\left(\sum_a J_a^\top m_a \ddot{r}_a\right)^\top \delta q = Q^\top \delta q. $     (35)

理想被动连接力在同一点反向作用,两侧虚位移相同,总虚功相消。重力、电机和地面作用保留,它们对这18个方向的总作用记为$Q$。

$\sum_a J_a^\top m_a \ddot{r}_a = Q. $  (36)

这里先保留未知地面力,不限制测试变化的接触方向;固定脚的条件最后联立加入。

因此可逐个比较18个方向的系数。

直接从牛顿惯性项收集出M和C

把加速度的两部分代入式(32)、(36)。各材料块使用同一个整机加速度和速度,分别提取出来:

$\begin{aligned} Q &= \sum_a J_a^\top m_a \left(J_a \dot{v} + \dot{J}_a v\right) \\ &= \left(\sum_a m_a J_a^\top J_a\right)\dot{v} + \left(\sum_a m_a J_a^\top \dot{J}_a\right)v. \end{aligned}$    (37)

把加速度前面的矩阵系数记为$M$;把第二组括号记为$C$:

$M(q) = \sum_a m_a J_a^\top J_a,\quad C(q,v) = \sum_a m_a J_a^\top \dot{J}_a. $        (38)

$M(q)\dot{v} + C(q,v)v = Q. $       (39)

$M$和$C$直接来自上面的牛顿方程:$M$收集加速度系数,$Cv$收集全部速度相关项。$C$可以有不同写法,运动方程使用其乘积$Cv$。

单摆只有一个坐标,$J$的一列是$le_t$,因此同一步会给出:

$M = mJ^\top J = ml^2 e_t^\top e_t = ml^2. $        (40)

整机只是把这一列扩成18列,并对所有材料块相加。

把重力换算到整机坐标

世界竖直方向向上;g是正的重力加速度大小。每块的实际重力直接写成向下的三维力:

$F_a^{\text{grav}} = \begin{bmatrix} 0 \\ 0 \\ -m_a g \end{bmatrix}. $       (41)

用刚才虚功关系得到的同一个力的换算系数,实际重力贡献为:

$\delta W_g = \sum_a \left(F_a^{\text{grav}}\right)^\top J_a \delta q = \left(\sum_a J_a^\top F_a^{\text{grav}}\right)^\top \delta q. $       (42)

把它移到动力学左侧,记成$G$:

$G(q) = -\sum_a J_a^\top F_a^{\text{grav}}.$      (43)

$M\dot{v} + Cv + G = Q_\tau + Q_c. $      (44)

此处大写$G$是18维的重力项;小写$g$始终是加速度数值。后两项分别表示电机与接触作用,接着逐项求出。

由关节电机的虚功得到选择矩阵

一个电机对关节两侧施加反向力矩。沿关节轴的两侧微小转动之差,就是关节相对转角变化:

$\delta W_{\tau,j} = \tau_j \delta\theta_j,\quad \delta W_\tau = \sum_{j=1}^{12}\tau_j \delta\theta_j = \tau^\top \delta\theta. $    (45)

$\delta q$的后12项就是关节角变化。定义$S$取出这部分:

$S = \begin{bmatrix}0_{12\times6} & I_{12}\end{bmatrix},\quad \delta\theta = S\delta q. $   (46)

代入电机虚功,把$\delta q$前面的系数收集起来:

$\delta W_\tau = \tau^\top S \delta q = \left(S^\top \tau\right)^\top \delta q. $    (47)

$Q_\tau = S^\top \tau = \begin{bmatrix}\boldsymbol{0}_6 \\ \tau\end{bmatrix}. $     (48)

前6项填零,后12项填入关节力矩。$S$转置只安排位置;电机力矩的数值和单位保持原样。

机身仍可通过与腿的惯性和接触耦合运动。

加入接触力,得到完整的速度形式

脚点位置与材料块位置形式相同,因此沿用已经完成的求导过程。Kc是脚点相对位置对12个关节角的系数表:

$r_{c,i} = p_B + R_B \rho_{c,i},\quad K_{c,i} = \frac{\partial \rho_{c,i}}{\partial \theta},\quad J_{c,i} = \begin{bmatrix}I_3 & -[R_B\rho_{c,i}]_\times & R_B K_{c,i}\end{bmatrix}. $    (49)

地面力乘以脚点虚位移,再按$\delta q$收集系数:

$\delta W_{c,i} = (f_c^{(i)})^\top J_{c,i}\delta q = \left(J_{c,i}^\top f_c^{(i)}\right)^\top \delta q,\quad Q_c = \sum_i J_{c,i}^\top f_c^{(i)}. $     (50)

$M(q)\dot{v}+C(q,v)v+G(q) = S^\top \tau + \sum_i J_{c,i}^\top f_c^{(i)}. $     (51)

固定支撑时,脚点真实速度为0。再求导一次,就得到与受力方程联立的接触条件:

$J_c v = 0,\quad J_c \dot{v} + \dot{J}_c v = 0. $      (52)

将各只脚的雅可比和地面力按相同顺序竖着叠起来,统一记为$J_c$、$f_c$:

$J_c = \begin{bmatrix}J_{c,1}\\\vdots\\J_{c,n_c}\end{bmatrix},\quad f_c = \begin{bmatrix}f_c^{(1)}\\\vdots\\f_c^{(n_c)}\end{bmatrix}.$

把动力学中的接触力移到左侧,与式 (52) 的加速度约束一起排成矩阵:

$\begin{bmatrix}M & -J_c^\top \\ J_c & \boldsymbol{0}\end{bmatrix} \begin{bmatrix}\dot{v}\\f_c\end{bmatrix} = \begin{bmatrix}S^\top \tau - Cv - G \\ -\dot{J}_c v\end{bmatrix}.$

上半行是受力方程,下半行是固定接触约束。给定q、v和电机力矩τ,联立求出整机加速度与地面接触力。

先看清位移向量,再写材料块速度

以下均用世界系表示。

$r_{\text{com},b}$是第$b$根连杆的质心位置,com是质心(center of mass)的缩写。材料块$a$相对质心的位置是:

$d_a = r_a - r_{\text{com},b},\quad r_a = r_{\text{com},b} + d_a.$

$d_a$有方向,$\|d_a\|$才是长度,单位为$\text{m}$。同一刚体上,这个长度不变,方向会随杆转动;它也不同于计算力矩时到力作用线的垂直力臂。

点号表示对时间求导。用$J_{p,b}$、$J_{\omega,b}$分别收集质心线速度、连杆角速度对整机速度$v$的系数:

$\dot{r}_{\text{com},b} = J_{p,b}v,\quad \omega_b = J_{\omega,b}v. $    (53)

同一刚体内,$d_a$随杆转动,所以$\dot{d}_a = \omega_b \times d_a$。对$r_a = r_{\text{com},b} + d_a$求导,材料块速度就是:

$\dot{r}_a = \dot{r}_{\text{com},b} + \omega_b \times d_a = \big(J_{p,b} - [d_a]_\times J_{\omega,b}\big)v. $      (54)

式(54)与前面的$\dot{r}_a = J_a v$描述同一个点。比较$v$前的系数,得到:

$J_a = J_{p,b} - [d_a]_\times J_{\omega,b}.$

把这份$J_a$代入式(38),先只对连杆$b$内的材料块求和:

$M_b = \sum_{a\in b} m_a \big(J_{p,b} - [d_a]_\times J_{\omega,b}\big)^\top \big(J_{p,b} - [d_a]_\times J_{\omega,b}\big). $

把一根杆内的材料块合并,得到式(57)

接下来只整理式(55)的求和。

总质量为$m_b=\sum_{a\in b}m_a$;质心和惯量的定义给出:

$\sum_{a\in b} m_a d_a = 0,\quad I_b = \sum_{a\in b} m_a [d_a]_\times^\top [d_a]_\times = \sum_{a\in b} m_a\big(\|d_a\|^2 I_3 - d_a d_a^\top\big). $    (56)

叉乘矩阵的每个元素都与位移分量成正比。因此,加权位移之和为0,也使加权叉乘矩阵之和为0:

$\sum_{a\in b} m_a [d_a]_\times = \left[\sum_{a\in b} m_a d_a\right]_\times = 0.$

将式(55)的两个括号展开成四项。同一根杆的$J_{p,b}$和$J_{\omega,b}$对所有材料块相同,可以提到求和号外;再用式(56)整理:

$ \begin{aligned} M_b &= m_b J_{p,b}^\top J_{p,b} + J_{\omega,b}^\top I_b J_{\omega,b} \\ &\quad - J_{p,b}^\top \underbrace{\left(\sum_{a\in b} m_a [d_a]_\times\right)}_{0} J_{\omega,b} \\ &\quad - J_{\omega,b}^\top \underbrace{\left(\sum_{a\in b} m_a [d_a]_\times^\top\right)}_{0} J_{p,b}. \end{aligned} $

后两项为0,留下质心平移项和绕质心转动项。再对所有连杆$b$求和:

$M(q) = \sum_b \big(m_b J_{p,b}^\top J_{p,b} + J_{\omega,b}^\top I_b J_{\omega,b}\big). $     (57)

式(55)使用的就是原来式(38)的材料块求和。式(57)只是先把同一根杆内的材料块合并,再对各杆求和,所以得到同一个M。

下面再把两种写法的矩阵直接对齐。

先明确三段位移与两组速度系数

先区分三段位移:$s_a$从机身原点到材料块,$s_b$从机身原点到连杆质心,$d_a$从质心到材料块。

下面把原有质心雅克比和角速度雅克比的后12列,分别简记为$P_b$和$W_b$。用前面已有的$S$取出这部分:

$P_b = J_{p,b}S^\top \in \mathbb{R}^{3\times 12},\quad W_b = J_{\omega,b}S^\top \in \mathbb{R}^{3\times 12}.$

$P_b\dot{\theta}$是关节运动带来的质心线速度,$W_b\dot{\theta}$是关节运动带来的连杆角速度。加上机身运动,便有:

$ \begin{aligned} \dot{r}_{\text{com},b} &= \dot{p}_B + \omega_B \times s_b + P_b\dot{\theta},\\ \omega_b &= \omega_B + W_b\dot{\theta}. \end{aligned} $

$W_b$的第$j$列是关节j的转轴在世界系中的单位方向;该关节不带动这根杆时,该列为0。$W_b$是系数矩阵,$\omega_b$才是实际角速度。

按$v=(\dot{p}_B,\omega_B,\dot{\theta})$的顺序收集上面两式的系数:

$J_{p,b} = \big[I_3\quad -[s_b]_\times \quad P_b\big],\quad J_{\omega,b} = \big[0\quad I_3 \quad W_b\big].$

需要对照的旧矩阵,是把式(26)的$J_a$直接代入式(38)得到的:

$ M(q) = \sum_{a} m_a \begin{bmatrix} I_3 & -[s_a]_\times & R_B K_a \\ [s_a]_\times & [s_a]_\times^\top [s_a]_\times & [s_a]_\times R_B K_a \\ K_a^\top R_B^\top & -K_a^\top R_B^\top [s_a]_\times & K_a^\top K_a \end{bmatrix}. $

展开两种点速度,推导关节列的关系

先看第一种写法:由式(26),直接用材料块的速度系数计算:

$\dot{r}_a = \dot{p}_B + \omega_B \times s_a + R_B K_a \dot{\theta}.$

再看第二种写法:按式(54),材料块速度等于质心平移速度加上绕质心转动的速度。把上一页的质心速度和连杆角速度代入:

$ \begin{aligned} \dot{r}_a &= \dot{r}_{\text{com},b} + \omega_b \times d_a \\ &= \big(\dot{p}_B + \omega_B \times s_b + P_b\dot{\theta}\big) + \big(\omega_B + W_b\dot{\theta}\big) \times d_a \\ &= \dot{p}_B + \omega_B \times (s_b + d_a) + P_b\dot{\theta} + (W_b\dot{\theta}) \times d_a. \end{aligned} $

位置关系给出$s_b + d_a = s_a$。接着处理最后一项:叉乘交换顺序要变号,所以:

$(W_b\dot{\theta}) \times d_a = -d_a \times (W_b\dot{\theta}) = -[d_a]_\times W_b\dot{\theta}.$

第二种写法因此整理成:

$\dot{r}_a = \dot{p}_B + \omega_B \times s_a + \big(P_b - [d_a]_\times W_b\big)\dot{\theta}.$

两种写法描述同一个点。减去相同的机身平移项和机身转动项,得到:

$R_B K_a \dot{\theta} = \big(P_b - [d_a]_\times W_b\big)\dot{\theta}.$

这个等式对任意关节速度都成立。

比较系数时,依次让一个关节速度取$1\ \text{rad/s}$、其余取0,矩阵乘积取出的就是对应的那一列。对12个关节逐列比较,得到:

$R_B K_a = P_b - [d_a]_\times W_b.$

这里比较的是每个关节角速度前的系数;仅凭某一次运动的点速度相同,不能得出整个矩阵相同。

代回关节列关系,把两种M写成相同矩阵

现在把上面证明的关系代回式(54)的速度系数,逐列整理:

$ \begin{aligned} J_a &= J_{p,b} - [d_a]_\times J_{\omega,b} \\ &= \big[I_3 \quad -[s_b]_\times \quad P_b\big] - [d_a]_\times \big[0 \quad I_3 \quad W_b\big] \\ &= \big[I_3 \quad -[s_b+d_a]_\times \quad P_b - [d_a]_\times W_b\big] \\ &= \big[I_3 \quad -[s_a]_\times \quad R_B K_a\big]. \end{aligned} $

第三行到第四行用了$s_a = s_b + d_a$和刚证明的$R_B K_a = P_b - [d_a]_\times W_b$,结果正是式(26)。

所以,两条路线从一开始用的就是相同的$J_a$。

将旧矩阵按连杆分组,再用式(56)合并材料块,得到:

$ M(q) = \sum_{b} \begin{bmatrix} m_b I_3 & -m_b [s_b]_\times & m_b P_b \\ m_b [s_b]_\times & m_b [s_b]_\times^\top [s_b]_\times + I_b & m_b [s_b]_\times P_b + I_b W_b \\ m_b P_b^\top & -m_b P_b^\top [s_b]_\times + W_b^\top I_b & m_b P_b^\top P_b + W_b^\top I_b W_b \end{bmatrix}. $

将式(57)中的$J_{p,b}$和$J_{\omega,b}$直接按列相乘,也正好得到上面这个矩阵。

为看清如何合并,以右下角(分块矩阵M(q)的第三行第三列) “关节与关节” 分块为例:

$ \begin{aligned} \sum_{a\in b} m_a K_a^\top K_a &= \sum_{a\in b} m_a (R_B K_a)^\top (R_B K_a) \\ &= \sum_{a\in b} m_a \big(P_b - [d_a]_\times W_b\big)^\top \big(P_b - [d_a]_\times W_b\big) \\ &= m_b P_b^\top P_b + W_b^\top I_b W_b. \end{aligned} $

第一行用$R_B^\top R_B = I_3$;最后一行展开后,两项交叉项由$\sum_{a\in b} m_a d_a = 0$消去,平方项按式(56)合成$I_b$。

其余分块使用同样的两条关系。

因此,“材料块形式的M”和式(57)的M相等,依据就是:同一套点速度系数,加上同一根杆内的质量求和。

退回单摆,核对全部系数

固定支点,只保留单摆角θ。此时整机速度只剩角速度,选择矩阵为1,没有脚底接触力。

$v = \dot{\theta},\quad J = l\begin{bmatrix}\cos\theta\\\sin\theta\end{bmatrix},\quad M = mJ^\top J = ml^2. $     (58)

切向与径向垂直,速度相关项的投影为0。重力与同一雅克比相乘,得到:

$Cv = mJ^\top \dot{J}\dot{\theta} = 0,\quad G = -J^\top \begin{bmatrix}0\\-mg\end{bmatrix} = mgl\sin\theta. $          (59)

$M\dot{v} + Cv + G = \tau \implies ml^2\ddot{\theta} + mgl\sin\theta = \tau. $        (60)

所以单摆和浮动基座用的是同一条路线:位置求导,整理速度系数,代入牛顿方程,按虚功投影并收集系数。


03    一条被冠军验证过的工程链路

本文出自:深蓝学院联合英国纽卡斯尔大学正教授、智身科技CTO潘为,推出《四足机器人:从动力学建模到强化学习》实战课程

——以IROS 2025四足挑战赛冠军的实战经验为核心,从动力学建模到实机部署,手把手教你掌握四足机器人全栈开发能力。

这是一门:教你理解四足机器人的物理本质,然后用 RL 控制它的工程实践课:

模型 → 辨识 → 仿真 → 训练 → 部署。

课程全程基于 MATRiX 一站式平台(MuJoCo 高精度物理引擎 + UE5 高保真视觉渲染 + GPU 并行训练),支持多种四足机器人型号,内置丰富地形。

从系统辨识到策略训练,从视觉验证到实机部署,全部在同一个平台内完成闭环。

我们不绑定任何一家硬件生态。CPU 能训,GPU 能训,Windows / macOS / Linux 都能跑。

Ref

1. 深蓝学院《四足机器人:从动力学建模到强化学习》-第1章 导论:四足控制的演进-课件.pdf(第4页)。

用于核对目标方程与单摆起点。

2. ETH Zurich, Robot Dynamics Lecture Notes, HS 2017,§2.10、§3.5-3.7。

https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf

用于核对浮动基座运动学、牛顿投影与接触动力学。

3. Kevin M. Lynch and Frank C. Park, Modern Robotics,§8.7 Constrained Dynamics。

用于核对约束力与约束运动的联立形式。

4. MathWorks, Understanding Model Predictive Control:https://www.mathworks.com/videos/understanding-model-predictive-control-part-2-what-is-mpc--1528106359076.html

Logo

DAMO开发者矩阵,由阿里巴巴达摩院和中国互联网协会联合发起,致力于探讨最前沿的技术趋势与应用成果,搭建高质量的交流与分享平台,推动技术创新与产业应用链接,围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐