Research on Jumping Method of Two Wheeled-leg Robot Based on Whole-body Torque Control
-
摘要: 双腿轮机器人由于内在不稳定性以及强耦合非线性特性, 其运动控制尤其是高动态运动控制非常困难. 为此, 提出基于最优力分配的全身力矩控制框架, 可同时实现双腿轮机器人的自平衡与躯干位姿控制; 为提高双腿轮机器人在高速运动时跨越垂直障碍物的能力, 提出应对垂直障碍的跳跃动作规划方法, 并基于全身力矩控制框架进行控制与实现; 通过分析简化的轮式倒立摆模型, 得到腾空时飞轮转动对俯仰姿态的动力学影响, 实现腾空阶段俯仰姿态的调整. 设置连续跳跃仿真实验与有无飞轮调整的俯仰姿态对比实验, 其仿真结果证明所提方法的有效性与鲁棒性.Abstract: The motion control, especially high dynamic motion control of a two wheeled-leg robot is difficult because of its inherent instability and strong coupling nonlinear characteristics. In this paper, a whole-body torque control framework based on optimal force distribution is proposed, which can realize the self-balancing and torso posture control of the two wheeled-leg robot simultaneously. A jumping movement planning method is proposed to improve the ability of the two wheeled-leg robot to step over vertical obstacles during high-speed movement based on the whole-body torque control method. By analyzing the simplified wheeled inverted pendulum model, the dynamic effect of the fly wheel rotation on the pitch attitude during flight is obtained, and the pitch attitude adjustment during the flight phase is realized. A continuous jump simulation experiment and a comparison experiment of pitch attitude with or without fly wheel adjustment are set up. The simulation results demonstrate the effectiveness and robustness of the proposed method.
-
近年来, 轮式移动方式以其高效率、高能量利用率引起了腿足式机器人研究者的广泛关注[1]. 试图搭建腿轮构型机器人, 将腿式机器人的高越障能力与轮式运行方式的快速移动能力相结合. 因此, 双腿轮机器人、四腿轮机器人等平台应运而生[2]. 其中, 双腿轮机器人具有更加灵活的移动与转向性能, 在多种行业工况下具有广阔的应用前景[3].
为使双腿轮机器人的地形适应能力进一步提高, 面临较高垂直障碍时, 可采用跳跃形式快速跨越. 苏黎世理工大学研发的双腿轮机器人Accento[4-5]在基于线性二次调节器(Linear quadratic regulator, LQR)平衡控制器的基础上进行跳跃控制器的设计, 实现了跳跃运动, 收到了显著的效果. 美国波士顿动力公司研发的Handle双腿轮机器人[6]实现了快速加减速、跳跃、搬运重物等功能, 展现了良好的动态平衡能力. 如何构建一个可以将运动与平衡功能集于一体的具有扩展性的双腿轮控制框架, 成为一个亟待解决的问题. 但是, 双腿轮机器人系统为内在不稳定系统, 且具有欠驱动特性, 运动过程中不存在稳定平衡点, 因此需要借助复杂的控制算法使其保持动态平衡. 而针对该系统的高动态运动控制研究尚处于起步阶段.
移动机器人经过半个世纪的发展, 基于静态稳定的控制算法已成熟, 主要应用对象包括双足机器人[7]、四足机器人[8]、六足机器人[9]等, 该类算法在保证机器人静态稳定裕度的前提下进行位置控制, 实现期望运动, 其依据为ZMP (Zero moment point)[10]、ESM (Energy stability margin)[11]、NESM (Normalized ESM)[12]等静态稳定判据, 因此无法实现高动态运动控制. 而双腿轮机器人的支撑面为两轮与地面接触点的连线, 不存在静态稳定区域, 因此静态稳定算法对于双腿轮机器人的借鉴意义不大.
双轮机器人的控制方法已渐趋成熟, 作为一个欠驱动不稳定多耦合系统, 常用于控制算法的有效性验证[13-14], 日本庆应大学搭建了两轮移动机械臂平台, 验证了基于零空间的共振比率控制器[15]、滑膜控制器[16]、非线性反步法控制器等控制轮的移动与机械臂运动的有效性; 意大利技术研究院控制双足机器人站立于两轮移动平台Segway上[17], 采用二次优化方法以质心位置与角动量为调节目标, 生成全身关节力矩, 能够保持平衡并控制平台移动. 大连理工大学团队采用模型预测控制(Model predictive control, MPC)实现了两轮移动平台的运动轨迹跟踪[18]. 以上研究都具有借鉴意义, 但与两轮移动平台不同, 双腿轮平台不再存在两轮同轴被动约束, 这也使得控制器的构造难度大大增加.
近几年来, 腿足式机器人动态控制算法成为研究热点. 得益于各种实时优化解算库[19-20]的开发, 控制中的多解问题可转化为优化问题, 得到局部甚至全局最优解[21-22]. 如美国麻省理工学院的四足机器人Mini-cheetah[23]构造了全身控制与模型预测控制相结合的控制器, 实现了4 m/s的奔跑和碎石路面行走. 双足机器人Atlas[24]跟踪采用模型预测控制器优化的参考轨迹, 实现了左右脚交替三连跳40 cm台阶、前滚翻等高动态运动.
为提高双腿轮机器人的地形适应性, 实现稳定的速度跟随与跳跃, 本文在前期研究的基础上[25], 构造了基于最优力分配的全身力矩控制器并实现了跳跃运动, 其创新点如下:
1)实现了基于优化的自平衡全身力矩控制. 该方法可自行生成同时满足平衡与期望运动的全身力矩, 不需要设计平衡控制器. 受到跳跃着陆冲击干扰时, 能够有效规划期望状态轨迹, 并恢复有界稳定.
2)提出了适用于双腿轮机器人的连续跳跃规划. 可在保证高速运动的前提下, 实现垂直障碍的跨越, 并能够在腾空阶段对俯仰姿态进行有效调整, 保证着陆姿态.
1. 运动学与动力学建模
本文的控制对象以美国HEBI Robotics公司的双腿轮机器人HEBI IGOR[26]为原型, 其简化模型如图1所示. 该机器人具有一个单刚体躯干与两条结构相同相对于躯干左右对称布置的腿轮. 躯干尺寸为$200\;{\rm{ mm}}\times300\;{\rm{ mm}}\times200\; {\rm{mm}}\; (L\times w\times h)$, 质量为40 kg. 腿轮完全伸展长度为1150 mm, 其中大腿长度(L1)为500 mm, 小腿长度(L2)为500 mm, 轮的半径(R)为150 mm. 每条腿轮布置3 个旋转关节, 所有关节都可以实现独立的力矩控制. 髋部与膝部为俯仰旋转关节, 能够实现腿部的收缩与伸展, 轮通过一个俯仰旋转关节与腿部末端相连, 从而实现机器人在三维空间中的移动.
1.1 运动学建模
双腿轮机器人为典型的动基座系统, 浮动坐标系${\Sigma _B}$固连于躯干质心处, 其在惯性系中的线速度与角速度分别描述为${}^{\boldsymbol{O}}{{\boldsymbol{v}}_{\boldsymbol{B}}}$与${}^{\boldsymbol{O}}{{\boldsymbol{\omega}} _{\boldsymbol{B}}}$. 各肢体末端均由关节、连杆与躯干相连, 因此, 肢体末端广义速度同时受到躯干动基座广义速度与各关节角速度的影响.
$$\left[ {\begin{array}{*{20}{c}} {{}^{\boldsymbol{O}}{{\boldsymbol{v}}_{i}}} \\ {{}^{\boldsymbol{O}}{{\boldsymbol{\omega}} _{i}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\boldsymbol{E}}&{ - {}^{\boldsymbol{O}}{{\vec {\boldsymbol{r}}}_{\boldsymbol{B}i}}} \\ {\boldsymbol{0}}&{\boldsymbol{E}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{}^{\boldsymbol{O}}{{\boldsymbol{v}}_{\boldsymbol{B}}}} \\ {{}^{\boldsymbol{O}}{{\boldsymbol{\omega}} _{\boldsymbol{B}}}} \end{array}} \right] + {{\boldsymbol{J}}_{i}}{\dot {{\boldsymbol{q}}}_{i}}$$ (1) 其中, 本文中编号$i= 1, 2$分别代表右腿与左腿, ${}^{\boldsymbol{O}}{{\boldsymbol{v}}_{i}}$与${}^{\boldsymbol{O}}{{\boldsymbol{\omega}} _{i}}$分别为腿i末端相对于世界坐标系的速度与角速度. ${}^{\boldsymbol{O}}{\vec {\boldsymbol{r}}_{\boldsymbol{B}i}}$为由躯干坐标系到腿i末端坐标系的位矢在世界坐标系中的表示, ${{\boldsymbol{q}}_{i}}$为腿${i}$主动关节角度.
1.2 动力学建模
为便于构造分布式控制器, 采用了分布式动力学建模方法[25]. 分布式动力学模型由躯干子系统以及腿轮子系统组成. 腿轮子系统通过广义力旋量与躯干子系统形成迭代递推关系, 其广义力旋量的表示如图1所示. 躯干的动力学模型为
$${{\boldsymbol{M}}_{\boldsymbol{b}}}\left( {\boldsymbol{X}} \right)\ddot {\boldsymbol{X}}{\rm{\;+\;}}{{\boldsymbol{G}}_{\boldsymbol{b}}}\left( {\boldsymbol{X}} \right) = {{\boldsymbol{J}}^{{\rm{T}}}}{\boldsymbol{W}}$$ (2) 其中, ${\boldsymbol{X}} = {\left[ {{}^{O}{x_B}}\;\;{{}^{O}{y_B}} \;\; {{}^{O}{z_B}}\;\;\gamma \;\;\beta \;\;\alpha \right]^{\rm{T}}}$为躯干坐标系${\Sigma _B}$在大地坐标系${\Sigma _O}$中的位姿描述, ${{\boldsymbol{M}}_{\boldsymbol{b}}}$为惯性矩阵, ${{\boldsymbol{G}}_{\boldsymbol{b}}}$为重力补偿项. ${\boldsymbol{W}} = {\left[ {{\boldsymbol{W}}_{1}}\;\;{{{\boldsymbol{W}}_{2}}} \right]^{\rm{T}}}$为腿i末端施加于躯干的广义力旋量, 其中, ${{\boldsymbol{W}}_{i}} = {\left[{{f_{ix}}}\;\;{{f_{iz}}}\;\;{{n_{iy}}} \right]^{\rm{T}}}$. 支撑工况下, 腿轮施加于躯干的力旋量可作为躯干子系统的输入, 从而实现躯干子系统的控制. 由于轮式结构造成的非完整约束, 世界坐标系中躯干侧向运动(${}^{O}{\dot y_B}$)与前向运动(${}^{O}{\dot x_B}$)由躯干坐标系中x方向速度${}^B{{v}_{{B}}}$和航向角度($\alpha $)耦合而成, 基于非完整约束运动学构造矩阵${\boldsymbol{S}}$:
$${\boldsymbol{S}}\;{\rm{ = }}\left[ {\begin{array}{*{20}{c}} {\cos \alpha}&0&0&0&0 \\ {\sin \alpha }&0&0&0&0 \\ 0&1&0&0&0 \\ 0&0&1&0&0 \\ 0&0&0&1&0 \\ 0&0&0&0&1 \end{array}} \right]$$ (3) 使得${\dot{\boldsymbol X}}\;{\rm{=}}\;{\boldsymbol{S\eta}}$, 其中${\boldsymbol{\eta}}\; {\rm{=}}\;[{ {{}^B{{v}_{{B}}}}\;\;{{}^B{{\dot z}_B}}\;\;{\dot \gamma }\;\;{\dot \beta }\;\;{{\omega}}^{\rm{T}}}]$, ${{\omega}} $为航向角速度, 求导得$\ddot{\boldsymbol{X}}{\rm{ \;= \;}}{\boldsymbol{S}}\dot{\boldsymbol{\eta}} $, 因此, 式(2)中动力学模型可改写为
$${\boldsymbol{\Psi}} \dot {\boldsymbol{\eta}}\; {\rm{ + }}\;{{\boldsymbol{G}}_{\boldsymbol{b}}}\left( {\boldsymbol{X}} \right) = {{\boldsymbol{J}}^{{\rm{T}}}}{\boldsymbol{W}}$$ (4) 其中, ${\boldsymbol{\Psi}} ={{\boldsymbol{M}}_{\boldsymbol{b}}}{\boldsymbol{S}}$. 此时, 状态量${\boldsymbol{\eta}}$为躯干刚体在自然坐标系(始终与机体坐标系重合)中的运动描述.
与腿足式机器人不同, 双腿轮机器人处于支撑状态时, 腿轮不仅要提供躯干所需虚拟力, 还要在不同运动状态下保持唯一的平衡姿态. 因此, 以轮为动基座, 采用递推牛顿−欧拉方法建立支撑状态下的腿轮动力学方程如下所示:
$$\begin{split} &{{\boldsymbol{M}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\boldsymbol{q}}_{iw}}} \right){{\ddot {\boldsymbol{q}}}_{i}} + {{\boldsymbol{C}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\dot {\boldsymbol{q}}}_{i}},{{\boldsymbol{q}}_{iw}},{{\dot {\boldsymbol{q}}}_{iw}}} \right)+\\ & \quad{{\boldsymbol{G}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\boldsymbol{q}}_{iw}}} \right){\rm{ + }}\;{{\boldsymbol{I}}}\left( {{{q}_{iw}},{{\dot q}_{iw}}} \right){{\ddot q}_{iw}} + {\boldsymbol{J}}_{i}^{\rm{T}}{{\boldsymbol{W}}_{i}} = {{\boldsymbol{\tau}} _{i}} \end{split} $$ (5) 其中, ${{\boldsymbol{\tau}} _{i}} = {\left[ {{\tau _{iw}}}\;\;{{\tau _{ik}}}\;\;{{\tau _{ih}}} \right]^{\rm{T}}}$分别为腿i的轮关节、膝关节与髋关节力矩. ${{\boldsymbol{q}}_{i}}={\left[ {{{q}_{ic}}}\;\;{{{q}_{ik}}}\;\;{{{q}_{ih}}} \right]^{\rm{T}}}$分别为腿i 的小腿俯仰角度、膝关节与髋关节角度. ${{\boldsymbol{M}}_{i}}\left( {{{\boldsymbol{q}}_{i}}} \right)$、${{\boldsymbol{C}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\dot {\boldsymbol{q}}}_{i}}} \right)$、${{\boldsymbol{G}}_{i}}\left( {{{\boldsymbol{q}}_{i}}} \right)$分别为惯性矩阵、科氏力与离心力、重力补偿矩阵. 值得注意的是, 所提出的分布式动力学建模方法, 其关节角度变量中不包括轮式动基座的线运动与角运动, 但当双腿轮机器人进行高速运动时, 腿部连杆会产生较大的惯性力, 从而影响控制精确度与系统的平衡. 因此, ${\boldsymbol{I}}\left( {{{q}_{iw}},{{\dot q}_{iw}}} \right)$为动基座惯性力补偿矩阵, 以补偿高速运动时动基座传导给各腿轮的惯性力.
当双腿轮机器人处于腾空状态时, 将腿轮子系统分解为两个简化的模型 —— 腿部模型与轮式倒立摆模型, 腿部模型中髋关节坐标系与躯干坐标系方向相同, 以髋关节坐标系为基座, 建立动力学模型如下:
$${\boldsymbol{M}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s}} \right)\ddot {\boldsymbol{q}}_{i}^{s}\;+\; {\boldsymbol{C}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s},\dot {\boldsymbol{q}}_{i}^{s}} \right)\;+\;{\boldsymbol{G}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s}} \right) + {\boldsymbol{J}}_{i}^{{s}{\rm{T}}}{{\boldsymbol{F}}_{is}}{\rm{\;=\;}}{\boldsymbol{\tau}} _{i}^{s}$$ (6) 其中, ${\boldsymbol{\tau}} _{i}^{s} = \left[ {{\tau _{ih}}}\;\;{{\tau _{ik}}} \right]$, ${\boldsymbol{q}}_{i}^{s} = {\left[ {{{q}_{ih}}}\;\;{{{q}_{ik}}} \right]^{\rm{T}}}$.
为研究腾空阶段轮的转动对机器人姿态的影响, 将模型简化为一级轮式倒立摆, 即只关注矢状面内的运动, 将除轮以外的杆件质量集中于一个质点, 形成虚拟质心, 虚拟质心与轮轴的连线为虚拟倒立摆摆杆. 进行无外部接触力的拉格朗日动力学建模, 其动力学模型为
$$\begin{split} &\left( {{m_u} + {m_w} + \dfrac{{{I_w}}}{{{R^2}}}} \right)R{{\ddot \theta }_w} + L{m_u}\ddot \sigma {{\rm{c}} _\sigma } - L{m_u}{{\dot \sigma }^2}{{s} _\sigma } = 0 \\ &L{m_u}R{{{c}} _\sigma }{{\ddot \theta }_w} + \left( {{I_{b}} + {L^2}{m_u}} \right)\ddot \sigma - gL{m_u}{{s} _\sigma } = - {\tau _w} \\[-10pt] \end{split} $$ (7) 其中, ${m_u}$为除轮外其他杆件的质量总和, $\sigma $为倒立摆的虚拟俯仰角, L为虚拟摆杆长度, ${I_{b}}$与${I_w}$分别为摆杆与轮的转动惯量.
2. 轨迹生成器
构造轨迹生成器使机器人可根据障碍物的高度规划此次跳跃的躯干参考轨迹与足端参考轨迹. 本节中, 首先根据障碍物高度确定关键点处的位置与速度, 然后采用三次样条插值法得到躯干质心点与轮最小离地点的连续位置与速度. 简称轮最小离地点为足端. 相关参数的含义如表1所示.
表 1 跳跃参数设置Table 1 Parameters of the jump motion参数 参数含义 H0 正常行走时躯干站立高度在${\Sigma _O}$中的表示 H1 腾空瞬间躯干站立高度在${\Sigma _O}$中的表示 Hpt 足端最大离地距离在${\Sigma _O}$中的表示 Hs 足端最大收缩量在${\Sigma _B}$中的表示 图2展示了机器人跳跃时躯干与足端轨迹, 虚线表示躯干正常站立高度H0, 实线为跳跃过程中躯干的期望轨迹. 支撑相时, 机器人双轮着地; 腾空相时, 机器人双轮离地, 处于自由落体状态. 将跳跃动作分解为三阶段, 分别为起跳($p_0-p_1$)、腾空($p_1- p_3$)、缓冲($p_3-p_4$). 起跳阶段, 通过腿部快速伸展, 为躯干纵向加速, 当达到期望速度($p_1$)时, 腿部开始收缩, 进入腾空相. 腾空相躯干受到重力加速度, 纵向速度不断减小, 在此过程中, 腿部完成了收缩, 并伸展到可保持正常站高H0的长度, 准备落地. 缓冲阶段将纵向速度调整为0, 并保持期望站高.
图 2 躯干与足端矢状面轨迹示意图Fig. 2 The trajectory schematic of the torso and feet in the sagittal plane图2中, $ H_0 $、$ H_1 $以及腿部最大收缩量$H_s $ 为常量, 由机器人本体结构确定, $H_{pt} $由所要跨越的障碍高度确定. $p_2 $ 点的高度也随之确定: ${H_2}=H_s + {H_{pt}} $, 由于p2点的速度${{v}_2} = 0$, 根据能量守恒定理, 可得:
$$\left\{\begin{aligned} &{{v}_1} = \sqrt {2g\left( {{H_{pt}} + {H_s} - {H_1}} \right)} \\ &{{v}_{\rm{3}}} = \sqrt {2g\left( {{H_{pt}} + {H_s} - {H_{\rm{0}}}} \right)} \\ &{t_{01}} = \dfrac{{2\left( {{H_1} - {H_0}} \right)}}{{\sqrt {2g\left( {{H_{pt}} + {H_s} - {H_1}} \right)} }} \\ & {t_{12}} = \dfrac{{\sqrt {2g\left( {{H_{pt}} + {H_s} - {H_1}} \right)} }}{g} \\ & {t_{{\rm{23}}}} = \dfrac{{\sqrt {2g\left( {{H_{pt}} + {H_s} - {H_{\rm{0}}}} \right)} }}{g} \end{aligned}\right. $$ (8) 其中, ${{v}_1}$和${{v}_{\rm{3}}}$分别为点p1和p3处的速度. 足端离地时间即腾空时间为${t_{12}} + {t_{23}}$. 为同时保证每一段期望轨迹的平滑以及初始点与结束点的位置$\left( {{y_{{\rm{init}}}}},{{y_{{\rm{end}}}}}\right)$、速度$\left( {{{v}_{{\rm{init}}}}},{{{v}_{{\rm{end}}}}} \right)$要求, 采用三次样条函数生成连续轨迹. 起跳阶段的纵向位置期望轨迹、$p_1$-$p_2 $阶段、${p_2}$-${p_3}$ 阶段足端期望轨迹均由式(9)生成:
$$ \left\{\begin{aligned} &y=a{t}^{3}+b{t}^{2}+ct+d\\ &c={v}_{{\rm{init}}}\\ &d={y}_{{\rm{init}}}\\ &a=\dfrac{{v}_{{\rm{end}}}t-2{y}_{{\rm{end}}}+{v}_{{\rm{init}}}t+2{y}_{{\rm{init}}}}{T^{3}}\\ &b=\dfrac{{v}_{{\rm{end}}}-3a{T}^{2}-c}{2T}\end{aligned}\right.$$ (9) 其中, T为该段三次样条插值的总时长.
3. 全身力矩控制器
为实现机器人奔跑与跳跃的有效控制, 本节介绍了分布式控制器的设计方法. 首先基于躯干子系统的动力学模型, 采用二次规划 (Quadratic programming, QP)二次优化得到合理的虚拟作动力以使得躯干位姿满足控制要求. 采用逆动力学前馈补偿与逆运动学反馈补偿方法得到腿轮关节力矩.
3.1 基于最优虚拟力分配的躯干控制器
对于躯干单刚体进行静力分析, 假设躯干质量集中于几何中心处, 建立躯干质心合力与躯干受到的腿轮虚拟作动力的映射关系如下所示:
$$\left[ {\begin{array}{*{20}{c}} {\boldsymbol{F}} \\ {\boldsymbol{N}} \end{array}} \right]{\rm{\;+ \;}}{{\boldsymbol{G}}_{b}} = {\boldsymbol{A}}\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{W}}_{1}}} \\ {{{\boldsymbol{W}}_{2}}} \end{array}} \right]$$ (10) 其中, ${\boldsymbol{F}}{\rm{\;= [}}{F_x},{F_y},{F_z}{]^{\rm{T}}},\;{\boldsymbol{N}} = {[{N_x},{N_y},{N_z}]^{\rm{T}}}$分别为躯干质心合力与力矩. 受到关节配置的约束, 腿轮末端只能主动输出延机体坐标系x和z的力与绕y轴的力矩, 式(10)可展开为式(11)的形式.
$$\begin{split} \left[ {\begin{array}{*{20}{c}} {\boldsymbol{F}} \\ {\boldsymbol{N}} \end{array}} \right] + &\left[ {\begin{array}{*{20}{c}} {{m_{{b}}}g\sin \beta } \\ 0 \\ {{m_{{b}}}g\cos \beta } \\ {\rm{0}} \\ {\rm{0}} \\ {\rm{0}} \end{array}} \right] = \\ &\underbrace {\left[ {\begin{array}{*{20}{c}} 1&0&0&1&0&0 \\ 0&0&0&0&0&0 \\ 0&1&0&0&1&0 \\ 0&{\dfrac{w}{2}}&0&0&{ - \dfrac{w}{2}}&0 \\ { - \dfrac{h}{2}}&0&1&{ - \dfrac{h}{2}}&0&1 \\ { - \dfrac{w}{2}}&0&0&{\dfrac{w}{2}}&0&0 \end{array}} \right]}_{\boldsymbol{A}}\left[ {\begin{array}{*{20}{c}} {{f_{1x}}} \\ {{f_{1z}}} \\ {{n_{1y}}} \\ {{f_{2x}}} \\ {{f_{2z}}} \\ {{n_{2y}}} \end{array}} \right]\end{split}$$ (11) 其中, $g = 9.8 \; {\rm{m}}/{{\rm{s}}^2}$. 根据不同维度的特点构造躯干期望虚拟力${\boldsymbol{F}}$与${\boldsymbol{N}}$的反馈部分, 基于动力学模型进行必要的前馈补偿, 为保证速度跟踪性能, 虚拟力${F_x}$的反馈部分采用自适应滑膜方法构造, 其他维度采用比例−微分控制方法构造, 具体方法为
$$\left\{\begin{aligned} &{F_x} = s + sat\left( s \right)c \\ &s = \dot e + \lambda e \\ &e = {{\dot x}_d} - {}^B{{{v}}_{{B}}}, \dot e = {{\ddot x}_d} - {}^B{{\dot v}_B} \\ &{F_z} = {k_{pz}}\left( {{z_d} - {}^B{z_B}} \right) + {k_{dz}}\left( {{{\dot z}_d} - {}^B{{\dot z}_B}} \right)+{m_{{b}}}{{\ddot z}_d} \\ &{N_x} = {k_{p\gamma }}\left( {{\gamma _d} - \gamma } \right) + {k_{n\gamma }}\left( {{{\dot \gamma }_d} - \dot \gamma } \right) \\ & {N_y} = {k_{p\beta }}\left( {{\beta _d} - \beta } \right) + {k_{n\beta }}\left( {{{\dot \beta }_d} - \dot \beta } \right) \\ &{N_z} = {k_{{\omega}} }\left( {{{{\omega}} _d} - {{\omega}} } \right) \end{aligned}\right. $$ (12) 矩阵${\boldsymbol{A}}$为奇异矩阵, 因此无法直接解得与躯干虚拟广义力一一对应的腿轮末端作动力, 因此, 将式(11)变形为
$${\boldsymbol{A}}\underbrace {\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{W}}_{1}}} \\ {{{\boldsymbol{W}}_{2}}} \end{array}} \right]}_x=\underbrace {{{\left[ {\begin{array}{*{20}{c}} {\boldsymbol{F}} \\ {\boldsymbol{N}} \end{array}} \right]}^{{\rm{desire}}}}+{{\boldsymbol{G}}_{b}}}_b$$ (13) 通过QP优化器进行作动力求解
$$\begin{split} &\mathop {\min }\limits_{\boldsymbol{x}} \;\;\;f =\; {\left( {{\boldsymbol{Ax}} - {\boldsymbol{b}}} \right)^{\rm{T}}}{\boldsymbol{Q}}\left( {{\boldsymbol{Ax}} - {\boldsymbol{b}}} \right)\;+ \\ & \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{\boldsymbol{P}}{\left\| {{\boldsymbol{x}} - {{\boldsymbol{x}}_{{\rm{pre}}}}} \right\|^2} + {\boldsymbol{J}}{\left\| {\boldsymbol{x}} \right\|^2} \\ &{\rm{s.t}}\; \left| {{f_{ix}}} \right| \leq \mu {f_{iz}} \end{split} $$ (14) 其中, Q, P, J为正定对角权重矩阵, ${{\boldsymbol{x}}_{{\rm{pre}}}}$为上一时刻的解.
3.2 腿部控制器
当机器人处于支撑相时, 躯干控制器将躯干期望合力分配为各条腿轮的末端力旋量. 为实现力伺服, 基于轮式基座动力学模型前馈补偿的逆动力学力矩解算器如下所示
$$\begin{split} {{\boldsymbol{\tau}} _{i}} =\;& {\boldsymbol{J}}_{i}^{\rm{T}}{{\boldsymbol{W}}_{i}} + {{\boldsymbol{C}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{\dot {\boldsymbol{q}}}_{i}},{{q}_{iw}},{{\dot q}_{iw}}} \right) + \\ &{{\boldsymbol{G}}_{i}}\left( {{{\boldsymbol{q}}_{i}},{{q}_{iw}}} \right) +{i}\left( {{{q}_{iw}},{{\dot q}_{iw}}} \right){{\ddot q}_{iw}} \end{split} $$ (15) 其中, ${q}_{iw}{\text{, }}{\dot{q}}_{iw}{\text{, }}{\ddot{q}}_{iw}$均由基于传感器的状态观测器得到. 基于动力学前馈补偿的控制方法可以使机器人自适应地保持在唯一的不稳定平衡点处.
当机器人处于腾空相时, 为保证腿轮末端与躯干的相对位置以及机器人在世界坐标系中姿态的稳定, 将腾空相腿部控制器分为两部分, 基于式(6)动力学模型, 对髋关节与膝关节进行力矩控制以实现腿轮末端轨迹跟踪.
$${\boldsymbol{\tau}} _{i}^{s} = {\boldsymbol{C}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s},\dot {\boldsymbol{q}}_{i}^{s}} \right) + {\boldsymbol{G}}_{i}^{s}\left( {{\boldsymbol{q}}_{i}^{s}} \right) + {\boldsymbol{J}}_{i}^{{s}{\rm{T}}}{{\boldsymbol{F}}_{is}}$$ (16) 其中, ${{\boldsymbol{F}}_{is}} = {\left[ {{f_{ipx}}}\;\;{{f_{ipz}}}\right]^{\rm{T}}}$.
$$\left\{\begin{aligned} &{f_{ipx}} = {k_{ipx}}\left( {{x_{pd}} - {x_{ip}}} \right) + {k_{idx}}\left( {{{\dot x}_{pd}} - {{\dot x}_{ip}}} \right) \\ &{f_{ipz}} = {k_{ipz}}\left( {{z_{pd}} - {z_{ip}}} \right) + {k_{idz}}\left( {{{\dot z}_{pd}} - {{\dot z}_{ip}}} \right) \end{aligned}\right. $$ (17) 其中, $\left( {{x_{ip}},{z_{ip}}} \right)$与$\left( {{x_{pd}},{z_{pd}}} \right)$分别为自然坐标系中的足端位置描述与期望足端位置描述.
在腾空阶段, 系统处于动量守恒状态, 因此腿部关节的运动会导致整体姿态的变化, 此时可通过控制飞轮转动对姿态的误差进行有效补偿. 将一级轮式倒立摆动力学模型式(7)变形, 得到:
$$\ddot \beta = - \frac{{L{m_{b}}R{{\rm{c}} _\beta }}}{{{I_{b}} + {L^2}{m_{b}}}}{\ddot \theta _w} - \frac{1}{{{I_{b}} + {L^2}{m_{b}}}}{\tau _w} + \frac{{gL{m_{b}}{{\rm{s}} _\beta }}}{{{I_{b}} + {L^2}{m_{b}}}}$$ (18) 此时轮不与地面接触, 可以看作调整平衡的飞轮, 通过式(18)可以看出, 飞轮的转动能够影响倒立摆的俯仰角运动状态, 当俯仰角逆时针偏离期望位置时, 飞轮应顺时针转动保证姿态平衡, 反之亦然. 基于这一结论, 可以根据躯干控制器中输出的俯仰姿态矫正力矩${N_y}$ (如式(12)所示)来规划轮的力矩, 从而得到轮关节的力矩为
$$ {\tau _{{\rm{1}}w}} = {\tau _{2w}} = - k{N_y} $$ (19) 其中, k为正比例增益.
4. 仿真实验
在Webots 8.4.0中建立与HEBI IGOR同尺寸3D模型, 与Visual Studio 2017、Gurobi 902优化解算库进行联合仿真. 为验证本文所提算法的有效性, 分别设置了连续跳跃实验与空中姿态调整对比实验, 其总体控制框图如图3所示. 仿真视频见
https://www.bilibili.com/video/BV1zz4y1X7jp/ .根据期望跳跃高度${H_{pt}}$的不同, 轨迹生成器会生成不同的起跳阶段躯干纵向期望轨迹. 分别针对${H_{pt}}\;{ = 0}.45\;{\rm{m}}$, ${H_{pt}}\;{ = 0}.35\;{\rm{m}}$, ${H_{pt}}\;{ = 0}.25\;{\rm{m}}$进行了弹跳实验, 机器人的躯干纵向轨迹${z_{b}}$以及足端位置轨迹${z_p}$如图4所示, 可以看出期望弹跳高度越大, 所规划的躯干纵向期望轨迹在起跳点处纵向速度越大, 从而腾空相时间越长.
图 4 不同离地高度下躯干与足端纵向位置曲线Fig. 4 Longitudinal position curves of the torso and feet at different ground clearance4.1 连续跳跃实验
本仿真实验主要验证: 1)该算法框架下的跳跃动作是否能够实现; 2)该算法框架是否能够对跳跃动作带来的强干扰进行抑制并保持有界稳定. 因此, 仿真实验设置如下: 机器人初始状态为站立静止, 在0 ~ 3 s内加速至2 m/s, 随后保持匀速运动. 当t = 3.35 s时, 通过外部遥控下达${{H}_{{pt}}}{\rm{\;= \;0}}.45\;{\rm{m}}$的跳跃命令, 机器人依次执行起跳−腾空−落地缓冲−调整; 当t = 5.85 s时, 再次下达${H_{pt}}=0.45\;{\rm{m}}$的跳跃命令, 机器人完成跳跃并最终将跳跃导致的状态偏移调整到有界稳定状态. 以首次跳跃为例, 其仿真截图如图5所示.
对于完全站立高度为1.35 m的腿轮机器人来说, 最大离地高度为0.45 m的跳跃会对其平衡状态产生较大冲击, 甚至失去平衡. 由图6和图7中曲线可看出, 每次跳跃结束后, 各状态量进行了及时调整. 0 ~ 3 s加速阶段躯干水平速度的最大跟踪误差不超过0.075 rad, 其他支撑阶段, 误差小于0.04 rad. 图7可见, 处于支撑相时, 加速度越大水平速度误差越大, 最大误差为0.06 m/s. 当机器人进入腾空相时, 与外界环境不再产生相互作用力, 此时, 机器人保持动量守恒状态, 因此腿部伸缩运动影响了躯干的水平速度, 最大误差不超过0.08 m/s. 由腾空相切换为支撑相的瞬间, 实际水平速度与期望速度(2 m/s)之间存在偏差, 控制器实时规划期望速度曲线, 并将速度调整为2 m/s. 此外, 如图6所示, 腾空相中机器人pitch与roll角波动较大, 但着陆后控制器可有效控制姿态角恢复到0 rad附近. 与之对应的, 如图8所示, 力与力矩在着陆后出现较明显波动, 用以调整状态量使其恢复临界稳定.
图8为躯干子系统所需的虚拟广义力曲线图, 可见, 前进方向虚拟作动力${F_x}$与躯干水平加速度相关, 0 ~ 3 s水平加速度始终为正, 因此, ${{F_x}> 0}$, 最大值约为46 N; 当站高保持常值时, 纵向虚拟作动力${F_z}$在388 N ~ 398 N (mb = 40 kg)区间内波动, 由于起跳瞬间躯干纵向期望轨迹的初始点处加速度不为零, 加速度跳变导致了${F_z}$的跳变.
躯干纵向位置跟踪曲线如图9所示, 最大误差为48 mm, 发生在起跳阶段, 当期望站高为常量${H_{\rm{0}}}$时, 最大误差为0.2 mm, 发生在落地缓冲阶段. 可见该控制框架具有对纵向位置跟踪的稳定性与抗扰能力. 腾空阶段, 足端位置基于躯干动基座计算得到, 从而躯干动基座的波动会影响足端轨迹跟随, 导致足端轨迹波动较大, 最大误差为50 mm.
通过最优力分配将躯干虚拟作动力分配为各腿轮末端期望的输出广义力如图10所示. 两腿轮的广义力输出基本保持一致, 在着陆后有明显的调整行为. 各关节力矩曲线如图11所示, 膝关节力矩输出较大, 髋关节与轮关节力矩均在0附近波动.
4.2 空中姿态调整对比实验
为验证轮轴力矩对俯仰姿态矫正的有效性, 设计了腾空姿态调整对比实验. 实验1中腾空相的轮轴力矩${\tau _w} = 0\;({\rm{N}}\cdot{\rm{m}})$, 实验2中腾空相的轮轴力矩${\tau _w} = - {N_y}$, 保证其他条件完全相同. 图12为对比实验的俯仰角曲线, 可见当${\tau _w} = 0\;({\rm{N}}\cdot{\rm{m}})$时, 俯仰角由腾空初始时刻到着陆瞬间, 一直处于单向递增状态, 若腾空时间过长会导致着陆姿态不稳定. 当${\tau _w} =- {N_y} $时, 以俯仰角状态量为反馈进行了有效的姿态调整, 使得腾空相俯仰角在 $(- 0.015\;{\rm {rad}} $, 0.02 rad)范围内波动. 证明了基于角动量守恒的轮的反向转动对腾空姿态的调整是有效的.
5. 结束语
为使双腿轮机器人实现高动态运动, 本文提出了基于最优力分配的全身力矩控制器, 该控制器基于动力学模型进行设计, 所生成的全身力矩可在不额外规划平衡控制器的基础上实现机器人的自平衡, 并有效控制躯干位姿. 仿真实验表明, 该控制器能够同时保证水平速度、站高等多个维度的状态量处于有界稳定. 为提高双腿轮机器人地形适应能力, 规划并实现了双腿轮机器人的跳跃运动, 使其能在不打断高速运动的同时跨越垂直障碍, 进行了连续跳跃实验, 验证了所提方法的有效性.
-
图 2 躯干与足端矢状面轨迹示意图
Fig. 2 The trajectory schematic of the torso and feet in the sagittal plane
图 4 不同离地高度下躯干与足端纵向位置曲线
Fig. 4 Longitudinal position curves of the torso and feet at different ground clearance
表 1 跳跃参数设置
Table 1 Parameters of the jump motion
参数 参数含义 H0 正常行走时躯干站立高度在${\Sigma _O}$中的表示 H1 腾空瞬间躯干站立高度在${\Sigma _O}$中的表示 Hpt 足端最大离地距离在${\Sigma _O}$中的表示 Hs 足端最大收缩量在${\Sigma _B}$中的表示 
下载: 导出CSV
-
[1] 高志伟, 代学武, 郑志达. 基于运动控制和频域分析的移动机器人能耗最优轨迹规划. 自动化学报, 2020, 46(5): 934-945 doi: 10.16383/j.aas.c180399Gao Zhi-Wei, Dai Xue-Wu, Zheng Zhi-Da. Optimal energy consumption trajectory planning for mobile robot based on motion control and frequency domain analysis. Acta Automatica Sinica, 2020, 46(5): 934-945 doi: 10.16383/j.aas.c180399 [2] Bjelonic M, Bellicoso C D, De Viragh Y, Sako D, Tresoldi F D, Jenelten F, et al. Keep Rollin’—whole-body motion control and planning for wheeled quadrupedal robots. IEEE Robotics and Automation Letters, 2019, 4(2): 2116-2123 doi: 10.1109/LRA.2019.2899750 [3] Dynamics B. Introducing handle [Online], available: https://youtu.be/-7xvqQeoA8c, February 28, 2017 [4] Klemm V, Morra A, Salzmann C, Tschopp F, Bodie K, Gulich L, et al. Ascento: A two-wheeled jumping robot. In: Proceedings of the International Conference on Robotics and Automation (ICRA). Montreal, Canada: IEEE, 2019. 7515−7521 [5] Klemm V, Morra A, Gulich L, Mannhart D, Rohr D, Kamel M, et al. LQR-assisted whole-body control of a wheeled bipedal robot with kinematic loops. IEEE Robotics and Automation Letters, 2020, 5(2): 3745-3752 doi: 10.1109/LRA.2020.2979625 [6] Handle[Online], available: https://baijiahao.baidu.com/s?id=1629321730842384760 &wfr=spider&for=pc, March 29, 2019 [7] Wang H F, Zheng Y F, Jun Y, Oh P. DRC-Hubo walking on rough terrains. In: Proceedings of the IEEE International Conference on Technologies for Practical Robot Applications (TePRA). Woburn, USA: IEEE, 2014. 1−6 [8] Zhang S, Gao J Y, Duan X G, Li H, Yu Z G, Chen X C, et al. Trot pattern generation for quadruped robot based on the ZMP stability margin. In: Proceedings of the ICME International Conference on Complex Medical Engineering. Beijing, China: IEEE, 2013. 608−613 [9] Zhao D, Revzen S. Multi-legged steering and slipping with low DoF hexapod robots. Bioinspiration and Biomimetics, 2020, 15(4): Article No. 045001 [10] Vukobratović M, Borovac B. Zero-moment point — thirty five years of its life. International Journal of Humanoid Robotics, 2004, 1(1): 157-173 doi: 10.1142/S0219843604000083 [11] Messuri D, Klein C. Automatic body regulation for maintaining stability of a legged vehicle during rough-terrain locomotion. IEEE Journal on Robotics and Automation, 1985, 1(3): 132-141 doi: 10.1109/JRA.1985.1087012 [12] Hirose S, Tsukagoshi H, Yoneda K. Normalized energy stability margin and its contour of walking vehicles on rough terrain. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Seoul, South Korea: IEEE, 2001. 181−186 [13] Liu L X, Guo R W, Ji J C, Miao Z H, Zhou J. Practical consensus tracking control of multiple nonholonomic wheeled mobile robots in polar coordinates. International Journal of Robust and Nonlinear Control, 2020, 30(10): 3831-3847 doi: 10.1002/rnc.4967 [14] Xin Y X, Rong X W, Li Y B, Li B, Chai H. Movements and balance control of a wheel-leg robot based on uncertainty and disturbance estimation method. IEEE Access, 2019, 7: 133265-133273 doi: 10.1109/ACCESS.2019.2940487 [15] Abeygunawardhana P K W, Murakami T. Vibration suppression of two-wheel mobile manipulator using resonance-ratio-control-based null-space control. IEEE Transactions on Industrial Electronics, 2010, 57(12): 4137-4146 doi: 10.1109/TIE.2010.2044115 [16] Shihabudheen K V, George N, Chinmaya K A, Thankachan J. Stability control and trajectory tracking of two wheeled mobile manipulator. In: Proceedings of the Annual IEEE India Conference (INDICON). New Delhi, India: IEEE, 2015. 1−6 [17] Xin S Y, You Y W, Zhou C X, Fang C, Tsagarakis N. A torque-controlled humanoid robot riding on a two-wheeled mobile platform. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Vancouver, Canada: IEEE, 2017. 1435−1442 [18] Yue M, An C, Sun J Z. An efficient model predictive control for trajectory tracking of wheeled inverted pendulum vehicles with various physical constraints. International Journal of Control, Automation and Systems, 2018, 16(1): 265-274 doi: 10.1007/s12555-016-0393-z [19] Park J, Haan J, Park F C. Convex optimization algorithms for active balancing of humanoid robots. IEEE Transactions on Robotics, 2007, 23(4): 817-822 doi: 10.1109/TRO.2007.900639 [20] Rocchi A, Hoffman E M, Caldwell D G, Tsagarakis N G. OpenSoT: A whole-body control library for the compliant humanoid robot COMAN. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Seattle, USA: IEEE, 2015. 6248−6253 [21] Neunert M, Stauble M, Giftthaler M, Bellicoso C D, Carius J, Gehring C, et al. Whole-body nonlinear model predictive control through contacts for quadrupeds. IEEE Robotics and Automation Letters, 2018, 3(3): 1458-1465 doi: 10.1109/LRA.2018.2800124 [22] Chen T, Sun X B, Xu Z, Li Y B, Rong X W, Zhou L L. A trot and flying trot control method for quadruped robot based on optimal foot force distribution. Journal of Bionic Engineering, 2019, 16(4): 621-632 doi: 10.1007/s42235-019-0050-3 [23] Kim D, Di Carlo J, Katz B, Bledt G, Kim S. Highly dynamic quadruped locomotion via whole-body impulse control and model predictive control. arXiv preprint arXiv: 1909.06586v1, 2019. [24] Dynamics B. Parkour atlas [Online], available: https://youtu.be/LikxFZZO2sk, October 12, 2018 [25] Xin Y X, Chai H, Li Y B, Rong X W, Li B, Li Y Y. Speed and acceleration control for a two wheel-leg robot based on distributed dynamic model and whole-body control. IEEE Access, 2019, 7: 180630-180639 doi: 10.1109/ACCESS.2019.2959333 [26] HEBI Robotics. IGOR: Wheeled, self-balancing robotic kit from HEBI robotics [Online], available: https://youtu.be/icLyS4RWYjg, September 12, 2019 期刊类型引用(5)
1. 任晓磊，刘辉，韩立金，陈前，聂士达，谢景硕，崔山. 复杂地形下轮腿复合机动平台动态运动控制. 兵工学报. 2024(09): 2993-3003 . 
百度学术2. 宋荆洲，宫兴龙，段嘉辰，张腾飞. 可跳跃移动机器人机构设计与跳跃过程控制研究综述. 机械工程学报. 2024(15): 1-17 . 百度学术3. 吴文青. 学前教育中基于CPG与DDPG结合的机器人运动控制与幼儿互动研究. 自动化与仪器仪表. 2024(09): 289-293 . 百度学术4. 魏塽，孙明跃，赵宇航，罗树梅，闫庚龙. 基于腿轮自平衡技术的救援巡逻小车. 现代信息科技. 2023(20): 151-156 . 百度学术5. 李佳宁. 基于PLC的工厂机器人自动上下料控制方法. 今日制造与升级. 2023(12): 84-86 . 百度学术其他类型引用(2)
-
下载:
计量
- 文章访问数: 2028
- HTML全文浏览量: 820
- PDF下载量: 364
- 被引次数: 7
