一种双足机器人的路径规划方法技术

本发明专利技术公开了一种双足机器人的路径规划方法，使用基于多源传感器RGBD的orbslam2建立3D稠密点云图，并使用Octomap进行压缩滤除地面信息。然后通过2D投影生成占据格地图，最后利用代价地图Costmap加障碍层，用改进的D*和DwA进行全局和局部路径规划导航实时避障。本方法采用全局与局部路径规划方法的融合，通过在D*算法的子节点实际代价值中加入角度评价，减少不必要的转弯过程，同时通过限制子节点的选取，使路线规划更加安全可靠。使路线规划更加安全可靠。使路线规划更加安全可靠。

【技术实现步骤摘要】
一种双足机器人的路径规划方法


[0001]本专利技术涉及机器人领域，尤其涉及一种双足机器人的路径规划方法。

技术介绍

[0002]移动机器人现有的路径规划根据地图环境和障碍物类型等可以分为全局路径规划和局部路径规划。全局路径规划主要用于规划机器人的路径，局部路径规划主要用于避障。D*算法为常用的局部路径规划算法，但是在避障中存在一定的缺点，D*算法是一种长度优先的算法并且向周围8个子节点进行扩展搜索，生成的路径会产生损坏路径质量的不必要转弯，在实际导航过程中，这些不必要的转弯会增加移动机器人系统的能耗和时间，长度优先也导致生成的路径十分贴近障碍物，某些时候会从两个“危险”障碍物之间穿过，对实际应用追求的安全性和低能耗考虑的并不周到。同时一般的算法一般是基于轮式机器人所编写的，要在双足机器人中实现，需要进行一定的转换，并不容易。

技术实现思路

[0003]本专利技术目的在于针对现有技术的不足，提出一种双足机器人的路径规划方法。
[0004]本专利技术的目的是通过以下技术方案来实现的：一种双足机器人的路径规划方法，包括以下步骤：
[0005]步骤一，构建地图
[0006]基于多源传感器RGBD的orbslam2建立3D稠密点云图，基于随机一致性采样(RANSAC)进行点云的地面滤除，然后将Octomap进行2D投影生成占据栅格地图，最后利用Costmap代价地图叠加动态障碍层作为最终的导航地图。
[0007]步骤二，路径规划
[0008]采用改进的基于启发式的D*算法，算法在选取子节点时，根据父节点与周围节点的代价函数C(X,Y)值，连续计算两层父子节点值，筛选掉靠近障碍物的子节点，防止算法生成路径贴近障碍物。同时在父节点周围扩展子节点，计算扩展子节点代价值时，根据角度余弦值进行修正，优先选择扩展使得路径平滑的子节点，减少路径转弯次数。
[0009]在根据步骤一构建的地图和目标位置，规划一条机器人到达目标位置的全局路径(只考虑工作空间的几何约束，不考虑机器人的运动学模型和约束)。
[0010]步骤三，避障规划
[0011]采用动态窗口算法，根据所得到的实时传感器测量信息计算最佳的速度指令，调整局部路径以避免发生碰撞。具体过程为：在机器人控制空间(dx,dy,dθ)中离散采样，对于每个采样速度，从机器人的当前状态执行正向移动模拟，以预测如果在某时间段内应用采样速度将会发生什么。然后从以下方面来评价正向移动模拟产生的每个轨迹：接近障碍物，接近目标，接近全局路径和速度。并且放弃非法轨迹，所述非法轨迹为与障碍物相撞的轨迹，选择最高得分的轨迹，并将最佳速度指令发送到下位机的移动基座。然后重复轨迹评价直到轨迹空间遍历完成。
[0012]步骤四，轨迹生成
[0013]将机器人模型转化为线性倒立摆模型，确定每步之间落脚点信息的转换关系，然后进行步态规划，包括实际落脚点的求算、质心轨迹规划、踝关节轨迹规划，以及逆运动学求算关节角度。根据机器人的运动学模型和约束，发送控制命令给机器人下位机，将可行的直线路径转化为可行的脚印轨迹。
[0014]进一步地，所述步骤2中，改进的基于启发式的D*算法具体步骤如下：
[0015]S1，在步骤一构建的地图中确定起始点S与目标点G的位置，将G点加入open表中，同时置空open表与close表。
[0016]S2，判断open表是否为空，若为空，表示目标不可达。
[0017]S3，若open表不为空，将当前节点作为父节点，将其周围的所有子节点全部放入open表中。
[0018]S4，将代价函数值最小的子节点作为下一节点next，并将节点next放入closed表中，若节点next为目标点，则找到路径，算法结束。若并非目标点，开始以节点next为父节点扩展周围子节点sub。
[0019]S5，计算节点next与节点sub之间的路径开销C(next，sub)，当开销为无穷大时，表明节点next周围存在障碍，则将节点next从close表中剔除，并按照代价函数值从小到大顺序再从open表中选择下一个子节点作为下一节点next，并返回S4进行计算。
[0020]S6，当节点next与节点sub之间的路径开销C(next，sub)均不为无穷大时，判断节点sub是否在open表中，若在，则计算节点sub的代价函数值，并与节点next的代价函数值进行比较。当节点sub的代价函数值小于节点next的代价函数值，则直接跳到S7，反之则舍弃节点sub。若节点sub不在open表中，继续判断是否在close表中，如果不在，则将节点sub插入open表中，然后执行S7。若节点sub在节点open表中，判断节点sub与节点next代价函数的大小关系，若节点sub代价函数值小于节点next的代价函数值，将节点sub从Close表移至Open表，并更新Close表代价值，反之舍弃节点sub。
[0021]S7，更新Open表中的代价值，并将指针指向Next。然后跳到S2。
[0022]进一步地，在子节点的实际代价值中加入角度评价从而减少不必要的转弯过程。若设父节点到当前节点的方向为向量A方向，当前节点到子节点的方向为向量B方向，角度偏移量的取值取决于向量A与向量B的夹角。
[0023]g
new
(n)＝Kg
old
(n)
[0024]K＝
‑
cos(∠AB)+2
[0025]其中，g
old
(n)为D*算法中的距离代价，g
new
(n)为改进后的D*算法中的距离代价，K为角度偏移系数，用于根据角度的变化来改变距离代价的值，∠AB为向量A与向量B的夹角。
[0026]进一步地，所述步骤四中，增加了双足机器人的关系映射，轨迹生成包括以下步骤：
[0027]S1建立倒立摆模型，同时将模型与机器人运动对应。假设机器人质心始终固定在机器人身上某处(实践中取两髋关节连线的中点)，对应线性倒立摆中的质点M。倒立摆的支撑点同样对应机器人支撑脚上位置固定的一个点(一般取踝关节中心点在脚面上的垂直投影处)。
[0028]S2，确定每步之间落脚点信息的转换关系。
[0029]当机器人开始迈完一步，开始新一步时，存在如下转换关系：
[0030]当前一步支撑脚转换为下一步摆动脚起点，当前一步摆动脚终点(实际落脚点)转换为下一步支撑脚。
[0031]S3，步态规划，包括以下步骤
[0032]S31，实际落脚点求算
[0033]在实际步态规划中，机器人的目标落脚点位置P用步行参数(S
x S
y S
θ
)描述，分别对应步长，步宽和步行方向，假设此时目标落脚点为P
n
，下一时刻目标落脚点为P
n+1
，两者满足以下关系：
[0034][0035]S32质心轨迹求算
[0036]基于P...

【技术保护点】

【技术特征摘要】
1.一种双足机器人的路径规划方法，其特征在于，包括以下步骤：步骤一，构建地图基于多源传感器RGBD的orbslam2建立3D稠密点云图，基于随机一致性采样(RANSAC)进行点云的地面滤除，然后将Octomap进行2D投影生成占据栅格地图，最后利用Costmap代价地图叠加动态障碍层作为最终的导航地图。步骤二，路径规划采用改进的基于启发式的D*算法，算法在选取子节点时，根据父节点与周围节点的代价函数C(X,Y)值，连续计算两层父子节点值，筛选掉靠近障碍物的子节点，防止算法生成路径贴近障碍物。同时在父节点周围扩展子节点，计算扩展子节点代价值时，根据角度余弦值进行修正，优先选择扩展使得路径平滑的子节点，减少路径转弯次数。在根据步骤一构建的地图和目标位置，规划一条机器人到达目标位置的全局路径(只考虑工作空间的几何约束，不考虑机器人的运动学模型和约束)。步骤三，避障规划采用动态窗口算法，根据所得到的实时传感器测量信息计算最佳的速度指令，调整局部路径以避免发生碰撞。具体过程为：在机器人控制空间(dx,dy,dθ)中离散采样，对于每个采样速度，从机器人的当前状态执行正向移动模拟，以预测如果在某时间段内应用采样速度将会发生什么。然后从以下方面来评价正向移动模拟产生的每个轨迹：接近障碍物，接近目标，接近全局路径和速度。并且放弃非法轨迹，所述非法轨迹为与障碍物相撞的轨迹，选择最高得分的轨迹，并将最佳速度指令发送到下位机的移动基座。然后重复轨迹评价直到轨迹空间遍历完成。步骤四，轨迹生成将机器人模型转化为线性倒立摆模型，确定每步之间落脚点信息的转换关系，然后进行步态规划，包括实际落脚点的求算、质心轨迹规划、踝关节轨迹规划，以及逆运动学求算关节角度。根据机器人的运动学模型和约束，发送控制命令给机器人下位机，将可行的直线路径转化为可行的脚印轨迹。2.根据权利要求1中所述的一种双足机器人的路径规划方法，其特征在于，所述步骤2中，改进的基于启发式的D*算法具体步骤如下：S1，在步骤一构建的地图中确定起始点S与目标点G的位置，将G点加入open表中，同时置空open表与close表。S2，判断open表是否为空，若为空，表示目标不可达。S3，若open表不为空，将当前节点作为父节点，将其周围的所有子节点全部放入open表中。S4，将代价函数值最小的子节点作为下一节点next，并将节点next放入closed表中，若节点next为目标点，则找到路径，算法结束。若并非目标点，开始以节点next为父节点扩展周围子节点sub。S5，计算节点next与节点sub之间的路径开销C(next，sub)，当开销为无穷大时，表明节点next周围存在障碍，则将节点next从close表中剔除，并按照代价函数值从小到大顺序再从open表中选择下一个子节点作为下一节点next，并返回S4进行计算。S6，当节点next与节点sub之间的路径开销C(next，sub)均不为无穷大时，判断节点sub
是否在open表中，若在，则计算节点sub的代价函数值，并与节点next的代价函数值进行比较。当节点sub的代价函数值小于节点next的代价函数值，则直接跳到S7，反之则舍弃节点sub。若节点sub不在open表中，继续判断是否在close表中，如果不在，则将节点sub插入open表中，然后执行S7。若节点sub在节点open表中，判断节点sub与节点next代价函数的大小...

【专利技术属性】
技术研发人员：甘春标，李子静，葛一敏，王小莹，袁璐，张金霖，朱小京，余天浩，
申请(专利权)人：浙江大学，
类型：发明
国别省市：