一种基于协作控制的双足机器人楼梯爬越步态规划方法

摘要

本发明涉及仿人机器人运动规则技术领域，公开了一种基于协作控制的双足机器人楼梯爬越步态规划方法，本方法包括步骤：S10：根据预设D‑H参数法建立第一机器人和第二机器人搬运物体的运动学模型；S20：通过三次多项式插值规划第一机器人的第一预设步态轨迹，基于预设算法得到第一机器人膝关节的角度，控制第一机器人完成第一预设步态动作。本方法通过利用三次多项式插值分别对两个机器人进行楼梯爬越的步态进行规划，得到其髋关节和踝关节关于时间的轨迹，并得到其完整步态周期内各关节的运动规律，实现两个机器人协作平稳搬运物体完成楼梯爬越。

说明书

技术领域

本发明涉及仿人机器人运动规则领域，尤其涉及一种基于协作控制的双足机器人楼梯爬越步态规划方法。

背景技术

随着人工智能技术的发展，多个机器人合作的研究越来越广泛，在救援行动、仓库合作和足球比赛等领域都有相关的应用。利用机器人来完成复杂繁琐的任务，并能够在同一环境中与人类和谐共处的想法日渐成熟。同时为进一步提高工业生产的质量和速度，并降低人类用户与机器人之间的协作中发生事故的风险，将机器人与机器人的协作视为理想的解决方案。

双足机器人是一种模仿人类结构特征的机器人，它的最终目标是实现与人类相似的运动行为。目前，国内外已经成功研发出了许多能够实现行走运动的双足机器人，如美国波士顿动力公司的ATLAS与Petman、日本AIST的HRP、日本本田公司的ASIMO、韩国AKIST的KHR、欧洲开源机器人iCub、我国哈尔滨工业大学的HIT与GoRoBoT、国防科技大学的KDW、北京理工大学的BHR、清华大学的THBIP等。尽管这些双足机器人都能实现稳定的行走运动，但其步行能效都较低，导致续航能力较弱，难以走向实际应用。实际上，经过千百年的自然进化，人类的步态变得十分高效，已公开的数据表明，人类的步行能效是ATLAS机器人的25倍，是ASIMO机器人的16倍。人类高能效步态背后的科学原理在于运动相的合理规划以及各关节的协调动作。

发明内容

本发明的目的是提出一种基于协作控制的双足机器人楼梯爬越步态规划方法，在保持两个机器人稳定性的前提下，能够在楼梯环境中实现两个机器人的动态步行。

本规划方法，具体包括以下技术方案：

一种基于协作控制的双足机器人楼梯爬越步态规划方法，包括步骤：

S10：根据预设D-H参数法建立第一机器人和第二机器人搬运物体的运动学模型；

S20：通过三次多项式插值规划第一机器人的第一预设步态轨迹，基于预设算法得到第一机器人膝关节的角度，控制第一机器人完成第一预设步态动作；

S30：当第一机器人完成第一预设步态动作后，通过调整第一机器人膝关节的角度来减小第一机器人完成第一预设步态动作后的重心；

S40：根据被搬运物体移动的末端速度和三次多项式插值对第二机器人的第二预设步态轨迹进行规划，控制第二机器人完成第二预设步态动作；

S50：当第二机器人完成第二预设步态动作后，通过调整第一机器人膝关节的角度使第一机器人恢复直立状态，重复第一预设步态动作和第二预设步态动作，进行下一级台阶的步态轨迹。

进一步地，所述运动学模型包括正运动学模型和逆运动学模型。

进一步地，根据预设D-H参数法对第一机器人的腿部建立正运动学模型，世界坐标系∑W建立在第一机器人的右脚踝关节中心位置，得到第一机器人右腿运动的齐次变换矩阵T为：

进一步地，第二号机器人的左脚踝关节的局部坐标系为Σα，则相对于世界坐标系ΣW的齐次变换坐标为：

进一步地，根据预设D-H参数法对第一机器人的腿部建立逆运动学模型，通过几何法将踝关节的关节坐标和髋关节的关节坐标进行逆运动学求解，得到踝关节坐标系中的髋关节位置矢量为：

其中，踝关节的关节坐标为(P

利用勾股定理得到踝关节与髋关节之间的距离为：

根据余弦定理：

L

得到膝关节的角度θ

进一步地，第一机器人髋关节的轨迹表达式为：

其中，[y

进一步地，第一机器人和第二机器人之间通过无线消息通道保持通信；在搬运过程中，第一机器人和第二机器人的重心保持在支撑区域之内且足部与地面水平。

进一步地，第一机器人和第二机器人均采用相同型号的NAO机器人，且第一机器人和第二机器人的每条腿均有六个旋转自由度，其中包括：髋关节的三个自由度，膝关节的一个自由度，踝关节的两个自由度。

本发明采用上述技术方案包括以下有益效果：

本发明通过新引入一个参数来解决相邻连杆平行时存在的奇异性问题，在每一个连杆的关节处都有一个正交参考坐标系，包含连杆参数及关节变量、角度等信息，通过一系列依次变换推导出关节位姿，进而表示出连杆之间的变换关系。本发明利用三次多项式插值分别对两个机器人进行楼梯爬越的步态规划，得到其髋关节和踝关节关于时间的轨迹，并得到其完整步态周期内各关节的运动规律，实现两个机器人协作搬运一块木板上楼梯。在此过程中，通过调整膝关节的角度来改变整体的重心轨迹，有助于保持两个机器人在爬楼梯过程中的稳定性。

附图说明

图1为本发明基于协作控制的双足机器人楼梯爬越步态规划方法的流程图；

图2为本发明基于协作控制的双足机器人楼梯爬越步态规划方法的参数设定及控制步态的流程图；

图3为本发明基于协作控制的双足机器人楼梯爬越步态规划方法的第一机器人和第二机器人的腿部连杆模型图；

图4为本发明基于协作控制的双足机器人楼梯爬越步态规划方法的第一机器人和第二机器人的逆运动学模型图；

图5为本发明基于协作控制的双足机器人楼梯爬越步态规划方法的第一机器人和第二机器人的运动轨迹模型图；

图6为本发明基于协作控制的双足机器人楼梯爬越步态规划方法的第一机器人和第二机器人的搬运物体仿真图。

具体实施方式

以下是本发明的具体实施例并结合附图，对本发明的技术方案作进一步的描述，但本发明并不限于这些实施例。

本实施例

本实施例提供了一种基于协作控制的双足机器人楼梯爬越步态规划方法，如图1所示，本方法包括步骤：S10：根据预设D-H参数法建立第一机器人和第二机器人搬运物体的运动学模型；S20：通过三次多项式插值规划第一机器人的第一预设步态轨迹，基于预设算法得到第一机器人膝关节的角度，控制第一机器人完成第一预设步态动作；S30：当第一机器人完成第一预设步态动作后，通过调整第一机器人膝关节的角度来减小第一机器人完成第一预设步态动作后的重心；S40：根据被搬运物体移动的末端速度和三次多项式插值对第二机器人的第二预设步态轨迹进行规划，控制第二机器人完成第二预设步态动作；S50：当第二机器人完成第二预设步态动作后，通过调整第一机器人膝关节的角度使第一机器人恢复直立状态，重复第一预设步态动作和第二预设步态动作，进行下一级台阶的步态轨迹。

具体的，运用预设的D-H参数法构建第一机器人和第二机器人搬木板的正逆运动学模型。刚性杆件的D-H表示法取决于连杆的以下四个参数：α

表1

其中，运动学模型包括正运动学模型和逆运动学模型。

根据预设D-H参数法对第一机器人的腿部建立正运动学模型，世界坐标系∑W建立在第一机器人的右脚踝关节中心位置，得到第一机器人右腿运动的齐次变换矩阵T为：

第二号机器人的左脚踝关节的局部坐标系为∑α，则相对于世界坐标系∑W的齐次变换坐标为：

具体的，将世界坐标系∑W建立在第一机器人的右脚踝关节中心处，可以将坐标系∑i中任意一点的位置转换到坐标系∑i-1的变换矩阵转化成是一个只与关节变量q

其中，cθ

第一机器人完成一级台阶的步态后，通过调整膝关节的角度以此来减小第二机器人上楼梯带来的重心上移的不稳定性。第一机器人通过降低重心来保持木板与地面平行，因此降低后的膝关节在z方向上的坐标位置应该满足：

0＜z

以此来提高协作运动过程中的稳定性，同时改变重心的位置，减小第二机器人承担的模板质量。

参阅图2，通过设定第一机器人和第二机器人的步长和落脚点位置等参数，采用3次多项式插值规划髋关节和踝关节的运动轨迹；所述预设算法包括通过逆运动学模型计算其他关节角度，基于ZMP公式判断第一机器人和第二机器人的稳定性，若是，控制各关节完成楼梯爬越步态；若否，则通过修正位姿，返回采用3次多项式插值规划髋关节和踝关节的运动轨迹，也就是再通过三次多项式插值规划第一机器人的第一预设步态轨迹，之后继续通过逆运动学模型计算其他关节角度，基于ZMP公式判断第一机器人和第二机器人的稳定性。

参阅图3和图4，根据预设D-H参数法对第一机器人的腿部建立逆运动学模型，通过几何法将踝关节的关节坐标和髋关节的关节坐标进行逆运动学求解，得到踝关节坐标系中的髋关节位置矢量为：

其中，踝关节的关节坐标为(P

利用勾股定理得到踝关节与髋关节之间的距离为：

根据余弦定理：L

得到膝关节的角度θ

具体的，摆动腿、踝关节和髋关节位置向量的夹角为α，在踝关节、髋关节与膝关节连线组成的直角三角形中，根据余弦定理有：

在踝关节坐标系中，根据髋关节的位置矢量可得到踝关节的俯仰角和滚动角分别为：

θ

其中，sign(x)为符号函数，当x为正时返回+1，x为负时返回-1。

对于髋关节的摆动、滚转和俯仰三个角度，各连杆之间的位姿关系为：

R

对上式变形可得：

将左边的式子展开，并计算右边的值，即得：

对比左边矩阵的各个元素，可得到髋关节的各个角度的表达式如下：

θ

θ

θ

参阅图5和图6，第一机器人髋关节的轨迹表达式为：

其中，[y

具体的，利用三次多项式：z

第一机器人z轴方向上的髋关节的运动轨迹的参数由初始和末端的约束条件确定。

其中在y轴方向上轨迹由双脚支撑周期和单脚支撑周期两个函数组成，其三次多项式的参数可以由以下的约束来推导：

假定初始加速度和末端速度是确定的，以便可以生成满足物理约束的髋关节轨迹。利用三次多项式对踝关节在单脚支撑周期的轨迹加以规划，必须满足的约束条件为：

其中，

第二机器人在左边摆动脚着地时，第一机器人恢复站立状态，整体的重心轨迹会受到影响，而且这种影响随着时间的累积会导致机器人协作控制的不稳定性增大。因此，在第二机器人完成一个步态周期后，有一段时间停止运动，有利于消除累积的误差。

假定和(x

根据前面所规划机器人各关节运动及ZMP计算的公式为：

第一机器人和第二机器人之间通过无线消息通道保持通信；在搬运过程中，第一机器人和第二机器人的重心保持在支撑区域之内且足部与地面水平。

第一机器人和第二机器人均采用相同型号的NAO机器人，且第一机器人和第二机器人的每条腿均有六个旋转自由度，其中包括：髋关节的三个自由度，膝关节的一个自由度，踝关节的两个自由度。

其中，髋关节的三个自由度包括：俯仰、滚动和偏转；膝关节的一个自由度包括：俯仰；踝关节的两个自由度包括：俯仰和滚动。

本发明通过三次多项式插值分别规划两个机器人髋关节和踝关节的运动轨迹，并通过调整膝关节角度来改变重心位置以保持行走过程中的稳定性，然后通过求解正运动学模型的逆解，得到各个关节对应的运动角度，来控制足部移动。以ZMP为动态步行稳定性的判断依据，保证ZMP始终位于机器人足部与地面形成的支撑多边形内。

在V-REP环境中使用NAO机器人模型，并与MATLAB软件进行联合仿真。利用MATLAB中的逆运动学算法，通过应用程序编程接口(API)接口，将虚拟机器人的腿部在VREP中的运动控制到指定的位置和姿态，从而验证逆运动学算法的正确性仿真与实验结果表明，该方法可以有效控制仿人机器人协作运动并确保其在爬楼环境中的稳定性。

本发明利用三次多项式插值分别对两个机器人进行楼梯爬越的步态规划，得到其髋关节和踝关节关于时间的轨迹，并得到其完整步态周期内各关节的运动规律，实现两个机器人协作搬运一块木板上楼梯。在此过程中，通过调整膝关节的角度来改变整体的重心轨迹，有助于保持两个机器人在爬楼梯过程中的稳定性。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。