一种基于脚力估计算法的多足机器人动态稳定性定量判定方法

摘要

本发明提供了一种基于脚力估计算法的多足机器人动态稳定性定量判定方法，适用于机器人行走于非结构化环境时的稳定性实时检测。本发明由传感器获得关节和机身运动状态后，通过正、逆运动学求得计算稳定裕度所需的变量值。然后通过脚力估计算法估计支撑腿的脚力，由脚力大小得到最易发生倾翻的支撑轴和落足点。由脚力在质心处的合力和合力矩计算对支撑轴和落足点的倾翻有效合力，进而可以由公式得到支撑轴和落足点的稳定角，选取稳定角中的最小者由公式计算得到机器人的归一化的力角稳定裕度值。本发明的优点在于：本方法直接判断对支撑轴和落足点的稳定裕度；可以表征质心的高低带来的稳定性变化；不需要采用易损的多维力传感器测量脚力。

说明书

技术领域

本发明属于机器人控制领域，具体是一种基于脚力估计算法的机器人动态稳定性定量判定方法，实时检测多足机器人运动过程中的稳定裕度。

背景技术

多足机器人是一种具有多支链、冗余和时变拓扑结构的复杂机器人，具有比轮式机器人更强的地形适应能力，在军事和民用领域具有巨大的应用潜力和广泛的应用前景，受到国内外机器人研究者的广泛关注和重视。

多足机器人一般按足的数目可分为四足、六足和八足机器人。由于多足机器人行走的环境往往较为复杂，可能会遇到崎岖地形，楼梯，障碍物，甚至滚石等，这些复杂地形和环境干扰对机器人的稳定性带来了很大的挑战。采用准确的稳定性判据实时监控机器人的稳定性对于保持机器人平衡，保证机器人不倾翻，完成救灾任务是至关重要的。机器人的稳定性判定主要是指由一定的稳定裕度值判断机器人会不会出现倾翻摔倒，稳定裕度大于零时机器人处于稳定状态，稳定裕度小于零时机器人将会出现倾翻，即机器人失稳。

目前，机器人的稳定性判定方法分为静态稳定判据和动态稳定性判据。静态稳定性判据没有考虑惯性力和外力对稳定性的影响，静态稳定性判据主要有两类，一类是基于重心投影与支撑多边形关系的方法，他们有重心投影法，静态稳定边界法，纵向稳定边界法和偏转纵向稳定边界法。另一类是基于能量的静态稳定判据，它们有能量稳定边界法，倾覆能量稳定边界法和规范化的能量稳定性边界法。动态稳定性判据是指考虑了惯性力和外力影响的稳定性判据，主要分为两类。第一类是基于零力矩点（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中的坐标p_i（i>n），同时也可以得到各个构件的坐标。

步骤2：利用质心计算公式计算机器人总体质心在参考坐标系中的坐标。

其中，r和s分别表示多足机器人腿的个数和每条腿含有独立构件的个数，m_b表示机身的质量，m_ij表示第i条腿第j个构件的质量。

步骤3：采用脚力估计算法实时获取支撑腿的脚力信息，并向机器人质心处等效各个脚力，得到对机器人质心的合力和合力矩。

支撑腿脚力信息由脚力估计算法估计得到，具体如图5所示。首先需要预先建立机器人腿部机构动力学方程和关节摩擦模型，并对未知参数进行参数辨识，得到精确的机器人腿部动力学模型。机器人在行走过程中，伺服电机可以实时将电机电流信息和编码器得到的角速度信息上传至控制器。

动力学模型中需要用到关节的角度、角速度和角加速度，而电机编码器只能得到角速度，因此需要状态观测器估计角度和角加速度，而且编码器直接输出的角速度是带有噪声的信号，状态观测器也可以滤除噪声。本发明中采用的状态观测器是卡尔曼滤波器。设，则通过卡尔曼滤波器，即可得到状态的最优估计值 、和。

由于电机不是直接驱动连杆的，往往是通过减速器或者其他运动副与连杆相连，而这些中间环节往往具有较大的惯量和摩擦，需要通过扰动观测器求解关节力矩τ_o。

对于关节i具有以下平衡方程：

式中Kⁱ为电机力矩系数，Iⁱ为电机电流，Jⁱ为折算到电机轴上的关节转动惯量，τ_disi是外界所有扰动之和。τ_disi定义如下，

式中即为需要求取的关节扭矩，由建立的摩擦模型求得。值得注意的是此处将其他扰动全部合并到摩擦力中，因此摩擦模型一般不能用简单的库仑摩擦加粘性摩擦来描述，需要根据实际情况合理建立新的摩擦模型。联立(2)和(3)式即可得到关节力矩为

预先建立的腿部机构的动力学模型为

其中M为3×3的惯性矩阵，C为科氏力和向心力项，G为重力项，T_f为除驱动关节摩擦外的其他关节摩擦。将状态观测器得到的状态估计值和代入(5)式中，即可得到τ_dyna。

关节驱动力包含两部分，一部分是无外力作用时驱动机构运动所需驱动力，另一部分是外力作用对关节驱动力的影响，即有下式

τ_o＝τ_dyna+τ_react(6)

则

τ_react＝τ_o-τ_dyna(7)

式(7)得到的关节空间中的力矩，通过雅克比矩阵可以映射到笛卡尔坐标系中，

设第i条支撑腿的脚力为令

b_i＝p_i-p_c(9)

式中b_i如图2所示。则可由下式得到脚力对质心的等效合力和合力矩

步骤4：计算最易发生倾翻的两条支撑轴的矢量，与脚力最大的脚相连的两条轴线为最易发生倾翻的两条轴线。

机器人发生倾翻时可能会绕一条支撑轴倾翻，也可能会绕一个落足点倾翻，出现倾翻时会导致倾翻轴线所在的两个脚受力最大，如果是绕落足点倾翻，则该足受力最大。因此可以据此减少计算量，只需计算与脚力最大的落足点相连的两条轴线的稳定裕度，并且计算绕脚力最大的落足点的稳定裕度，从而不需要再计算其他支撑轴和落足点的稳定裕度。设第m条支撑腿的脚力最大，则有

则最易发生倾翻的两条支撑轴的矢量为α_m和a_m+1。

步骤5：分别计算合力和合力矩对两条最易发生倾翻的轴线及步骤4中脚力最大的脚的倾翻有效合力。

合力f_r中平行于支撑轴线的分量不会造成机器人的倾翻，如图3所示，不失一般性，合力f_r对a_i轴的倾>

其中E₃表示3×3的单位举证，表示a_i的单位向量。

同样，合力矩n_r中也只有垂直于a_i轴的分量对机器人产生绕a_i轴的倾翻作用，该有效倾翻力矩分量为

如图3中所示，将n_i分解为一对力偶f_ni，则可将力矩的倾翻作用并入f_i中，形成最终的倾翻有效合力。

其中I_i表示过质心的轴a_i的垂线，为I_i的单位向量。则合力和合力矩对a_i的倾翻有效合力为

类似地，同样可以计算合力和合力矩对落足点i的倾翻有效合力。此时合力f_r全部会对落足点产生倾翻，因此设对f_bi为f_r对落足点i的有效倾翻力为

f_bi＝f_r(16)

然而合力矩n_r中只有垂直于b_i的分量才是对落足点i的有效倾翻力矩分量，由下式计算得到

其中为b_i的单位矢量。同样将该有效倾翻力矩分量n_bi分解为一对力偶，

则可得则合力和合力矩对落足点i的倾翻有效合力为

将i替换为m和m+1，可得对应最易发生倾翻的两条轴线a_m和a_m+1及落足点m的三个倾翻有效合力和

步骤6：分别计算步骤5中得到的三个倾翻有效合力与相应的倾翻轴过质心的垂线的夹角和与质心—落足点连线的夹角。

对支撑轴a_i的稳定角是指对应的倾翻有效合力与倾翻轴的垂线I_i的夹角，由下式得到

其中表示的单位矢量，ξ_i可由下式得到

类似地，对落足点的有效倾翻合力与质心—落足点连线构成了绕落足点的稳定角，由下式计算得到

对于已经判定出的最易发生倾翻的两条支撑轴和一个落足点，即与具有最大脚力的落足点m相关的三个稳定角为θ_m，θ_m+1和其中稳定角的正负表示了机器人的状态，如果稳定角小于零则机器人处于倾翻状态，如果大于零则机器人处于稳定状态，如果等于零则机器人处于临界稳定状态。

步骤7：选取步骤6中得到的三个夹角中最小者作为机器人的稳定角，稳定裕角与合力相乘后作归一化处理得到稳定裕度值。

步骤6中一旦有一个稳定角出现了小于零的情况，机器人就处于倾翻状态，因此选取三者中最小的夹角为机器人的稳定角。值得注意的是，在稳定角相同的情况下，合力f_r的大小也表征了机器人的稳定程度，如在机器人稳定角大于零的情况下，f_r越大，需要更大的外界扰动力才能使机器人倾翻，因此定义机器人的力角稳定裕度为机器人稳定角与f_r的模的乘积。为方便起见，进行归一化处理，经归一化的机器人的力>

其中θ_nom和f_nom定义为机器人处于水平地面没有运动没有受到外界干扰力时的稳定角和质心处脚力的合力，此时机器人的稳定裕度最大。

因此本发明基于力角稳定裕度的机器人动态稳定性判定方法可以表述为：力角稳定裕度α大于零时机器人处于稳定状态，且α越大，即越接近1，机器人稳定性越高；α小于零时机器人处于不稳定状态，即处于倾翻状态，且α越小机器人的不稳定程度越大；α等于零是机器人处于临界稳定状态。

本发明能够体现质心高度对稳定性的影响在于在其他条件不变的情况下质心高度的增加会带来稳定角θ_i和的减小，如图4所示，从而导致式(24)中α的减小，即力角稳定裕度的减小。