绪论
双足轮腿机器人,在RoboMaster比赛中一般以平衡步兵形态出现(25赛季上交以其轮腿英雄震惊全场)。因轮腿式机器人系统具有非线性、欠驱动、强耦合、多输入多输出(MIMO)特性,传统PID算法非常难以使其达到收敛稳定状态(因为即使理想情况下,每一个腿长都对应着左右腿各三个电机,共六组参数,非常难调),故需要使用LQR、MPC等使用最优思想的控制算法,并对其建模仿真,才能使系统趋于稳定。
幸运的是自从23年底哈工程的王洪玺(很多交流群提到的王工、玺佬)发布了LQR控制五连杆轮腿机器人的电控开源,各个队伍调轮腿的难度也大大降低,后续也出现了基于LQR串联MPC(电科中山柳幸之)、强化学习(交龙等队伍)等更优的控制方法,RM赛场上的轮腿越来越多也越发强大。
本文主要针对尚未系统学习过自动控制原理(自动化大二下)、现代控制理论(自动化大三上),但希望能调腿的队员设计,主要针对控制理论中一些较晦涩难懂的概念及名词用较为通俗的例子进行解释,点明这里是什么,为什么,该怎么做。由于笔者水平有限,部分内容可能解释略微浅显,各队员也可结合我推荐的众多队伍电控开源进行学习理解。
轮腿机器人是跃鹿战队“年年不忘”的每年电控必吃榜。自23赛季曾老师首次调出五连杆并联腿,到26赛季换成上交构型四连杆串腿平步,在轮腿上的电控技术积累逐渐趋于稳定,已经积累了跃鹿自己的文档和仿真(来自24赛季腿控王硕凯)。在保证电控组 人手、资金充足的条件下, 我个人认为无论规则倾向如何,队伍的新队员都应该尝试进行轮腿机器人的调试,这会成为电控组员控制水平提高的一个很好的机会。
学习路径
轮腿机器人门槛愈发降低,但对电控的基本数理要求依然摆在那里,为了尽可能减少理论不扎实带来的调车痛苦,要求腿控至少要学习的基础理论有:
高数、线性代数、大学物理力学部分 (必学)
自动控制原理中的闭环控制、反馈等概念
现代控制理论中的状态空间方程、代价函数、观测器等概念
理论力学中的广义力、虚位移、虚功原理等概念
也就是说,腿控不建议早于大一下开始调腿,最佳时间为赛季末至下赛季初的暑假开始知识储备,以保证新赛季初能在机械出车后立刻开始调车。
接下来我会以王工的论文(见绪论超链接)为范本进行顺序解释,希望大家在看本篇之前至少看过他的论文一遍。
力学分析与系统建模

以下的所有分析都基于单侧腿
文章将五连杆腿简化为了一个二阶倒立摆,为了计算和建模方便,可以将腿简化成的单杆视作一个匀质杆,杆的中点视作质心(需要注意无论四连杆还是五连杆轮腿都可以如此等效,而实际上四连杆构型会存在些许质心偏移,但总体对我们后续的分析影响不大)
文中使用的是经典力学分析,也可通过拉格朗日方程进行分析,式子更简略,对数理要求更高,此处暂不叙述。这部分大家不需要自己手算分析,因为过程和结果都比较复杂,可以用MATLAB solve函数进行求解。
经典力学分析后得到的核心方程即原文中的式(3)(6)(9)
$$ \ddot{x} = \frac{T - NR}{\frac{I_{\text{W}}}{R} + m_{\text{W}} R} $$$$I_{\text{P}} \ddot{\theta} = \left(PL + P_{\text{M}} L_{\text{M}}\right) \sin\theta - \left(NL + N_{\text{M}} L_{\text{M}}\right) \cos\theta - T + T_{\text{P}} $$$$I_{\text{m}} \ddot{\varphi} = T_{\text{P}} + N_{\text{M}} l \cos\varphi + P_{\text{M}} l \sin\varphi$$其中
$$x = x_b - (L + L_{\text{M}})\sin\theta $$$$\dot{x} = \dot{x}_b (L + L_{\text{M}})\dot{\theta}\cos\theta$$$$\ddot{x} = \ddot{x}_b - (L + L_{\text{M}})\ddot{\theta}\cos\theta + (L + L_{\text{M}})\dot{\theta}^2\sin\theta$$进行运动学分解,目的是为了给状态空间方程建立做准备。
3.状态空间方程
首先大家需要理解一件事,矩阵本质是描述线性映射/线性变换的数学工具,我们在校所学的线性代数可能未能很好阐述矩阵、行列式、线性的作用和概念,建议大家去看MIT的线代教学或3B1B的线代科普,会有新的收获。 状态空间方程基本形式如下
$$ \dot{x} = Ax+Bu \\ y = Cx+Du $$其中,
A是状态矩阵,描述了系统自身演化规律
B是输入矩阵,描述输入对系统状态变化的影响(中学学过,导数可以理解为系统的变化率)
C是输出矩阵,描述系统状态x到实际输出的线性映射关系
D是前馈矩阵,代表了输入对输出的直接实际影响
为了使大家更好理解状态空间矩阵,可以从此超链接到弹簧阻尼系统的例子。
同时,你需要理解以下这句话:
LQR本身是线性系统的最优控制方法,处理形如$\dot{x} = Ax+Bu$的线性状态空间模型。而轮腿是典型的非线性系统,真实动力学满足$\dot{x} = f(x,u)$这个函数(或者说,映射,这个f是什么在这里你不用在意),因此需要用雅可比矩阵在平衡点处进行线性化。
(非线性关系“线性化”比较直观的体现)
雅可比矩阵第一次接触应该会在高数下,从直角坐标系到球坐标系柱坐标系的变换,其具体作用可参考3B1B的视频。 定义系统非线性模型$\dot{x} = f(x,u)$,其中状态向量$x $与输入向量$u$分别为
$$ \boldsymbol{x} = \begin{bmatrix} \theta\\\dot{\theta} \\ x_b \\\dot{x}_b \\\varphi\\\dot{\varphi} \end{bmatrix}, \quad\boldsymbol{u} = \begin{bmatrix} T \\ T_P \end{bmatrix} $$然后按照论文部分往下推导,在平衡点处进行线性化变成$\dot{x} = Ax+Bu$的形式,$AB$矩阵均基于我们上述的力学运动学推导,那么把矩阵展开会有
$$ \begin{bmatrix} \dot{\theta}\\\ddot{\theta} \\ \dot{x_b} \\\ddot{x}_b \\\dot{\varphi}\\\ddot{\varphi} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 & 0 & 0 \\ A_1 & 0 & 0 & 0 & A_2 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0\\A_3 & 0 & 0 & 0 & A_4 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ A_5 & 0 & 0 & 0 & A_6 & 0 \end{bmatrix} \begin{bmatrix} \theta\\\dot{\theta} \\ x_b \\\dot{x}_b \\\varphi\\\dot{\varphi} \end{bmatrix} + \begin{bmatrix} 0 & 0 \\ B_1 & B_2 \\ 0 & 0 \\B_3 & B_4\\ 0 & 0 \\ B_5 & B_6 \end{bmatrix} \begin{bmatrix} T \\ T_P \end{bmatrix} $$其中
$$ \begin{gather*} A_1 = &\;\frac{g\,(L m_p + L M + L_M M)\left(I_{\text{M}} I_w + I_{\text{M}} M R^2 + I_w M l^2 + I_{\text{M}} R^2 m_p + I_{\text{M}} R^2 m_w + M R^2 l^2 m_p + M R^2 l^2 m_w\right)}{\Delta} \\[4pt] A_2 = &\;\frac{M^2 g l^2 \left(I_w L + I_w L_M + L R^2 m_w + L_M R^2 m_p + L_M R^2 m_w\right)}{\Delta} \\[4pt] A_3 = &\;\frac{g\,(I_{\text{M}} + M l^2)\,(L m_p + L M + L_M M)\left(I_w L + I_w L_M + L R^2 m_w + L_M R^2 m_p + L_M R^2 m_w\right)}{\Delta} \\[4pt] A_4 = &\;\frac{M^2 g l^2 \left(I_w L^2 + I_w L_M^2 + I_p R^2 + L^2 R^2 m_w + L_M^2 R^2 m_p + L_M^2 R^2 m_w + 2 I_w L L_M + 2 L L_M R^2 m_w\right)}{\Delta} \\[4pt] A_5 = &\;\frac{M g l\,(L m_p + L M + L_M M)\left(I_w L + I_w L_M + L R^2 m_w + L_M R^2 m_p + L_M R^2 m_w\right)}{\Delta} \\[4pt] A_6 = &\;\frac{M g l\left(I_p I_w + I_w L^2 M + I_w L_M^2 M + I_p M R^2 + I_w L^2 m_p + I_p R^2 m_p + I_p R^2 m_w + L^2 M R^2 m_w + L_M^2 M R^2 m_p + L_M^2 M R^2 m_w + 2 I_w L L_M M + L^2 R^2 m_p m_w + 2 L L_M M R^2 m_w\right)}{\Delta} \\[4pt] B_1 = &\;-\frac{I_{\text{M}} I_w + I_{\text{M}} M R^2 + I_w M l^2 + I_{\text{M}} R^2 m_p + I_{\text{M}} R^2 m_w + I_{\text{M}} L M R + I_{\text{M}} L_M M R + M R^2 l^2 m_p + M R^2 l^2 m_w + I_{\text{M}} L R m_p + L M R l^2 m_p}{\Delta} \\[4pt] B_2 = &\;\frac{I_{\text{M}} I_w + I_{\text{M}} M R^2 + I_w M l^2 + I_{\text{M}} R^2 m_p + I_{\text{M}} R^2 m_w + M R^2 l^2 m_p + M R^2 l^2 m_w + I_w L M l + I_w L_M M l + L M R^2 l m_w + L_M M R^2 l m_p + L_M M R^2 l m_w}{\Delta} \\[4pt] B_3 = &\;-\frac{(I_{\text{M}} + M l^2)\left(I_w L + I_w L_M - I_p R + L R^2 m_w + L_M R^2 m_p + L_M R^2 m_w + L L_M R m_p\right)}{\Delta} \\[4pt] B_4 = &\;\frac{I_{\text{M}} I_w L + I_{\text{M}} I_w L_M + I_w L M l^2 + I_w L^2 M l + I_w L_M M l^2 + I_w L_M^2 M l + I_p M R^2 l + I_{\text{M}} L R^2 m_w + I_{\text{M}} L_M R^2 m_p + I_{\text{M}} L_M R^2 m_w + L M R^2 l^2 m_w + L^2 M R^2 l m_w + L_M M R^2 l^2 m_p + L_M^2 M R^2 l m_p + L_M M R^2 l^2 m_w + L_M^2 M R^2 l m_w + 2 I_w L L_M M l + 2 L L_M M R^2 l m_w}{\Delta} \\[4pt] B_5 = &\;-\frac{M l\left(I_w L + I_w L_M - I_p R + L R^2 m_w + L_M R^2 m_p + L_M R^2 m_w + L L_M R m_p\right)}{\Delta} \\[4pt] B_6 = &\;\frac{I_p I_w + I_w L^2 M + I_w L_M^2 M + I_p M R^2 + I_w L^2 m_p + I_p R^2 m_p + I_p R^2 m_w + L^2 M R^2 m_w + L_M^2 M R^2 m_p + L_M^2 M R^2 m_w + 2 I_w L L_M M + L^2 R^2 m_p m_w + I_w L M l + I_w L_M M l + 2 L L_M M R^2 m_w + L M R^2 l m_w + L_M M R^2 l m_p + L_M M R^2 l m_w}{\Delta} \\[4pt] \Delta = &\;I_{\text{M}} I_p I_w + I_p I_w M l^2 + I_{\text{M}} I_w L^2 m_p + I_{\text{M}} I_p R^2 m_p + I_{\text{M}} I_p R^2 m_w \\&+ I_{\text{M}} I_w L^2 M + I_{\text{M}} I_w L_M^2 M + I_{\text{M}} I_p M R^2 + I_{\text{M}} L^2 M R^2 m_w + I_{\text{M}} L_M^2 M R^2 m_p \\&+ I_{\text{M}} L_M^2 M R^2 m_w + 2 I_{\text{M}} I_w L L_M M + I_w L^2 M l^2 m_p + I_p M R^2 l^2 m_p + I_p M R^2 l^2 m_w \\&+ I_{\text{M}} L^2 R^2 m_p m_w + 2 I_{\text{M}} L L_M M R^2 m_w + L^2 M R^2 l^2 m_p m_w \end{gather*} $$以上公式由MATLAB直接导出,可以看出篇幅确实不小,在MATLAB中进行仿真时也不必我们计算,使用solve函数即可,只需要记住$AB$矩阵中的各个系数可视作仅以腿长$L$作为变量,其余参数在建模之初便已赋值为常量。而系统输出设为$y=Cx=I_{6}x$,其中$I_{6}$是六维单位阵,即不经过系数转化直接将状态作为输出;输入不直接对输出产生影响,$D$矩阵为0。
注:在经过关于控制系统可控性和可观性的学习后,我们可以知道系统的能控性矩阵和能观性矩阵均满秩,从而可控且可观。
4.控制器设置
4.1线性二次调节器LQR
轮腿机器人的控制系统框图如下,估计器中既包含了姿态解算得到的$\hat{\psi}$(即IMU读到的航向角yaw),也有经过速度解算得到的$\dot{x}$( 这个值还可能经过卡尔曼滤波进行速度估计,后面会讲到),控制器中可以是LQR、MPC或二者串联的方法,当前我们只考虑最基本的LQR控制。
先思考一下,我们从状态空间方程$\dot{x} = Ax+Bu $得到了状态变化量与当前状态和输入的关系,但我们并未建立起反馈机制,即状态又如何作用于我们的输入使我们做出调整。
这就好像我们开车时需要变道,我们知道打方向盘可以使车身偏离,这是输入对状态的影响;但我们不能一直打方向,看到变道成功后需要把方向盘回正,这是状态给输入的反馈。

在PID中,我们通过$error=measure-target$得到误差,再对误差进行比例微分积分计算,得到新输出量;在LQR中,这一机制是由$u=-Kx$(负号体现反馈性)此线性控制率决定的,同时为了使我们能跟踪参考输入,取参考值矩阵为$x_d$,于是有$$ u=K({x_d}-x) \
意识流地感受一下,$x$越接近参考值,控制量$u$越小,符合我们的直觉,感觉上有点像纯P控制。在LQR中,我们的最优化过程即体现在对$K$求最优解上。那么,我们需要一个工具来衡量我们这个$K$是不是我们需要的最优解,于是我们引入代价函数: $J=\int_{0}^{\infty} (x^TQx+u^TRu) dt $
对时间从0积分到无穷体现的是LQR在无限时域中的最优性(和MPC等控制器的代价函数有很大区别),不必纠结这个结论怎么来的,先接受他。代价函数中的两项均为二次型,也就是说$x^TQx$作为矩阵的运算就如同一般代数中的$qx^2$,$Q$我们称为状态权重矩阵(对角半正定),$R$我们称为控制权重矩阵(对角正定);$x 、u$代表状态和输入,$J$是全局代价。
关于正定半正定矩阵:正定矩阵$\boldsymbol{A}$可使$x^TAx>0$,半正定矩阵$A$可使$x^TAx≥0$。为了方便调参、解耦等需求,我们取了QR为对角阵,此时则要求Q对角线上每一项严格≥0,R对角线上每一项严格>0
也许看到这里你会有点晕:这个式子里明明没有$K$,为何会说$J$与反馈矩阵有关呢,而且凭空多出来一个$Q$和$R$是为什么?在此我再举一个浅薄的例子:
作为一名普通高中生,做数学题时,我们总要制定做题策略以争取拿到更高分数:是慢慢写追求正确率,还是追求省多点时间做多点题?这些策略可以抽象为一个个权重,对于一系列状态和输入,自然有一系列权重,这就是我们的Q、R矩阵。
$QR$矩阵即为整个LQR系统中最需要我们动态调整的参数矩阵。当我们希望系统更稳定,就对应调大$Q$中的参数,比如希望机体俯仰角$\theta$快速收敛,就调大对应的$Q_1$权重;当调大$R$中的参数,会导致$J$变大,对应的输入$u$更小。
现在我们知道$QR$矩阵对$J$的影响,那么大概可以想象出这个二次型的图像。由于QR是半正定/正定矩阵,“碗”的开口始终向上,J总能求出一个最小值。
想象一个静态问题:$J=Qx^2+Ru^2$,我们能控制的是输入$u$($u$是变量),求导得$\frac{\partial J}{\partial u} =2Q\frac{\partial x}{\partial u} + 2Ru= 0 $,如果J、Q、R、K都是标量,且u=-Kx,则会有K=Q/R,也就得出了K的表达式,这当然很容易;而实际上他们都是矩阵,为使代价函数$J$最小,输入$u$应满足下式
其中$P$矩阵满足黎卡提方程
$$A^TP+PA−PBR^{−1}B^TP+Q=0$$黎卡提方程推理请查看超链接。这需要一些泛函分析的知识,大家有兴趣可以去学习。
因此,我们得出结论$K=-R^{-1}B^TP$,在MATLAB中,我们可以很容易地用lqr函数求出增益矩阵(求解代码形如$K=lqr(A,B,Q,R)$)。此时还有一个问题:我们的$AB$矩阵会因不同腿长$L$的变化而变化,造成增益矩阵均有所不同,而每次变腿长时都对增益矩阵求解计算量太大,无法将该算法部署在单片机上。因此如论文所述,我们在腿长每变化10mm时进行一次线性化并求此时的增益矩阵,拟合则相当于对此时每个增益矩阵中的参数$K_{ij}$进行记录后,用一条“光滑的曲线”把它们连接起来,同时预测没有计算的点会以一个什么样的趋势变化。 理论上,6X2大小的增益矩阵$K$中每个$K_{ij}$均进行三次多项式拟合后应共有12*(3+1)=48个系数,而经过实测检验二次多项式也可以满足需求,因此战队24赛季的平衡步兵采用了一个[12][3]的数组存放系数,便于直接调用。

4.2运动学解算,虚拟模型控制VMC
好了,费了那么大劲我们总算是求出了$K$矩阵——在单片机中每个$K_{ij}$的形式是一个与“腿长”$L_0$有关的函数。但你理应意识到这个“腿长”只是我们简化分析设置的等效杆长,我们求出的$T_p$也只是等效中心轴输出力矩,我们接下来要做的工作就是通过虚拟模型控制,把这个等效量转化为我们实际需要让电机输出的关节力矩。
- 正运动学解算
(五连杆机构简化图,实际模型轮毂在C的位置,对于四连杆可等效为$AE=0$)
为了方便新学习的同学补充一些知识,这里简单解释何为正逆运动学解算
- 正运动学解算:关节空间->工作空间,即知道角度($\theta,\phi…$)求位姿($x,y,z$)
- 逆运动学解算:工作空间->关节空间,即知道位姿反求角度 在此也就是通过$\phi_{i},l_j$求出C点坐标。根据图像,左右两边分别列出$x_c,y_c$的表达式 $$\begin{cases} x_B + l_2 \cos\phi_2 = x_D + l_3 \cos\phi_3 \\ y_B + l_2 \sin\phi_2 = y_D + l_3 \sin\phi_3 \end{cases}$$
解出$\phi_2$:
$$\phi_2 = 2 \arctan\left( \frac{B_0 + \sqrt{A_0^2 + B_0^2 - C_0^2}}{A_0 + C_0} \right)$$其中的参数为以下式子,而$x_D,y_D$又与$\phi_4$相关
$$ \begin{gather*} &A_0 = 2l_2 (x_D - x_B) \\ &B_0 = 2l_2 (y_D - y_B) \\ &C_0 = l_2^2 + l_{BD}^2 - l_3^2\\ &l_{BD} = \sqrt{(x_D - x_B)^2 + (y_D - y_B)^2} \\ \end{gather*}$$从而解出C的坐标,仅使用可直接观测的量$l_1,\phi_1$和解算可得的$\phi_2$(与$\phi_4$相关)表示:
$$ \begin{cases} x_C = l_1 \cos\phi_1 + l_2 \cos\phi_2 \\ y_C = l_1 \sin\phi_1 + l_2 \sin\phi_2 \end{cases} $$并进一步得到极坐标,即等效的单摆倒立摆和真实角度与腿长的映射:
$$\begin{cases} L_0 = \sqrt{(x_C - l_5/2)^2 + y_C^2} \\ \phi_0 = \arctan\left( \frac{y_C}{x_C - l_5/2} \right) \end{cases} $$正运动学解算即完成,该过程理解上并不复杂,也不需要我们进行手算,直接将公式套入MATLAB中即可算出来,不必在这上面过多纠结停留。
- VMC雅可比矩阵
王工关于此处更详细的解释如下:五连杆运动学解算与VMC 对于上面的参数我们定义向量$\boldsymbol{x} = {\begin{bmatrix} L_0 & \phi_0\ \end{bmatrix}}^T$,$\boldsymbol{q} = {\begin{bmatrix} \phi_1 & \phi_4\ \end{bmatrix}}^T$,在式(13)中如果我们结合式(12)展开$x_c,y_c$,那么显然可以得到函数映射关系$\boldsymbol{x}=f(\boldsymbol{q})$。再提醒一次,$L_0$是腿的等效杆,$\phi_0$是等效杆与实际髋关节连线的夹角。 再定义向量$\boldsymbol{T} = {\begin{bmatrix} T_1 & T_2\ \end{bmatrix}}^T$和$\boldsymbol{F} = {\begin{bmatrix} F_0 & T_p\ \end{bmatrix}}^T$,其中$T_1,T_2$分别为前后两关节电机输出力矩,$F_0$为沿等效单杆机构的推力(新的未知量,等下提到如何解),$T_p$为沿等效中心轴的力矩。在论文中,玺佬核心思路是求出将关节速度$\boldsymbol{\dot{q}}$映射为五连杆姿态变化率$\boldsymbol{\dot{x}}$ 的雅可比矩阵$J$,再由虚功原理得出矩阵$T$与$F$的关系,从而解出真实髋关节力矩参考值$T_1,T_2$,从而核心结论为: $$ \begin{gathered} \delta\boldsymbol{x} = \boldsymbol{J}\delta\boldsymbol{q} \
\boldsymbol{T} = \boldsymbol{J}^\mathrm{T} \boldsymbol{F} \end{gathered} $$ 于是到目前为止,我们对各个参数可知道的信息为:
- $\boldsymbol x$:其中的$L_0,\phi_0$ 经过一系列变换可直接由观测得到的$\phi_1,\phi_4$解得,已知量
- $\boldsymbol J$:雅可比矩阵,可知,待求量
- $\boldsymbol F$:$T_p$由反馈$u=-Kx$可求得,已知量;$F_0$,尚未得到它与已知量的关系
- $\boldsymbol T$:真实髋关节力矩,目标值 对于待求的雅可比矩阵,论文中提供了两种方法: ①直接对$\boldsymbol{x}=f(\boldsymbol{q})$求全微分,从而得到雅可比矩阵: $$ \begin{gathered} \begin{cases} \delta L_0 = \dfrac{\partial f_1}{\partial \varphi_1} \partial \varphi_1 + \dfrac{\partial f_1}{\partial \varphi_4} \partial \varphi_4 \\[6pt] \delta \varphi_0 = \dfrac{\partial f_2}{\partial \varphi_1} \partial \varphi_1 + \dfrac{\partial f_2}{\partial \varphi_4} \partial \varphi_4 \end{cases} \\ \boldsymbol J = \begin{bmatrix} \dfrac{\partial f_1}{\partial \varphi_1} & \dfrac{\partial f_1}{\partial \varphi_4} \\[8pt] \dfrac{\partial f_2}{\partial \varphi_1} & \dfrac{\partial f_2}{\partial \varphi_4} \end{bmatrix} \end{gathered} $$
然而,只凭感觉我们会发现上述的模型表达式中包含大量平方与三角函数及其嵌套运算(展开式里既有arctanx也有他的平方与开根运算),即使用了MATLAB求解也会无法部署至单片机上。
②由于雅可比矩阵$J$实际描述的是两坐标微分的线性映射关系,因此我们可以通过计算速度映射关系来得到雅可比矩阵,即论文1.2.3静力学解算的方法
对式(12)求导:
$$ \begin{cases} \dot{x}_C = -l_1 \dot{\phi}_1 \sin\phi_1 - l_2 \dot{\phi}_2 \sin\phi_2 \\\dot{y}_C = l_1 \dot{\phi}_1 \cos\phi_1 + l_2 \dot{\phi}_2 \cos\phi_2 \end{cases} $$经过一系列消去变换得到:
$$ \begin{cases} \dot{x}_C = -l_2 \dot{\phi}_1 \sin\phi_1 - l_2 \left( \frac{(\dot{x}_D - \dot{x}_B)\cos\phi_3 + (\dot{y}_D - \dot{y}_B)\sin\phi_3}{l_2 \sin(\phi_3 - \phi_2)} \right) \sin\phi_2 \\\dot{y}_C = l_2 \dot{\phi}_1 \cos\phi_1 + l_2 \left( \frac{(\dot{x}_D - \dot{x}_B)\cos\phi_3 + (\dot{y}_D - \dot{y}_B)\sin\phi_3}{l_2 \sin(\phi_3 - \phi_2)} \right) \cos\phi_2 \end{cases} $$用MATLAB进行符号计算后:
$$ \begin{bmatrix} \dot{x}_C \\\dot{y}_C \end{bmatrix} = \begin{bmatrix} \dfrac{l_1 \sin(\phi_1 - \phi_2) \sin\phi_3}{\sin(\phi_2 - \phi_3)} & \dfrac{l_4 \sin(\phi_3 - \phi_4) \sin\phi_2}{\sin(\phi_2 - \phi_3)} \\[1em] -\dfrac{l_1 \sin(\phi_1 - \phi_2) \cos\phi_3}{\sin(\phi_2 - \phi_3)} & -\dfrac{l_4 \sin(\phi_3 - \phi_4) \cos\phi_2}{\sin(\phi_2 - \phi_3)} \end{bmatrix} \begin{bmatrix} \dot{\phi}_1 \\\dot{\phi}_4 \end{bmatrix} $$记作:
$$ \begin{bmatrix} \dot{x}_C \\\dot{y}_C \end{bmatrix} = \boldsymbol{J} \begin{bmatrix} \dot{\phi}_1 \\\dot{\phi}_4 \end{bmatrix} $$当然,方法②的$J$与式(14)的$J$必然不同,从而$\boldsymbol{T}$与$\boldsymbol{F}$的关系也有所不同。事实上,若用此雅可比矩阵,则真实关节力矩与虚拟力映射中间还应左乘一变换矩阵$\boldsymbol{M}$和一旋转矩阵$\boldsymbol{R}$,展开后的形式为:
$$ \begin{gathered} \boldsymbol{R} = \begin{bmatrix} \cos(\phi_0 - \pi/2) & -\sin(\phi_0 - \pi/2) \\\sin(\phi_0 - \pi/2) & \cos(\phi_0 - \pi/2) \end{bmatrix}\\ \boldsymbol{M} = \begin{bmatrix} 0 & -\dfrac{1}{L_0} \\ 1 & 0 \end{bmatrix}\\ \begin{bmatrix} T_1 \\ T_2 \end{bmatrix} = \boldsymbol{J}^T \boldsymbol{R} \boldsymbol{M} \begin{bmatrix} F_0 \\ T_p \end{bmatrix} \end{gathered} $$MATLAB求解后得到最终结果:
$$ \begin{bmatrix} T_1 \\ T_2 \end{bmatrix} = \begin{bmatrix} \dfrac{l_1 \sin(\phi_0 - \phi_3) \sin(\phi_1 - \phi_2)}{\sin(\phi_3 - \phi_2)} & \dfrac{l_1 \cos(\phi_0 - \phi_3) \sin(\phi_1 - \phi_2)}{L_0 \sin(\phi_3 - \phi_2)} \\[1.5em] \dfrac{l_4 \sin(\phi_0 - \phi_2) \sin(\phi_3 - \phi_4)}{\sin(\phi_3 - \phi_2)} & \dfrac{l_4 \cos(\phi_0 - \phi_2) \sin(\phi_3 - \phi_4)}{L_0 \sin(\phi_3 - \phi_2)} \end{bmatrix} \begin{bmatrix} F_0 \\ T_p \end{bmatrix} $$十分简洁。
4.3未求解的数
在上面,我们已经解决了雅可比矩阵的求解,找到了$\boldsymbol T$与$\boldsymbol F$之间的映射关系,但还有一个量$F_0$我们还未将其表示。容易想到对于单腿有固定腿长下沿杆推力$F_l = 0.5Mgcos\theta$,其中$M$为机体质量;而若我们希望能实现变腿长且“不僵硬”,则需将腿杆建模为一个弹簧-阻尼系统,并使用PD控制器对其进行控制,将这个力叠加在$F_l$上,即$F_0=F_l+K_p(Height_{Ref}-Height)+K_d(0-\dot{Height})$(注,为保持机身稳定,这里的参考值是高度)
$$ \begin{gathered} \text{LegLength} = \sqrt{(x_C - l_5/2)^2 + y_C^2} \\ \text{Height} = \text{LegLength} \cdot \cos\theta \end{gathered} $$
4.4速度解算
王工的论文中对于轮速的处理为
$$ \begin{gathered} \dot{x_b} = \omega R+(L_0sin\theta)^{'} \end{gathered} $$其中$\omega$为驱动轮转子相对大地角速度。 根据式(23),轮腿机器人的机体平动速度 $\dot{x}_b$ 可通过驱动轮角速度 $\omega$ 与腿部运动项 $(L_0 \sin \theta)’$ 直接解算得到。然而,在实际系统中,这一方法存在三方面固有缺陷:
- 测量噪声放大:驱动轮角速度 $\omega$ 由编码器测量,高频噪声明显;微分项 $(L_0 \sin \theta)’$ 进一步放大了角度 $\theta$ 与腿长 $L_0$ 的噪声,导致速度估计剧烈抖动。
- 打滑与模型误差:式(23) 假设驱动轮纯滚动,一旦地面湿滑或加速过快,$\omega R$ 不再等于轮心相对大地的平动速度,直接计算会引入不可观测的偏差。
- 积分漂移:若采用加速度积分估计速度(如 IMU 数据),短时精度高但长时间积分后零偏漂移严重;而式(23) 虽然无长期漂移,但动态响应滞后且易受腿部运动耦合影响。 为了融合两者的互补特性,卡尔曼滤波 成为一种最优的实时速度估计方案。它能根据系统动态模型与传感器噪声统计特性,动态调整加速度积分(预测)与轮速测量(校正)的权重,输出最小方差意义下的最优速度估计。
卡尔曼滤波原理与速度估计模型
要推导卡尔曼滤波全式子所需篇幅过大,也不适合文档这个载体,因此此处我只会给出一些关键式子,具体的板书推导我会给出DR.CAN的视频卡尔曼滤波数学推导,我十分推荐大家把他的视频看完,感受何为醍醐灌顶。 先明确卡尔曼滤波是什么:他是一个观测器,它根据预测值(模型推演)和测量值(传感器),按各自的不确定性(方差)动态计算权重,实时估计出系统状态。 设置这么一个场景:我们有两个秤,对于同一物品,A称出来重$Z_1$kg,B称出来$Z_2$kg,同时我们已知A、B秤称重的标准差分别为$\sigma_1$和$\sigma_2$,且服从正态分布,我们需要通过数据融合的思想使其称重结果更可信,这个结果记为估计值$\hat{Z}$。 由于正态分布的性质,我们设$\hat{Z}=(1-K)Z_1+Z_2=Z_1+K(Z_2-Z_1)$。显然$K=0$时$\hat{Z}=Z_1$,$K=1$时$\hat{Z}=Z_2$ 。我们希望找到一个最“优”的$K$值使$\hat{Z}$置信度最高,即求出使融合后的方差$\sigma_{\hat{Z}}$最小的$K$。 由概率论知识,$\sigma_{\hat{Z}}=Var((1-K)Z_1+Z_2)$,由于AB称出来的结果相互独立,则$\sigma_{\hat{Z}}=Var((1-K)Z_1)+Var(Z_2)=(1-K)^2Var(Z_1)+Var(Z_2)=(1-K)^2\sigma_1^2+\sigma_2^2$ 考虑有一维离散系统,状态变量为机体速度 $v_k$加速度计提供控制输入 $a_m$,轮速提供观测值 $z_k = v_m$。系统模型如下: 状态预测方程(基于加速度积分):$\hat{v}{k|k-1} = \hat{v}{k-1} + a_m \cdot \Delta t$ 预测协方差:$P_{k|k-1} = P_{k-1} + Q$ 其中 $Q$ 为过程噪声协方差,反映加速度计积分的不确定性。 测量更新方程(利用轮速校正): 卡尔曼增益:
$$ \begin{gathered} K_k = \frac{P_{k|k-1}}{P_{k|k-1} + R} \end{gathered} $$状态估计更新:
$$ \begin{gathered} \hat{v}_k = \hat{v}_{k|k-1} + K_k \left( z_k - \hat{v}_{k|k-1} \right) \end{gathered} $$估计协方差更新:
$$ \begin{gathered} P_k = (1 - K_k) P_{k|k-1} \end{gathered} $$其中 $R$ 为测量噪声协方差,反映轮速测量的可信度(打滑时 $R$ 增大)。
通过递归执行上述五步,卡尔曼滤波在每一时刻都给出当前速度的最优估计。该估计在统计意义上融合了加速度积分的高频响应与轮速测量的无漂移特性,同时对打滑和噪声具有鲁棒性。将滤波后的速度 $\hat{v}_k$ 替代原有的直接微分项,可显著提升轮腿的纵向运动控制的稳定性与精度。
4.5综合运动控制
该部分回到了我们熟悉的PID控制且几乎都使用了PD控制器。以IMU获取到的Yaw角度角速度作为反馈,即可完成转向控制,进一步实现轮腿小陀螺;以Roll的角度角速度作为反馈,即可完成横滚控制,从而保持单腿上坡等情景下机身平衡;腿长控制在4.3中也进行了叙述。进而我们可以将第4部分一开始的图加上各种控制器变成论文中的模样。

该部分在理论上并没有难理解的部分,PD控制器在培训阶段应有涉及,实际参数需结合调车情况分析调整,可能会与仿真有较大差别。
5.仿真软件选择
针对不同的建模与仿真需求,这里介绍四款常用的仿真软件,可根据自身平台与侧重点选择。
MATLAB/Simulink
MATLAB/Simulink是商业软件,也是最常用于算法原理验证与数学建模的工具,支持Windows、macOS、Linux。其在矩阵运算、符号计算和控制系统工具箱方面几乎无可替代,例如前文求解增益矩阵所用的
lqr函数、solve符号求解,直接在MATLAB中一条命令即可完成。Simulink提供丰富的模块库,可快速搭建控制框图,验证LQR、MPC等算法的正确性。但其3D物理仿真能力很弱(Simscape Multibody在接触、碰撞、打滑等动力学细节上表现一般),无法模拟轮腿与地面真实的摩擦、打滑过程,因此更适合做"算法正确性"验证,而不是"整机真实感"仿真。Gazebo
Gazebo是ROS生态中最流行的开源3D物理仿真软件,主要面向Linux(Ubuntu)平台,Windows/macOS支持较差。它通过URDF/SDF格式建模,支持ODE、Bullet、DART等多种物理引擎,能较真实地模拟接触、摩擦、打滑等细节,并可配合Rviz等工具搭建完整的感知-控制闭环,社区资料丰富、用户基数大。缺点是环境配置繁琐(需要Ubuntu+ROS+Gazebo版本严格匹配),学习曲线陡峭,物理引擎精度一般,高频运行时容易出现数值抖动,且对新手不太友好。
Webots
Webots是开源免费的3D机器人仿真软件,跨平台支持Windows/macOS/Linux,图形界面友好,上手门槛远低于Gazebo。它基于ODE物理引擎,内置丰富的轮式机器人模型,支持C/C++、Python、ROS等多种控制器语言,非常适合教学、算法快速验证和课程作业。缺点是ROS集成不如Gazebo成熟,社区生态相对较小,物理细节(如打滑、柔性接触)不如专业引擎精细,自定义场景需要花时间学习其节点式的建模方式。
MuJoCo
MuJoCo是DeepMind开源的物理仿真引擎,以速度快、数值稳定性好、接触动力学精度高著称,是当下强化学习(RL)训练的主流选择。轮腿这类欠驱动、强耦合的系统若采用RL方法训练,MuJoCo几乎是首选平台。其Python接口
dm_control简洁易用,使用MJCF格式建模,也支持C++接口,跨平台(Windows/macOS/Linux)。缺点是它更偏"引擎"而非"软件",缺少完整的三维可视化界面与调试面板,需要配合其他工具使用,对以理论学习为主、尚未接触RL的队员来说,入门曲线不如MATLAB直观。
6.更新
2026.4.1 完成初版
2026.4.15 完成控制器设置、新增仿真软件选择部分
2026.5.7 完成卡尔曼滤波部分,略显粗糙
2026.8.10 完成对博客格式适配,添加各仿真的说明