Motion control method of wheeled-legged self-balancing robots based on model predictive control
-
摘要: 对于五连杆结构的轮腿式自平衡机器人，传统的倒立摆建模方法未考虑航向角等自由度，控制性能较差。对此提出一种新的建模与控制方法。首先，通过虚拟模型控制和辅助控制器实现机器人部分自由度的控制，同时对系统进行解耦，建立较为精简的整体运动模型。其次，因传统线性二次型调节器控制器无法解决腿部运动范围受限问题，设计模型预测控制器以预测未来周期内的运动状态，优化求解约束范围内的控制量，控制器输出通过虚拟模型控制映射为关节电机力矩，从而实现机器人的运动控制。最后，对轮腿式机器人进行仿真实验。仿真结果表明，该方法能够较好地跟踪控制指令的效果，具有较好的抗干扰能力。Abstract: A wheeled-legged self-balancing robot with a five-link structure in its legs has poor control performance due to that traditional modeling method of inverted pendulum does not take account of heading. This paper proposes a new modeling and control approach to solve this problem. Firstly, by employing the virtual model control and auxiliary controllers, control of the robot’s partial degrees of freedom is achieved while decoupling the system to establish a simplified overall motion model. Secondly, due to the limitation of traditional linear quadratic regulator controllers in addressing the restricted range of leg motion, a model predictive controller is designed to predict the motion state within future cycles, optimizing control inputs within the constraint range. The controller output is mapped into a joint motor torque through virtual model control to achieve robot motion control. Finally, simulation experiments on the wheeled-legged robot demonstrate that this method effectively tracks control commands and exhibits good disturbance rejection capabilities.
-
近年来，机器人技术已经逐渐应用到人类生活的各个领域。凭借自主运动或远程控制能力，机器人已经开始取代人类执行各种繁杂且危险的任务，例如野外勘探、设施巡检以及搬运装卸等。特别是在复杂多变的地形条件下，研究具备适应性强、机动性高以及运动范围广的移动机器人已经成为机器人研究的重要领域。通过综合分析巡检、勘探、快递等应用需求以及地形条件，考虑到经济性、平稳性以及快速性等因素，轮腿式机器人移动结构相较于单一设计方案具有明显优势[1−3]，在相对平缓的地面上，可以通过轮式结构实现快速移动；而在崎岖地形中，可利用腿部作为主动悬挂，保证上层机构的平稳运动，从而更高效地完成各类作业任务。
国内外机器人研究机构都对轮腿式机器人展开了研究，取得了丰富的研究成果。波士顿动力公司的双足轮腿机器人Handle，苏黎世联邦理工的机器人ANYmal[4−5]和Ascento[6−8]，哈尔滨工业大学的液压驱动双轮腿机器人WLR[9−10]；腾讯Robotics X实验室的两足轮腿机器人Ollie[11]，该机器人使用的机械结构各有千秋，而轮腿式机器人的控制也有很多实现方法。例如陈阳等[12]基于单腿倒立摆模型建立了数学模型，并使用线性二次型调节器（linear quadratic regulator, LQR）配合一些辅助控制器完成对机器人的控制。Feng等[13]使用LQR完成机器人的平衡和运动控制，使用自抗扰控制器控制机器人腿部运动。Zhang等[14]使用全身控制（whole body control,WBC）与基于强化学习的自适应控制器相结合，增强机器人多自由度的灵活性。
本文使用文献[15]中的五连杆结构作为机器人的腿部结构，相较于串联式腿部对关节电机扭矩要求更低，具有更强的负载能力。使用虚拟模型控制（virtual model control, VMC）[16]的方法简化腿部结构带来的建模与控制问题；为了优化单腿模型中对于航向角的控制以及减少辅助控制器的数量，建立基于双腿的整体运动模型；为了防止机器人腿部运动受限导致的不稳定问题，设计模型预测控制（model predictive control, MPC）[17−19]实现对机器人的控制。
1. 系统建模
轮腿式平衡机器人是一类复杂的非线性多输入多输出系统，为了得到较为精简且准确的数学模型，首先做出一些假设以对系统进行简化。
1）将五连杆视为一根长度变化的摆杆。
2）已通过腿长控制器控制五连杆的长度恒定。
3）已通过滚转角控制器控制滚转角恒为0。
1.1 变量定义
将机器人视为由左右轮、左右腿、机体组成的模型，其中左右轮与左右腿对称，如图1所示。
图 1 轮腿式机器人模型
下载:
全尺寸图片
表 1 轮腿式机器人模型变量定义符号 含义 单位 ${N_{{\text{f}}1}},{N_{{\text{f2}}}}$ 地面对轮子摩擦力，箭头所示 N ${N_{{\text{wl1}}}},{N_{{\text{wl2}}}}$ 轮子对腿部水平力，箭头所示 N ${P_{{\text{wl1}}}},{P_{{\text{wl2}}}}$ 轮子对腿部竖直力，箭头所示 N ${N_{{\text{lb1}}}},{N_{{\text{lb2}}}}$ 腿部对机身水平力，箭头所示 N ${P_{{\text{lb1}}}},{P_{{\text{lb2}}}}$ 腿部对机身竖直力，箭头所示 N ${x_{{\text{w1}}}},{x_{{\text{w2}}}}$ 轮子的位移，箭头所示 m ${T_{{\text{w1}}}},{T_{{\text{w2}}}}$ 轮子输出力矩，顺时针 $ \text{N}\cdot \text{m} $ ${T_{{\text{l1}}}},{T_{{\text{l2}}}}$ 关节输出力矩，顺时针 $ \text{N}\cdot \text{m} $ ${x_{\text{b}}}$ 机体的位移，箭头所示 m $\psi $ 航向角，图示为正 rad $\phi $ 机身俯仰角，图示为正 rad ${\theta _1},{\theta _2}$ 腿部与竖直方向夹角，图示为正 rad 表 2 轮腿机器人模型参数定义符号 含义 单位 ${m_{\text{w}}},{m_{\text{l}}},{m_{\text{b}}}$ 轮子、腿部、机体的质量 kg ${I_{\text{w}}},{I_{{\text{l1}}}},{I_{{\text{l2}}}},{I_{\text{b}}}$ 轮子、腿部、机体俯仰角转动惯量 $ \text{kg}\cdot {\text{m}}^{2} $ $R$ 轮子半径 m ${L_{11}},{L_{12}}$ 腿重心到轮转轴的距离 m ${L_{21}},{L_{22}}$ 腿重心到机体转轴的距离 m $l$ 机体重心到转轴的距离 m $I_{\text{z}}$ 机体航向角的转动惯量 $ \text{kg}\cdot {\text{m}}^{2} $ ${l_{\text{w}}}$ 两腿距离的一半 m 1.2 动力学分析
对左轮部有
$$ {m_{\text{w}}}{\ddot x_{{\text{w}}1}} = {N_{{\text{f}}1}} - {N_{{\text{wl}}1}} $$ (1) $$ {I_{\text{w}}}\frac{{{{\ddot x}_{{\text{w1}}}}}}{R} = {T_{{\text{w1}}}} - {N_{{\text{f1}}}}R $$ (2) 对左腿部有
$$ {N_{{\text{wl1}}}} - {N_{{\text{lb1}}}} = {m_l}\frac{{{\partial ^2}}}{{\partial {t^2}}}\left( {{x_{{\text{w1}}}} + {L_{11}}\sin {\theta _1}} \right) $$ (3) $$ {P_{{\text{wl1}}}} - {P_{{\text{lb1}}}} - {m_{\text{l}}}g = {m_{\text{l}}}\frac{{{\partial ^2}}}{{\partial {t^2}}}\left( {{L_{11}}\cos {\theta _1}} \right) $$ (4) $$ \begin{split} & {I_{{\text{l1}}}}{{\ddot \theta }_1} = \left( {{P_{{\text{wl1}}}}{L_{11}} + {P_{{\text{lb1}}}}{L_{21}}} \right)\sin {\theta _1} - \\& \left( {{N_{{\text{wl1}}}}{L_{11}} + {N_{{\text{lb1}}}}{L_{21}}} \right)\cos {\theta _1} - {T_{{\text{w1}}}} + {T_{{\text{l1}}}} \end{split} $$ (5) 对右轮部和右腿部，有与式（1）~（5）同样形式的5个方程。
对机体有
$$ {N_{{\text{lb1}}}} + {N_{{\text{lb2}}}} = {m_{\text{b}}}\frac{{{\partial ^2}}}{{\partial {t^2}}}\left( {{x_{\text{b}}} - l\sin \phi } \right) $$ (6) $$ \begin{split} & {P_{{\text{lb1}}}} + {P_{{\text{lb1}}}} - {m_{\text{b}}}g = \frac{1}{2}{m_{\text{b}}}\frac{{{\partial ^2}}}{{\partial {t^2}}}[\left( {{L_{11}} + {L_{21}}} \right)\cos {\theta _1} + \\&\qquad\qquad \left( {{L_{12}} + {L_{22}}} \right)\cos {\theta _2} + 2l\cos \phi ] \end{split} $$ (7) $$ \begin{split} & {I_{\text{b}}}\ddot \phi = \left( {{T_{{\text{l1}}}} + {T_{{\text{l2}}}}} \right) + \left( {{N_{{\text{lb1}}}} + {N_{{\text{lb2}}}}} \right)l\cos \phi +\\&\qquad\qquad \left( {{P_{{\text{lb1}}}} + {P_{{\text{lb2}}}}} \right)l\sin \phi \end{split} $$ (8) 式中
$$\begin{split} & {x_{\text{b}}} = \frac{1}{2} \times [{x_{{\text{w1}}}} + \left( {{L_{11}} + {L_{21}}} \right)\sin {\theta _1} + \\&\qquad {x_{{\text{w2}}}} + \left( {{L_{12}} + {L_{22}}} \right)\sin {\theta _2}] \end{split}$$ (9) 对航向角有
$$ {I_{\text{z}}}\ddot \psi = \left( { - {N_{{\text{lb1}}}} + {N_{{\text{lb2}}}}} \right){l_{\text{w}}} $$ (10) 式中
$$ \begin{split} & 2{l_{\text{w}}}\psi = - {x_{{\text{w1}}}} - \left( {{L_{11}} + {L_{21}}} \right)\sin {\theta _1} + \\&\qquad {x_{{\text{w2}}}} + \left( {{L_{12}} + {L_{22}}} \right)\sin {\theta _2} \end{split} $$ (11) 因为滚转角恒为0，所以左右轮支持力相等，即
$$ {P_{{\text{wl1}}}} = {P_{{\text{wl2}}}} $$ (12) 联立式（1）~（5）及其在另一侧的表示，以及式（6）~（12），共计17个方程，消去12个中间变量
$ {N_{{\text{f1}}}} $ 、$ {N_{{\text{f2}}}} $ 、$ {N_{{\text{wl1}}}} $ 、$ {N_{{\text{wl2}}}} $ 、$ {P_{{\text{wl1}}}} $ 、$ {P_{{\text{wl2}}}} $ 、$ {N_{{\text{lb1}}}} $ 、$ {N_{{\text{lb2}}}} $ 、$ {P_{{\text{lb1}}}} $ 、$ {P_{{\text{lb2}}}} $ 、$ {x_{{\text{w1}}}} $ 、$ {x_{{\text{w2}}}} $ ，解得系统微分方程。定义状态向量与控制向量：
$$ \begin{gathered} {\boldsymbol{x}} = \left[ {{x_b},{{\dot x}_b},\psi ,\dot \psi ,\phi ,\dot \phi ,{\theta _1},{{\dot \theta }_1},{\theta _2},{{\dot \theta }_2}} \right]^{\mathrm{T }} \\\qquad {\boldsymbol{u}} = \left[ {{T_{w1}},{T_{w2}},{T_{l1}},{T_{l2}}} \right]^{\mathrm{T }} \\ \end{gathered} $$ 系统微分方程可以表示为
$$ \dot {\boldsymbol{x}} = f({\boldsymbol{x}},{\boldsymbol{u}}) $$ (13) 在系统平衡点
$ {\boldsymbol{x}} = \left[ {{x_b},{{\dot x}_b},\psi ,\dot \psi ,0,0,0,0,0,0} \right]^{\mathrm{T }} $ 处进行线性化。$$ \begin{gathered} {\boldsymbol{A}} = \frac{{\partial f}}{{\partial x}}({\boldsymbol{x}},{\boldsymbol{u}}),{\boldsymbol{B}} = \frac{{\partial f}}{{\partial u}}({\boldsymbol{x}},{\boldsymbol{u}}), \dot {\boldsymbol{x}} = {\boldsymbol{Ax}} + {\boldsymbol{Bu}} \\ \end{gathered} $$ (14) 1.3 平面五连杆运动学分析
为了解算
${L_1}$ 、${L_2}$ 、${\theta _1}$ 、${\theta _2}$ ，通过虚拟模型控制得到实际关节电机力矩与控制器输出控制量的映射关系，需要对腿部五连杆结构进行正运动学解算。五连杆结构示意图如图2所示。
图 2 五连杆结构示意下载:
全尺寸图片
图2中
${l_1} = {l_4}$ ，${l_2} = {l_3}$ ，参考系为机体坐标系，根据图2可得：$$ \left\{ {\begin{array}{*{20}{l}} {{x_{\text{N}}} = {x_{\text{M}}} + {l_2}\cos {\alpha _2} = {x_{\text{G}}} + {l_3}\cos {\alpha _3}} \\ {{y_{\text{N}}} = {y_{\text{M}}} + {l_2}\sin {\alpha _2} = {y_{\text{G}}} + {l_3}\sin {\alpha _3}} \end{array}} \right. $$ (15) 求解可得：
$$ {\alpha _2} = 2\tan \frac{{d_{\mathrm{temp2}} - \sqrt {{d_{\mathrm{temp1}}^2} + {d_{\mathrm{temp2}}^2} - {d_{\mathrm{temp3}}^2}} }}{{d_{\mathrm{temp1}} - d_{\mathrm{temp3}}}} $$ 式中：
$$ \begin{gathered}d_{\mathrm{temp1}}=2l_2(x_{\text{M}}-x_{\text{G}}) \\ d_{\mathrm{temp2}}=2l_2(y_{\text{M}}-y_{\text{G}}) \\ d_{\mathrm{temp3}}=l_2^2-l_3^2+(x_{\text{M}}-x_{\text{G}})^2+(y_{\text{M}}-y_{\text{G}})^2 \\ \end{gathered} $$ 则有：
$$ \left\{\begin{array}{*{20}{l}}x_{\mathrm{N}}=-0.5l_5+l_1\cos\alpha_1+l_2\cos\alpha_2 \\ y\mathrm{_N}=l_1\sin\alpha_1+l_2\sin\alpha_2\end{array}\right. $$ (16) $$ \alpha = \frac{{\text{{\text{π}} }}}{2} - {\tan ^{ - 1}}\frac{{{y_{\text{N}}}}}{{{x_{\text{N}}}}},{\alpha _3} = {\tan ^{ - 1}}\frac{{{y_{\text{N}}} - {y_{\text{G}}}}}{{{x_{\text{N}}} - {x_{\text{G}}}}} $$ 对式（15）求导可得：
$$ \left\{ {\begin{array}{*{20}{l}} {{{\dot x}_{\text{N}}} = {{\dot x}_{\text{M}}} - {l_2}{{\dot \alpha }_2}\sin {\alpha _2} = {{\dot x}_{\text{G}}} - {l_3}{{\dot \alpha }_3}\sin {\alpha _3}} \\ {{{\dot y}_{\text{N}}} = {{\dot y}_{\text{M}}} + {l_2}{{\dot \alpha }_2}\cos {\alpha _2} = {{\dot y}_{\text{G}}} + {l_3}{{\dot \alpha }_3}\cos {\alpha _3}} \end{array}} \right. $$ 消去
$ {\dot \alpha _3} $ ，求解可得：$$ {\dot \alpha _2} = \frac{{({{\dot x}_{\text{G}}} - {{\dot x}_{\text{M}}})\cos {\alpha _3} + ({{\dot y}_{\text{G}}} - {{\dot y}_{\text{M}}})\sin {\alpha _3}}}{{{l_2}\sin ({\alpha _3} - {\alpha _2})}} $$ (17) 对式（16）求导并代入式（17）可得：
$$ \left[\begin{array}{*{20}{l}}\dot{x}_{\text{N}} \\ \dot{y}_{\text{N}}\end{array}\right]=\boldsymbol{J}\left[\begin{array}{*{20}{l}}\dot{\alpha}_1 \\ \dot{\alpha}_4\end{array}\right] $$ (18) $$ \begin{gathered} {{\boldsymbol{J}}_{11}} = \frac{{{l_1}\sin ({\alpha _1} - {\alpha _2})\sin {\alpha _3}}}{{\sin ({\alpha _2} - {\alpha _3})}},{{\boldsymbol{J}}_{12}} = \frac{{{l_4}\sin ({\alpha _3} - {\alpha _4})\sin {\alpha _2}}}{{\sin ({\alpha _2} - {\alpha _3})}} \\ {{\boldsymbol{J}}_{21}} = - \frac{{{l_1}\sin ({\alpha _1} - {\alpha _2})\cos {\alpha _3}}}{{\sin ({\alpha _2} - {\alpha _3})}},{{\boldsymbol{J}}_{22}} = - \frac{{{l_4}\sin ({\alpha _3} - {\alpha _4})\cos {\alpha _2}}}{{\sin ({\alpha _2} - {\alpha _3})}} \\ \end{gathered} $$ 式中
$ {\boldsymbol{J }}$ 为雅可比矩阵。2. 控制系统设计
所设计的机器人整体控制系统结构如图3所示，通过状态估计器对传感器数据进行融合处理，得到机器人状态量；使用模型预测控制对状态量
$ x $ 进行闭环控制，同时根据建模相关假设，设计腿长控制器与滚转角补偿器；最后，使用VMC将输出的${T_l}$ 、${F_l}$ 映射到实际关节电机力矩。
图 3 机器人控制系统结构下载:
全尺寸图片
2.1 模型预测控制
将连续的系统状态方程按一定的采样周期离散化，模型预测控制的周期表示为
$\Delta T$ ，近似认为在一个周期中，状态微分$\dot {\boldsymbol{x}}$ 不变，根据式（14）有$$ \dot{\boldsymbol{x}}(k)=\frac{\boldsymbol{x}(k+1)-\boldsymbol{x}(k)}{\Delta T}=\boldsymbol{A} \boldsymbol{x}(k)+\boldsymbol{B} \boldsymbol{u}(k) $$ 则系统的离散状态方程可以表示为
$$ \boldsymbol{x}(k+1)=\tilde{\boldsymbol{A}} \boldsymbol{x}(k)+\tilde{\boldsymbol{B}} \boldsymbol{u}(k)$$ 式中：
$$ \tilde{\boldsymbol{A}}=\boldsymbol{I}+\Delta T \cdot \boldsymbol{A}, \tilde{\boldsymbol{B}}=\Delta T \cdot \boldsymbol{B} $$ 由此得到未来
$ {N_P} $ 个周期内系统状态的预测值为$$ \begin{gathered} \boldsymbol{x}(k+1)=\tilde{\boldsymbol{A}} \boldsymbol{x}(k)+\tilde{\boldsymbol{B}} \boldsymbol{u}(k) \\ \boldsymbol{x}(k+2)=\tilde{\boldsymbol{A}}^2 \boldsymbol{x}(k)+\tilde{\boldsymbol{A}} \tilde{\boldsymbol{B}} \boldsymbol{u}(k)+\tilde{\boldsymbol{B}} \boldsymbol{u}(k+1) \\ \vdots \\ \boldsymbol{x}\left(k+N_{\mathrm{P}}\right)=\tilde{\boldsymbol{A}}^{N_{\mathrm{p}}} \boldsymbol{x}(k)+\tilde{\boldsymbol{A}}^{N_{\mathrm{p}}-1} \tilde{\boldsymbol{B}} \boldsymbol{u}(k) +\\ \tilde{\boldsymbol{A}}^{N_{\mathrm{p}}-2} \tilde{\boldsymbol{B}} \boldsymbol{u}(k+1)+\cdots+\tilde{\boldsymbol{B}} \boldsymbol{u}\left(k+N_{\mathrm{P}}-1\right) \end{gathered} $$ 为了减小计算量，提高控制实时性，认为
$ {N_C} $ ~$ {N_P} $ 范围内的控制量不变。将未来$ {N_C} $ 个周期内的控制量表示为${\boldsymbol{U}}(k)$ 。$$ {\boldsymbol{U}}(k) = {\left[ {\begin{array}{*{20}{l}} {{{\boldsymbol{u}}^{\text{T}}}(k)}&{{{\boldsymbol{u}}^{\text{T}}}(k + 1)}& \cdots &{{{\boldsymbol{u}}^{\text{T}}}(k + {N_{\text{C}}} - 1)} \end{array}} \right]^{\text{T}}} $$ 将预测值输出表示为
$$ \boldsymbol{X}(k)=\boldsymbol{A}_{\mathrm{QP}} \boldsymbol{x}(k)+\boldsymbol{B}_{\mathrm{QP}} \boldsymbol{U}(k) $$ (19) 式中：
$$ {\boldsymbol{X}}(k) = \left[ {\begin{array}{*{20}{l}} {{\boldsymbol{x}}(k + 1)} \\ {{\boldsymbol{x}}(k + 2)} \\ \vdots \\ {{\boldsymbol{x}}(k + {N_P})} \end{array}} \right],{{\boldsymbol{A}}_{{\text{QP}}}} = \left[ {\begin{array}{*{20}{l}} {\boldsymbol{A}} \\ {{{\boldsymbol{A}}^2}} \\ \vdots \\ {{{\boldsymbol{A}}^{{N_{\text{P}}} - 1}}} \end{array}} \right], $$ $$ {{\boldsymbol{B}}_{{\text{QP}}}} = \left[ {\begin{array}{*{20}{l}} {\boldsymbol{B}}&0& \cdots &0 \\ {{\boldsymbol{AB}}}&{\boldsymbol{B}}& \cdots &0 \\ \vdots & \vdots & \ddots & \vdots \\ {{{\boldsymbol{A}}^{{N_{\text{P}}} - 1}}{\boldsymbol{B}}}&{{{\boldsymbol{A}}^{{N_{\text{P}}} - 2}}{\boldsymbol{B}}}& \cdots &{\boldsymbol{B}} \end{array}} \right]\left[ {\begin{array}{*{20}{l}} 1&0& \cdots &0 \\ 0&1& \cdots &0 \\ \vdots & \vdots & \ddots & \vdots \\ 0&0& \cdots &1 \end{array}} \right] $$ 由式（19）可知，系统未来的状态量由未来的控制量决定，由此可以构建优化问题，求解一个
${\boldsymbol{U}}(k)$ ，使状态$ {\boldsymbol{X}}(k) $ 与期望$D$ 的误差尽可能小：$$ \begin{aligned} \mathop {\min }\limits_{{\boldsymbol{U}}(k)} J[{\boldsymbol{U}}(k)] = & {[{\boldsymbol{X}}(k) - {\boldsymbol{D}}]^{\text{T}}}{\boldsymbol{Q}}[{\boldsymbol{X}}(k) - {\boldsymbol{D}}] + {{\boldsymbol{U}}^{\text{T}}}(k){\boldsymbol{RU}}(k) \\ \end{aligned} $$ (20) 式中第1项
$ {[{\boldsymbol{X}}(k) - {\boldsymbol{D}}]^{\text{T}}}{\boldsymbol{Q}}[{\boldsymbol{X}}(k) - {\boldsymbol{D}}] $ 是为了让$ {\boldsymbol{X}}(k) - {\boldsymbol{D}} $ 的差值尽可能小，${\boldsymbol{ Q}} $ 为反馈权重矩阵；第2项$ {{\boldsymbol{U}}^{\text{T}}}(k){\boldsymbol{RU}}(k) $ 是为了保证控制量不太大，$ {\boldsymbol{R}} $ 为控制量权重矩阵。将式（19）代入到式（20）：
$$\begin{split} & J[{\boldsymbol{U}}(k)] = {{\boldsymbol{U}}^{\text{T}}}(k)({\boldsymbol{B}}_{{\text{QP}}}^{\text{T}}{\boldsymbol{Q}}{{\boldsymbol{B}}_{{\text{QP}}}} + {\boldsymbol{R}}){\boldsymbol{U}}(k) + \\&\qquad 2{{\boldsymbol{E}}^{\text{T}}}{\boldsymbol{Q}}{{\boldsymbol{B}}_{{\text{QP}}}}{\boldsymbol{U}}(k) + {{\boldsymbol{E}}^{\text{T}}}{\boldsymbol{QE}} \end{split} $$ (21) 式中
${\boldsymbol{E}} = {{\boldsymbol{A}}_{{\text{QP}}}}{\boldsymbol{x}}(k) - {\boldsymbol{D}}$ ，$ {{\boldsymbol{E}}^{\text{T}}}{\boldsymbol{QE}} $ 为常数，可以忽略。至此，可以将这个优化问题转化为二次规划的一般形式：
$$ \begin{array}{c} \mathop {\min }\limits_x f({\boldsymbol{x}}) = \frac{1}{2}{{\boldsymbol{x}}^{\text{T}}}{\boldsymbol{Hx}} + {{\boldsymbol{g}}^{\text{T}}}{\boldsymbol{x}} \\ {\boldsymbol{x}} = {\boldsymbol{U}}(k) \\ {\boldsymbol{H}} = 2({\boldsymbol{B}}_{{\text{QP}}}^{\text{T}}{\boldsymbol{Q}}{{\boldsymbol{B}}_{{\text{QP}}}} + {\boldsymbol{R}}) \\ {\boldsymbol{g}} = {(2{{\boldsymbol{E}}^{\text{T}}}{\boldsymbol{Q}}{{\boldsymbol{B}}_{\text{QP}}})^{\text{T}}} = 2{\boldsymbol{B}}_{{\text{QP}}}^{\text{T}}{\boldsymbol{Q}}({{\boldsymbol{A}}_{{\text{QP}}}}{\boldsymbol{x}}(k) - {\boldsymbol{D}}) \\ \end{array} $$ 实际控制系统中，还需要满足控制量和状态量的约束条件，尤其是腿部机构前后摆动的范围，否则腿部会撞击到限位块。
$$ \begin{gathered} - {{\boldsymbol{u}}_{\max }} \leqslant {\boldsymbol{u}}(k + i) \leqslant {{\boldsymbol{u}}_{\max }},i = 0,1, \cdots ,{N_C} - 1 \\ - {{\boldsymbol{x}}_{\max }} \leqslant {\boldsymbol{x}}(k + j) \leqslant {{\boldsymbol{x}}_{\max }},j = 0,1, \cdots ,{N_P} - 1 \\ \end{gathered} $$ 接下来使用二次规划求解库求解带约束的二次规划问题，将得到的优化解
$ {\boldsymbol{U}}(k) $ 的第一个分量作为当前周期的控制量。2.2 虚拟模型控制
为了将模型预测控制得到的关节输出力矩
${T_l}$ 和腿长控制器输出的${F_l}$ 转化为关节电机的输出力矩，使用VMC得到输出力矩与${T_l}$ 、${F_l}$ 之间的映射关系。对如图2中一侧五连杆进行分析，VMC通过构建虚拟力，使产生的虚拟力驱动机器人实现期望的控制目标，这种虚拟力所产生的效果与关节力矩相同。建立虚拟力与关节扭矩之间的关系，需要先将工作空间的力映射到关节空间的关节力矩，即正运动学模型
$$ {\boldsymbol{x}} = f({\boldsymbol{q}}) $$ $$ {\boldsymbol{x}} = {[\begin{array}{*{20}{l}} {{x_{\text{C}}}}&{{y_{\text{C}}}} \end{array}]^{\text{T}}},{\boldsymbol{q }}= {\left[ {\begin{array}{*{20}{l}} {{\alpha _1}}&{{\alpha _4}} \end{array}} \right]^{\text{T}}} $$ 对其求微分：
$$ {\boldsymbol{\dot x}} = {\boldsymbol{J\dot q}} $$ (22) 式中
$ {\boldsymbol{J}} $ 为式（18）中的雅可比矩阵。根据虚功原理可知
$$ {{\boldsymbol{T}}^{\text{T}}}{\boldsymbol{\dot q}} + {( - {\boldsymbol{F}})^{\text{T}}}{\boldsymbol{\dot x}} = 0 $$ $$ {\boldsymbol{T}} = {\left[ {\begin{array}{*{20}{l}} {{{\boldsymbol{T}}_{\boldsymbol{1}}}}&{{{\boldsymbol{T}}_{\boldsymbol{2}}}} \end{array}} \right]^{\text{T}}},{\boldsymbol{F}} = {\left[ {\begin{array}{*{20}{l}} {{{\boldsymbol{F}}_{\boldsymbol{x}}}}&{{{\boldsymbol{F}}_{\boldsymbol{y}}}} \end{array}} \right]^{\text{T}}} $$ (23) 将式（23）代入到式（22）,可得
$$ {\boldsymbol{T}} = {{\boldsymbol{J}}^{\text{T}}}{\boldsymbol{F}} $$ (24) 同时由于
${T_l}$ 垂直于${l_0}$ ，$ \boldsymbol{F}_l $ 与${l_0}$ 共线，所以$$ F = \left[ {\begin{array}{*{20}{l}} { - \cos \alpha }&{\sin \alpha } \\ {\sin \alpha }&{\cos \alpha } \end{array}} \right]\left[ {\begin{array}{*{20}{l}} {\dfrac{{{{\boldsymbol{T}}_l}}}{{{l_0}}}} \\ {{{\boldsymbol{F}}_l}} \end{array}} \right] $$ (25) 将式（25）代入到式（24），即可得到
${{\boldsymbol{T}}_1}$ 、${{\boldsymbol{T}}_2}$ 与${{\boldsymbol{T}}_l}$ 、${{\boldsymbol{F}}_l}$ 之间的映射关系。2.3 腿长与滚转角控制
在轮腿式平衡机器人中，腿部机构除了起到控制机体俯仰角等的作用以外，还需要完成控制机体滚转角的任务，并且作为机器人整体的悬挂装置。以实现机器人在复杂地面移动时，始终保持机体的稳定。腿长与滚转角的控制，使用比例积分微分控制（proportional-integral-derivative control，PID）并联控制方式。
腿长控制部分使用PID控制，由于腿部需要达到弹簧阻尼的效果，使用较大的
$ K\mathrm{_D} $ 以模拟阻尼。左右两侧分别使用2个腿长控制器。滚转角补偿部分，使用PID控制，将输出以相反的符号叠加到两侧腿长控制的输出上。3. 仿真与实验验证
3.1 MPC控制器仿真实验
为了验证所设计的MPC控制器，在Simulink中搭建仿真模型，机器人模型使用式（13）中的非线性模型。控制器参数设计为
$$ \begin{array}{c}\boldsymbol{\mathbf{\mathit{N}}}_{\text{P}}=20,N_{\text{C}}=6, \\ \boldsymbol{Q}=\text{diag}(5000,40,50,0.25,100,0.2,50,0.07,50,0.07), \\ \boldsymbol{R}=\text{diag}(0.01,0.01,0.01,0.01) \end{array} $$ 式中
$ {\text{diag}} $ 为对角矩阵。作为对比，基于式（14）中的线性模型，设计 LQR控制器，参数为
$$\begin{array}{c} {\boldsymbol{Q}} = {\text{diag}}(1\;000,30,2\;000,20,200,10,500,10,500,10), \\ {\boldsymbol{R}} = {\text{diag}}(4,4,4,4) \end{array} $$ 设机器人在1 s时，期望速度为1 m/s，2种控制器对比仿真结果如图4所示，期望参考值用引用（reference，REF）曲线表示。
图 4 LQR/MPC控制曲线（1 m/s）下载:
全尺寸图片
设期望速度为2 m/s，仿真结果如图5所示，期望参考值用REF曲线表示。
图 5 LQR/MPC控制曲线（2 m/s）下载:
全尺寸图片
由图4可知，在期望速度为1 m/s时，LQR与MPC对机器人控制效果相近，都可以较快跟踪期望，保持机体俯仰角的稳定，仅有小幅波动。在LQR控制下，机体俯仰角的波动相对较大，但整体控制效果并无明显差距。
由图5可知，在期望速度为2 m/s时，LQR相对MPC有更快的响应速度，但在实际系统中，左右腿的摆角有一定限制，只能在
$[ - {28^ \circ },{28^ \circ }]$ 的范围内运动。LQR控制器无法处理该限制，因此仿真结果中腿部摆角已经超出限制；而MPC控制器可以设置约束条件，仿真结果中腿部摆角在接近${28^ \circ }$ 时，停止增大，所以响应速度相对LQR较慢。设机器人在1 s时，期望航向角速度为
${300^ \circ }$ /s，仿真结果如图6所示，期望参考值用REF曲线表示。在期望航向角速度为${300^ \circ }$ /s 时，2种控制器控制效果相近，都能够快速的跟踪期望值。
图 6 LQR/MPC控制曲线（1 s）下载:
全尺寸图片
对比仿真结果可得，MPC控制器对比LQR控制器在响应速度上没有明显优势，但可以通过预测未来周期内的运动状态，优化求解约束范围内的控制量，使得系统运动不超过运动范围限制。因此，MPC控制器更适用于轮腿式机器人的控制。
为验证控制器抵抗作用于机体的外力扰动的能力，在1 s时，对机器人施加前后方向的扰动力
$ \Delta F={m}_{b}\cdot \Delta a $ ，$ \Delta a = 1\;{\text{m/}}{{\text{s}}^{\text{2}}} $ ，仿真结果如图7所示。
图 7 扰动下机器人状态曲线下载:
全尺寸图片
由图7可知，机器人受外部扰动时，可以通过调整俯仰角和腿部摆角来保持机器人的稳定，具有较好的抗干扰能力。
3.2 整体控制系统仿真实验
为验证所设计的控制系统对轮腿式机器人的控制效果，在Webots中导入所设计的实物机器人模型。在机器人操作系统ROS2中编写控制代码，使用QuadProg++作为二次规划求解库，每次计算MPC大约需要0.4 ms。图8给出了仿真中机器人在复杂地形下，可以保持机身的水平。
图 8 Webots中机器人地形适应能力下载:
全尺寸图片
为验证机器人在倒地状态下，是否能够自启动，在1 s时发送启动命令，启动过程速度姿态如图9所示。
图 9 Webots中机器人倒地自启的状态曲线下载:
全尺寸图片
由图9可知，机器人能够快速从倒地状态恢复到站立状态，速度短暂波动后快速收敛到0，3个姿态角也快速收敛到0。机器人具有倒地自启的能力，使得其鲁棒性具有一定的保证。
设机器人在1 s时，期望速度为1 m/s，仿真结果如图10所示。设机器人在1 s时，期望航向角速度为
${300^ \circ }$ /s ，仿真结果如图11所示。上述实验结果中，期望参考值均用REF曲线表示。由图10和图11可知，机器人的速度和航向角速度能够快速收敛到期望附近，各状态量变化趋势也和第3.1节中的仿真结果相近，验证了本文所推导的机器人数学模型准确，所设计的控制器有效。
图 10 Webots中速度控制曲线下载:
全尺寸图片
图 11 Webots中航向角速度控制曲线下载:
全尺寸图片
4. 结束语
本文针对轮腿式自平衡机器人的运动控制问题，首先建立了完整且精简的运动学与动力学模型，并根据轮腿式机器人系统具有非线性、欠驱动、强耦合特性，提出了基于MPC和VMC的控制系统，解决轮腿式机器人数学建模较为复杂、计算量较大、控制性能较差等问题。仿真实验结果表明系统在确保机动性的同时，保持了机体部分的稳定性，展现出良好的适应复杂地形的能力，且避免了腿部运动幅度过大导致的限位碰撞问题。轮腿式结构的运用显著提升了机器人的性能，拓展了其适用场景，对机器人研究具有重要意义。未来研究将重点解决自适应外界扰动和自身参数摄动的问题，并探索通过跳跃等方法适应更加复杂的场景。
-
图 1 轮腿式机器人模型
下载:
全尺寸图片
图 2 五连杆结构示意
下载:
全尺寸图片
图 3 机器人控制系统结构
下载:
全尺寸图片
图 4 LQR/MPC控制曲线（1 m/s）
下载:
全尺寸图片
图 5 LQR/MPC控制曲线（2 m/s）
下载:
全尺寸图片
图 6 LQR/MPC控制曲线（1 s）
下载:
全尺寸图片
图 7 扰动下机器人状态曲线
下载:
全尺寸图片
图 8 Webots中机器人地形适应能力
下载:
全尺寸图片
图 9 Webots中机器人倒地自启的状态曲线
下载:
全尺寸图片
图 10 Webots中速度控制曲线
下载:
全尺寸图片
图 11 Webots中航向角速度控制曲线
下载:
全尺寸图片
表 1 轮腿式机器人模型变量定义
符号 含义 单位 ${N_{{\text{f}}1}},{N_{{\text{f2}}}}$ 地面对轮子摩擦力，箭头所示 N ${N_{{\text{wl1}}}},{N_{{\text{wl2}}}}$ 轮子对腿部水平力，箭头所示 N ${P_{{\text{wl1}}}},{P_{{\text{wl2}}}}$ 轮子对腿部竖直力，箭头所示 N ${N_{{\text{lb1}}}},{N_{{\text{lb2}}}}$ 腿部对机身水平力，箭头所示 N ${P_{{\text{lb1}}}},{P_{{\text{lb2}}}}$ 腿部对机身竖直力，箭头所示 N ${x_{{\text{w1}}}},{x_{{\text{w2}}}}$ 轮子的位移，箭头所示 m ${T_{{\text{w1}}}},{T_{{\text{w2}}}}$ 轮子输出力矩，顺时针 $ \text{N}\cdot \text{m} $ ${T_{{\text{l1}}}},{T_{{\text{l2}}}}$ 关节输出力矩，顺时针 $ \text{N}\cdot \text{m} $ ${x_{\text{b}}}$ 机体的位移，箭头所示 m $\psi $ 航向角，图示为正 rad $\phi $ 机身俯仰角，图示为正 rad ${\theta _1},{\theta _2}$ 腿部与竖直方向夹角，图示为正 rad 表 2 轮腿机器人模型参数定义
符号 含义 单位 ${m_{\text{w}}},{m_{\text{l}}},{m_{\text{b}}}$ 轮子、腿部、机体的质量 kg ${I_{\text{w}}},{I_{{\text{l1}}}},{I_{{\text{l2}}}},{I_{\text{b}}}$ 轮子、腿部、机体俯仰角转动惯量 $ \text{kg}\cdot {\text{m}}^{2} $ $R$ 轮子半径 m ${L_{11}},{L_{12}}$ 腿重心到轮转轴的距离 m ${L_{21}},{L_{22}}$ 腿重心到机体转轴的距离 m $l$ 机体重心到转轴的距离 m $I_{\text{z}}$ 机体航向角的转动惯量 $ \text{kg}\cdot {\text{m}}^{2} $ ${l_{\text{w}}}$ 两腿距离的一半 m -
[1] 谢哲东, 马婧, 孟祥雨, 等. 轮腿式越障机器人的研究及发展[J]. 机械工程与自动化, 2017(1): 219−220. [2] 张弨. 双足轮腿机器人系统设计与运动控制研究[D]. 哈尔滨: 哈尔滨工业大学, 2020: 2−11. [3] 孙俊凯. 轮腿式星表探测机器人运动规划与控制算法研究[D]. 长春: 吉林大学, 2023: 10−18. [4] BJELONIC M, BELLICOSO C D, DE VIRAGH Y, et al. Keep rollin’—whole-body motion control and planning for wheeled quadrupedal robots[J]. IEEE robotics and automation letters, 2019, 4(2): 2116−2123. doi: 10.1109/LRA.2019.2899750 [5] BJELONIC M, SANKAR P K, BELLICOSO C D, et al. Rolling in the deep–hybrid locomotion for wheeled-legged robots using online trajectory optimization[J]. IEEE robotics and automation letters, 2020, 5(2): 3626−3633. doi: 10.1109/LRA.2020.2979661 [6] KLEMM V, MORRA A, SALZMANN C, et al. Ascento: a two-wheeled jumping robot[C]//2019 International Conference on Robotics and Automation. Piscataway: IEEE, 2019: 7515−7521. [7] KLEMM V, MORRA A, GULICH L, et al. LQR-assisted whole-body control of a wheeled bipedal robot with kinematic loops[J]. IEEE robotics and automation letters, 2020, 5(2): 3745−3752. doi: 10.1109/LRA.2020.2979625 [8] KLEMM V, DE VIRAGH Y, ROHR D, et al. Non-smooth trajectory optimization for wheeled balancing robots with contact switches and impacts[J]. IEEE transactions on robotics, 2023, PP(99): 1−20. [9] LI Xu, ZHOU Haitao, FENG Haibo, et al. Design and experiments of a novel hydraulic wheel-legged robot (WLR)[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway: IEEE, 2018: 3292−3297. [10] LI Xu, YU Haoyang, FENG Haibo, et al. Design and control for WLR-3P: a hydraulic wheel-legged robot[J]. Cyborg and bionic systems, 2023, 4: 0025. doi: 10.34133/cbsystems.0025 [11] WANG Shuai, CUI Leilei, ZHANG Jingfan, et al. Balance control of a novel wheel-legged robot: design and experiments[C]//2021 IEEE International Conference on Robotics and Automation. Piscataway: IEEE, 2021: 6782−6788. [12] 陈阳, 王洪玺, 张兰勇. 轮腿式平衡机器人控制[J]. 信息与控制, 2023, 52(5): 648−659. [13] FENG Xujiong, LIU Shuaishuai, YUAN Qiang, et al. Research on wheel-legged robot based on LQR and ADRC[J]. Scientific reports, 2023, 13(1): 15122. doi: 10.1038/s41598-023-41462-1 [14] ZHANG Jingfan, WANG Shuai, WANG Haitao, et al. An adaptive approach to whole-body balance control of wheel-bipedal robot ollie[C]//2022 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway: IEEE, 2022: 12835−12842. [15] 于红英, 唐德威, 王建宇. 平面五杆机构运动学和动力学特性分析[J]. 哈尔滨工业大学学报, 2007, 39(6): 940−943. [16] 谢惠祥, 罗自荣, 尚建忠. 四足机器人对角小跑动态控制[J]. 国防科技大学学报, 2014, 36(4): 146−151. [17] 韩连强, 陈学超, 余张国, 等. 面向离散地形的欠驱动双足机器人平衡控制方法[J]. 自动化学报, 2022, 48(9): 2164−2174. [18] 侯兰东. 双足机器人模型预测控制算法的研究[D]. 济南: 齐鲁工业大学, 2023. [19] 张博雯. 基于模型预测控制的四足机器人运动稳定策略研究[D]. 哈尔滨: 哈尔滨工业大学, 2022.
