一种基于局部高程图的落足点选择方法与流程

本发明属于自动控制，具体涉及一种基于局部高程图的落足点选择方法。

背景技术：

1、四足机器人越障中在没有感知的状态下非常容易与环境和障碍物发生碰撞，特别是通过复杂非结构地形时如果采用传统倒立摆落足点规划，会出现踏空、踩踏障碍物边缘造成机器人的失稳。因此，在发生上述问题时引入视觉感知信息，基于局部高程地图完成对最优落足点的选择并进一步开展轨迹的重规划，从而实现更加可靠的越障和智能越障。

技术实现思路

1、(一)要解决的技术问题

2、本发明要解决的技术问题是：为了实现四足机器人在复杂地形中的落足点主动选择，如何提供一种基于局部高程图的落足点选择方法。

3、(二)技术方案

4、为解决上述技术问题，本发明提供一种基于局部高程图的落足点选择方法，所述方法包括如下步骤：

5、步骤s1：针对当前第i条腿，由倒立摆模型slip计算其slip落足点在视觉局部高程地图中的栅格位置{gxslip,gyslip}，进一步基于倒立摆模型slip落足、避碰、边缘和速度匹配的原则进行最优落足点选择；

6、步骤s2：将选择的最优落足点作为下一次落足位置，基于当前足端位置进行摆动轨迹的规划，采用摆线函数完成对摆动轨迹的计算；

7、步骤s3：最终，将实时计算的足端位置，采用运动学逆解计算得到关节角度，并驱动执行器完成闭环控制和摆动轨迹跟踪。

8、其中，所述步骤s1包括步骤如下：

9、步骤s1-1：采用局部高程地图中各栅格的高程信息来对落足点进行选择，考虑到落足点与倒立摆模型slip求取位置间的误差会对机器人造成速度扰动，因此落足点优化以最小化与slip落足点的误差为总体目标，同时考虑肢体的工作空间约束，需要保证落足点选择在安全地形平面内时避免运动学求解的歧义；

10、步骤s1-2：构建高程网格搜索区域；以当前slip落足点对应栅格位置{gxslip,gyslip}构建一个长为h宽为w的矩形搜索区域s，并依次变换各个网格进行后续约束判断和计算；

11、步骤s1-3：slip落足点速度约束：首先计算权重，设当前栅格对应落足点位置为pi，落足点优化的目标首先是找到与slip落足点位置距离最小的落足点位置，即：

12、

13、其中lnorm为腿部最大的运动空间，slip落足点基于当前质心速度和步态周期进行计算，即：

14、

15、其中，tsw为步态周期，为当前机器人质心速度估计，vd为期望的质心运动速度，kv为速度控制反馈系数；

16、步骤s1-4：高程图落足权重：遍历矩形搜索区域s，计算各栅格位置的奖励函数sf(hmap,s)，对于安全的落足点位置，其需要考虑包含当前栅格的地形坡度系数、边缘异变系数以及地形崎岖度在内的综合权重，因此构建奖励函数为：

17、

18、其中r为地形崎岖度，d为边缘异变系数，p为地形坡度系数，λ1+λ2+λ3＝1为地形因子，通过人工参数进行整定；

19、步骤s1-5：支撑区域最大化约束：为了保证机器人下一次踏步更加稳定，其下一时刻的落足点位置应该尽量增加支撑区域，因此设计如下代价函数为：

20、

21、其中为上一次的落足位置；

22、步骤s1-6：肢体避碰约束：为了避免在肢体摆动运动中与其他肢体碰撞，运动规划会避开当前支撑足的落足位置，其代价函数为：

23、

24、其中为此次开始摆动时对角腿的位置，dmin为设置的避碰距离；

25、步骤s1-7：最小探腿约束：该代价函数希望在越障时探腿的距离最短：

26、

27、其中pi,lf为此次开始摆动时对角腿的位置，pi,td为触地的位置；

28、步骤s1-8：地图边界区域约束：当slip落足位置不在局部高程图内部时，即平台运动速度很快，则选择slip落足点位置作为最终结果：

29、

30、其中pi,hip为该条腿的胯关节位置。

31、其中，所述步骤s2中，将优化的结果作为下一次落足位置，基于当前足端位置进行摆动轨迹的规划，采用摆线函数完成对摆动轨迹的计算，具体计算公式如下：

32、

33、

34、

35、其中，xt,i为当前时刻轨迹的x位置，yt,i为当前时刻轨迹的y位置，zt,i为当前时刻轨迹的z位置，px,i与py,i为摆动起始点的xy坐标，与为落足点xy坐标，hsw为摆动高度。

36、其中，所述方法将视觉局部高程地图作为输入，结合机器人倒立摆落足点位置、机器人运动学约束、本体运动状态，构建多约束优化模型，完成在高程地图上对落足点的细粒度迭代优化，机器人本体传感器数据构建力觉感知与检测模型，通过雅克比动力学求解足端力结合摆动轨迹完成对碰撞的检查，并在出现碰撞后进行落足点的重规划和摆动轨迹的重规划。

37、(三)有益效果

38、与现有技术相比较，本发明提出一种基于局部高程图的落足点选择方法，主要步骤包括高程图搜索区域选择、落足点迭代优化、摆动轨迹规划，实现视觉引导下的落足点选择，并进一步规划摆动轨迹，有效增强了机器人越障的能力。

技术特征：

1.一种基于局部高程图的落足点选择方法，其特征在于，所述方法包括如下步骤：

2.如权利要求1所述的基于局部高程图的落足点选择方法，其特征在于，所述步骤s1包括步骤如下：

3.如权利要求2所述的基于局部高程图的落足点选择方法，其特征在于，所述步骤s2中，将优化的结果作为下一次落足位置，基于当前足端位置进行摆动轨迹的规划，采用摆线函数完成对摆动轨迹的计算，具体计算公式如下：

4.如权利要求1所述的基于局部高程图的落足点选择方法，其特征在于，所述步骤s2中，xt,i为当前时刻轨迹的x位置。

5.如权利要求1所述的基于局部高程图的落足点选择方法，其特征在于，所述步骤s2中，yt,i为当前时刻轨迹的y位置。

6.如权利要求1所述的基于局部高程图的落足点选择方法，其特征在于，所述步骤s2中，zt,i为当前时刻轨迹的z位置。

7.如权利要求1所述的基于局部高程图的落足点选择方法，其特征在于，所述步骤s2中，px,i与py,i为摆动起始点的xy坐标。

8.如权利要求1所述的基于局部高程图的落足点选择方法，其特征在于，所述步骤s2中，与为落足点xy坐标。

9.如权利要求1所述的基于局部高程图的落足点选择方法，其特征在于，所述步骤s2中，hsw为摆动高度。

10.如权利要求1所述的基于局部高程图的落足点选择方法，其特征在于，所述方法将视觉局部高程地图作为输入，结合机器人倒立摆落足点位置、机器人运动学约束、本体运动状态，构建多约束优化模型，完成在高程地图上对落足点的细粒度迭代优化，机器人本体传感器数据构建力觉感知与检测模型，通过雅克比动力学求解足端力结合摆动轨迹完成对碰撞的检查，并在出现碰撞后进行落足点的重规划和摆动轨迹的重规划。

技术总结
本发明属于自动控制技术领域，具体涉及一种基于局部高程图的落足点选择方法，所述方法将视觉局部高程地图作为输入，结合机器人倒立摆落足点位置、机器人运动学约束、本体运动状态，构建多约束优化模型，完成在高程地图上对落足点的细粒度迭代优化，机器人本体传感器数据构建力觉感知与检测模型，通过雅克比动力学求解足端力结合摆动轨迹完成对碰撞的检查，并在出现碰撞后进行落足点的重规划和摆动轨迹的重规划。本发明实现视觉引导下的落足点选择，并进一步规划摆动轨迹，有效增强了机器人越障的能力。

技术研发人员：邢伯阳,刘宇飞,王志瑞,许鹏,梁振杰,赵建新,邱天奇,苏波,江磊
受保护的技术使用者：中兵智能创新研究院有限公司
技术研发日：
技术公布日：2024/1/16