一种双足机器人爬楼梯步态规划方法、装置及机器人与流程

本发明涉及智能机器人领域，尤其涉及一种双足机器人爬楼梯步态规划方法、装置及机器人。
背景技术：
：仿人双足机器人应该实现以下三个特征：可以在人所处的环境工作、能够使用人类的工具和具有人的形状。虽然仿人机器人拥有与人类类似的双足，但由于关节自由度的限制，不能像人类一样完成许多高难度动作。参照法国一公司推出的nao机器人，其总共拥有25个自由度，可以实现踢足球，打太极，打高尔夫等诸多复杂运动。为了推广仿人双足机器人在日常生活中的应用，机器人能否在现实生活中的诸多复杂地形中自由地行走就变的极其重要。诸多学者在仿人双足机器人走楼梯方面做出了巨大贡献：matthewj.powell等人提出了提出了一种用于多运动行为的双足机器人控制技术的开发方法，陈广龙等人提出了规划步行和控制与期望的zmp楼梯两足步行机器人的一种新方法：从迭代最优算法得到的期望zmp不仅具有足够的稳定裕度并且满足执行器规格，而且可用于实现和节能，然后，提出了一种具有力敏感和可变阻抗的控制器，可以很好地感知和补偿环境干扰。楼梯作为日常生活中最常出现的地形，更是需要针对性地规划出步态规划的可行方案。目前为止，机器人仍无法在楼梯上行走，所以规划一种有效的行走方式应用在这些特殊地面上就变得至关重要。技术实现要素：本发明的目的是提供一种双足机器人爬楼梯步态规划方法、装置及机器人，可以实现一种有效的机器人行走方式应用,使得机器人像人类一样自如地在特殊地面上行走。为实现以上目的，本发明提供的双足机器人爬楼梯步态规划方法的技术方案如下：建立双足机器人的摆动腿与支撑腿在走楼梯过程中的关键步态的几何模型；其中，所述关键步态包括起步步态、中步步态以及止步步态；所述中步步态包括前向步态和侧向步态；在爬楼梯过程中，采用变长一阶倒立摆模型对前向步态进行轨迹规划，根据预设的约束条件约束踝关节的运动轨迹以及采用一阶线性倒立摆模型对侧向步态进行轨迹规划，实现双足机器人爬楼梯；其中，爬楼梯的过程为：当面对第一楼梯，控制身体重心左移；运用踝关节的约束条件控制抬起摆动腿右脚，令右脚落到楼梯上；将身体重心转移到右脚，并再次运用踝关节的约束条件控制抬起摆动腿左脚，令左脚落到楼梯上；重复以上步骤直至第一楼梯为最后一级楼梯；判断重心是否落在了稳定区域内；若是，则结束行走；若否，则控制两腿进行靠拢，回到起步状态。优选地，所述运用踝关节的约束条件控制抬起摆动腿右脚，还包括：控制摆动腿离开地面和踏上楼梯的瞬时速度接近0，且摆动腿抬起的最高点处的高度高于所述楼梯的高度，并运用样条插值对摆动腿的移动轨迹进行平滑处理。优选地，所述双足机器人为nao机器人，所述关键步态的几何模型基于nao机器人的物理结构及参数进行运动学建模生成。优选地，所述双足机器人的起始位置与楼梯平行，且行走过程中世界坐标系保持不变，即世界坐标系的y轴始终与楼梯平行；则还包括：当所述双足机器人到达楼梯跟前时，通过计算所述双足机器人的双脚的连线与y轴的夹角来获取双足机器人的双脚的连线与楼梯的夹角，以调整所述双足机器人与楼梯的位置。进一步地，本发明实施例还提供了一种双足机器人爬楼梯步态规划装置，包括：几何模型建立单元，用于建立双足机器人的摆动腿与支撑腿在走楼梯过程中的关键步态的几何模型；其中，所述关键步态包括起步步态、中步步态以及止步步态；所述中步步态包括前向步态和侧向步态；爬楼梯规划单元，用于在爬楼梯过程中，采用变长一阶倒立摆模型对前向步态进行轨迹规划、根据预设的约束条件约束踝关节的运动轨迹以及采用一阶线性倒立摆模型对侧向步态进行轨迹规划，实现双足机器人爬楼梯；其中，爬楼梯规划单元具体包括：重心左移模块，用于当面对第一楼梯，控制身体重心左移；右腿控制模块，用于运用踝关节的约束条件控制抬起摆动腿右脚，令右脚落到楼梯上；左腿控制模块，用于将身体重心转移到右脚，并再次运用踝关节的约束条件控制抬起摆动腿左脚，令左脚落到楼梯上；重心判断模块，用于判断重心是否落在了稳定区域内；若是，则结束行走；若否，则控制两腿进行靠拢，回到起步状态。优选地，所述右腿控制模块具体用于：控制摆动腿离开地面和踏上楼梯的瞬时速度接近0，且摆动腿抬起的最高点处的高度高于所述楼梯的高度，并运用样条插值对摆动腿的移动轨迹进行平滑处理。优选地，所述双足机器人为nao机器人，所述关键步态的几何模型基于nao机器人的物理结构及参数进行运动学建模生成。优选地，所述双足机器人的起始位置与楼梯平行，且行走过程中世界坐标系保持不变，即世界坐标系的y轴始终与楼梯平行；则还包括：调整单元，用于当所述双足机器人到达楼梯跟前时，通过计算所述双足机器人的双脚的连线与y轴的夹角来获取双足机器人的双脚的连线与楼梯的夹角，以调整所述双足机器人与楼梯的位置。本发明实施例还提供了一种机器人，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时包括上述的双足机器人爬楼梯步态规划方法。发明实施例提供的双足机器人爬楼梯步态规划方法、装置及机器人，可实现一种有效的机器人行走方式应用,使得机器人像人类一样自如地在特殊地面上行走。附图说明为了更清楚地说明本发明的技术方案，下面将对实施方式中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一种实施方式，图1为本发明实施例提供的的一种双足机器人爬楼梯步态规划方法的走楼梯流程图。图2为本发明实施例提供的的一种双足机器人爬楼梯步态规划装置的支撑腿模型。图3为本发明实施例提供的的一种双足机器人爬楼梯步态规划装置的摆动腿模型。图4为本发明实施例提供的的一种双足机器人爬楼梯步态规划装置的变长一阶倒立摆模型。图5为本发明实施例提供的的一种双足机器人爬楼梯步态规划装置的踝关节的运动轨迹模型。图6为本发明实施例提供的的一种双足机器人爬楼梯步态规划装置的一阶倒立摆模型。图7为本发明实施例提供的的一种双足机器人爬楼梯步态规划装置的侧向运动的调整关节角度模型。图8为本发明实施例提供的的一种双足机器人爬楼梯步态规划装置的上楼梯时不同时刻的关键步态截图。图9为本发明实施例提供的的一种双足机器人爬楼梯步态规划装置的下楼梯时不同时刻的关键步态截图。图10为本发明实施例提供的的一种双足机器人爬楼梯步态规划装置的上楼梯实际步态截图。图11为本发明实施例提供的的一种双足机器人爬楼梯步态规划装置的下楼梯实际步态截图。具体实施方式下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。请参阅图1，本发明第一实施例提供了一种双足机器人爬楼梯步态规划方法，包括：建立双足机器人的摆动腿与支撑腿在走楼梯过程中的关键步态的几何模型；其中，所述关键步态包括起步步态、中步步态以及止步步态；所述中步步态包括前向步态和侧向步态；在爬楼梯过程中，采用变长一阶倒立摆模型对前向步态进行轨迹规划，根据预设的约束条件约束踝关节的运动轨迹以及采用一阶线性倒立摆模型对侧向步态进行轨迹规划，实现双足机器人爬楼梯；其中，爬楼梯的过程为：当面对第一楼梯，控制身体重心左移；运用踝关节的约束条件控制抬起摆动腿右脚，令右脚落到楼梯上；将身体重心转移到右脚，并再次运用踝关节的约束条件控制抬起摆动腿左脚，令左脚落到楼梯上；重复以上步骤直至第一楼梯为最后一级楼梯；判断重心是否落在了稳定区域内；若是，则结束行走；若否，则控制两腿进行靠拢，回到起步状态。其中，所述倒立摆模型是假设仿人机器人的所有质量都集中在质心上，将两条腿看做两个无质量、可伸缩的摆杆，建立仿人机器人的三维线性倒立摆模型，并设出相应的步态参数，通过建立并求解运动学方程的方法，得到期望的质心和摆动腿的运动轨迹。优选地，所述运用踝关节的约束条件控制抬起摆动腿右脚，还包括：控制摆动腿离开地面和踏上楼梯的瞬时速度接近0，且摆动腿抬起的最高点处的高度高于所述楼梯的高度，并运用样条插值对摆动腿的移动轨迹进行平滑处理。其中，所述样条插值是一种以可变样条来作出一条经过一系列点的光滑曲线的数学方法，具体为：由一些多项式组成的，每一个多项式都是由相邻的两个数据点决定的，且得到的任意的两个相邻的多项式以及它们的导数在连接点处都是连续的。优选地，所述双足机器人为nao机器人，所述关键步态的几何模型基于nao机器人的物理结构及参数进行运动学建模生成。其中，所述nao机器人是身高57cm的可编程人形机器人，主要内容如下：具有机身25度自由度(dof)，电机和执行器的关键部件；一系列传感器：2个摄像头，4个麦克风，1个超声波距离传感器，2个红外发射器和接收器，1个惯性板，9个触觉传感器和8个压力传感器；用于自我表达的设备：语音合成器，led灯和2个高品质扬声器；一个cpu(位于机器人头部)，运行linux内核，并支持aldebaran自己的专有中间件；第二个cpu(位于机器人躯干内部)；一个55瓦的电池，可以提供1.5小时甚至更长时间；nao的视觉技术，使用两个高清摄像头，有效像素高达920万，每秒30帧，其中一台摄像头位于机器人的前额，拍摄了前面的水平屏幕；另一个位于嘴里，用于扫描周围的环境。nao的每只脚都有四个压力传感器用于检测地面。如图1.2所示，nao从重心到颈部的长度为12.65cm，到肩膀的关节为10cm，到臀部为8.5cm，到大腿是10cm。从膝盖到脚踝为10.75cm，脚的高度为4.51cm，颈部和肩膀关节的长度为9.8cm，手的宽度为1.59cm。所述nao机器人的骨盆关节包含右髋前关节和左髋前关节两个关节，这两个关节绕y轴和z轴的中间旋转，共用同一个电机，所以两个关节不能独立控制；以右脚为例，nao的脚部由五个关节组成，包含绕着x轴左右旋转的臀部滚动关节、绕着y轴方向前后移动的臀前关节、绕着y轴旋转的膝前关节、绕着y轴方向从后面到前面移动的脚踝前关节、绕着x轴从左到右旋转的脚踝滚动关节；进一步地，根据d-h矩阵参数法和坐标系间的旋转矩阵，得出左腿相邻两个坐标之间的齐次变换矩阵如下,其中，所述d-h矩阵对应的模型是将仿人机器人的每个关节都设定一个参考坐标系，然后，通过一定的变换关系，实现关节(坐标)间的变换；例如，要得到第n个关节的变换矩阵((4阶方阵)，就需要从基坐标开始推到第1个坐标、第2个坐标直至第n个坐标，最后将之前的n-1次变换结合起来就得到了第n个关节的变化矩阵。将各个连杆的齐次变换矩阵依次相乘，就可以得到nao的左腿对应的总变换矩阵：优选地，所述双足机器人的起始位置与楼梯平行，且行走过程中世界坐标系保持不变，即世界坐标系的y轴始终与楼梯平行；则还包括：当所述双足机器人到达楼梯跟前时，通过计算所述双足机器人的双脚的连线与y轴的夹角来获取双足机器人的双脚的连线与楼梯的夹角，以调整所述双足机器人与楼梯的位置。其中，通过对nao机器人的程序进行相应的代码计算与y轴的夹角来调整与楼梯的位置，从而实现运动过程位置的正确调整。以下详细介绍基于上述的双足机器人爬楼梯步态规划方法的支撑腿模型，即参阅图2与图3：建立图2的支撑腿模型，其中l1是nao的小腿长度，l2是nao的大腿长度，l3是支撑腿踝关节c1到同侧髋关节a1之间的长度，l4是摆动腿的踝关节c2到同侧髋关节a2之间的长度，zch为nao的质心pcom和两髋连线中点k沿z轴的长度，xcom与zcom分别是机器人质心在中的x轴坐标值和y轴坐标值，xankle和zankle是nao摆动腿的踝关节c2在中的x轴坐标值和z轴坐标值；根据其中的几何约束关系，对机器人支撑腿进行逆运动学求解，可以得到a1到c1的距离l3和a1c1与z轴的夹角为：在δa1b1c1中，根据三角形的余弦定理可支撑脚的关节序列为：进一步地，基于上述支撑腿模型详细介绍双足机器人爬楼梯步态规划方法的摆动腿模型，即参阅图3：建立图3的摆动腿模型，其中参数参阅支撑腿模型的模型介绍，再根据图3中的几何关系，对机器人的摆动腿进行逆运动学求解，可以得到在δa2b2c2中，根据三角形的余弦定理可以得到摆动腿的关节角序列为以下详细介绍基于上述的双足机器人爬楼梯步态规划方法的变长一阶倒立摆模型，即参阅图4：建立图4的变长一阶倒立摆模型，其中pcom为nao的质心，c1支撑腿踝关节的中心，c2摆动腿踝关节的中心，l3为中步阶段初始时刻pcom到c1的距离，l4为中步阶段初始时刻pcom到c2的距离，lstep为楼梯的宽度，h为楼梯的高度；将pcom到c2的距离作为倒立摆的摆长，记作l(t)，把c2当作摆点；假设双腿的落脚点与楼梯的接触点处于楼梯的中心，中步阶段初始时刻pcom位于楼梯竖直边界的延长线上；根据其对应的几何关系，可得：设θ(t)为质心pcom与摆动腿踝关节的连线z轴的夹角，已知倒立摆方程如下：由于l(t)变化很小，所以可以简化为常量，令l(t)＝l1，可得上式的二阶微分方程的通解：当t＝0时，为初始时刻，有：当t＝t时，为终止时刻，有：由此可得变长一阶倒立摆的摆角θ(t)为：令此倒立摆杆长变化率是一个常数，则此倒立摆的摆长l(t)为：由上述两个式子可得nao的质心的前向运动轨迹为：xcom(t)＝l(t)sinθ(t)zcom(t)＝l(t)cosθ(t)以下详细介绍基于上述的双足机器人爬楼梯步态规划方法的踝关节的运动轨迹模型，即参阅图5：建立图5的踝关节运动轨迹模型，其中nao的踝关节的运动可分为两个部分，分别是向上和向前，即分别为z轴正向和x轴正向；首先是关于nao的踝关节前向运动轨迹的规划，在一个步行周期内，向x轴正向的移动量为lstep，同时，为了机器人的行走过程更加地平稳，应使摆动腿离开地面和踏上楼梯的瞬时速度接近0；其次是关于nao的踝关节向上运动轨迹的规划，为了不让摆动腿和楼梯出现碰撞的情况，在nao摆动腿抬起的最高点c3处的高度必须要高于楼梯的高度h，所以本文假设c3处的z轴坐标为h+0.01(m)，且在t＝t/2时刻到达c3点；确定了踝关节运动的三个关键点之后，为保证轨迹的平滑性和运动过程中不与楼梯发生碰撞，其中，确定踝关节的运动轨迹的关键点如图5的灰色圆点所示；最后，使用样条插值对轨迹进行平滑处理。以下详细介绍基于上述的双足机器人爬楼梯步态规划方法的一阶倒立摆与侧向运动的调整关节角度模型，即参阅图6、图7：建立图6的一阶倒立摆模型，其中c1为支撑腿踝关节的中心，pcom为nao的质心，以c1为摆点，将pcom到c1的距离记为摆长，摆角为θ′(t)；由一阶倒立摆方程可得：解得微分方程的两个边界条件为：当t＝0时，为初始时刻，有：当t＝t/2时，有：由上式可以得到：由此可得nao的质心的侧向运动轨迹为：ycom(t)＝l5sinθ′(t)zcom(t)＝l5cosθ′(t)在整个侧向运动过程中，nao机器人的双足均不离开地面，所以踝关节的位置不变；进一步地，基于建立图7的侧向运动的调整关节角度模型，可知，θ7＝θ8＝θ9＝θ10＝θ11＝θ12，可求得这些关节角为：进一步地，本发明实施例还提供了一种双足机器人爬楼梯步态规划装置，包括：几何模型建立单元，用于建立双足机器人的摆动腿与支撑腿在走楼梯过程中的关键步态的几何模型；其中，所述关键步态包括起步步态、中步步态以及止步步态；所述中步步态包括前向步态和侧向步态；爬楼梯规划单元，用于在爬楼梯过程中，采用变长一阶倒立摆模型对前向步态进行轨迹规划、根据预设的约束条件约束踝关节的运动轨迹以及采用一阶线性倒立摆模型对侧向步态进行轨迹规划，实现双足机器人爬楼梯；其中，爬楼梯规划单元具体包括：重心左移模块，用于当面对第一楼梯，控制身体重心左移；右腿控制模块，用于运用踝关节的约束条件控制抬起摆动腿右脚，令右脚落到楼梯上；左腿控制模块，用于将身体重心转移到右脚，并再次运用踝关节的约束条件控制抬起摆动腿左脚，令左脚落到楼梯上；重心判断模块，用于判断重心是否落在了稳定区域内；若是，则结束行走；若否，则控制两腿进行靠拢，回到起步状态。优选地，所述右腿控制模块具体用于：控制摆动腿离开地面和踏上楼梯的瞬时速度接近0，且摆动腿抬起的最高点处的高度高于所述楼梯的高度，并运用样条插值对摆动腿的移动轨迹进行平滑处理；优选地，所述双足机器人为nao机器人，所述关键步态的几何模型基于nao机器人的物理结构及参数进行运动学建模生成。优选地，所述双足机器人的起始位置与楼梯平行，且行走过程中世界坐标系保持不变，即世界坐标系的y轴始终与楼梯平行；则还包括：调整单元，用于当所述双足机器人到达楼梯跟前时，通过计算所述双足机器人的双脚的连线与y轴的夹角来获取双足机器人的双脚的连线与楼梯的夹角，以调整所述双足机器人与楼梯的位置。还需说明的是，nao机器人可通过有线或者无线网络与pc建立连接，其软件是基于gentoolinux并支持多种程序语言以及基于图形接口编程的choregraphe，支持在choregraphe中建立指令盒，并在指令盒中编写程序、与虚拟的nao机器人或者真实的nao机器人连接、灌输和下载程序；还可以通过本地的ip和端口与webots中的虚拟机器人nao相连，继而可对编写的程序进行仿真，测试程序的安全性和可靠性；其中，所需要实现的主要程序组成有：样条插值对踝关节轨迹做处理、侧向质心规划、前向质心规划、调整机器人相对于楼梯的位置以及起步函数与止步函数。综上所述，本发明实施例提供的双足机器人爬楼梯步态规划方法、装置及机器人，通过对机器人进行运动学建模，在了解了双足机器人自身的步态特点和人类的行走方式后，再根据楼梯的特点，对机器人走楼梯的关键步态的几何约束进行分析；接着通过一阶倒立摆模型对机器人侧向步态进行规划，用变长一阶倒立摆模型对机器人前向步态进行规划，实现对机器人的起步和止步步态进行规划。本发明实施例还提供了一种机器人，所述机器人包括如上述任一实施例所述的双足机器人爬楼梯步态规划方法。为了便于对本发明的理解，下面将以一个仿真的结果和物理样机实验结果来详细说明本发明。一、webots仿真如图8所示，当webots中仿真楼梯高度为4cm时，得到机器人在上楼梯时不同时刻的截图，图9则为机器人下楼梯时不同时刻的截图；分别以高度为4cm和5cm的楼梯通过webots进行仿真，其仿真结果如表1所示：表1由此可得，本实施例所使用的步态规划方法是可行的，但仍需进一步优化以提高在高一点的楼梯上行走的成功率。二、nao机器人物理样机实验如图10所示为nao上楼梯的实际步态截图，图11所示为nao下楼梯的实际步态截图；由于nao机器人会随着使用时间的加长，出现关节发热，卡带，抽搐等异常，加上本实施例使用的代码没有达到最优状态，所以使用nao的时候由人全程保护，避免摔倒引起故障。经过多次实验，得出为楼梯高度为4cm时的成功率如表2所示：表2实验上楼梯下楼梯实验次数1010成功次数46成功率40％60％本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。当前第1页12