1.清华大学 机械工程系, 北京 100084;
2.精密超精密制造装备及控制北京市重点实验室, 北京 100084;
3.摩擦学国家重点实验室, 北京 100084
收稿日期: 2013-08-05
基金项目: 国家自然科学基金资助项目（51175288，51105219）
作者简介: 张继文(1983-),男,博士研究生
通讯作者: 陈恳,教授,E-mail:kenchen@tsinghua.edu.cn
摘要：以实现仿人机器人步行稳定控制为目的，为克服零力矩点（ZMP）反馈控制算法不能补偿较大冲击干扰以及反射响应方法打断当前步行任务的缺点，以低成本微机电系统（MEMS）传感器及方向余弦矩阵描述的补偿算法构成航姿参考系统。提出了由姿态转角和角速度的线性组合表达的步行稳定性判据，并实现了躯干姿态校正的比例-微分控制器。基于仿人机器人全方位步态规划方法，调整下一步步幅和步频为行为方式，实现了步行稳定控制。样机实验结果表明：在原地踏步和稳定前进等情况下，机器人受到冲击干扰以致倾斜角度达20°时，仍能够不打断全方位步行且有效避免倾覆。
关键词： 仿人机器人 航姿参考系统 步行稳定 全方向步行
Stabilizing control of humanoids' walking based on AHRS feedback
ZHANG Jiwen1,2,3, LIU Li1,2,3, CHEN Ken1,2,3
1.Department of Mechanical Engineering, Tsinghua University, Beijing 100084, China;
2.Beijing Key Laboratory of Precision/Ultra-precisionManufacturing Equipments and Control, Tsinghua University, Beijing 100084, China;
3.State Key Laboratory of Tribology, Tsinghua University, Beijing 100084, China
Abstract:An attitude and heading reference system (AHRS) feedback system was established using micro-electro-mechanical system (MEMS) sensors with low cost and the compensation algorithm described by the direction cosine matrix to realize the stabilizing control of humanoids' walking, being aimed to overcome the defect of incapability to compensate the large disturbance when using the zero moment point (ZMP) feedback control algorithm and the shortcoming of the current walking task interruption when using the reflex response control method. The walking stability criterion expressed by the linear composition of posture angles and angular velocities was also proposed, with a proportion-derivative controller for torso pose adjusting also implemented. Walking stabilization control was realized by the action style of tuning the next stride and walking frequency using the omni-directional walking gait planning method. Experimental results with a physical robot show that the robot can effectively avoid omni-directional walking task interruption and fall over even if disturbed by an impulse to reach the maximum inclination of 20° when marching on the spot and walking forward steadily.
Key words: humanoid robotattitude and heading reference systemwalking stabilizationomni-directional walking
仿人机器人双足步行运动具有本质不稳定的特征。通常在仿人机器人上安装多个传感器对步行状态进行检测以实现实时在线稳定控制。在与人类尺寸相近的仿人机器人中，通常会安装位于躯干的惯性测量单元(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所示。
-56-8-818-1.jpg "点击查看原图") |
| 图 1 基于DCM矩阵的AHRS补偿滤波算法示意图 |
| 图选项 |
图 1中，a、 m和ω分别是3轴加速度计、 3轴磁强计和3轴陀螺的输出，ωC是经加速度计和磁强计补偿后的AHRS旋转角速度向量。DCM的本质是传感器固连坐标系相对于世界坐标系的旋转矩阵，只要能够求出DCM，即可转化为2个坐标系间的横滚—俯仰—偏航(roll-pitch-yaw,RPY)变换角度，从而完成AHRS的姿态解算。设RPY角度分别为φ、 θ和ψ，则方向余弦矩阵RD可表示为
| ${{R}_{D}}={{R}_{x}}\left( \varphi \right){{R}_{y}}\left( \theta \right){{R}_{z}}\left( \psi \right).$ | (1) |
2 步行运动不稳定性判据机器人步行失稳时，将绕支撑脚的边缘做旋转运动，因而可据此判定机器人是否处于稳定状态。设： 机器人单腿具有常见的6自由度布置； 步行期间，支撑腿各个关节在某个时刻转角为(θ1,θ2,θ3,θ4,θ5,θ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}_{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}_{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) |
此外，机器人稳定步行时，躯干姿态的侧向摆角和俯仰摆角很小，其躯干接近于在平面内做刚体的二维平动。由图 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) |
-56-8-818-2.jpg "点击查看原图") |
| 图 2 俯仰方向规划与实际姿态偏差及倾斜角速度示意图 |
| 图选项 |
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) |
第n个周期的步幅和步频的变化以第(n-1)个周期内依据式(6)计算得到的偏差(εφ,ωr)和(εθ,ωp)为依据，从而形成一个反馈控制系统，其原理如图 3所示。其中给定的全方位步行运动指令为(xc,yc,αc)，即单步步幅同时完成前进xc、 侧移yc和旋转αc； Δx和Δy分别是前进方向和侧移方向的步幅调整量； Tn是由调节器输出的新单步频率。
-56-8-818-3.jpg "点击查看原图") |
| 图 3 步行稳定反馈控制系统原理图 |
| 图选项 |
由于被控对象的物理模型难于采用明确的微分方程予以描述，且具有很强的非线性以及大时间延迟特性，而图 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) |
考虑到机器人出现较大倾斜以致出现倾倒风险时，摆动足提前或延迟着地的问题，在调整机器人落地足位置的同时，还应适当调整步行的周期T。经验表明该数值与机器人倾斜角度和机器人实际步幅有关，因此建立一个机器人倾覆的倒立摆模型如图 4所示。
-56-8-818-4.jpg "点击查看原图") |
| 图 4 机器人即将倾覆摔倒时的简化倒立摆模型 |
| 图选项 |
在不考虑姿态角偏差的条件下，机器人摆动足的落脚点将位于图 4中的A点，而当出现εθ偏差时，机器人规划轨迹结果期望落脚在A′点，但由于地面的限制，摆动足提前在B点着地，其行程发生的变化导致单步运行时间T发生了变化。在机器人前进步幅为xc+Δ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) |
| $~\frac{{{T}_{\theta }}}{T}=1-{{K}_{\theta }}\left( {{x}_{c}}+\Delta x \right)\cdot {{{\bar{\varepsilon }}}_{\theta }}.$ | (9) |
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给出第一组实验结果。
表 1 MOS-Strong机器人的AHRS传感器参数
| 传感器名称 | 型号 | 量程 | 灵敏度 |
| 加速度计 | ADXL345 | ±156.8 m/s2 | 0.039 2 m/s2 |
| 陀螺 | L3G4200D | ±500 (°)/s | 17.5 (°)/s |
| 磁强计 | HMC5883 | ±2.5 Ga | 1.51 mGa |
表选项
-56-8-818-5.jpg "点击查看原图") |
| 图 5 机器人静止状态受到俯仰方向冲击后步幅和步频调整 |
| 图选项 |
微处理器对传感器的数据采集频率为100 Hz，姿态解算与数据融合的频率亦为100 Hz。
首先保持机器人稳定站立，未执行任何运动，即xc=0、 yc=0、 αc=0，同时也不具备原地踏步的动作。此时手动控制机器人的整体姿态，仅观察步幅和步频调节算法的调整量。如图 5所示，沿俯仰方向快速转动机器人时，角速度反馈作用较为明显，能够有效补偿突发的干扰，而躯干角度所产生的调节量则在倾斜角平稳时占主导作用，从而消除缓慢变化的干扰。
随后，对机器人原地踏步的情况进行测试。对机器人施加俯仰方向的冲击，步幅和步频响应曲线如图 6所示，步态稳定控制算法有效地抑制了冲击，避免机器人倾覆，恢复倾斜角度最大可达20°。
-56-8-818-6.jpg "点击查看原图") |
| 图 6 机器人原地踏步时受到俯仰方向冲击干扰响应 |
| 图选项 |
图 7所示的是机器人稳定前进过程中突然受到冲击，通过调整步行的步幅和频率，有效地避免了机器人的倾覆。由于机器人具备较高的向前步行速度，冲击造成了机器人姿态震荡，但得到了有效的抑制，使机器人恢复稳定前进步行。图 8所示的是实验过程中的机器人自稳定步行时受到外部突然冲击后由不稳定状态恢复稳定状态的连拍图像。
-56-8-818-7.jpg "点击查看原图") |
| 图 7 机器人快速前进受到俯仰方向的冲击干扰响应 |
| 图选项 |
-56-8-818-8.jpg "点击查看原图") |
| 图 8 机器人快速前进期间受到较大冲击干扰后恢复稳定步行运动实验截图 |
| 图选项 |
实验过程同时测试了式(9)计算得到Tφ和Tθ的不同参数选择方式。由于侧向受到左右脚交替落地自激震荡的影响，难以选择Tφ作为步行周期以避免侧向倾覆，甚至会干扰到前进方向的调整性能，因此主要以Tθ调整机器人的步频，而仅在机器人俯仰偏差较小、 俯仰姿态较为稳定的状况下，才采用Tφ调整左右交替落脚的步频，从而消除侧向受到的干扰。经实验所得的步行周期Tn选择方法如下(其中σ1和σ2是经实验确定的正常数)。
| $\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}$ |
参考文献
| [1] | Journal of Central South University(Science and Technology), 41(2):649-654.--> Gupta G S,Barlow P,David S.Review of sensors and sensor integration for the control of a humanoid robot[C]//IEEE Instrumentation and Measurement Technology Conference (I2MTC).Hangzhou,China:IEEE,2011:1-5. |
| [2] | Journal of Central South University(Science and Technology), 41(2):649-654.-->Mergner T, Schweigart G, Fennell L. Vestibular humanoid postural control[J]. Journal of Physiology Paris, 2009, 103(3-5) : 178–194.DOI:10.1016/j.jphysparis.2009.08.002 |
| [3] | Journal of Central South University(Science and Technology), 41(2):649-654.-->Mergner T. A neurological view on reactive human stance control[J]. Annual Reviews in Control, 2010, 34(2) : 177–198.DOI:10.1016/j.arcontrol.2010.08.001 |
| [4] | Journal of Central South University(Science and Technology), 41(2):649-654.--> Song S.Development of an Omni-directional Gait Generator and a Stabilization Feedback Controller for Humanoid Robots[D].Blacksburg,Virginia,USA:Virginia Polytechnic Institute and State University,2010. |
| [5] | Journal of Central South University(Science and Technology), 41(2):649-654.--> Renner R,Behnke S.Instability detection and fall avoidance for a humanoid using attitude sensorsand reflexes[C]//IEEEInternational Conference on Intelligent Robots and Systems.Beijing,China:IEEE,2006:2967-2973. |
| [6] | Journal of Central South University(Science and Technology), 41(2):649-654.--> Yi S J,Zhang B T,Hong D,et al.Learning full body push recovery control for small humanoid robots[C]//2011 IEEE International Conference on Robotics and Automation.Shanghai,China:IEEE,2011:2047-2052. |
| [7] | Journal of Central South University(Science and Technology), 41(2):649-654.-->Hohn O, Gerth W. Probabilistic balance monitoring for bipedal robots[J]. International Journal of Robotics Research, 2009, 28(2) : 245–256.DOI:10.1177/0278364908095170 |
| [8] | Journal of Central South University(Science and Technology), 41(2):649-654.--> Radkhah K,Scholz D,Anjorin A,et al.Simple yet effective technology for robust real-time instability detection for humanoid robots using minimal sensor input[C]//The 13th International Conference on Climbing and Walking Robots (CLAWAR2010).Nagoya,Japan:World Scientific Publishing,2010:680-689. |
| [9] | Journal of Central South University(Science and Technology), 41(2):649-654.--> Missura M,Behnke S.Lateral capture steps for bipedal walking[C]//Proceedings of the 11th IEEE-RAS International Conference on Humanoid Robots (Humanoids).Bled,Slovenia:IEEE,2011:401-408. |
| [10] | Journal of Central South University(Science and Technology), 41(2):649-654.--> Ruiz-Del-Solar J,Moya J,Parra-Tsunekawa I.Fall detection and management in biped humanoid robots[C]//2010 IEEE International Conference on Robotics and Automation.Anchorage,Alaska,USA:IEEE,2010:3323-3328. |
| [11] | Journal of Central South University(Science and Technology), 41(2):649-654.--> Bachmann E R,McGhee R B,Yun X,et al.Inertial and magnetic posture tracking for inserting humans into networked virtual environments[C]//Proceedings of ACM Symposium on Virtual Reality Software and Technology.Banff,Alta,Canada:ACM,2001:9-16. |
| [12] | Journal of Central South University(Science and Technology), 41(2):649-654.-->Mahony R, Hamel T, Pflimlin J M. Nonlinear complementary filters on the special orthogonal group[J]. IEEE Transactions on Automatic Control, 2008, 53(5) : 1203–1218.DOI:10.1109/TAC.2008.923738 |
| [13] | Journal of Central South University(Science and Technology), 41(2):649-654.--> Gouaillier D,Collette C,Kilner C.Omni-directional closed-loop walk for NAO[C]//IEEE International Conference on Humanoid Robots (Humanoids).Nashville,TN,USA:IEEE,2010:448-454. |
| [14] | Journal of Central South University(Science and Technology), 41(2):649-654.--> Li C,Zhang J,Liu L.Team description paper for team TH-MOS[EB/OL].[2012-03-24].http://www.tzi.de/humanoid/bin/view/Website/Teams2012. |
