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

1.本发明涉及机器人领域，尤其涉及一种双足机器人的路径规划方法。


背景技术：

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


技术实现要素：

3.本发明目的在于针对现有技术的不足，提出一种双足机器人的路径规划方法。
4.本发明的目的是通过以下技术方案来实现的：一种双足机器人的路径规划方法，包括以下步骤：
5.步骤一，构建地图
6.基于多源传感器rgbd的orbslam2建立3d稠密点云图，基于随机一致性采样(ransac)进行点云的地面滤除，然后将octomap进行2d投影生成占据栅格地图，最后利用costmap代价地图叠加动态障碍层作为最终的导航地图。
7.步骤二，路径规划
8.采用改进的基于启发式的d*算法，算法在选取子节点时，根据父节点与周围节点的代价函数c(x,y)值，连续计算两层父子节点值，筛选掉靠近障碍物的子节点，防止算法生成路径贴近障碍物。同时在父节点周围扩展子节点，计算扩展子节点代价值时，根据角度余弦值进行修正，优先选择扩展使得路径平滑的子节点，减少路径转弯次数。
9.在根据步骤一构建的地图和目标位置，规划一条机器人到达目标位置的全局路径(只考虑工作空间的几何约束，不考虑机器人的运动学模型和约束)。
10.步骤三，避障规划
11.采用动态窗口算法，根据所得到的实时传感器测量信息计算最佳的速度指令，调整局部路径以避免发生碰撞。具体过程为：在机器人控制空间(dx,dy,dθ)中离散采样，对于每个采样速度，从机器人的当前状态执行正向移动模拟，以预测如果在某时间段内应用采样速度将会发生什么。然后从以下方面来评价正向移动模拟产生的每个轨迹：接近障碍物，接近目标，接近全局路径和速度。并且放弃非法轨迹，所述非法轨迹为与障碍物相撞的轨迹，选择最高得分的轨迹，并将最佳速度指令发送到下位机的移动基座。然后重复轨迹评价直到轨迹空间遍历完成。
12.步骤四，轨迹生成
13.将机器人模型转化为线性倒立摆模型，确定每步之间落脚点信息的转换关系，然后进行步态规划，包括实际落脚点的求算、质心轨迹规划、踝关节轨迹规划，以及逆运动学求算关节角度。根据机器人的运动学模型和约束，发送控制命令给机器人下位机，将可行的直线路径转化为可行的脚印轨迹。
14.进一步地，所述步骤2中，改进的基于启发式的d*算法具体步骤如下：
15.s1，在步骤一构建的地图中确定起始点s与目标点g的位置，将g点加入open表中，同时置空open表与close表。
16.s2，判断open表是否为空，若为空，表示目标不可达。
17.s3，若open表不为空，将当前节点作为父节点，将其周围的所有子节点全部放入open表中。
18.s4，将代价函数值最小的子节点作为下一节点next，并将节点next放入closed表中，若节点next为目标点，则找到路径，算法结束。若并非目标点，开始以节点next为父节点扩展周围子节点sub。
19.s5，计算节点next与节点sub之间的路径开销c(next，sub)，当开销为无穷大时，表明节点next周围存在障碍，则将节点next从close表中剔除，并按照代价函数值从小到大顺序再从open表中选择下一个子节点作为下一节点next，并返回s4进行计算。
20.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。
21.s7，更新open表中的代价值，并将指针指向next。然后跳到s2。
22.进一步地，在子节点的实际代价值中加入角度评价从而减少不必要的转弯过程。若设父节点到当前节点的方向为向量a方向，当前节点到子节点的方向为向量b方向，角度偏移量的取值取决于向量a与向量b的夹角。
23.g
new
(n)＝kg
old
(n)
24.k＝
‑
cos(∠ab)+2
25.其中，g
old
(n)为d*算法中的距离代价，g
new
(n)为改进后的d*算法中的距离代价，k为角度偏移系数，用于根据角度的变化来改变距离代价的值，∠ab为向量a与向量b的夹角。
26.进一步地，所述步骤四中，增加了双足机器人的关系映射，轨迹生成包括以下步骤：
27.s1建立倒立摆模型，同时将模型与机器人运动对应。假设机器人质心始终固定在机器人身上某处(实践中取两髋关节连线的中点)，对应线性倒立摆中的质点m。倒立摆的支撑点同样对应机器人支撑脚上位置固定的一个点(一般取踝关节中心点在脚面上的垂直投影处)。
28.s2，确定每步之间落脚点信息的转换关系。
29.当机器人开始迈完一步，开始新一步时，存在如下转换关系：
30.当前一步支撑脚转换为下一步摆动脚起点，当前一步摆动脚终点(实际落脚点)转换为下一步支撑脚。
31.s3，步态规划，包括以下步骤
32.s31，实际落脚点求算
33.在实际步态规划中，机器人的目标落脚点位置p用步行参数(s
x s
y s
θ
)描述，分别对应步长，步宽和步行方向，假设此时目标落脚点为p
n
，下一时刻目标落脚点为p
n+1
，两者满足以下关系：
[0034][0035]
s32质心轨迹求算
[0036]
基于p
n
，p
n+1
的约束，设定质心的目标末状态为
[0037][0038]
双足机器人落脚位置；
[0039]
双足机器人步行单元的速度；
[0040]
z
c
为倒立摆模型约束面的截距，是一个取决于质心高度和重力加速度的常数；为代价函数值；
[0041]
t
sup
为双足机器人支撑脚切换时间；
[0042][0043]
得到双足支撑期质心始、末状态后，即可根据样条函数插值求双足支撑期质心轨迹。双足支撑期质心末状态，也就是接下来单足支撑期的质心始状态。运用轮式运动学模型，将整车线位移映射到双足机器人质心线位移上，但是在转换系数上除二，角速度以步行方向在单位时间δt的改变来实现。
[0044]
s33踝关节轨迹规划
[0045]
根据摆动脚起点以及求算得到的摆动脚终点(实际落脚点)即可以在x、y方向上规划机器人的摆动脚的运动，其运动由样条插值得到。
[0046]
进一步地，双足机器人的最大步长为s
xm
＝12。直立行走情况下，s
y
可固定设置为两脚自然状态下的间距，为保证舵机性能和防止结构干涉，步行方向的改变幅度不能过大，限制为正负30度以内。
[0047]
本发明的有益效果：与传统路径规划相比，本发明的方法通过将角度偏移量加入
评价机制中，优先选择使得路径偏转少的子节点进行扩展，这样可以有效减少多次转弯的情况。同时通过f值判断周围子节点中是否存在障碍物，舍弃存在障碍物或者是靠近障碍物的子节点，从而避免贴近障碍物行走的不安全状况发生。
附图说明
[0048]
图1为双足机器人路径规划方法的流程图。
[0049]
图2为改进的d*算法流程图。
具体实施方式
[0050]
以下结合附图对本发明具体实施方式作进一步详细说明。
[0051]
如图1所示，本发明提供的一种为了使本发明的目的和技术方案更加清晰和便于理解，以下结合附图和实施例，对本发明进行进一步的详细说明，此处描述的具体实施例仅用于解释本发明，并非用于限定本发明。本发明具体步骤如下：
[0052]
1、构建地图
[0053]
基于多源传感器rgbd的orbslam2建立3d稠密点云图，基于随机一致性采样(ransac)进行点云的地面滤除，然后将octomap进行2d投影生成占据栅格地图，最后利用代价地图costmap加障碍层，作为最终的导航地图。
[0054]
2、路径规划
[0055]
采用改进的d*算法进行路径规划，改进的d*算法主要基于两个方面进行改进，一个是基于扩展子节点代价值的改变，一个是基于子节点的选取。算法在选取子节点时，根据父节点与周围节点的代价函数c(x,y)值，连续计算两层父子节点值，筛选掉靠近障碍物的子节点，防止算法生成路径贴近障碍物。
[0056]
(1)d*算法采用如下的评估函数来进行计算
[0057]
f(n)＝g(n)+h(n)
[0058]
其中g(n)是基本项，代表终点到当前节点的实际成本；h(n)为启发项，代表当前节点到起点的最小成本估计值；n为当前节点。当通过子节点在父节点周围进行扩展的时候，计算下一扩展节点代价时，将偏转角度作为评价标准加入评价机制中，优先选择使得路径偏转少的子节点进行扩展，这样可以有效减少多次转弯的情况。
[0059]
本发明引入偏转角∠ab的概念，将父节点到当前节点的方向设为向量a的方向，当前节点到子节点的方向作为向量b的方向，偏转角为向量a与向量b的夹角。
[0060]
g
new
(n)＝kg
old
(n)
[0061]
k＝
‑
cos(∠ab)+2
[0062]
其中，g
old
(n)为d*算法中的距离代价，g
new
(n)为改进后的d*算法中的距离代价，k为角度偏移系数，用于根据角度的变化来改变距离代价的值，∠ab为向量a与向量b的夹角。
[0063]
(2)子节点的选取。
[0064]
d*算法扩展当前节点的8个邻居子节点并且这8个节点是同时加入openlist中，没有先后顺序。同时扩展的8个子节点会存在障碍物，也会存在贴近障碍物行进的情况。改进的算法中通过f值判断周围子节点中是否存在障碍物，舍弃存在障碍物或者是靠近障碍物的子节点，从而避免贴近障碍物行走的不安全状况发生。
[0065]
主要步骤为：
[0066]
s1，在地图中确定起始点s与目标点g的位置，将g点加入open表中，同时置空open表与close表。
[0067]
s2，判断open表是否为空，若为空，表示目标不可达。
[0068]
s3，若open表不为空，将当前节点作为父节点，将其周围的8个子节点全部放入open表中。
[0069]
s4，将代价函数f值最小的子节点作为下一节点next，并将节点next放入closed表中，若节点next为目标点，则找到路径，算法结束。若并非目标点，开始以节点next为父节点扩展周围子节点sub.
[0070]
s5，计算子节点next与周围子节点sub之间的路径开销c(next，sub)，当开销为无穷大时，表明节点next周围存在障碍，将此节点next从close表中剔除，并f值从小到大顺序再从open表中选择下一个子节点作为下一节点next，并返回s4进行计算。
[0071]
s6，当节点next与节点sub之间的路径开销c(next，sub)均不为无穷大时，判断节点sub是否在open表中，若在，则计算节点sub的代价函数f值，并与节点next的f值进行比较。当节点sub的f值小于节点next的f值，则直接跳到s7，反之则舍弃节点sub。若节点sub不在open表中，继续判断是否在close表中，如果不在，则将节点sub插入open表中，然后执行s7。若节点sub在节点open表中，判断节点sub与节点next代价函数的大小关系，若节点sub代价函数值小于节点next的代价函数值，将节点sub从close表移至open表，并更新close表代价值，反之舍弃节点sub。
[0072]
s7更新open表中的代价值，并将指针指向next。然后跳到s2。
[0073]
3、避障规划
[0074]
采用dwa动态窗口算法进行局部路径规划，根据所得到的实时传感器测量信息计算最佳的速度指令，调整局部路径以避免发生碰撞。具体过程为：在机器人控制空间(dx,dy,dθ)中离散采样，对于每个采样速度，从机器人的当前状态执行正向移动模拟，以预测如果在某时间段内应用采样速度将会发生什么。然后从以下方面来评价正向移动模拟产生的每个轨迹：接近障碍物，接近目标，接近全局路径和速度。并且放弃非法轨迹，所述非法轨迹为与障碍物相撞的轨迹，选择最高得分的轨迹，并将最佳速度指令发送到下位机的移动基座。然后重复轨迹评价直到轨迹空间遍历完成。
[0075]
4、轨迹生成，为了使算法落地更加方便，增加了双足机器人的关系映射，轨迹生成包括以下步骤：
[0076]
s1，建立倒立摆模型，同时将模型与机器人运动对应。
[0077]
常见的移动式机器人都是轮式或者是麦克纳姆轮式的，机器人坐标递推公式为：
[0078]
x＝x+v
t
δtcos(θ
t
)
[0079]
y＝y+v
t
δtsin(θ
t
)
[0080]
θ
t
＝θ
t
+ω
t
δt
[0081]
其中，x表示轮式机器人位置横坐标，y表示轮式机器人位置纵坐标，θ
t
表示轮式机器人偏转角度。v
t
表示轮式机器人线速度，w
t
表示轮式机器人角速度，δt表示时间间隔。
[0082]
s2，确定每步之间落脚点信息的转换关系。
[0083]
当机器人开始迈完一步，开始新一步时，存在如下转换关系：
[0084]
当前一步支撑脚转换为下一步摆动脚起点，当前一步摆动脚终点(实际落脚点)转换为下一步支撑脚。
[0085]
s3，步态规划，包括以下步骤：
[0086]
s31，实际落脚点求算
[0087]
实际步态规划中，机器人的目标落脚点位置p用步行参数(s
x s
y s
θ
)描述，分别对应步长，步宽和步行方向，假设此时目标落脚点为p
n
下一时刻目标落脚点为p
n+1
，两者满足以下关系：
[0088][0089]
s32质心轨迹求算：
[0090]
基于p
n
，p
n+1
的约束，设定质心的目标末状态为
[0091][0092]
双足机器人落脚位置；
[0093]
双足机器人步行单元的速度；
[0094]
z
c
为倒立摆模型约束面的截距，是一个取决于质心高度和重力加速度的常数；为代价函数值；
[0095]
t
sup
为双足机器人支撑脚切换时间；
[0096][0097]
得到双足支撑期质心始、末状态后，即可根据样条函数插值求双足支撑期质心轨迹。双足支撑期质心末状态，也就是接下来单足支撑期的质心始状态。运用轮式运动学模型，将整车线位移映射到双足机器人质心线位移上，但是在转换系数上除二，角速度以步行方向在单位时间δt的改变来实现。
[0098]
s33踝关节轨迹规划
[0099]
根据摆动脚起点以及求算得到的摆动脚终点(实际落脚点)即可以在x、y方向上规划机器人的摆动脚的运动，其运动由样条插值得到。
[0100]
根据机器人的运动学模型和约束，发送适当的控制命令给机器人下位机，将可行的直线路径转化为可行的脚印轨迹。
[0101]
上述实施例用来解释说明本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明作出的任何修改和改变，都落入本发明的保护范围。