一种基于脚力估计算法的多足机器人动态稳定性定量判定方法技术领域
本发明属于机器人控制领域，具体是一种基于脚力估计算法的机器人动态稳定性
定量判定方法，实时检测多足机器人运动过程中的稳定裕度。
背景技术
多足机器人是一种具有多支链、冗余和时变拓扑结构的复杂机器人，具有比轮式
机器人更强的地形适应能力，在军事和民用领域具有巨大的应用潜力和广泛的应用前景，
受到国内外机器人研究者的广泛关注和重视。
多足机器人一般按足的数目可分为四足、六足和八足机器人。由于多足机器人行
走的环境往往较为复杂，可能会遇到崎岖地形，楼梯，障碍物，甚至滚石等，这些复杂地形和
环境干扰对机器人的稳定性带来了很大的挑战。采用准确的稳定性判据实时监控机器人的
稳定性对于保持机器人平衡，保证机器人不倾翻，完成救灾任务是至关重要的。机器人的稳
定性判定主要是指由一定的稳定裕度值判断机器人会不会出现倾翻摔倒，稳定裕度大于零
时机器人处于稳定状态，稳定裕度小于零时机器人将会出现倾翻，即机器人失稳。
目前，机器人的稳定性判定方法分为静态稳定判据和动态稳定性判据。静态稳定
性判据没有考虑惯性力和外力对稳定性的影响，静态稳定性判据主要有两类，一类是基于
重心投影与支撑多边形关系的方法，他们有重心投影法，静态稳定边界法，纵向稳定边界法
和偏转纵向稳定边界法。另一类是基于能量的静态稳定判据，它们有能量稳定边界法，倾覆
能量稳定边界法和规范化的能量稳定性边界法。动态稳定性判据是指考虑了惯性力和外力
影响的稳定性判据，主要分为两类。第一类是基于零力矩点（ZMP, Zero Moment Point）的
动态稳定性判据。南斯拉夫学者Miomir Vukobratovic于1970年提出了ZMP稳定性判据，该
方法成为了应用于两足机器人稳定性判定的经典方法，此后众多学者在此概念的基础上提
出了自己的判定方法，如压力中心法、有效质量中心法等，其实质都属于ZMP理论。然而有学
者指出ZMP方法无法用于行走于非平面地形上的多足机器人的稳定性判定，因为由ZMP的定
义可知，ZMP点是惯性力与重力的合力与脚底支撑平面的交点，当机器人支撑腿的数目大于
等于四条时，地面地形为非平面地形时，机器人脚与地面的接触点将不会共面，此时ZMP点
将无法求得。另外一类动态稳定性判定方法是基于动量的稳定性判据，主要有动态稳定边
界法，倾倒稳定性判别法和支撑力矩判别法。这类方法对于具有外力扰动，地面不平和惯性
力存在的情况也是完全可行的，但是这类判据没有考虑质心位置变动对稳定性的影响，因
为对于一个地面上的物体而言，其质心位置距离地面越高稳定性越差。本发明利用多足机
器人位姿信息和足端脚力信息对机器人进行动态稳定性判断，不利用支撑平面判断稳定
性，直接判断绕支撑轴的倾翻和绕落足点的倾翻，使该方法能够适用于不平路面的情形，而
且考虑了质心位置变动对机器人稳定性的影响。
无论是基于ZMP的稳定性判定方法，还是本发明提出的稳定性判定方法，都需要采
用足底与地面接触力计算，因此正确检测足底接触力成为关键和先决条件。目前都采用在
足底安装FSR（压力传感器）或者多维力传感器的方法实现对足底力的检测。但是FSR只能检
测正压力，无法检测其余方向的力；多维力传感器价格昂贵，且易损坏。本发明提出一种基
于动力学模型的脚力估计算法，通过机器人的运动状态和电机扭矩估算外界接触力，可以
有效检测足底力的大小和方向，而无需力传感器。
发明内容
本发明的目的在于提出一种基于脚力估计的多足机器人动态稳定性定量判定方
法，该方法考虑了惯性力、外力干扰，而且考虑了质心变动对稳定性的影响，具有计算量少、
可靠性高等特点，且采用脚力估计方法替代易损的多维力传感器，降低机器人制造成本，提
高检测可靠性。
本发明提供的一种基于脚力估计算法的多足机器人动态稳定性定量判定方法，其
特征在于，该方法包括下述步骤：
步骤1：实时获取关节角度、机身位姿信息，并利用这些信息对机器人进行正运动学计
算，得到机器人各个构件的质心在参考坐标系中的坐标，以及各支撑腿足端点的坐标；
步骤2：利用质心计算公式计算机器人总体质心在参考坐标系中的坐标；
步骤3：采用脚力估计算法实时获取支撑腿的脚力信息，并向机器人质心处等效各个脚
力，得到对机器人质心的合力和合力矩；
步骤4：计算最易发生倾翻的两条支撑轴的矢量，与脚力最大的脚相连的两条轴线为最
易发生倾翻的两条轴线；
步骤5：分别计算合力和合力矩对两条最易发生倾翻的轴线及步骤4中脚力最大的脚的
倾翻有效合力；
步骤6：分别计算步骤5中得到的三个倾翻有效合力与倾翻轴过质心的垂线的夹角和质
心与脚力最大落足点的连线的夹角，三个夹角分别为相应轴线和落足点的稳定角；
步骤7：选取步骤6中得到的三个夹角中最小者作为机器人的稳定裕角，稳定裕角与合
力相乘后作归一化处理得到稳定裕度值。
本发明是一种基于脚力估计算法的多足机器人动态稳定性检测方法，用于多足机
器人在非结构化环境下运动时稳定性判定，判定方法简洁明确。本发明的核心是由脚力估
计算法估计足底地面接触力，所有脚力在质心处合成合力和合力矩，然后计算对两条最易
发生倾翻的支撑轴和落足点的稳定裕度角。在脚力大小和方向都不变的情况下，如果机器
人质心高度增高，有效合力与倾翻轴线过质心的垂线之间的夹角一定会减小，因此该方法
能够反映质心变动对稳定性带来的影响。
附图说明
图1是多足机器人动态稳定性定量检测方法流程图。
图2是六足机器人结构与坐标系示意图。
图3是绕倾翻轴稳定裕角计算示意图。
图4是绕落足点稳定裕角计算示意图。
图5是脚力估计算法计算框图。
具体实施方式
以下结合实施例及附图对本发明作进一步详细的说明，但本发明实施方式不限于
此。该具体实施例选取多足机器人中六足串联机器人（如图2所示）作为作用对象，但实施对
象不限于此，四足、八足或并联机器人等同样适用。
如图1所示，本发明实例提供的一种基于脚力估计算法的多足机器人动态稳定性
定量判定方法包括以下步骤。
步骤1：实时获取关节角度、机身位姿信息，并利用这些信息对机器人进行正运动
学计算，得到机器人各个构件的质心在参考坐标系中的坐标，以及各支撑腿足端点的坐标。
图2中Oxyz为参考坐标系，由电机绝对值编码器和机身位姿传感器可以进行机器
人正逆运动学计算，从而可以得到图2中所示的各支撑腿落足点在参考坐标系Oxyz中的坐
标pi（i =1,2,…,n），同时也可以得到各个构件的坐标。
步骤2：利用质心计算公式计算机器人总体质心在参考坐标系中的坐标。
其中，r和s分别表示多足机器人腿的个数和每条腿含有独立构件的个数，mb表示机身
的质量，mij表示第i条腿第j个构件的质量。
步骤3：采用脚力估计算法实时获取支撑腿的脚力信息，并向机器人质心处等效各
个脚力，得到对机器人质心的合力和合力矩。
支撑腿脚力信息由脚力估计算法估计得到，具体如图5所示。首先需要预先建立机
器人腿部机构动力学方程和关节摩擦模型，并对未知参数进行参数辨识，得到精确的机器
人腿部动力学模型。机器人在行走过程中，伺服电机可以实时将电机电流信息和编码器得
到的角速度信息上传至控制器。
动力学模型中需要用到关节的角度、角速度和角加速度，而电机编码器只能得到
角速度，因此需要状态观测器估计角度和角加速度，而且编码器直接输出的角速度是带有
噪声的信号，状态观测器也可以滤除噪声。本发明中采用的状态观测器是卡尔曼滤波器。设
，则通过卡尔曼滤波器，即可得到状态的最优估计值 、和。
由于电机不是直接驱动连杆的，往往是通过减速器或者其他运动副与连杆相连，
而这些中间环节往往具有较大的惯量和摩擦，需要通过扰动观测器求解关节力矩τo。
对于关节i具有以下平衡方程：
式中Ki为电机力矩系数，Ii为电机电流，Ji为折算到电机轴上的关节转动惯量，
τdisi是外界所有扰动之和。τdisi定义如下，
式中即为需要求取的关节扭矩，由建立的摩擦模型求得。值得注意的是此处将
其他扰动全部合并到摩擦力中，因此摩擦模型一般不能用简单的库仑摩擦加粘性摩擦来描
述，需要根据实际情况合理建立新的摩擦模型。联立(2)和(3)式即可得到关节力矩为
预先建立的腿部机构的动力学模型为
其中M为3×3的惯性矩阵，C为科氏力和向心力项，G为重力项，Tf为除驱动关节摩
擦外的其他关节摩擦。将状态观测器得到的状态估计值和代入(5)式中，即可得到
τdyna。
关节驱动力包含两部分，一部分是无外力作用时驱动机构运动所需驱动力，另一
部分是外力作用对关节驱动力的影响，即有下式
τo＝τdyna+τreact (6)
则
τreact＝τo-τdyna (7)
式(7)得到的关节空间中的力矩，通过雅克比矩阵可以映射到笛卡尔坐标系中，
设第i条支撑腿的脚力为令
bi＝pi-pc (9)
式中bi如图2所示。则可由下式得到脚力对质心的等效合力和合力矩
步骤4：计算最易发生倾翻的两条支撑轴的矢量，与脚力最大的脚相连的两条轴线为最
易发生倾翻的两条轴线。
机器人发生倾翻时可能会绕一条支撑轴倾翻，也可能会绕一个落足点倾翻，出现倾翻
时会导致倾翻轴线所在的两个脚受力最大，如果是绕落足点倾翻，则该足受力最大。因此可
以据此减少计算量，只需计算与脚力最大的落足点相连的两条轴线的稳定裕度，并且计算
绕脚力最大的落足点的稳定裕度，从而不需要再计算其他支撑轴和落足点的稳定裕度。设
第m条支撑腿的脚力最大，则有
则最易发生倾翻的两条支撑轴的矢量为αm和am+1。
步骤5：分别计算合力和合力矩对两条最易发生倾翻的轴线及步骤4中脚力最大的脚的
倾翻有效合力。
合力fr中平行于支撑轴线的分量不会造成机器人的倾翻，如图3所示，不失一般性，合
力fr对ai轴的倾 翻有效力为
其中E3表示3×3的单位举证，表示ai的单位向量。
同样，合力矩nr中也只有垂直于ai轴的分量对机器人产生绕ai轴的倾翻作用，该有效倾
翻力矩分量为
如图3中所示，将ni分解为一对力偶fni，则可将力矩的倾翻作用并入fi中，形成最终的
倾翻有效合力。
其中Ii表示过质心的轴ai的垂线，为Ii的单位向量。则合力
和合力矩对ai的倾翻有效合力为
类似地，同样可以计算合力和合力矩对落足点i的倾翻有效合力。此时合力fr全部会对
落足点产生倾翻，因此设对fbi为fr对落足点i的有效倾翻力为
fbi＝fr (16)
然而合力矩nr中只有垂直于bi的分量才是对落足点i的有效倾翻力矩分量，由下式计
算得到
其中为bi的单位矢量。同样将该有效倾翻力矩分量nbi分解为一对力偶，
则可得则合力和合力矩对落足点i的倾翻有效合力为
将i替换为m和m+1，可得对应最易发生倾翻的两条轴线am和am+1及落足点m的三个倾翻
有效合力和
步骤6：分别计算步骤5中得到的三个倾翻有效合力与相应的倾翻轴过质心的垂线的夹
角和与质心—落足点连线的夹角。
对支撑轴ai的稳定角是指对应的倾翻有效合力与倾翻轴的垂线Ii的夹角，由下式得
到
其中表示的单位矢量，ξi可由下式得到
类似地，对落足点的有效倾翻合力与质心—落足点连线构成了绕落足点的稳定角，由
下式计算得到
对于已经判定出的最易发生倾翻的两条支撑轴和一个落足点，即与具有最大脚力的落
足点m相关的三个稳定角为θm，θm+1和其中稳定角的正负表示了机器人的状态，如果稳定
角小于零则机器人处于倾翻状态，如果大于零则机器人处于稳定状态，如果等于零则机器
人处于临界稳定状态。
步骤7：选取步骤6中得到的三个夹角中最小者作为机器人的稳定角，稳定裕角与合力
相乘后作归一化处理得到稳定裕度值。
步骤6中一旦有一个稳定角出现了小于零的情况，机器人就处于倾翻状态，因此选取三
者中最小的夹角为机器人的稳定角。值得注意的是，在稳定角相同的情况下，合力fr的大小
也表征了机器人的稳定程度，如在机器人稳定角大于零的情况下，fr越大，需要更大的外界
扰动力才能使机器人倾翻，因此定义机器人的力角稳定裕度为机器人稳定角与fr的模的乘
积。为方便起见，进行归一化处理，经归一化的机器人的力 角稳定裕度计算公式如下
其中θnom和fnom定义为机器人处于水平地面没有运动没有受到外界干扰力时的稳定角
和质心处脚力的合力，此时机器人的稳定裕度最大。
因此本发明基于力角稳定裕度的机器人动态稳定性判定方法可以表述为：力角稳定裕
度α大于零时机器人处于稳定状态，且α越大，即越接近1，机器人稳定性越高；α小于零时机
器人处于不稳定状态，即处于倾翻状态，且α越小机器人的不稳定程度越大；α等于零是机器
人处于临界稳定状态。
本发明能够体现质心高度对稳定性的影响在于在其他条件不变的情况下质心高度的
增加会带来稳定角θi和的减小，如图4所示，从而导致式(24)中α的减小，即力角稳定裕度
的减小。