基于变参数速度映射和二次规划的双足机器人落脚点规划方法

1.本发明属于双足机器人规划技术领域，具体涉及一种基于变参数速度映射和二次规划的双足机器人落脚点规划方法。


背景技术：

2.双足机器人在实际应用场景中，存在动态避障与灵活行走的需求。双足机器人运动形式与人相似，面对人类的生活环境，具有强大的环境适应潜力。但采用双足行走，与其他移动机器人相比，在上层运动规划层面，首先需要考虑机器人落脚点的位姿与步行周期的规划问题。人类生活环境中存在各类障碍物，机器人想要穿梭自如，就需要具备灵活动态避障的能力。而如果上层规划不能根据环境变化快速解算、调整期望的落脚点位姿与步行周期，即使下层的运动稳定控制完备可靠，也无法实现双足机器人的灵活行走。
3.现有的方法以离散迭代搜索的方法为主，此类方法采用离线步态库和在线启发式搜索的方式，在机器人的运动过程中，通过a*、rrt等算法搜索最佳运动路径，并从步态库中选取固定步态进行拼接，形成机器人的运动轨迹，主要保证了机器人在静态障碍物环境下的运动空间可行性。现有技术方法大多局限于静态复杂环境，关注机器人落脚点位置和姿态的选取以避免自身、环境的碰撞，保证运动空间的可行性，但几乎没有考量运动时间的可行性，落脚点时空关联弱，应对具有动态障碍物的复杂环境不能实时调整，且计算复杂度高、在线求解较为困难。


技术实现要素：

4.针对现有技术中存在不足，本发明提供了一种基于变参数速度映射和二次规划的双足机器人落脚点规划方法，兼顾空间可行性与时间可行性，能够在线规划机器人的落脚点位姿与步行周期，开发机器人的快速运动变化极限，提高双足机器人应对动态障碍物的快速反应能力和机动性。
5.本发明是通过以下技术手段实现上述技术目的的。
6.基于变参数速度映射和二次规划的双足机器人落脚点规划方法，具体为包括：基于变参数速度映射的步行周期规划，基于所述步行周期规划实现落脚点姿态规划，基于步行周期规划和所述落脚点姿态规划实现落脚点位置规划；
7.所述基于变参数速度映射的步行周期规划具体为：
[0008][0009]
其中：为预测区间内第j步的落脚点时刻，σ为速度映射因子，v表征机器人的期望简化质心运动速度，表征速度的名义中心值，表征的步时；
[0010]
根据落脚点时刻序列计算得到预测区间内的步行周期向量；
[0011]
所述落脚点姿态规划具体为：考虑机器人连续两步之间的姿态角变化需满足实际
机器人运动学的限制，形成二次规划，求解出期望落脚点姿态向量：
[0012][0013]
其中：代表在预测区间内第j步的世界坐标系下z方向姿态角，为落脚点姿态向量，f表征预测区间范围内的期望落脚点的个数，θ
max
是连续两步之间允许的最大姿态角绝对变化幅度，表征对应步长周期内的角速度积分值，且其中为世界坐标系下的z方向角速度；
[0014]
所述落脚点位置规划具体为：设计运动可达域约束，形成二次规划，求解出期望落脚点位置向量：
[0015][0016]
其中：代表在预测区间内的落脚点位置向量，代表在预测区间内第j步的世界坐标系下的位置，和表示相邻落脚点x、y方向期望位置变化量；
[0017]
将落脚点的位置、姿态信息分别与时序信息逐一匹配，得到描述预测区间内每一步完整时空信息的落脚点向量。
[0018]
进一步的技术方案，所述变参数速度映射具体为在线调节参数：
[0019]
将期望简化质心运动速度v与名义速度参数阈值vf、vs作比较，当v≥vf时，速度参数构成的名义速度参数集为当vs《v《vf时，速度参数构成的名义速度参数集为当v≤vs时，速度参数构成的名义速度参数集为其中分别为高速运动对应的速度中心值、步时中心值、步长中心值，分别为中速运动对应的速度中心值、步时中心值、步长中心值，分别为低速运动对应的速度中心值、步时中心值、步长中心值；
[0020]
当期望的步长变化量δls≥δl
max
且期望的步时变化量δts≤δt
min
时，σ＝δt
min
/δl
max
；当期望的步长变化量δls≤δl
min
且期望的步时变化量δts≥δt
max
时，σ＝δt
max
/δl
min
；当期望的步长变化量δls和步长变化量δls的取值不属于上述两种情况时，σ＝(δt
min
δt
max
)/(δl
min
δl
max
)；其中δl
max
、δl
min
分别为期望步长变化量上限阈值和下限阈值，δt
max
、δt
min
分别为期望步时变化量上限阈值和下限阈值。
[0021]
进一步的技术方案，所述落脚点姿态规划中的二次规划的规范形式为：
[0022][0023]
其中：h
θ
、f
θ
分别为目标函数hessian矩阵和一次项系数矩阵，a
θ
、b
θ
为表征相邻步
角度差线性约束的系数矩阵；
[0024]
将系数矩阵h
θ
、f
θ
、a
θ
、b
θ
输入求解器即可求得落脚点姿态向量。
[0025]
进一步的技术方案，运动可达域是一种基于机器人本体运动学特征的单步运动可行范围界定，具体为能最大程度囊括可行范围的凸n边形。
[0026]
更进一步的技术方案，所述落脚点位置规划中的二次规划的规范形式为：
[0027][0028]
其中：h
l
、f
l
分别为目标函数hessian矩阵和一次项系数矩阵，a
l
、b
l
为预测区间内运动可达域约束的系数矩阵，为整合后的目标优化变量；
[0029]
将系数矩阵h
l
、f
l
、a
l
、b
l
输入求解器即可求得落脚点位置向量。
[0030]
更进一步的技术方案，所述运动可达域单步约束为：
[0031][0032]
其中：a
krr
、b
krr
是运动可达域边界的线性表征，
wfj 1
、
wfj
为游动脚的落脚点位置，是第j步落脚点在世界坐标系下的姿态矩阵rj的转置。
[0033]
进一步的技术方案，所述和通过如下公式计算：
[0034][0035]
其中：r
θ
表征机器人矢状面朝向的姿态矩阵，而rj和r
j-1
分别表征第j和j-1步落脚点在世界坐标系下的姿态矩阵，l为机器人两髋间距，v
x
、vy分别表征在简化质心坐标系下x方向的期望运动线速度、y方向的期望运动线速度。
[0036]
进一步的技术方案，所述期望简化质心平面运动位置轨迹是按照如下方式获取的：对世界坐标系下的期望运动速度按照步行周期进行积分，得到对应步行周期内机器人简化质心的位置变化量和z方向姿态变化量。
[0037]
本发明的有益效果为：
[0038]
(1)本发明根据期望速度输入，能够在线快速生成“变步长”、“变步行周期”的落脚点向量，落脚点时空信息完备，为双足机器人的灵活动态避障行走提供重要基础；生成的落脚点向量安全可靠性高，通过设计优化约束保证了步态的运动学可行性；
[0039]
(2)本发明不依赖于离线步态库，采用变参数速度映射和二次规划方法，扩展了双足机器人的步态多样性；
[0040]
(3)本发明根据期望速度输入，能够生成多种运动模式，包括模式之间的流畅切换，同一模式下的速度变化，如前进后退、左右横移、转弯对角行走等。
附图说明
[0041]
图1为本发明所述基于变参数速度映射和二次规划的双足机器人落脚点规划流程图；
[0042]
图2为本发明所述速度映射参数在线调节图；
[0043]
图3为本发明所述相邻落脚点期望位置变化量图解示意图；
[0044]
图4为本发明所述运动可达域约束说明图。
具体实施方式
[0045]
下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。
[0046]
二次规划是非线性规划中的一类特殊数学规划问题，规范的二次规划一般表达式为：
[0047][0048]
其中：为优化向量，为目标成本函数为目标函数hessian矩阵，为目标函数一次项系数矩阵，为线性不等式约束系数矩阵，为线性等式约束系数矩阵，是优化向量不等式约束系数矩阵，为向量空间，n为优化向量中变量个数，neq为等式约束的个数，nineq为不等式约束的个数，beq为系数的个数。
[0049]
二次规划问题是在满足相关线性约束的条件下，求解能够使得二次型成本函数最小化的最优优化控制向量。二次规划的优点在于，当hessian矩阵为半正定，则该问题是凸二次规划，如果可行域不为空，且目标函数在可行域有下界，则该凸二次规划问题有全局最小值。针对形成的二次规划问题，目前业界已有许多成熟一流的二次规划求解器，只需要转化成求解器要求的规范输入形式，就能够进行快速求解，得到满足条件的最优优化控制向量。
[0050]
针对动态复杂环境，双足机器人的落脚点规划不仅需要考虑运动空间碰撞干涉问题，还需要赋予机器人快速灵活改变运动速度的能力。而运动速度的变化与步行周期、步长直接相关，如果仅规划、调节落脚点位置，将会限制机器人运动速度的变化极限。因此，双足机器人的灵活避障行走步态应当具备“变步行周期”、“变步长”的特点，需要进行步行周期、落脚点位姿的在线规划，并保证位姿与周期的关联性。
[0051]
如图1所示，本发明设计了一种基于变参数速度映射和二次规划的双足机器人落脚点规划方法，当时间t＝tk时，落脚点在线规划模块输入为预测区间[tk，tk t
p
]内的期望的机器人运动速度轨迹v
x
、vy、w，所述输入可由其他环境感知与智慧决策模块得到或是直接由操作员进行遥操作实时发送运动速度指令；落脚点在线规划模块输出为预测区间内的落脚点位置向量姿态向量以及步行周期向量其中tk表征机器人当前的运动时刻，t
p
表征预测区间的时间范围，v
x
、vy、w分别表征在简化质心坐标系下x方向的期望运动线速度、y方向的期望运动线速度以及z方向的期望运动角速度。可表示为如下形式：
[0052][0053][0054][0055][0056]
其中代表在预测区间内第j步的世界坐标系下的位置与z方向姿态角，而则是第(j-1)步落脚的单足支撑期起始时刻到第j步落脚的单足支撑期起始时刻的步行时间，f表征预测区间范围内的期望落脚点的个数，且将随期望速度输入的不同而动态变化。
[0057]
落脚点在线规划模块包含三个部分，如图1所示，依据时序分别是基于变参数速度映射的步行周期规划、基于二次规划的落脚点姿态规划和落脚点位置规划。
[0058]
对于步行周期规划部分，设计如下名义速度模型：
[0059][0060]
其中：ls和ts分别表征v对应的期望步长和期望步时，表征速度的名义中心值，则表征的步时和步长中心值，它们的关系为：
[0061][0062]
v表征机器人的期望简化质心运动速度，其计算公式为：
[0063][0064]
δv表征速度变化量，δts、δls表征δv步时和步长的变化量。
[0065]
并设计如下比例关系：
[0066]
δts＝σδls(8)
[0067]
联立式(5)、(6)、(8)，可得到期望步时ts的计算公式为：
[0068][0069]
其中σ为速度映射因子，通过调节该因子可控制同一速度变化δv所映射得到的步长变化δls和步时变化δts之间的比例。因此，通过合理配置速度映射因子σ及名义速度参数数该映射方法对双足机器人运动范围、运动性能及部署环境差异性具有较强的鲁棒性。
[0070]
在预测区间内循环使用式(9)，可得到对应的落脚点时刻：
[0071][0072]
其中，为预测区间内第j步的落脚时刻；的初始值是当前采样时刻tk之前的上一步落脚点对应的落脚时刻(当前支撑腿落脚时刻)，在支撑腿发生切换时进行更新。
[0073]
当出现某个时，则该步已经超出预测区间范围，需要将其剔除，并由此
确定f的值。而根据落脚点时刻序列可计算得到预测区间内的步行周期向量，如式(4)所示，其中：
[0074][0075]
速度映射的参数集合可表达为：
[0076][0077]
在实际应用场景中，固定的速度映射参数往往不足以适应动态环境的复杂性及机器人自身的本体运动特征，因此，设计如图2所示的速度映射参数在线调节规则，实现可变参数的速度映射，保证步态的灵活性与可靠性。
[0078]
如图2所示，参数在线调节规则的输入是期望简化质心速度v和机器人的期望运动特性。环境感知与智慧决策模块给出期望运动线速度输入后，经式(7)可得到期望简化质心运动速度v，将期望简化质心运动速度v与名义速度参数阈值vf、vs作比较，根据比较情况选取对应的名义速度参数，该部分不同情况对应的速度参数可构成名义速度参数集，图2中为三种名义速度参数取值，分别对应v≥vf、vs《v《vf、v≤vs三种情况。其中分别为高速运动对应的速度中心值、步时中心值、步长中心值，分别为中速运动对应的速度中心值、步时中心值、步长中心值，分别为低速运动对应的速度中心值、步时中心值、步长中心值。同时，考虑机器人机械本体的特性，环境感知与智慧决策模块可给出期望的双足机器人运动步态特性要求，包括期望的步时变化量和步长变化量。依据三种情况选取速度映射因子σ，当期望的步长变化量δls≥δl
max
并且期望的步时变化量δts≤δt
min
时，σ＝δt
min
/δl
max
，速度映射因子取得最小值；当期望的步长变化量δls≤δl
min
并且期望的步时变化量δts≥δt
max
时，σ＝δt
max
/δl
min
，速度映射因子取得最大值；当期望的步长变化量δls和步长变化量δls的取值不属于上述两种情况时，σ＝(δt
min
δt
max
)/(δl
min
δl
max
)，速度映射因子取得适中值。其中δl
max
、δl
min
分别为期望步长变化量上限阈值和下限阈值，δt
max
、δt
min
分别为期望步时变化量上限阈值和下限阈值。
[0079]
当环境、速度输入发生动态变化，上述规则根据环境感知与智慧决策模块输入在线调节速度映射的参数集合pv，并通过式(10)实现期望简化质心运动速度向步行周期的映射。
[0080]
预测区间内的步行周期确定以后，再进行落脚点位姿的规划。机器人简化质心坐标系下的运动速度(v
x
、vy和w)可通过坐标变换转换到世界坐标系下描述：
[0081][0082]
其中和分别表征机器人在世界坐标系x、y方向的线速度和z方向的角速度，而v
x
、vy和w表征机器人简化质心坐标系x、y方向的线速度和z方向的角速度。简化质心坐标系固连于机器人，原点位于机器人两髋之间，骨盆中心处。
[0083]rz
为简化质心坐标系相对于世界坐标系的旋转矩阵，表达式为：
[0084][0085]
其中θ为表征机器人在世界坐标系z方向的角度，是的积分值。
[0086]
对世界坐标系下的期望运动速度(和)按照步行周期进行积分，可得到对应步行周期内机器人简化质心的位置变化量、z方向姿态变化量，并得到机器人简化质心的平面运动位置轨迹，简称为模型积分路径。
[0087]
落脚点位姿求解的核心思想是，要求求解得到的预测区间内的所有两两相邻的落脚点之间的z方向姿态角度变化差、位置变化差尽可能地跟踪相邻落脚点的期望z方向姿态角度变化量和期望位置变化量。上述相邻落脚点之间的z方向姿态角度变化差统称为实际运动变化差，期望z方向姿态角度变化量和期望位置变化量统称为期望运动变化量。但同时为防止期望变化量不合理，导致步态超出双足机器人本体的运动极限，或是造成机器人双腿的互相干涉碰撞等现象，需要将实际运动变化差约束在机器人的运动允许范围内以保证双足机器人运动的安全性。因此落脚点位姿求解的目标是求得最优的落脚点位置和姿态向量，这两个向量能够让预测区间内实际运动变化差和期望运动变化量之间的误差总和最小化，同时满足相关约束，可将其设计为二次规划问题进行描述并通过求解器进行求解。
[0088]
如果将落脚点位置和姿态的求解整合在同一个二次规划问题下，会产生非线性约束，考虑到二次规划要求约束必须是一次线性约束，因此将落脚点位姿的求解拆解为两个二次规划问题依次进行求解。
[0089]
对预测区间内的落脚点姿态规划，考虑机器人连续两步之间的姿态角变化需满足实际机器人运动学的限制，可将其设计为如下二次规划：
[0090][0091]
其中为目标优化向量，也即式(3)中的落脚点姿态向量，的初始值为当前采样时刻tk的机器人支撑脚z方向姿态。表征相邻两步期望z方向姿态角度变化量，等效为简化质心在对应步行周期内z方向的姿态变化量，其计算公式为：
[0092][0093]
式(15)中，θ
max
是连续两步之间允许的z方向最大姿态角绝对变化幅度，其设置与实际机器人的髋关节间距、脚底板大小等直接相关。“姿态角变化裕度”约束要求规划得到的落脚点姿态向量必须满足相邻两步姿态角的变化差值不能超过机器人承受极限的条件，同时根据目标函数最小化的目标尽量使实际姿态差值跟踪期望姿态变化量。
[0094]
二次规划问题(15)可化为如下规范形式：
[0095][0096]
其中h
θ
、f
θ
分别为目标函数hessian矩阵和一次项系数矩阵，a
θ
、b
θ
为表征相邻步角
度差线性约束的系数矩阵，并且有将系数矩阵h
θ
、f
θ
、a
θ
、b
θ
输入求解器即可求得落脚点姿态向量(式(3))。
[0097]
对预测区间内的落脚点位置规划，设计为如下二次规划：
[0098][0099]
其中为目标优化向量，即式(1)、(2)表达的落脚点位置向量，的初始值表征当前采样时刻tk的机器人支撑脚位置，在支撑腿发生切换时进行更新。
[0100]
而对于相邻落脚点x、y方向期望位置变化量和则可以通过式(19)计算：
[0101][0102]
其中r
θ
表征机器人矢状面朝向的姿态矩阵，而rj和r
j-1
分别表征第j和j-1步落脚点在世界坐标系下的姿态矩阵，l为机器人两髋间距。机器人每个时刻矢状面的朝向由对应时刻两只脚的姿态决定。式(18)第一项和第三项的正负号对应第j-1步左右脚属性。
[0103]
式(19)图解如图3所示，相邻落脚点期望位置变化量由三段组成，第一段与式(19)右侧第一项对应，表征第(j-1)落脚点相对于简化质心坐标系的偏置，第二段与式(19)右侧第二项对应，表征对应步行周期内的模型积分路径，第三段与式(19)右侧第三项对应，表征第j落脚点相对于简化质心坐标系的偏置。图中是第j-1步落脚点位置描述，同理是第j步落脚点位置描述。
[0104]
对运动可达域约束作进一步说明，本发明提出一种双足机器人落脚点“运动可达域”的概念，运动可达域是一种基于实际机器人运动特性与关节限位等机器人本体运动学特征的单步运动可行范围界定，具体简化为能最大程度囊括可行范围的凸n边形以保证二次规划约束的线性。以凸五边形为例，运动可达域约束如图4所示。
[0105]“运动可达域”定义为与当前脚具有相同z方向姿态角的凸多边形，“可达中心点”定义为与当前支撑脚落脚点冠状面距离为l(两髋间距)的点，“运动可达域”范围边界通过“可达中心点”以及凸多边形顶点位置向量进行描述。上述边界的具体取值及多边形的边数n由双足机器人的运动学特征得到。“运动可达域”约束要求下一步的落脚点位置必须处于“运动可达域”的范围边界内。“运动可达域”单步约束可表达为：
[0106][0107]
其中是运动可达域边界的线性表征。
[0108]
利用式(20)将预测区间内的所有相邻两步间约束进行整合，二次规划问题(18)可
化为如下规范形式：
[0109][0110]
其中h
l
、f
l
分别为目标函数hessian矩阵和一次项系数矩阵，a
l
、b
l
为预测区间内“运动可达域”约束的系数矩阵，为整合后的目标优化向量，并且有为整合后的目标优化向量，并且有将系数矩阵h
l
、f
l
、a
l
、b
l
输入求解器即可求得落脚点位置向量(式(1)和式(2))。
[0111]
得到落脚点在线规划模块的输出式(1)-(4)后，将落脚点的位姿信息和时序信息逐一匹配，如图1所示，得到描述预测区间内的每一步完整时空信息的落脚点全维信息向量fk，简称为落脚点向量，其表示形式为：
[0112]fk
＝(f1…fj
…ff
)
t
(22)
[0113]
其中为第j步的时空信息。
[0114]
在落脚点位姿规划中合理配置“姿态角变化裕度”约束、“运动可达域”约束，能够保证落脚点向量fk中的每一步的时空信息在执行时都不会超过机器人的运动极限，印证了本发明生成的落脚点向量的安全可靠性。即使环境感知与智慧决策模块给出了极端的期望速度输入，由于上述两种约束的施加，最终执行的落脚位姿，步行周期都将处于机器人的运动允许范围内。
[0115]
依据期望速度输入取值的不同，本发明生成的落脚点向量具有不同的特点，可将其归类为27种运动模式(由v
x
、vy和w进行排列组合得到)，以下是其中几种运动模式的举例：
[0116]
当v
x
＝0、vy＝0、w＝0时，本发明可生成机器人原地踏步的运动模式，依据式(10)此时步行周期取得最大值；
[0117]
当v
x
＞0、vy＝0、w＝0时，本发明可生成机器人前向行走的运动模式，且v
x
越大，机器人前向运动的速度越快；
[0118]
当v
x
《0、vy＝0、w＝0时，本发明可生成机器人后退行走的运动模式，且v
x
越小，机器人后退运动的速度越快；
[0119]
当v
x
＝0、vy＞0、w＝0时，本发明可生成机器人向左横移行走的运动模式，且vy越大，机器人向左横移运动的速度越快；
[0120]
当v
x
＞0、vy＞0、w＝0时，本发明可生成机器人左前对角行走的运动模式，且v
x
或vy越大，机器人左前对角运动的速度越快；
[0121]
当v
x
＞0、vy＝0、w＞0时，本发明可生成机器人向前左转弯的运动模式，且v
x
越大，机器人向前转向速度越快，w越大，机器人左转弯的转弯幅度越大。
[0122]
随着采样时刻tk的向前推移，预测区间[tk，tk t
p
]会向前滑动，双足机器人行走运动的支撑脚会发生切换，支撑脚时空信息(规划初始值)会进行更新，以此不断循环往复，直至落脚点在线规划模块终止(机器人停止运动)。并且，当落脚点在线规划模块的期望运动速度输入v
x
、vy和w发生实时变化时，该模块基于当前支撑脚时空信息，能够响应速度输入的变化，在线调整、规划符合期望运动速度的预测区间内的落脚点向量，在不同的运动模式之间进行切换，或是更改同一运动模式下的运动速度。本发明配合其他的双足
机器人在线质心轨迹规划和足端轨迹规划方法(为现有技术)，可以实现双足机器人在具有动态障碍物的复杂环境中的灵活变速行走，为双足机器人的灵活动态避障行走提供重要基础。落脚点向量中的每一步的时空信息均可变化调整，因此，使用本发明在线规划的机器人步态时空关联性强，具有“变步长”、“变步行周期”的特点。
[0123]
所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。