仿人机器人双足步行运动具有本质不稳定的特征。通常在仿人机器人上安装多个传感器对步行状态进行检测以实现实时在线稳定控制。在与人类尺寸相近的仿人机器人中，通常会安装位于躯干的惯性测量单元(inertial measurement unit,IMU)和位于足底的力传感器^[1]，前者测得躯干六维位姿，后者测得脚底受力情况，进而实现多种步行稳定控制，如零力矩点(zero moment point,ZMP)跟随方法、 步行不稳定检测和摔倒保护等。由于近年来生物学及临床医学的研究发现双足步行稳定控制过程中，躯干姿态的感知和反馈起主要作用^[2-3]，且由于姿态传感器的机械安装比足底力传感器简易，因此本文主要通过躯干姿态感知实现仿人机器人的步行稳定控制。

机器人步行过程中可能受到微弱或者强烈的干扰，前者可以由姿态的反馈校正予以消除，而后者则多采用反射响应的方法避免摔倒。目前，这2种干扰消除策略均具有一定研究成果。文^[4]提出控制踝关节位置及落地足位置，保持上身姿态处于竖直状态，进而实现机器人的步行稳定控制。而文^[5]为实现仿人机器人在较大扰动情况下的全方向稳定步行，提取多个稳定开环步行速度下的低频模型进行模式匹配，并采用减慢速度或降低身高这2种反射方式恢复到稳定状态。文^[6]采取反射动作实现了站立状态受到外部干扰时的推动—恢复过程。文^[7]以Gauss混合模型(Gaussian mixture model,GMM)及隐Markov模型(hidden Markov model,HMM)实现了机器人对摔倒状态的分类，并采用反射响应方式维持双足运动的稳定性。文^[8]则采用2轴加速度计和2轴陀螺以Kalman滤波器实现传感器融合，进而估计躯干的姿态，以角速度和倾角偏离的线性组合作为机器人失稳的判据，并改变摆动足的落地位置防止跌倒。文^[9]以简化行走稳定控制的计算负荷为目的，以关节角度传感器、 惯性测量单元传感器以及低成本的控制器为硬件基础，在步行过程中受到侧向干扰的情况下，通过修改步长和步频等行走参数，实现了侧向平面下的行走稳定控制。文^[10]以3轴加速度计和2轴陀螺及Kalman滤波器估算躯干姿态，分别针对站立状态和行走状态设定不同的摔倒检测门限，以反射控制实现了稳定步行。

上述研究中的姿态传感器轴数配置存在较大差异，解算和滤波算法效率不高，甚至为避免解算而直接采用传感器输出数据进行反馈控制，且所采用的反射响应控制过程通常会打断当前步行执行任务，不利于后续步态的延续执行。

本文拟采用低成本的微机电系统(micro-electro-mechanical system,MEMS)器件构成的航姿参考系统(attitude and heading reference system，AHRS)感知姿态，采用通用的空间三维姿态解算算法获取机器人姿态信息，以全方位步行运动落脚点以及步行频率控制方式实现可靠的双足步行稳定补偿控制，减少反射动作执行前后机器人步态执行的不连续影响。

1 AHRS传感器及姿态解算算法

AHRS是一种主要采用惯性传感器以数据融合算法精确测量姿态方向的传感系统。AHRS的物理基础是相互正交的3轴陀螺仪、 3轴加速度计以及3轴磁强计。它可以在重力场方向和地磁场的水平方向所构成的坐标系下给出准确的姿态朝向。

尽管大量的研究及商业化AHRS系统均以Kalman滤波器或扩展Kalman滤波器实现姿态求解，但其线性化过程会产生较大系统误差，而且计算负荷较高，不利于算法在低成本处理器上实现。针对这些缺陷，Bachmann等^[11]和Mahony等^[12]提出了补偿滤波器，以加速度计对重力场的测量和磁场计对地磁场的测量来补偿陀螺的漂移，再对补偿后的角速度数值积分求得姿态转角。本文采用方向余弦矩阵(direction cosine matrix,DCM)描述的补偿滤波器实现机器人躯干姿态的求解，其原理框图如图 1所示。

图 1中，a、 m和ω分别是3轴加速度计、 3轴磁强计和3轴陀螺的输出，ω_C是经加速度计和磁强计补偿后的AHRS旋转角速度向量。DCM的本质是传感器固连坐标系相对于世界坐标系的旋转矩阵，只要能够求出DCM，即可转化为2个坐标系间的横滚—俯仰—偏航(roll-pitch-yaw,RPY)变换角度，从而完成AHRS的姿态解算。设RPY角度分别为φ、 θ和ψ，则方向余弦矩阵R_D可表示为

${{R}_{D}}={{R}_{x}}\left( \varphi \right){{R}_{y}}\left( \theta \right){{R}_{z}}\left( \psi \right).$

(1)

其中，R_x、 R_y和R_z分别是沿世界坐标系3个坐标轴的旋转矩阵。有关补偿滤波器理论基础及具体算法可参见文^[12]。

2 步行运动不稳定性判据

机器人步行失稳时，将绕支撑脚的边缘做旋转运动，因而可据此判定机器人是否处于稳定状态。设： 机器人单腿具有常见的6自由度布置； 步行期间，支撑腿各个关节在某个时刻转角为(θ₁,θ₂,θ₃,θ₄,θ₅,θ₆)，脚底与水平地面的夹角为(φ_f,θ_f)。则存在函数关系

${{R}_{t}}={{R}_{f}}\left( {{\varphi }_{f}},{{\theta }_{f}} \right){{R}_{s}}\left( {{\theta }_{1}},{{\theta }_{2}},{{\theta }_{3}},{{\theta }_{4}},{{\theta }_{5}},{{\theta }_{6}} \right).$

(2)

其中R_t和R_f分别是躯干和支撑脚在世界坐标系下的旋转矩阵。R_t可由固连在机器人躯干内部的AHRS测得，只是不考虑式(1)中绕z轴的偏航角ψ，即

${{R}_{t}}={{R}_{D}}\left| _{\psi =0} \right.=\left[ \begin{matrix} cos\theta & sin\varphi sin\theta & cos\varphi sin\theta \\ 0 & cos\varphi & -sin\varphi \\ sin\theta & sin\varphi cos\theta & cos\varphi cos\theta \\ \end{matrix} \right].$

(3)

R_s是腿部各个关节旋转后所对应的旋转矩阵。由式(2)可得

${{R}_{f}}\left( {{\varphi }_{f}},{{\theta }_{f}} \right)={{R}_{t}}R_{s}^{-1}\left( {{\theta }_{1}},{{\theta }_{2}},{{\theta }_{3}},{{\theta }_{4}},{{\theta }_{5}},{{\theta }_{6}} \right).$

(4)

因而可根据式(4)求得(φ_f,θ_f)。当该角度与理论规划所得的足部与地面夹角出现较大偏差时，即可认定机器人将失稳摔倒。

此外，机器人稳定步行时，躯干姿态的侧向摆角和俯仰摆角很小，其躯干接近于在平面内做刚体的二维平动。由图 1所示的算法可求得躯干在补偿滤波后的侧向和俯仰方向的角速度分量分别是ω_r和ω_p，则两者的数值均应接近0。当机器人失去稳定时，ω_r和ω_p将显著增大，因此也可用于机器人的失稳判定准则和控制动作产生的依据。同时，躯干角速度的变化提前于(φ_f,θ_f)，因而能够显著提升机器人对行走失稳的感知灵敏度。综合考虑机器人整体的倾角和角速度后，机器人的稳定性判定指标可以表示为

$~\chi ={{\left[ \begin{matrix} {{\varepsilon }_{\varphi }}+{{p}_{x}}{{\omega }_{r}} & {{\varepsilon }_{\theta }}+{{p}_{y}}{{\omega }_{p}} \\ \end{matrix} \right]}^{T}}.$

(5)

式中： ε_φ和ε_θ分别是φ_f和θ_f实际数值与步态规划理论数值之间的偏差，ε_θ及ω_p如图 2所示，p_x和p_y分别表示角度和角速度的检测的权重。当‖χ‖<‖χ‖_max时，可以认为机器人处在稳定状态，反之则机器人开始失去稳定性并摔倒。

3 步态稳定控制算法

检测步行状态的最终目标是在机器人步行失稳前，施加必要的控制动作，使机器人在一步或者几步之内恢复稳定步行。近年来，仿人机器人全方位步态规划方法迅速发展，机器人可以根据上层步幅指令任意放置落地足位置，并实时在线规划出可行关节轨迹，使得机器人按指令以给定的方向和速度步行^[13]。借助这一研究成果，可在机器人即将失去步行稳定时，修改原有步幅指令和步态的执行频率，期间并未中断机器人的正常步行，因而确保了机器人步态任务执行的延续性。

设机器人的单步周期为T，关节控制周期为τ，则一步内将执行N=T/τ次关节控制指令更新。此外，姿态传感器的数据融合解算及关节转角传感器的采样周期也为τ。而步幅和步频的调整以单个步伐为基本单位，即检测到步行失去稳定后，在下一个步行周期内予以调整。

设在第(n-1)步行周期内观测到的脚底倾角偏差为ε_φ(k)和ε_θ(k)，躯干角速度为ω_r(k)和 ω_p(k)，k=1,2,…,N。由于在机器人双脚支撑期间，各个传感器易受到落脚和抬脚震动的影响，因此对单个步行控制时间段的两端观测值赋予较小的权值，而对单脚支撑期下所获取的传感数据赋予较高的权值，由此求得一步之内倾斜角度和角速度偏差的加权平均值：

$\begin{align} & {{{\bar{\varepsilon }}}_{\varphi }}=\sum\limits_{k=1}^{N}{{}}{{c}_{k}}{{\varepsilon }_{\varphi }}\left( k \right), \\ & {{{\bar{\omega }}}_{r}}=\sum\limits_{k=1}^{N}{{}}{{c}_{k}}{{\omega }_{r}}\left( k \right). \\ \end{align}$

(6)

其中，c_k≥0是置信系数，且满足$\sum\limits_{k=1}^{N}{{}}$c_k=1。

第n个周期的步幅和步频的变化以第(n-1)个周期内依据式(6)计算得到的偏差(ε_φ,ω_r)和(ε_θ,ω_p)为依据，从而形成一个反馈控制系统，其原理如图 3所示。其中给定的全方位步行运动指令为(x_c,y_c,α_c)，即单步步幅同时完成前进x_c、 侧移y_c和旋转α_c； Δx和Δy分别是前进方向和侧移方向的步幅调整量； T_n是由调节器输出的新单步频率。

由于被控对象的物理模型难于采用明确的微分方程予以描述，且具有很强的非线性以及大时间延迟特性，而图 3所示的本质是一种比例—微分控制器，ε_φ和 ε_θ可视作机器人整体姿态相对目标数值偏离的直接观测结果，而 ω_r和 ω_p则是该偏差的变化律。从而可以形成比例环节和微分环节以实现步幅调节器，步幅的控制量为：

$\begin{align} & \Delta x={{K}_{x}}\left( {{{\bar{\varepsilon }}}_{\theta }}+{{p}_{x}}{{{\bar{\omega }}}_{p}} \right), \\ & \Delta y={{K}_{y}}\left( {{{\bar{\varepsilon }}}_{\varphi }}+{{p}_{y}}{{{\bar{\omega }}}_{r}} \right). \\ \end{align}$

(7)

式中，K_x和K_y分别是比例系数。为达到与式(5)所示的稳定判据一致，比例环节与微分环节的相对比例采用与式(5)相同的p_x和p_y。最终机器人实际执行的步幅将修正为(x_c+Δx,y_c+Δy,α_c)。

考虑到机器人出现较大倾斜以致出现倾倒风险时，摆动足提前或延迟着地的问题，在调整机器人落地足位置的同时，还应适当调整步行的周期T。经验表明该数值与机器人倾斜角度和机器人实际步幅有关，因此建立一个机器人倾覆的倒立摆模型如图 4所示。

在不考虑姿态角偏差的条件下，机器人摆动足的落脚点将位于图 4中的A点，而当出现ε_θ偏差时，机器人规划轨迹结果期望落脚在A′点，但由于地面的限制，摆动足提前在B点着地，其行程发生的变化导致单步运行时间T发生了变化。在机器人前进步幅为x_c+Δx、 俯仰方向的姿态角偏差为ε_θ时，摆动腿自抬脚到落脚的过程中竖直方向的行程变化量为

$~\Delta d=\left( {{x}_{c}}+\Delta x \right)sin{{{\bar{\varepsilon }}}_{\theta }}\approx \left( {{x}_{c}}+\Delta x \right)\cdot {{{\bar{\varepsilon }}}_{\theta }}.$

(8)

Δd越大，表明摆动足落地越提前，单步周期应越短；相反，若ε_θ为负，摆动足将延迟落地，其值越小，则新一步步行周期T越长，因此采用式(9)来描述新一步步行周期与原周期的比例关系。

$~\frac{{{T}_{\theta }}}{T}=1-{{K}_{\theta }}\left( {{x}_{c}}+\Delta x \right)\cdot {{{\bar{\varepsilon }}}_{\theta }}.$

(9)

K_θ是需要在线优化的系数，而T_θ是依据前进步幅和姿态俯仰偏差计算所得的新一步步行周期。同理，针对机器人侧移步幅和姿态侧摆偏差亦存在最佳周期T_φ，最终通过样机实验的方法从T_φ和T_θ中进行优化选择。

4 步行稳定控制实验

为测试机器人步行稳定控制算法的有效性，采用小型仿人机器人MOS-Strong^[14] 进行样机实验。该机器人整体具有21个自由度，左右腿各6个自由度，腿部均采用Robotis公司的RX64驱动器，其最大输出力矩为6.4 N·m。机器人运动控制器的核心处理器为LPC1768，主频率为100 MHz，具有64 kB内存并扩展了4 MB flash存储器。MOS-Strong的身高为583 mm，总质量为5.34 kg，重心高度为345 mm，脚板尺寸为130 mm×90 mm。

机器人关节运动控制周期为10 ms，其能够以350 ms单步周期实现全方位步行运动^[14]。其最高步速在前进、 后退、 侧移、 旋转方向分别为 20.0 cm/s、 12.0 cm/s、 10.0 cm/s和37.5 (°)/s。构成AHRS的9轴传感器安装在机器人胸腔内部，各传感器参数如表 1所示。图 5给出第一组实验结果。

微处理器对传感器的数据采集频率为100 Hz，姿态解算与数据融合的频率亦为100 Hz。

首先保持机器人稳定站立，未执行任何运动，即x_c=0、 y_c=0、 α_c=0，同时也不具备原地踏步的动作。此时手动控制机器人的整体姿态，仅观察步幅和步频调节算法的调整量。如图 5所示，沿俯仰方向快速转动机器人时，角速度反馈作用较为明显，能够有效补偿突发的干扰，而躯干角度所产生的调节量则在倾斜角平稳时占主导作用，从而消除缓慢变化的干扰。

随后，对机器人原地踏步的情况进行测试。对机器人施加俯仰方向的冲击，步幅和步频响应曲线如图 6所示，步态稳定控制算法有效地抑制了冲击，避免机器人倾覆，恢复倾斜角度最大可达20°。

图 7所示的是机器人稳定前进过程中突然受到冲击，通过调整步行的步幅和频率，有效地避免了机器人的倾覆。由于机器人具备较高的向前步行速度，冲击造成了机器人姿态震荡，但得到了有效的抑制，使机器人恢复稳定前进步行。图 8所示的是实验过程中的机器人自稳定步行时受到外部突然冲击后由不稳定状态恢复稳定状态的连拍图像。

实验过程同时测试了式(9)计算得到T_φ和T_θ的不同参数选择方式。由于侧向受到左右脚交替落地自激震荡的影响，难以选择T_φ作为步行周期以避免侧向倾覆，甚至会干扰到前进方向的调整性能，因此主要以T_θ调整机器人的步频，而仅在机器人俯仰偏差较小、 俯仰姿态较为稳定的状况下，才采用T_φ调整左右交替落脚的步频，从而消除侧向受到的干扰。经实验所得的步行周期T_n选择方法如下(其中σ₁和σ₂是经实验确定的正常数)。

$\begin{align} & If~\left| \frac{{{T}_{\theta }}}{T}-1 \right|<{{\sigma }_{1}}~and~\left| \frac{{{T}_{\varphi }}}{T}-1 \right|>\left| \frac{{{T}_{\theta }}}{T}-1 \right|+{{\sigma }_{2}} \\ & Then~{{T}_{n}}={{T}_{\varphi }} \\ & Else~{{T}_{n}}={{T}_{\theta }} \\ \end{align}$

5 结 论

本文在不采用脚底力传感器和高精度姿态传感器的情况下，通过MEMS传感器及其数据融合算法实现了低成本的躯干姿态角度和角速度估算，将两者的线性组合作为机器人步行稳定性判据，以此调整机器人下一步的步幅和步频，本质上实现了仿人机器人步行这一复杂非线性大延迟系统的比例—微分控制。该方法不需要建立仿人机器人稳定步行的精确物理模型，而是通过增强机器人从不稳定性过渡到稳定步行的趋势实现，控制的强度随着机器人不稳定程度的改变而改变，因此同时具备反射响应和反馈控制的特征。方法以较小的传感器成本和较低的计算代价，有效实现了仿人机器人步行过程中受到较大冲击和干扰时在线稳定步行补偿控制。