最短路径算法C语言,c++求最短路径
1.算法介绍DWA算法被称为动态窗口应用,其原理主要是速度空间(v,w)中采样多组速度,它模拟了这些速度在一定时间内的轨迹,http://www。
动态窗口DWA的实现包括两个步骤。
(1)约束机器人速度,形成速度采样的动态窗口;
)2)根据速度采样点,找出应评价的轨迹,最大化评价函数,选择最佳速度指令。
动态窗口由一系列约束组成,包括微分自动机的不完全约束、环境障碍物的约束以及受结构和电机影响的动态约束。
使用C时,DWA在Qt上的可视化效果较低,所以使用python。
c项目源文件
程序引用PythonRobotics-master
原创DWA
2.机器人运动学模型
下图是两轮差动机器人的运动模型示意图。v和W分别表示机器人在世界坐标系中的平移速度和旋转速度。
在一个采样时间内,机器人的移动距离很小,两个样本之间的轨迹可以视为一条直线,得到如下公式:
三。DWA算法的采样空间下图是传统DWA算法的速度矢量空间示意图。图中横轴是机器人的角速度,纵轴是机器人的线速度,整个区域是Va of的所有白色区域Va都是可以避免碰撞的区域。灰色区域的速度表示机器人在执行该速度时可能会与障碍物发生碰撞。Vd是机器人在控制周期内考虑电机转矩限制时可达到的速度范围。Vr是上述三组的交集,最终的动态滑动窗口主要受以下三个方面的限制:移动机器人受自身最大速度和最小速度的限制,移动机器人受电机性能(加速度)的影响,移动机器人受障碍物的影响。
(1)移动机器人受限于其自身的最大速度和最小速度。
2)移动机器人受电机性能的影响。
3)正如移动机器人受障碍物影响一样,DWA的动态窗口是上述三个速度集的交集。
在速度空间Vr中,以相等的间隔收集离散的点(V,W ),并且通过使用采样的点基于机器人运动模型在随后的t时间模拟机器人。(T与上不同)T .在仿真时间T内,默认的机器人速度V和角速度W不变,都是采样点的速度))仿真图如下图所示。
第四,DWA算法的评价函数需要根据采样速度得到相应的轨迹,然后对轨迹进行评价,选择当前状态下的最佳轨迹。评估函数定义如下。
Heading(v,w) heading) v,w)主要推动机器人在移动过程中继续将其航向角指向目标点。
的方位角图像如下所示。
distance(v,w)distance(v,w)的主要含义是机器人位于预测轨迹终点时,与地图上最近的障碍物之间的距离。对靠近障碍物的采样点进行惩罚,以保证机器人的越障能力。机器人
L的目的是在赛道上没有障碍物的情况下,将函数输出为一个较大的定值,使其在评价函数中的比重不会变得太大。
velocity(v,W)促进机器人快速到达目标所采用的评价函数具体定义如下:
5.DWA算法优化DWA算法没有考虑到需要评估的轨迹方向性、乘坐舒适性和到达目标点的速度等缺点。因此,可以添加评估函数来优化DWA算法,可以使用评价函数。
参考以下论文。
面向家庭环境的移动机器人局部路径规划算法研究
例如,当从终点开始增加距离评估函数时,您可以看到以下内容。
评价函数DWA的优化
郑重声明:本文由网友发布,不代表盛行IT的观点,版权归原作者所有,仅为传播更多信息之目的,如有侵权请联系,我们将第一时间修改或删除,多谢。