【技术实现步骤摘要】
本专利技术涉及四足机器人规划领域，特别是涉及基于高程图的四足机器人落足规划方法。
技术介绍
1、四足机器人相对于轮式和履带式机器人具有运动能力强的特点，可以跨越废墟、楼梯和沟壑等复杂地形，而其运动能力的提升主要源于足端的灵活运动，因此合理规划足端运动是拓展四足机器人运动能力的关键因素。在跨越复杂地形时，机器人面临着足端附近地形未知、以及可能会由于地形中出现的边缘、沟壑导致行走时不安全的问题，考虑同时地形安全性和运动鲁棒性的落足规划成为四足机器人在复杂地形上行走的关键。落足规划本质上是根据地形建模信息和机器人状态反馈，求解其满足地形安全性、运动学约束、动力学平衡和跟踪上层目标轨迹的一个优化问题。这个问题非常具有挑战性，因为它旨在搜索当前地形信息约束下，整个候选落足空间的解决方案。由于四足机器人模型的复杂性，求解出满足全身运动的足端位置是落足规划问题的一个难点。此外，由于计算资源受限，对落足规划的有效性和效率也提出了更高的要求。
2、为了解决落足规划的问题，目前一部分的落足规划方法采用单落足点优化的策略，该种策略会导致落足点是作为等式约束给到后续的全身轨迹规划模块的，会大大提升后续运动规划的求解难度，降低求解成功率，从而降低整个运动的鲁棒性。还有一部分的落足规划方法，是将落足规划任务纳入了全身轨迹规划中，将地图直接引入全身轨迹规划，该种策略需要在全身轨迹规划中处理地形的非凸性质，将地形信息直接引入全身轨迹规划的优化问题，使得优化问题规模变大，算力要求高。
技术实现思路
1、
2、本专利技术具体采用的技术方案如下：
3、一种基于高程图的四足机器人高效鲁棒落足规划方法，包括以下步骤：
4、s1.可移动的四足机器人通过激光雷达实时采集行进过程中的激光点云数据，得到四足机器人的里程计信息，并根据所述里程计信息，利用不确定性估计方法，构建以四足机器人身体为中心的局部高程地图；
5、s2.由所述局部高程图计算地形特征，得到所述局部高程地图中每个栅格点的坡度、粗糙度和凹凸度；
6、s3.由所述地形特征生成通行性地图，利用所述坡度、所述粗糙度以及所述凹凸度，构建每个栅格点的地形代价函数；
7、s4.通过预设的阈值划分所述地形代价函数，得到二值化的每个栅格点的通行性地图；
8、s5.根据时间弹性带算法得到四足机器人的质心轨迹，获取一个包含四足机器人落足信息的步态时间表，指定四足机器人规划的步态周期，由所述步态周期和所述步态时间表得到指定时间序列；结合所述指定时间序列，由线性插值方法对所述质心轨迹进行插值，得到实际用于落足规划参考的质心位姿序列；
9、s6.根据四足机器人的默认站立构型，得到所述质心位姿序列的每个子目标位置下的候选落足点；
10、s7.在所述候选落足点附近提取足端子图，使用连通域提取方法对所述足端子图进行处理，得到足端子图内多个连通域集合；
11、s8.在每个所述连通域集合内，采用基于双极限点原理的预处理算法对所述足端子图进行初步预处理，得到预处理足端子图，再使用葛立恒扫描法获得所述预处理足端子图内的候选凸包；
12、s9.根据所述候选凸包和所述候选落足点，构建优化问题的总目标函数和优化问题的约束项，利用一个非线性优化库优化求解所述总目标函数，得到最优凸包和最优落足点；所述总目标函数包括多个子目标函数，由所述多个子目标函数进行线性加权得到所述总目标函数，所述约束项包括四足机器人最大步高约束、四足机器人腿间碰撞约束和四足机器人伸腿极限约束；
13、s10.通过所述最优落足点计算质心位置，将所述质心位置输入到时间弹性带算法中进行滚动优化，得到同时满足四足机器人身体和足端运动的质心轨迹；所述时间弹性带算法的收敛判断条件为所述滚动优化后的质心轨迹必经过所述落足点修正后的质心位置。
14、作为优选，所述s2步骤的具体过程如下：
15、s21.由所述局部高程地图mf(x,y)中每个栅格点pij的坐标为(xij,yij,zij)，构建以每个栅格点pij为球心的球体，将所述球体包含的所有地面栅格实际坐标点构成第一点集合，所述第一点集合为：
16、
17、其中，i为局部高程地图中栅格的横坐标索引，j为局部高程地图中栅格的纵坐标索引；k为所述球体中包含的栅格点个数；k＝1,2,…,k；中的分别表示所述球体中第k个栅格点的三维坐标；
18、s22.利用最小二乘法将所述第一点集合拟合成一个栅格平面，所述栅格平面q的函数形式为：
19、q＝a1x+a2y+a3
20、其中，a1、a2、a3为栅格平面参数；
21、s23.将所述栅格平面的一组法向量表示为(-a1,-a2,1)，由所述法向量计算所述坡度，所述坡度s(pij)的函数形式为：
22、
23、s24.利用标准差计算所述粗糙度，所述粗糙度r(pij)的函数形式为：
24、
25、s25.计算所述局部高程地图mf(x,y)中每个栅格点pij与对应的所述第一点集合中其余栅格点的距离，将获得的距离结果从大到小排序并从中选出与pij距离最小的个点，以计算所述凹凸度c(pij)，所述凹凸度c(pij)的函数形式为：
26、
27、其中，
28、作为优选，所述地形代价函数g(pij)∈[0,1]的函数形式为：
29、
30、其中，ω1、ω2、ω3均为大于0的权重系数，ω1、ω2、ω3满足：
31、ω1+ω2+ω3＝1。
32、作为优选，所述通行性地图mb(pij)的函数形式为：
33、
34、其中，nthr为所述阈值。
35、作为优选，所述s5步骤的具体过程如下：
36、s51.由所述时间弹性带算法，得到四足机器人的质心轨迹，所述质心轨迹bc的函数形式为：
37、bc＝[k1,δt1,k2,δt2,k3,…δtn-1,kn]
38、其中，n为所述质心轨迹的包含的点个数，k为四足机器人的位姿；δt为所述质心轨迹的时间间隔；
39、s52.获取一个包含四足机器人落足信息的步态时间表，指定四足机器人规划的步态周期n，根据所述步态周期和所述步态时间表，得到所述指定时间序列tf，所述指定时间序列的函数形式为：
40、tf＝[t1,t2,t3,…tn-1,tn]
41、s53.结合所述指定时间序列tf，由线性插值方法对所述质心轨迹进行插值，得到所述质心位姿序列bf，所述质心位姿序列的函数形式为：
42、bf＝[k1,δt1,k2,δt2,k3,…δtn-1,kn]。
43、作为优选，所述每个子目标位置下的候选落足点pi,本文档来自技高网...
【技术保护点】
1.一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，包括以下步骤：
2.如权利要求1所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述S2步骤的具体过程如下：
3.如权利要求2所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述地形代价函数g(pij)∈[0,1]的函数形式为：
4.如权利要求3所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述通行性地图Mb(pij)的函数形式为：
5.如权利要求4所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述S5步骤的具体过程如下：
6.如权利要求1所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述每个子目标位置下的候选落足点pi,nom的函数形式如下：
7.如权利要求1所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述基于双极限点原理的预处理算法的具体过程为：
8.如权利要求6所述的一种基于高程图的四足机器人高效鲁棒落足规划方法
9.如权利要求8所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述四足机器人最大步高约束的函数形式为：
10.如权利要求1所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述质心位置s*的函数形式为：
...【技术特征摘要】
1.一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，包括以下步骤：
2.如权利要求1所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述s2步骤的具体过程如下：
3.如权利要求2所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述地形代价函数g(pij)∈[0,1]的函数形式为：
4.如权利要求3所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述通行性地图mb(pij)的函数形式为：
5.如权利要求4所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述s5步骤的具体过程如下：
6.如权利要求1所述的一种基于高程图的四足机器人高效鲁棒落足规划方法，其特征在于，所述每个...
【专利技术属性】
技术研发人员：张睿，余纪宇，王东奇，吴双鹏，熊蓉，王越，
申请(专利权)人：浙江大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。