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

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


背景技术：

2.随着航空与勘探业的发展，移动机器人在人类不能够进入的地形或环境发挥着重大的作用(如火灾现场、星球探测、深海探测等)。与轮式、履带式机器人相比，六足机器人具有冗余的肢体结构、较多的关节自由度和丰富的步态，因此其对非结构化环境的适应能力更强。然而，在实际应用场景中，地形复杂、环境恶劣，外部破环等因素会导致六足机器人极易出现机械部件损坏、电控系统故障等不良状态，并进一步导致其腿部动力缺失或失效。在此种状态下，虽然机器人可以采用容错步态进行其运动的修正，但是使用容错步态运动时的六足机器人稳定性与柔顺性均会有明显降低。
3.常规的足式机器人运动规划方法研究中，或仅考虑运动能力健全条件下的六足机器人运动规划，而对运动功能受限条件下(如单腿失效、多腿失效等) 的六足机器人规划问题研究较少，或仅对机器人的运动柔顺性及运动稳定性进行单一考量，无法同时兼顾二者的协同提高，限制了机器人运动表现的提高。因此，研究六足机器人在腿部失效情况下的运动稳定性与运动柔顺性协同优化方法，无疑可以更好的提高六足机器人在实际复杂场景下的运动表现与现实应用性。


技术实现要素：

4.本发明的目的是提出一种基于力矩平衡点与运动稳定性协同优化的六足机器人容错步态落足点规划方法。以机身姿态角变化最小与zmp稳定裕度最大为期望分别设计优化目标函数，并建立基于两优化目标的评价函数，依据评价函数在两模块的输出间搜索最优解。力矩平衡点优化弥补了zmp稳定裕度与外界信息交互上的缺失，运动稳定性优化解决了机身姿态角累积变化量的消除问题。协同的对稳定性与运动柔顺性进行考虑，改进了当前研究中对稳定性和柔顺性考虑的单一性。
5.本发明能够实时解算高稳定性与高柔顺性协同要求下的最优落足点，提高单腿失效条件下的六足机器人运动稳定性与柔顺性，保障六足机器人的运动可靠性与运动安全性。
6.本发明通过以下技术方案来实现：步骤一：在仿真平台搭建具备关节位置驱动的单腿缺失六足机器人模型,设定各关节初值，初始化仿真环境,并基于所使用模型的几何构型对各足进行标号，推导各足在以机身质心为原点的坐标系下的逆运动学方程，给出符合容错步态步序的振荡信号；步骤二：将支撑足位置作为输入，以平面几何的方法设计将摆动足落点作为变量的基于zmp稳定裕度的优化算法。将支撑足位置及其足端纵向力反馈作为输入，通过力矩平
衡点到机身几何中心间的距离形成以摆动足落点作为变量的基于力矩平衡点的优化算法。结合收到的容错步态步序振荡信号将算法组合成代数环形成单足与其余足之间的耦合，使足端位置在支撑相时保持，在摆动相时向优化位置运动；步骤三：由运动稳定性与力矩平衡点两优化模块所使用的评价函数总结出新的评价函数。以步骤二得出的两结果作为边界对新评价函数进行一维搜索得到最终结果，并引入步骤二所述代数环中。
7.本发明的有益效果如下：所述的一种基于力矩平衡点与运动稳定性协同优化的六足机器人容错步态落足点规划方法，一方面能够通过基于zmp稳定裕度的优化在地面特征变化较小的情况下提高稳定性并消除姿态角累计变化量，另一方面能够通过对力矩平衡点位置的调整使机器人在地面特征变化较大的情况下维持姿态角，进一步减小运动过程中的姿态角波动。结合上述两部分对六足机器人以容错步态运动时摆动足的运动轨迹进行调整，将得到的信号输入到推导出的足端相对于机体质心位置的逆运动学中，使单腿缺失的六足机器人以容错步态运动时能够协同提高机器人的运动稳定性与柔顺性。
附图说明
图1为该方法控制流程图图2为adams模型示意图图3全系统框图
具体实施方式
8.下面结合附图和实施例对本发明的实施方式作进一步详细描述。
9.本发明的一个实施例：一种基于力矩平衡点与运动稳定性协同优化的六足机器人容错步态落足点规划方法，其控制流程如图1所示，主要包含以下内容：(1)：如图2所示，模型使用正六边形六足机器人构型，相较于左右对称的构型该设计使得机器人单腿失效的情况相同，使控制方案具有一般性，进一步设定各关节初值，初始化虚拟仿真环境。本发明采用的仿真环境为adams与 simulink联合仿真进行验证，在仿真引擎adams中可以设置关节材料，受力大小等实际仿真功能。为了使机体的稳定性表现更为明显，模型与地面均设置为刚体。在adams仿真环境中设置关节铰链，关节驱动，碰撞力等相应基本设置,包括驱动方式，碰撞穿透，阻尼值以及摩擦系数及重力等。同时依据使用的逆运动学公式对当前模型各关节位置设置初值。随后将该机械模型导出生成m文件再到 simulink中调用即可。基于所使用的模型的几何构型对各足进行标号，并推导各足在以机体质心为原点的坐标系下的逆运动学方程；1).常用的三自由度机械臂的逆运动学公式为足端到根关节位置，现只需按照模型根据各足的标号添加根关节相对于机体质心的坐标变换即可：设ki为当前足的标号,pi＝[x
i y
i zi]
t
为足端坐标,θ1,θ2,θ3分别为该足的根髋膝目标角， l0，l1，l2，l3表示机身几何中心至髋关节的水平距离与由髋关节指向足端的三条腿长，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
×fjudge
+f
judge2
)-k
add
×
(k
add1
×fjudge
×fjudge2
) (6)其中求和部分表示两模块同时存在取小的目标，乘积部分表示尽可能使两者同时取小，参数k
add
在初始化阶段给定，用以调整最终解在求和与乘积部分的优先级，参数k
add1
在初始化阶段给定，用以调整最终解在两模块间的优先级，此时步骤二中两模块的解为最终解的边界，设置可接受最大误差后使用一维搜索 (二分法)迭代寻找即可，最终全系统流程如图3所示。