一种双足机器人不平路面行走规划和控制方法与流程

本发明属于机器人控制，尤其涉及一种双足机器人不平路面行走规划和控制方法。

背景技术：

1、双足机器人发展至今，基本实现在平地或者结构性地形中稳定行走，但是在现有规划及控制框架下，其在不平路面行走的抗扰性能较弱，鲁棒性依旧较差，导致其在户外场景以及一些特殊的工业场景中应用受限，如何实现双足机器人在不平路面上的稳定行走，对于它们的进一步实际应用具有非常重要的意义。

2、目前，国内外相关研究中应对不平地面行走的控制方式，一方面，通过深度视觉感知，激光雷达扫描等方式来获取地形情况的先验，但是在户外往往受光照强度、地貌等因素影响而产生较大的误差，进而影响行走的稳定性；另一方面，通过足底主动的压力分布感知来估计地面可支撑区域的情况，但感知的精度往往也是影响稳定性的主要因素，而且行走速度和动态性也因此受限。此外，除了主动检测，一些研究在摆动脚着地过程中添加一定的阻尼和顺应性，但是其在抗扰能力上依旧很有限。在平地行走的研究中，为了在扰动中维持平衡，陆续挖掘出了“脚踝策略”、“髋部策略”以及“迈步策略”，其中“迈步策略”表现出最大的抗扰能力。类似行为在人类于行走时同样表现明显。而常规迈步策略多是依赖于固定支撑时间下简化模型的线性特征，仅对迈步位置进行调整，其面对较大扰动无法及时迈腿来使之恢复平衡。并且未见相关策略在不平路面行走的应用。

技术实现思路

1、本发明目的在于提供一种双足机器人不平路面行走规划和控制方法，以解决迈步策略在面对较大扰动无法及时迈腿来使机器人恢复平衡的技术问题。

2、为解决上述技术问题，本发明的一种双足机器人不平路面行走规划和控制方法的具体技术方案如下：

3、一种双足机器人不平路面行走规划和控制方法，包括如下步骤：

4、步骤1：构建步态生成器，在支撑相切换时，激活步态生成器，基于简化的线性倒立摆模型，生成下一支撑相的质心及摆动脚期望轨迹；

5、步骤2：构建全身控制器，在每个控制周期，激活全身控制器，整合当前质心及摆动脚期望状态、身体期望姿态等任务空间目标，求解最优力矩输出。

6、进一步地，所述步骤1构建步态生成器，包括如下步骤：

7、步骤1.1：给定期望的步行参数，以当前支撑脚位置为原点，根据简化的线性倒立摆模型构建一个理想的步行周期单元，并得到当前支撑相和下一支撑相的期望末状态，作为之后的优化目标的参考量；

8、步骤1.2：捕捉支撑相切换时刻机器人状态，并根据关节状态估计得到当前支撑相质心平面位置和速度的初状态，对线性倒立摆模型的解析解关于支撑时间做一阶近似，得到关于支撑时间的线性表示；

9、步骤1.3：对于摆动脚可行的落脚区域进行划定，将其组织为线性不等式约束下围成的凸域，最后，将当前及下一支撑相的末状态与期望末状态之间的误差作为代价函数，以当前支撑相的落脚位置和时间作为优化变量，构建二次规划问题，求解得到当前支撑相阶段摆动脚最优的落脚时间和位置；

10、步骤1.4：得到最优落脚位置和时间后，以当前质心状态为初始值，根据线性倒立摆解析解生成当前支撑相的质心位置和速度轨迹；以三次贝塞尔曲线规划摆动脚轨迹，引导其至期望的落脚位置；然后，质心和摆动脚轨迹采用一种冗余策略，在期望落脚时间段轨迹之后附加一段匀速下降的轨迹，当地面存在较大的落差时，主动引导摆动脚迈向可能存在的“坑”。进一步地，所述步骤1.1包括如下具体步骤：

11、名义前向步距sx和名义侧向步距sy，以当前支撑脚位置为原点，根据简化的线性倒立摆模型构建一个理想的步行周期单元，并得到当前支撑相和下一支撑相的期望末状态为：

12、

13、其中，ξd,c和ξd,n为当前步和下一步的期望质心平面状态，由设定的步行参数决定，为线性倒立摆模型动力学特征时间，s≡sinh(tsup/tc)，c≡cosh(tsup/tc)，tsup为名义支撑时间。

14、进一步地，所述步骤1.2关于支撑时间的线性表示为：

15、

16、其中，为质心平面状态，进而根据上式预测当前支撑相及下一支撑相的质心末状态：

17、

18、其中，

19、ξ(0)为步态生成器调用时捕捉到的当前支撑相的初始质心状态，下一支撑相的支撑时间在该步预测中设定为名义支撑时间tsup，i2为单位阵；

20、系数矩阵t(t)和u(t)来自于线性倒立摆的解析解：ξ(t)＝t(t)ξ(0)+u(t)pfoot，且系数矩阵和b来自于该解析解对时间的一阶近似，存在关系：进一步地，所述步骤1.3构建如下二次规划问题，求解得到当前支撑相阶段摆动脚最优的落脚时间和位置：

21、

22、

23、

24、其中，qc和qn为当前步和下一步预测状态的误差权重系数；lmin为最小侧向步距；lmax为最大跨步半径；tc和为允许的支撑时间上下界；指示不同的支撑相。

25、进一步地，所述步骤2的全身控制器构建如下：

26、

27、

28、

29、-τmax≤τ≤τmax  (6)

30、其中，q为广义位置，v是广义速度，τ为驱动力矩，为优化变量，ρc指示接触力，gtask为任务空间代价函数，包括来自步态生成器的期望质心及摆动脚轨迹追踪任务，盆骨和躯干姿态保持任务等，φtask是任务空间的目标量；jtask是该任务量对应的关节空间雅各比矩阵；是pd控制下得到的任务空间期望二阶量，kp,task和kd,task为相应的位置和速度增益，φd,task和为期望的目标量和一阶目标量，qtask为任务空间目标的权重矩阵；表示关节空间目标，s为指定关节的选择矩阵，关节空间的位置和速度目标由pd控制整合为期望的加速度指令q′d和为指定关节的期望位置和速度，kp,j和kd,j是对应的位置和速度增益矩阵；为能耗代价，和qτ是相应的权重矩阵；公式(4)描述机器人动力学约束，m(q)为惯性矩阵，b为驱动映射矩阵，c(q,v)包含了科氏力、离心力项，g(q)为重力项，公式(5)描述支撑脚接触保持约束，包含了支撑脚的位置和姿态加速度，jcf是支撑脚的速度雅各比，kd,cf是接触阻尼系数；理想可达半径约束形成一个圆域，支撑脚接触保持约束(5)中是对该圆域的内接正n边形近似，以形成线性约束，公式(6)描述机器人各关节可输出的最大力矩约束。

31、进一步地，所述步骤2动力学约束(4)将足底的接触模型构建为分布点接触，每个接触点的“摩擦锥”进一步近似为“内接摩擦棱锥”，每一个接触点的接触力表示为内接摩擦棱锥基向量的正组合：其中rf(i)为接触脚的朝向，是摩擦棱锥基向量的集合，为基向量的权重，pi＝pf(i)β为该接触点的接触映射矩阵。动力学约束的接触点速度雅各比矩阵jc(q)由各个接触点的速度雅各比矩阵组合而成：

32、

33、接触映射矩阵p(q)则由接触点各自的映射阵组合而成：

34、。

35、进一步地，理想可达半径约束形成一个圆域，支撑脚接触保持约束(5)中是对该圆域的内接正n边形近似，以形成线性约束。

36、本发明的一种双足机器人不平路面行走规划和控制方法具有以下优点：本发明进一步发掘了“迈步策略”在机器人抗扰中的应用，在优化落脚位置的基础上，进一步将落脚时间融入到优化中，使得当机器人遭遇较大扰动时，能够通过快速摆腿来使机器人恢复平衡，提高了机器人的抗扰能力和鲁棒性，并将该规划方法与全身运动控制器结合，成功应用到不平路面的行走。该方法的应用场景不仅限于不平路面行走，将其应用于平地行走同样也能大大提高机器人抵御扰动的能力。