基于多传感器信息融合的仿人机器人跌倒检测及控制*

毕盛 刘皓熙 闵华清 董敏 黄鑫龙

(1. 华南理工大学 计算机科学与工程学院, 广东 广州 510006; 2. 华南理工大学 软件学院, 广东 广州 510006)

基于多传感器信息融合的仿人机器人跌倒检测及控制*

毕盛1刘皓熙1闵华清2董敏1黄鑫龙1

(1. 华南理工大学 计算机科学与工程学院, 广东 广州 510006; 2. 华南理工大学 软件学院, 广东 广州 510006)

仿人机器人在行走过程中,需要及时检测到是否要摔倒,从而阻止摔倒或通过保护动作在摔倒过程中减少机器人损伤.文中基于姿态传感器(包括加速度传感器和陀螺仪)、力感应传感器(FSR)及步行阶段建立了多传感器信息融合模型,采用模糊逻辑决策方法建立了机器人跌倒检测的综合判定方法,并设计相应的控制器,该控制器可检测到仿人机器人运动时是否会摔倒,若无法避免摔倒,则采取相应的保护动作;若可避免摔倒,则通过控制髋关节阻止机器人摔倒.最后在SCUT-I仿人机器人上进行实验,结果表明,文中方法能及时检测到机器人在运动过程中发生跌倒的时刻,并及时通过稳定控制器阻止摔倒或通过倒地控制器产生保护动作,减少机器人的损伤.

仿人机器人;控制;多传感器信息融合;跌倒检测

目前对仿人机器人在特定环境下的行走,如平地行走[1]、上斜坡[2-3]和走楼梯[4-5]等已有较多的研究,但对仿人机器人在非特定环境下的行走问题,由于环境是未知的,故需要在行走过程中对仿人机器人的步行稳定状态进行判定.当机器人快要摔倒时,通过反馈控制相应的关节来阻止机器人摔倒.但有时机器人要摔倒时,由于机器人的状态已超出了可控范围,无法避免摔倒.此时唯一能做的就是让机器人以损伤最小的姿态倒地.

如何判定机器人当前的行走状态是否稳定,这需要一个综合的判定方法，主要借助仿人机器人身上的各种传感器信息来进行判定.目前已有一些研究机构在这方面进行了一系列研究,Renner等[6]通过仿真实验建立了基于步态速度和腰部姿态信息的跌倒检测模型,并在仿真和真实机器人中进行验证.Ogata等[7]根据腰部姿态传感器信息、零力矩点(ZMP)传感器信息和重心参考中心轨迹信息计算出相对于正常状态的角度偏差,从而判断机器人是否要摔倒.Höhn等[8]把机器人是否跌倒描述为一个分类问题,把力矩传感器信息、腰部姿态传感器信息和步行阶段信息作为一个向量进行归一化处理,通过主成分分析(PCA)进行降维,并利用高斯混合模型(GMM)进行分类.这些方法主要是直接利用传感器数据并结合机器人行走模型来判断机器人是否要跌倒,并没有对传感器数据之间的关系展开深入的分析.Chen等[9]通过扩展卡尔曼滤波对关节编码器及姿态传感器数据进行融合,获得机器人姿态状态并进行反馈控制.

为提高姿态传感器数据精度和运算速度,文中对Sage-Husa自适应卡尔曼滤波算法进行了改进,利用力感应传感器(FSR)信息计算出ZMP位置,并根据FSR传感器数据和离线步态确定出机器人的真实步行阶段;然后在传感器数据最高融合层次上,建立了基于姿态传感器、ZMP位置信息和机器人步行阶段的多传感器信息融合模型,并采用模糊逻辑决策方法判定机器人是否要跌倒,采取相应的防止摔倒的控制策略或者做出合理的摔倒保护动作;最后将机器人跌倒检测的综合判定框架应用于SCUT-I仿人机器人进行验证.

1 SCUT-I仿人机器人

SCUT-I仿人机器人是由华南理工大学智能软件与机器人实验室开发,是一款具有视觉和自主决策功能的全自主机器人,身高580 mm,具有21个自由度(DOF)[10],见图1.

图1 SCUT-I仿人机器人及自由度配置

SCUT-I仿人机器人安装姿态传感器和ZMP测量传感器.其中姿态传感器是由两轴的IDG300陀螺仪传感器和ADXL204加速度传感器构成,并安装在机器人腰部.ZMP测量传感器采用INTERLINK电子公司直径为1.27 cm的圆形力感应传感器[11],可以测量1～100 N之间的力.在SCUT-I仿人机器人的每个脚底板上安装了4个FSR传感器,共8个FSR传感器用来计算ZMP位置,分别安装在每个脚板的4个角处.

2 仿人机器人多传感器融合模型

根据传感器信息融合的特点,文中提出了一种分层结构的传感器信息融合模型,用于实现对仿人机器人步行过程的稳定性判定,见图2.

整个传感器信息融合模型主要分为低层和高层两个层次.低层融合模型主要包括姿态传感器信息融合、FSR传感器信息融合和步行阶段确定.姿态传感器信息融合的输入为加速度和陀螺仪数据,通过Sage-Husa自适应卡尔曼滤波,输出准确的姿态角度.FSR传感器信息融合的输入为每个脚底板安装的4个FSR传感器数据,根据力与受力点位置之间的关系计算出ZMP点位置并作为输出.此过程的约束条件为两只脚上的FSR传感器都必须接触地面,这样计算得到的ZMP位置才是有效的.步行阶段确定的输入为离线步态规划和安装在脚底板的FSR传感器与地面的接触情况,结合两部分信息,输出为和地面接触情况相关的步行阶段划分,包括单脚面接触支撑阶段、单脚非面接触支撑阶段、双脚面接触支撑阶段和双脚非面接触支撑阶段.高层融合模型选择模糊决策的方法来做稳定可控性判定,其输入为融合后的姿态信息、ZMP位置信息和真实机器人行走的步行阶段,输出为机器人是否稳定可控状态,从而为机器人控制提供决策的依据.

图2 仿人机器人传感器信息融合模型

3 姿态传感器信息融合

姿态传感器主要由加速度传感器和陀螺仪(角速度传感器)组成,其中加速度传感器容易受到平移加速度的干扰,测量响应速度慢;陀螺仪通过积分得到角度时会造成很大的累计误差.因此需要对这两种传感器信息进行融合,从而得到仿人机器人准确的姿态信息[12].通常利用传统卡尔曼滤波方法进行信息融合,此时认为系统过程噪声向量Wk～N(0,Qk)和观测噪声向量Vk～N(0,Rk)中的系统噪声Qk和测量噪声Rk是不变的,但实际观测环境以及系统环境都会有变化,从而引起Qk和Rk值发生变动.为了解决这个问题,文中采用改进的Sage-Husa自适应卡尔曼滤波算法[13]对姿态传感器信息建立融合模型，解决算法滤波发散问题，设定启动条件提高整体算法实时性.

(1)

(2)

本模型中E[Wk]=qk=0,E[Vk]=rk=0,则传统的Sage-Husa自适应滤波算法可简化为

(3)

(4)

(5)

(6)

dk=(1-b)/(1-bk+1)

(7)

在系统噪声方差阵和观测噪声方差阵均未知的情况下,Sage-Husa算法不能对Qk和Rk同时进行估计,否则会引起滤波器的发散.通过实验与分析发现:在传感器模型初始化时进行补偿后,Qk在滤波过程中变化不大;Rk对滤波的影响较为明显.因此在利用Sage-Husa算法时,仅根据式(5)对测量噪声方差阵Rk进行调整,而不对系统噪声Qk进行调整.

Sage-Husa算法中增加了对噪声统计特性的计算,从而加大了计算量,降低了实时性.文中通过设定合理的Sage-Husa算法启动条件,在不影响融合效果的前提下提高了算法的实时性.

根据式(2)和(3)可得

(8)

若滤波异常,则实际的观测误差的方差必然会与式(8)差别较大.因此,文中根据式(9)来判断滤波是否异常:

(9)

式中,η为常数,可自行设定.若k时刻式(9)成立,则说明实际误差将超过理论预计值的η倍,滤波发散,此时需要启动Sage-Husa自适应卡尔曼滤波算法来预测出新的R.

4 FSR传感器信息融合

在机器人的每只脚上安装4个FSR传感器,根据采集到的传感器数据计算出ZMP数值.FSR传感器位置见图3.

图3 FSR传感器安装位置

图3中,序号1-8分别表示安装点,(xi,yi)为安装点的坐标(i=1,2,…,8),这些坐标可以根据机器人脚的长(Lf)和宽(Wf)、x方向的步长(sx)和y方向的步长(sy)计算得到;fi为在安装点i上承受力的大小,即采集到的FSR传感器数据.ZMP点的坐标PZMP(xZMP,yZMP)计算式如下:

其中:在双脚支撑阶段时,k=1,m=8;在左脚支撑阶段时,k=1,m=4;在右脚支撑阶段时,k=5,m=8.

支撑面中心M的坐标PM(xM,yM)可以分别在双脚支撑阶段、左脚支撑阶段和右脚支撑阶段根据几何关系计算得到.

文中用ΔP表示机器人当前的稳定状态:

(10)

为了分析方便,把ΔP映射到[-1,1]的空间,用ΔP[-1,1]表示.在单脚支撑阶段,xΔP[-1,1]=2xΔP/Lf,yΔP[-1,1]=2yΔP/Wf;在双脚支撑阶段,xΔP[-1,1]=2xΔP/(Lf+sx),yΔP[-1,1]=2yΔP/(Wf+sy).

文中主要通过对ΔP[-1,1]的分析来得到ZMP的位置信息,从而判断机器人是否可以稳定的行走.

5 机器人跌倒状态判定及控制

根据仿人机器人当前的步行状态、姿态信息和ZMP位置信息可以对机器人的稳定可控性进行判定,并根据跌倒是否可控进行机器人倒地控制或机器人稳定控制.整个控制框图见图4.

图4 仿人机器人的控制框图

文中采用模糊决策方法[14-15]对步行阶段、姿态信息和ZMP位置信息进行融合,从而得到最终的机器人跌倒状态判定结果.

将步行阶段、姿态信息和ZMP位置信息作为模糊决策的输入,通过模糊决策后得到机器人是否处于可控状态或向哪个方向倒下的结果.由于判定机器人是否前后倒下,主要与前、后方向的姿态信息和ZMP位置信息有关,与左、右方向的数据没有太大关系,而判定是否左右方向倒下也如此,因此前后方向倒和左右方向倒的判定分两个部分来完成,这样就大大简化了判定方法和处理过程，机器人稳定可控性判断的整个框架如图5所示.

图5 机器人稳定可控性判定的整体框架

针对前后模糊决策系统,文中将机器人步行阶段(GS)、ZMP位置信息xΔP[-1,1]和绕y轴俯仰角θ作为输入.其中GS直接根据步行的4个阶段进行指定;将xΔP[-1,1]模糊化为5种状态(NB、NS、Z、PS、PB),Z的隶属函数采用三角形隶属函数,NS和PS的隶属函数采用梯形隶属函数,NB和PB的隶属函数采用半梯形隶属函数;将θ模糊化为5种状态(NB、NS、Z、PS、PB),NS、Z和PS的隶属函数采用三角形隶属函数,NB和PB的隶属函数采用半梯形隶属函数.左右模糊决策系统的输入隶属函数与前后模糊决策系统类似.

由于决策系统的输出是一个确定的结果,用Ffb表示.文中采用Sugeno模糊系统,设其输出为-1时,表示要向前倒下(FF);输出为0时,表示稳定可控(NF);输出为1时,表示要向后倒下(BF).左右模糊决策输出类似.

根据多次实验的经验设计以下推理规则:双脚面接触支撑阶段规则、双脚非面接触支撑阶段规则、单脚面接触支撑阶段规则和单脚非面接触支撑阶段规则.

根据前后模糊决策系统可以得到机器人前后的状态Ffb,根据左右模糊决策系统可以得到机器人左右的状态Flr,把两者组合成1个二维向量Fo:

(11)式(11)可描述仿人机器人的可控性,如Fo=[-1 1]表示机器人倒向前右方.机器人稳定可控性综合判定结果共有9种状态:可控状态、倒向前方、倒向后方、倒向左方、倒向右方、倒向前左方、倒向前右方、倒向后左方、倒向后右方.

根据前面稳定可控性综合判定结果,机器人不可避免要摔倒时的倒地方向共有8种状态.针对这8种状态,文中设计出4个倒地保护动作控制器,当机器人向前、向前左和向前右方摔倒时采取向前倒地控制器,当机器人向后、向后左和向后右方摔倒时采取向后倒地控制器,当机器人向左摔倒时采用向左倒地控制器,当机器人向右摔倒时采用向右倒地控制器.

这4种控制器控制的具体思路如下:通过控制手臂关节和膝关节使机器人在倒地过程中利用手臂进行支撑和在倒地前重心尽量低,从而减缓碰到地面时对机器人的冲击.

根据稳定可控性综合判定结果,若机器人处于可控状态,则通过步态稳定控制器使机器人保持稳定行走.稳定控制器主要包括姿态控制器和ZMP控制器.

文中对机器人姿态的控制主要使仿人机器人在行走过程中身体和水平面保持垂直.

文中通过对SCUT-I仿人机器人摆动脚踝关节绕x轴滚动的角度和绕y轴的俯仰角度进行调整来实现对机器人的姿态控制.

ZMP控制器是控制仿人机器人的实际ZMP位置,使其接近理想的ZMP位置,即使ZMP在一个理想的稳定区域内,从而使机器人能够实现稳定的行走.ZMP控制器只能在仿人机器人在支撑脚与接触面保持面接触时起作用.ZMP控制器的控制框图见图6.

图6 仿人机器人ZMP控制器框图

6 实验及结果分析

文中通过SCUT-I仿人机器人对提出的稳定可控性综合判定及控制方法进行测试实验.实验采集到的姿态传感器数据和ΔP[-1,1]数据见图7.由于机器人刚刚倒地此时同一侧的FSR接触点开始离开地面,此时根据式(10)计算出的ZMP位置点变化剧烈;而在倒地过程中当多个FSR接触点离开地面时根据式(10)计算出的结果不一定剧烈变化. 在第3秒至第3.5秒x方向的ZMP突然变化到-1,这是因为只有在此方向上的FSR接触点还在接触地面,而其他FSR点都不接触地面了,所以根据式(10)计算出的结果以此方向位置点为主,变成了-1.当机器人彻底倒地时,由于所有接触点的值为0,ZMP计算结果最后为0.

图7 机器人前进摔倒的姿态曲线和ΔP[-1,1]曲线

Fig.7 Attitude and ΔP[-1,1]curves for robot’s fall down during walking forward

文中方法的稳定可控性综合判定输出结果见图8,从图中可知,文中方法对SCUT-I仿人机器人的稳定可控性综合判定输出是正确的.同时机器人在摔倒过程中,倒地控制器起作用,减缓了地面对机器人的冲击.

图8 前进摔倒机器人的实验判定结果

Fig.8 Experimental result for robot’s fall down during walking forward

SCUT-I仿人机器人要在不平整的地面上行走,如图9所示.其中机器人前进步长sx=0.1 m,步行周期Tsup=1 s,地面有0～15 mm高、低不平的阶梯.

图9 机器人成功行走在不平整地面上的结果

Fig.9 Robot’s successful walking result on the uneven ground

采用文中的稳定控制器对机器人进行控制,其行走时的姿态曲线和ΔP[-1,1]曲线见图10.

图10 机器人行走在不平整地面的姿态曲线和ΔP[-1,1]曲线

Fig.10AttitudeandΔP[-1,1]curvesforrobot’swalkingunevenground

从图10(a)可知,姿态控制器控制的姿态角在-10°～10°之间;从图10(b)可知,由于在非平整路面上行走时,脚底的FSR传感器在行走过程中经常会离开地面,甚至在结束阶段,由于机器人站在不平整的地面上,脚底的FSR传感器没有和地面接触,所以ΔP[-1,1]在x轴和y轴方向经常达到-1和1,但由于此时脚底没有和地面完全接触,故不能用ZMP判定理论来判定.但在FSR都和地面接触的情况下,ZMP控制器的控制起作用,控制ΔP[-1,1]趋向稳定裕度内.由于机器人行走是一个连续的过程,在脚底FSR都接触地面的情况下,ZMP控制器能有效的工作,因此在地面环境变化不是很剧烈的情况下,文中方法可以实现机器人的稳定控制行走.

7 结论

文中建立了一整套基于多传感器信息融合模型的机器人步态稳定判定框架及相应的控制器;对Sage-Husa滤波算法进行了改进以提高姿态传感器融合数据的精度和运算速度;根据FSR传感器信息计算出ZMP位置并确定步行阶段；在传感器数据最高融合层次上建立基于姿态传感器、ZMP位置信息和步行阶段的Sugeno型模糊决策系统,同时设计相应的倒地控制器和稳定控制器，并在SCUT-I仿人机器人上进行实验.结果表明:文中方法能及时检测到机器人在运动过程中发生跌倒的时刻,并及时通过稳定控制器阻止摔倒或通过倒地控制器产生保护动作,减少机器人的损伤.

[1] 梶天秀司.Humanoid Robots [M].管贻生,译.北京:清华大学出版社,2006:1- 227.

[2] HUANG Wei-wei,CHEW Chee-Meng,ZHENG Yu,et al.Pattern generation for bipedal walking on slopes and stairs [C]∥Proceedings of the 8th IEEE-RAS International Conference on Humanoid Robots.Daejeon:IEEE,2008:205- 210.

[3] 毕盛,闵华清,陈强,等.一种仿人机器人斜坡运动步态规划方法 [J].华南理工大学学报(自然科学版),2010,38(11):148-160.

BI Sheng,MIN Hua-qing,CHEN Qianq,et al.Gait planning of humanoid robots walking on slope [J].Journal of South China University of Technology(Natural Science Edition),2010,38(11):148-160.

[4] PARK Chan-Soo,HA Taesin,KIM Joohyung,et al.Trajectory generation and control for a biped robot walking upstairs [J].International Journal of Control,Automation and Systems,2010,8(2):339-351.

[5] KIM J Y,PARK I W,OH J H.Realization of dynamic stair climbing for biped humanoid robot using force/torque sensors [J].Journal of Intelligent & Robotic Systems,2010,56(4):389- 423.

[6] RENNER R,BEHNKE S.Instability detection and fall avoidance for a humanoid using attitude sensors and reflexes [C]∥Proceedings of 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.Beijing:IEEE,2006:2967- 2973.

[7] OGATA K,TERADA K,KUNIYOSHI Y.Falling motion control for humanoid robots while walking [C]∥Procee-dings of IEEE-RAS the 7th International Conference on Humanoid Robots.Pittsburgh:IEEE,2007:306-311.

[8] HÖHN O,GERTH W.Probabilistic balance monitoring for bipedal robots [J].International Journal of Robotics Research,2009,28(2):245- 256.

[9] CHEN Ching-Pei,CHENJing-Yi,HUANG Chun-Kai,et al.Sensor data fusion for body state estimation in a bipedal robot and its feedback control application for stable wal-king [J].Sensors,2015,15(3):4925- 4946.

[10] BI Sheng,ZHUANG Zhongjie,MIN Huaqing,et al.Design and dynamic walking control of humanoid robot SCUT-I [J].Applied Mechanics and Materials,2012,187:165-172.

[11] Interlink Electronics,Inc.Force sensing resistor integration guide and evaluation parts catalog [EB/OL].(2015-12- 01) [2015-12- 20].http:∥www.interlinkelectronics.com/datasheets/Datasheet_FSR.pdf.

[12] KIM Jung-Hoon,KIM Jung-Yup,OH Jun-Ho.Adjustment of home posture of a biped humanoid robot using an iner-tial sensor and force torque sensors [C]∥Proceedings of 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems.San Diego:IEEE,2007:2223- 2229.[13] 鲁平,赵龙,陈哲.改进的Sage-Husa自适应滤波及其应用 [J].系统仿真学报,2007,19(15):3503-3505.

LU Ping,ZHAO Long,CHEN Zhe.Improved Sage-Husa-adaptive filtering and its application [J].Journal of System Simulation,2007,19(15):3503-3505.[14] 刘永,彭正洪.基于MATLAB的模糊逻辑控制系统的设计与仿真 [J].武汉大学学报(工学版),2008,41(2):132-135.

LIU Yong,PENG Zheng-hong.Design and simulation of fuzzy logic control system based on MATLAB [J].Engineerring Journal of Wuhan University,2008,41(2):132-135.

[15] 张文修,梁文锡.模糊控制与系统 [M].西安:西安交通大学出版社,1998.

Falling Detection and Control of Humanoid Robots Based on Multi-Sensor Information Fusion

BISheng1LIUHao-xi1MINHua-qing2DONGMin1HUANGXin-long1

(1. School of Computer Science and Engineering, South China University of Technology, Guangzhou 510006, Guangdong, China;2. School of Software, South China University of Technology, Guangzhou 510006, Guangdong, China)

When humanoid robots are walking, it is essential to detect in time whether the robots are going to fall, so as to prevent the robots from the falling or reduce the damage from the falling with the help of the corresponding protection actions. In this paper, a multi-sensor information fusion model in the stage of walking is constructed by using attitude sensors (including an acceleration sensor and a gyroscope) and force sensor resistors (FSR). Then, a comprehensive judgment method of the falling detection of a humanoid robot is proposed by adopting fuzzy logic decision method, and the corresponding controllers are designed to detect whether the robot is going to fall. If the falling is unavoidable, the corresponding protection actions will be adopted. Otherwise, if the falling can be avoided, it will be prevented by controlling hip joints. The experimental results of SCUT-I humanoid robot show that the proposed method can detect the moment at which the robot is going to fall, and then prevent the falling by using the stability controller or reduce the damage to the robot by using the falling controller to bring about the corresponding protection actions.

humanoid robot; control; multi-sensor fusion; falling detection

1000-565X(2017)01- 0095- 07

2016- 01- 04

广东省-教育部产学研合作专项资金项目(2013B090500093);广东省科技计划项目(2015A020219001);国家自然科学基金面上项目(61372140);广州市机器人软件及复杂信息处理重点实验室(15180007);广州市科技创新委员会项目(201609010075)

Foundation items： Supported by the Industry-University-Research Joint Project of Guangdong Province and the Ministry of Education (2013B090500093),the Science and Technology Planning Projects of Guangdong Province(2015A020219001) and the General Program of the National Natural Science Foundation of China(61372140)

毕盛(1978-),男,博士,副教授,主要从事智能仿人机器人研究.E-mail:picy@scut.edu.cn

TP 242.6

10.3969/j.issn.1000-565X.2017.01.014