本发明涉及一种用于静步态上楼梯的四足机器人运动控制方法，属于四足机器人控制方法领域。
背景技术：
四足机器人在平滑连续的环境下，展现出了高效的动态性和强大的鲁棒性。波士顿动力的bigdog成功地实现了各种动步态。
但是在只有少数可能落足点的地形下，比如台阶，楼梯和凌乱的房间，四足机器人的动态性和鲁棒性受到了很大的考验。
技术实现要素：
本发明提出一种针对楼梯地形的具有普适性的运动控制方法，为实现躯干运动的有效控制，建立躯干质心位置与支撑足足端位置之间的运动学关联，由于四足机器人的参考坐标系为躯干动基座，为消除躯干动基座在惯性系中的相对位置变化，采用离散运动学模型。对机器人系统进行离散运动学建模，建立躯干质心位置与支撑足足端位置之间的运动学关联，通过规划质心位置和躯干姿态，实现静步态实时全方位控制。利用基于触地检测和楼梯沿检测的状态机用来规划落足点和决定机器人的运动状态。
本发明提出的静步态上下楼梯的运动控制方法，具体步骤如下：
(1)针对静步态的迈步顺序，规划四足机器人的主步态；
每个主步态周期包含迈右后腿、四组支撑、迈左后腿、四足支撑、迈左前腿、四足支撑、迈右前腿、四足支撑共8个状态，在保证足端工作空间约束满足的前提下，通过腿部协调运动，推动躯干质心投影尽量向下一支撑三角形的形心靠近。
(2)基于机器人的运动学，建立以躯干运动与姿态作为输入的四足机器人的离散运动学模型；具体过程是：
①建立坐标系：
躯干坐标系∑b：此坐标系固定在机器人的躯干上，原点位于机器人躯干质心，xb指向躯干正前方，yb指向机器人左方，zb垂直于躯干向上；
惯性坐标系∑l：此坐标系固定在大地上，zl与重力方向相反，xl指向和xb在水平面的投影相同；
全局坐标系∑g：此坐标系固定在机器人的躯干上，原点位于机器人躯干质心，xg、yg、zg指向和惯性系相同；
②建立离散运动学模型：
四足机器人的当前时刻躯干质心运动状态变量记为η(t)＝[xb(t)yb(t)zb(t)]τ，当前时刻期望足端位置描述为：
p(t)＝[x1(t)y1(t)z1(t)x2(t)y2(t)z2(t)x3(t)y3(t)z3(t)x4(t)y4(t)z4(t)]τ，
则下一时刻期望足端位置描述为
其中，s12×12为步态选择矩阵，当四足撑地时，
(3)对躯干运动进行规划，根据四足机器人的离散运动学模型计算得到支撑足的足端期望位置；
具体过程是，包括躯干质心位置规划和躯干姿态规划两部分，
①躯干质心位置规划
躯干质心位置规划作用于四足支撑阶段，利用zmp稳定判据，通过计算下一个三足支撑阶段三个支撑足构成的支撑三角形的形心位置，将躯干质心运动至该形心位置，使得躯干质心投影与支撑三角形的形心重合，减小从四足支撑切换到三足支撑时造成的影响。
在全局坐标系∑g下，记下一次三足支撑阶段的三个足的足端位置分别为：
gp1＝[x1y1z1]t，gp2＝[x2y2z2]t，gp3＝[x3y3z3]t，机器人站高为zh，躯干质心的最终位置gpcog＝[xcogycogzcog]t通过计算支撑足的平面三角形的形心位置得到；
机器人躯干的姿态角记为θb＝[rollpitchyaw]t，为适应有坡度的地形，将躯干质心高度设为zcog＝zhcos(pitch)，
根据质心的初始位置和最终位置，以及速度要求，通过三次样条插值拟合出质心轨迹曲线η(t)。
②躯干姿态规划：
在四足支撑初始阶段，记4个足端位置分别为gpi(t0)＝[xi(t0)yi(t0)zi(t0)]t，下标i＝1,2,3,4分别表示机器人右前、左前、右后和左后四个足，由机器人的四个足端位置得地形角度θa＝[αβγ]t；
已知机器人躯干的姿态角θb＝[rollpitchyaw]t，记姿态调整速度为v＝[v1v2v3]t；
根据姿态调整速度，获得姿态调整旋转矩阵
通过躯干运动规划，得到质心轨迹曲线η(t)和姿态调整旋转矩阵
(4)针对楼梯地形，规划摆动足的足端期望位置；
具体过程是：
①计算步长：
在四足支撑阶段结束时，记接下来要摆动的足端位置为bp(t1)＝[x(t1)y(t1)z(t1)]t，足端所在的髋关节的位置为bphip＝[x0y0z0]t则前进步长step和侧方向步长sidestep分别为：
step＝x0-x(t1)+const，
sidestep＝y0-y(t1)+const，
其中const为常量，本文取const＝0.15m。
②摆动足足端轨迹规划：
设机器人每个状态的运动周期为t，步高为h；根据摆动周期，摆动高度，起始点和步长这四个要素，设计出满足要求的摆动足运动轨迹；
当状态机检测到摆动足碰到楼梯沿时，记当前时刻足端期望位置为：
bp(t2)＝[x(t2)y(t2)z(t2)]t，落足点xb＝x(t2)-const2，const2根据楼梯宽度设置，根据x(t2)，xb，剩余时间t-t2，利用三次样条插值重新规划x轴轨迹。
本发明将通过规划躯干运动来实现四足机器人的运动。对躯干运动进行分析时，以躯干质心运动为状态变量，通过离散运动学模型建立与支撑足足端位置之间的联系。以躯干的运动空间作为任务空间，将期望的躯干运动与姿态通过离散运动学模型，转化为机器人的支撑足足端期望位置。
本发明通过离散运动学模型，简单的建立了躯干运动与足端运动的联系，大大降低了模型的复杂度，使得机器人在运动的同时可以规划躯干的位置与姿态，实现四足机器人的全方位移动，在面对楼梯等复杂地形时也可以保持强大的鲁棒性。
附图说明
图1是本发明静步态上楼梯的四足机器人运动控制方法的控制框图。
图2是四足机器人主步态示意图。
图3是机器人坐标系关系图。
图4是x-z平面内足端运动轨迹图。
具体实施方法
对于陡峭的楼梯，四足机器人可以利用一系列典型的无固定步态的静态或准静态运动策略来进行运动。本发明提出了一种可应用于四足机器人在楼梯等复杂地形下的运动控制方法。具体过程如图1所示，首先，针对静步态的迈步顺序，规划四足机器人的主步态。然后基于机器人的运动学，建立以躯干运动与姿态作为输入的四足机器人的离散运动学模型，对躯干质心位置和躯干姿态进行规划，根据四足机器人的离散运动学模型计算得到支撑足的足端期望位置。针对楼梯地形，规划摆动足的足端轨迹。
1.规划四足机器人的主步态
步态是四足机器人运动控制中最基本的部分,处于静步态的四足机器人，存在三足支撑与四足支撑两种不同闭链构型。机器人的迈步运动可分为8个阶段，采用有限状态机控制，每个阶段对应一个状态，于是每个步态周期包含迈右后腿、四组支撑、迈左后腿、四足支撑、迈左前腿、四足支撑、迈右前腿、四足支撑共8个状态，如图2所示。在保证足端工作空间约束满足的前提下，通过腿部协调运动，推动躯干质心投影尽可能向下一支撑三角形的形心靠近。
2.离散运动学模型的建立
(1)建立坐标系
为了得出机器人的躯干运动和姿态调整策略，首先定义如下几个坐标系，如图3所示：躯干坐标系∑b、惯性坐标系∑l、全局坐标系∑g。
躯干坐标系∑b：此坐标系固定在机器人的躯干上，原点位于机器人躯干质心，xb指向躯干正前方，yb指向机器人左方，zb垂直于躯干向上。
惯性坐标系∑l：此坐标系固定在大地上，zl与重力方向相反，xl指向和xb在水平面的投影相同。
全局坐标系∑g：此坐标系固定在机器人的躯干上，原点位于机器人躯干质心，xg、yg、zg指向和惯性系相同。
(2)建立离散运动学模型
为实现静步态实时全方位控制，四足机器人的当前时刻躯干质心运动状态变量记为η(t)＝[xb(t)yb(t)zb(t)]τ，当前时刻期望足端位置描述为：
p(t)＝[x1(t)y1(t)z1(t)x2(t)y2(t)z2(t)x3(t)y3(t)z3(t)x4(t)y4(t)z4(t)]τ，则下一时刻期望足端位置描述为
3.躯干运动规划
躯干运动规划可以拆分为躯干质心位置规划和躯干姿态规划两部分。
(1)躯干质心位置规划
躯干质心位置规划作用于四足支撑阶段，利用zmp稳定判据，通过计算下一个三足支撑阶段三个支撑足构成的支撑三角形的形心位置，将躯干质心运动该形心位置，使得躯干质心投影与支撑三角形的形心重合，减小从四足支撑切换到三足支撑时造成的影响。
在全局坐标系下，记下一次三足支撑阶段的三个足的足端位置分别为gp1＝[x1y1z1]t，gp2＝[x2y2z2]t，gp3＝[x3y3z3]t，机器人站高为zh，躯干质心的最终位置gpcog＝[xcogycogzcog]t可通过计算支撑足的平面三角形的形心位置得到。
通过传感器测得的机器人躯干的姿态角记为θb＝[rollpitchyaw]t。为适应有坡度的地形，将躯干质心高度设为zcog＝zhcos(pitch)。
根据质心的初始位置和最终位置，以及速度要求，通过三次样条插值拟合出质心轨迹曲线η(t)。
(2)躯干姿态规划
躯干姿态规划主要用于机器人适应地形。
在四足支撑初始阶段，记4个足端位置分别为gpi(t0)＝[xi(t0)yi(t0)zi(t0)]t，下标i＝1,2,3,4分别表示机器人右前，左前，右后，左后四个足。由机器人的四个足端位置可得地形角度θa＝[αβγ]t。
已知机器人躯干的姿态角θb＝[rollpitchyaw]t。记姿态调整速度为v＝[v1v2v3]t，可依照简单的分段函数获得。
根据姿态调整速度，获得姿态调整旋转矩阵
通过躯干运动规划，得到质心轨迹曲线η(t)和姿态调整旋转矩阵
4.摆动足的足端期望位置规划
(1)计算步长
在四足支撑阶段结束时，记接下来要摆动的足端位置为bp(t1)＝[x(t1)y(t1)z(t1)]t，足端所在的髋关节的位置为bphip＝[x0y0z0]t则前进步长step和侧方向步长sidestep分别为：
step＝x0-x(t1)+const
sidestep＝y0-y(t1)+const
其中const为常量，本文取const＝0.15m。
(2)摆动足足端轨迹规划
设机器人每个状态的运动周期为t，步高为h。有了摆动周期，摆动高度，起始点和步长这四个要素，可以设计出成千上万种满足要求的摆动足运动轨迹。为适应楼梯地形，本发明采用简单的分段函数，以x-z平面为例，足端运动轨迹如图4所示。
当状态机检测到摆动足碰到楼梯沿时，记当前时刻足端期望位置为：
bp(t2)＝[x(t2)y(t2)z(t2)]t，落足点xb＝x(t2)-const2，const2可根据楼梯宽度设置。根据x(t2)，xb，剩余时间t-t2，利用三次样条插值重新规划x轴轨迹。