CN115857328B - 一种基于力矩平衡点与运动稳定性协同优化的六足机器人容错步态落足点规划方法 - Google Patents

Abstract

本发明属于足式机器人技术领域。本发明以单腿失效条件下的六足机器人对运动稳定性及运动柔顺性的同步高要求为目标，公开了一种基于力矩平衡点与运动稳定性协同优化的六足机器人容错步态落足点规划方法。本发明所述的一种基于力矩平衡点与运动稳定性协同优化的六足机器人容错步态落足点规划方法包括：(1)基于ZMP稳定裕度优化的六足机器人容错步态落足点规划模块；(2)基于机身力矩平衡点优化的六足机器人容错步态落足点规划模块；(3)稳定性‑力矩平衡点协同优化评价函数模块；本发明既提供了单腿失效条件下六足机器人以容错步态运动时的运动稳定性及力矩平衡点单独优化模块，又协同的对二者进行了有效融合，以同步实现六足机器人在单腿失效条件下的稳定、柔顺运动。

Description

一种基于力矩平衡点与运动稳定性协同优化的六足机器人容 错步态落足点规划方法

技术领域

本发明属于足式机器人技术领域，具体涉及一种基于力矩平衡点与运动稳定性协同优化的六足机器人容错步态落足点规划方法。

背景技术

随着航空与勘探业的发展，移动机器人在人类不能够进入的地形或环境发挥着重大的作用(如火灾现场、星球探测、深海探测等)。与轮式、履带式机器人相比，六足机器人具有冗余的肢体结构、较多的关节自由度和丰富的步态，因此其对非结构化环境的适应能力更强。然而，在实际应用场景中，地形复杂、环境恶劣，外部破环等因素会导致六足机器人极易出现机械部件损坏、电控系统故障等不良状态，并进一步导致其腿部动力缺失或失效。在此种状态下，虽然机器人可以采用容错步态进行其运动的修正，但是使用容错步态运动时的六足机器人稳定性与柔顺性均会有明显降低。

常规的足式机器人运动规划方法研究中，或仅考虑运动能力健全条件下的六足机器人运动规划，而对运动功能受限条件下(如单腿失效、多腿失效等) 的六足机器人规划问题研究较少，或仅对机器人的运动柔顺性及运动稳定性进行单一考量，无法同时兼顾二者的协同提高，限制了机器人运动表现的提高。因此，研究六足机器人在腿部失效情况下的运动稳定性与运动柔顺性协同优化方法，无疑可以更好的提高六足机器人在实际复杂场景下的运动表现与现实应用性。

发明内容

本发明的目的是提出一种基于力矩平衡点与运动稳定性协同优化的六足机器人容错步态落足点规划方法。以机身姿态角变化最小与ZMP稳定裕度最大为期望分别设计优化目标函数，并建立基于两优化目标的评价函数，依据评价函数在两模块的输出间搜索最优解。力矩平衡点优化弥补了ZMP稳定裕度与外界信息交互上的缺失，运动稳定性优化解决了机身姿态角累积变化量的消除问题。协同的对稳定性与运动柔顺性进行考虑，改进了当前研究中对稳定性和柔顺性考虑的单一性。

本发明能够实时解算高稳定性与高柔顺性协同要求下的最优落足点，提高单腿失效条件下的六足机器人运动稳定性与柔顺性，保障六足机器人的运动可靠性与运动安全性。

本发明通过以下技术方案来实现：

步骤一：在仿真平台搭建具备关节位置驱动的单腿缺失六足机器人模型,设定各关节初值，初始化仿真环境,并基于所使用模型的几何构型对各足进行标号，推导各足在以机身质心为原点的坐标系下的逆运动学方程，给出符合容错步态步序的振荡信号；

步骤二：将支撑足位置作为输入，以平面几何的方法设计将摆动足落点作为变量的基于ZMP稳定裕度的优化算法。将支撑足位置及其足端纵向力反馈作为输入，通过力矩平衡点到机身几何中心间的距离形成以摆动足落点作为变量的基于力矩平衡点的优化算法。结合收到的容错步态步序振荡信号将算法组合成代数环形成单足与其余足之间的耦合，使足端位置在支撑相时保持，在摆动相时向优化位置运动；

步骤三：由运动稳定性与力矩平衡点两优化模块所使用的评价函数总结出新的评价函数。以步骤二得出的两结果作为边界对新评价函数进行一维搜索得到最终结果，并引入步骤二所述代数环中。

本发明的有益效果如下：

所述的一种基于力矩平衡点与运动稳定性协同优化的六足机器人容错步态落足点规划方法，一方面能够通过基于ZMP稳定裕度的优化在地面特征变化较小的情况下提高稳定性并消除姿态角累计变化量，另一方面能够通过对力矩平衡点位置的调整使机器人在地面特征变化较大的情况下维持姿态角，进一步减小运动过程中的姿态角波动。结合上述两部分对六足机器人以容错步态运动时摆动足的运动轨迹进行调整，将得到的信号输入到推导出的足端相对于机体质心位置的逆运动学中，使单腿缺失的六足机器人以容错步态运动时能够协同提高机器人的运动稳定性与柔顺性。

附图说明

图1为该方法控制流程图

图2为Adams模型示意图

图3全系统框图

具体实施方式

下面结合附图和实施例对本发明的实施方式作进一步详细描述。

本发明的一个实施例：一种基于力矩平衡点与运动稳定性协同优化的六足机器人容错步态落足点规划方法，其控制流程如图1所示，主要包含以下内容：

(1)：如图2所示，模型使用正六边形六足机器人构型，相较于左右对称的构型该设计使得机器人单腿失效的情况相同，使控制方案具有一般性，进一步设定各关节初值，初始化虚拟仿真环境。本发明采用的仿真环境为Adams与 simulink联合仿真进行验证，在仿真引擎Adams中可以设置关节材料，受力大小等实际仿真功能。为了使机体的稳定性表现更为明显，模型与地面均设置为刚体。在Adams仿真环境中设置关节铰链，关节驱动，碰撞力等相应基本设置,包括驱动方式，碰撞穿透，阻尼值以及摩擦系数及重力等。同时依据使用的逆运动学公式对当前模型各关节位置设置初值。随后将该机械模型导出生成m文件再到simulink中调用即可。

基于所使用的模型的几何构型对各足进行标号，并推导各足在以机体质心为原点的坐标系下的逆运动学方程；

1).常用的三自由度机械臂的逆运动学公式为足端到根关节位置，现只需按照模型根据各足的标号添加根关节相对于机体质心的坐标变换即可：设k_i为当前足的标号,P_i＝[x_i y_i z_i]^T为足端坐标,θ₁,θ₂,θ₃分别为该足的根髋膝目标角， L₀，L₁，L₂，L₃表示机身几何中心至髋关节的水平距离与由髋关节指向足端的三条腿长，h0表示机身几何中心至髋关节的竖直距离，则

通过上述方程，得到足端关于机体质心的逆运动学公式。

2).引入一个单腿缺失六足机器人容错32步态的振荡器，生成一个占空比为与/>交替变化且幅值为/>与1交替变化的信号波形；

(2)：通过平面几何的方法分别得出基于稳定裕度与力矩平衡的针对即将落足点的简易优化算法，结合步骤一给出的步态次序将算法组合成代数环形成单足与机体之间的耦合，使足端位置在支撑相时保持，在摆动相时向优化位置运动，优化算法包括：

1).对于多足机器人，其稳定裕度为机体质心到支撑多边形的最短距离，多边形的边数等价于需要比较的点线距离的数量，再加上需要对机器人的落点进行范围限制，使其在工作空间内运动，导致方程的阶数极高，而且由于方程中至少有x，y两个元，所以通过偏导为0求极值得出最优解几乎成为不可能的事情。而使用BP神经网络或是遗传算法等机械学习智能算法则会因为上述过程为动态过程，需要实时对运动状态给出新的落足点，所以需要较高的硬件需求来满足计算速度。

考虑到3-2容错步态的支撑相固定为三角型，且落足点的优化无需求出稳定裕度的数值。当两点确定时，一条边的点线距离已无法改变，故此时稳定裕度最优的位置可以通过平面几何得出。确定的两点与两点中垂线所在且尽可能远离两点的中点的点所组成的三角形拥有最大的内切圆，那么当只固定一点而将另外两点以代数环的方式代入以上理论给出的方程，将得到两个点的位置解，它们互相接近对方与固定点的中垂线且尽可能远离两者的中点，这样能得到一个拥有最大内切圆的三角形，再考虑到正六边形机体的结构分布，该算法可以得到两个最优稳定裕度的落足点，同时32容错步态的步序中每步的切换足数量也是2。

设给定两点坐标为(a，b)，(c，d)，LQ为足端位置与机身几何中心的期望距离，P，Q为权重参数，则下式为评价函数

对其求偏导可得：

当固定点给出，例如(a，b)固定，将上式并行运行两次，依照容错步态的步序，式二的输出加半个水平步幅作为式一的输入，式一的输出作为式二的输入，即可得出两点的解，随后其中一解依照步序经过处理作为下一步序的固定点。

2).对于力矩平衡的优化部分则是通过计算机器人力矩和为零的点到机身几何中心的距离最小作为评价，忽略腿部相对于机身的质量，将机器人质心近似在机身的几何中心处，所以当零力矩点越接近机身几何中心机器人质心处受到的力矩越小，足端力反馈采取每一步态周期求积分除时间的方式，一方面是为了得到足地力的稳定信号用以判断地形与机器人姿态数据排除突发事件以及步序切换过程中的信号干扰，另一方面可以将地面看作另一个求解器，机器人输入足地力，输出位置，地面相反，而上述处理方式是一种滤波方式，当地形不发生剧烈变化时机器人支撑足不与地面分离，反馈力可以得到较为稳定的波形，且实际力与期望力间的误差上限将逐渐减小，从而使地面下一时刻输出的足地力在机器人正常运动的情况下趋于稳定，该优化方法的评价公式如下：

求偏导后得：

至此为力矩平衡优化模块所有推导公式，其中F,K,M均为权重系数，f_max为完整机体重力，初始点位于以机体几何中心为圆心，LQ为半径的圆上，各初值点间相差/>上式算上力反馈为九输入单输出模块，但其对于步态的依赖不如稳定裕度模块。

将上述两种优化模块分别形成代数环输出完好足的足端坐标，输入信号由振荡器给出。使用代数环的耦合性与步态信号起到的断点作用，将支撑相所在足的位置锁死，保证支撑足不会因为位置调整产生内力对抗，此时代数环将通过剩余的摆动相所在足形成新的平衡，达到步态优化的目的。

(3)：观察步骤二所述两优化模块使用的评价函数，建立新评价函数：

f_judge*＝(K_add1×f_judge+f_judge2)-K_add×(K_add1×f_judge×f_judge2) (6)

其中求和部分表示两模块同时存在取小的目标，乘积部分表示尽可能使两者同时取小，参数K_add在初始化阶段给定，用以调整最终解在求和与乘积部分的优先级，参数K_add1在初始化阶段给定，用以调整最终解在两模块间的优先级，此时步骤二中两模块的解为最终解的边界，设置可接受最大误差后使用一维搜索 (二分法)迭代寻找即可，最终全系统流程如图3所示。

Claims (1)

1.一种基于力矩平衡点与运动稳定性协同优化的六足机器人容错步态落足点规划方法，其特征在于：对以运动稳定性优化与机身力矩平衡点位置优化为目标下的机器人容错步态落足点进行分别规划，并通过设计带权重分配的落足点评价函数的方式，对两种优化目标下的规划落足点进行融合，最终实现单腿失效六足机器人容错步态运动下的最优落足点规划，保证机器人全时间历程下的运动稳定性与运动柔顺性；

其具体实现可分以下步骤：

步骤1：由基于ZMP稳定裕度优化的六足机器人容错步态落足点规划模块求解得到保证机器人稳定运动的规划落足点；

步骤2：由基于机身力矩平衡点优化的六足机器人容错步态落足点规划模块求解得到保证机器人运动柔顺性的规划落足点；

步骤3：依据步骤1及步骤2得到的规划落足点，通过稳定性-力矩平衡点协同优化落足点评价函数的设计及求解，得到最终规划落足点；

依据六足机器人单腿失效条件下的容错步态特征，以平面几何原理建立ZMP稳定裕度优化方程；由于容错32步态的支撑多边形恒为三角形，其内切圆原点为固定三角形的ZMP最大点，同时考虑到六足机器人的几何构型使其重心徘徊于机身几何中心附近，等边三角形成为最理想的支撑面；将支撑点与对边中垂线距离的平方减去支撑点与对边中点距离的平方作为目标建立优化公式，实时地以机器人在当前支撑状态与下一支撑状态中共有的支撑足的足端位置作为输入，通过对优化公式的两次迭代使用，在落足点约束内求解另外两个下一状态支撑足的落点位置使下一个支撑三角形趋于相似等边三角形：

式中存在(a,b)，(c,d)两个假定已知点，其中一点为两个不同支撑状态的共同支撑足位置，另外一点起迭代作用，参数Q控制支撑点与对边中点距离的平方相较于支撑点与对边中垂线距离的平方的权重，参数P控制目标足端软约束在全公式中的权重，参数k与参数L_Q确定了目标足的初始位置；参数均在初始化阶段依据步态期望给出，其余为实时足端信息，从而规划了ZMP稳定裕度最优条件下的当前摆动足最优落足点(x,y)，并得到优化目标f_judge；

依据一个步态周期内各足纵向平均受力以及当前足端位置，给出以机器人姿态角变化最小为目标的下一步序时刻足端水平方向优化位置；将机器人质心近似在机身的几何中心处，当机身几何中心处在姿态角方向的力矩最小时，机器人姿态角变化的加速度也将最小；将反馈得到的足地力信号在一步态周期内取平均值，从而忽略由于落足点不能及时响应而无法处理的突发状况带来的影响，将处理得到的一步态周期内平均足地力信号与对应的足端位置作为输入计算出力矩平衡点的位置；上述力矩均由足端纵向力产生，当力臂缩短时力矩将变小，将机器人几何中心与力矩平衡点间距离的平方作为评价目标得出的优化公式能够有效减小机器人受到的力矩：

式中存在(x_a,y_a)，(x_b,y_b)，(x_c,y_c)，(x_d,y_d)四个已知点，以参数l对四个已知点进行枚举，则l可取a，b，c，d四个标记，标记为j的对应足在一周期内力反馈的平均值为f_j，标记为i的目标足在一周期内力反馈的平均值为f_i，参数f_max为机身重力在每条腿上的均分量，参数F,K,M均为足端位置软约束权重系数，参数k，参数k_i与参数L_Q确定了目标足的初始位置；参数均在初始化阶段依据步态期望给出，其余为实时足端信息，从而解算得到使机器人运动过程中的力矩平衡点向机身几何中心处靠近的最优落足点(x,y)，并得到优化目标f_judge2，保证机器人在崎岖地形下运动时的运动柔顺性，减小姿态波动；

根据平面上所有周长相同的四边形中正方形面积最大的原理，通过两个优化目标的乘积使最终评价函数满足均衡优化的目的；最终评价函数如下：

f_judge*＝(K_add1×f_judge+f_judge2)-K_add×(K_add1×f_judge×f_judge2) (3)

将优化目标f_judge与f_judge2作为输入，根据路况以及个人需求，对稳定性与柔顺性存在不同的重视程度，对作用于单个优化目标上的权重系数K_add1的调整能够改变评价函数在两优化目标间的优化倾向，对作用于乘积项的权重系数K_add进行调整能够改变优化目标间的协同程度；基于得到的两个最优落足点建立区间，在其中以评价函数搜索得到最终落足点，按照权重系数的要求同时优化了机器人的运动稳定性与柔顺性；

基于所使用的模型的几何构型对各足进行标号，并推导各足在以机体质心为原点的坐标系下的逆运动学方程；

通过平面几何的方法分别得出基于稳定裕度与力矩平衡的针对即将落足点的简易优化算法，结合给出的步态次序将算法组合成代数环形成单足与机体之间的耦合，使足端位置在支撑相时保持，在摆动相时向优化位置运动。