[发明专利]狭窄通道环境下基于自适应动态窗口法的曲率一致性路径规划算法

说明书

本发明公开了一种狭窄通道环境下基于自适应动态窗口法的曲率一致性路径规划算法，属于机器人导航技术领域。本发明计算和最近障碍物间的距离，若距离D小于阈值Dt，则计算目标函数中速度项vel的动态权值γ；计算每一个采样速度对应轨迹的曲率k，从而计算曲率相似度因子C_sim；计算标准动态窗口法的目标函数中heading项、dis项和vel项的值，并代入曲率相似度因子C_sim和vel项的动态权值γ，得到下一时刻最优的运动轨迹和对应的执行速度。本发明解决了机器人在狭窄通道内进行局部路径规划时，容易陷入局部最优导致无法到达终点和运动规划轨迹平滑性较差的问题，以提高狭窄通道环境下轨迹规划的合理性和一致性。

技术领域

本发明属于一种移动机器人导航技术领域，具体涉及狭窄通道环境下基于自适应动态窗口法的曲率一致性路径规划算法。

背景技术

移动机器人运动规划根据需要完成任务的不同分为全局路径规划和局部运动规划。局部运动规划主要负责生成运动行为，依据局部环境信息进行动态避障，是体现移动机器人智能水平的关键因素。

目前应用较为广泛的移动机器人局部运动规划算法是动态窗口法(DynamicWindow Approach,DWA)。它首先在速度空间对满足约束的速度进行采样；然后根据机器人的运动学模型模拟这些采样速度在一定周期内的运动轨迹；最后根据目标函数给这些运动轨迹进行打分，选择分数最高的运动轨迹作为下一时刻的运动轨迹，并把对应的速度下发给底盘控制模块。但是基于动态窗口法的移动机器人局部运动规划存在以下问题：1)狭窄通道内机器人容易陷入局部最优从而无法到达终点。2)狭窄通道内机器人的运动轨迹不够平滑。

发明内容

本发明的目的在于：针对现有移动机器人局部路径规划算法在狭窄通道内无法平滑通过的问题，提供一种狭窄通道环境下基于自适应动态窗口法的曲率一致性路径规划算法。

本发明的技术方案如下：

S1、在速度空间中进行采样，对每个模拟出的轨迹计算和最近障碍物的距离D，判断D和阈值Dt的关系，若D＞Dt，则进行第二步，反之则计算线速度v的动态权值γ；

S2、对每个采样的速度计算对应的曲率并和前一时刻轨迹曲率进行比较从而计算曲率相似度因子C_sim。其中为防止某一项对结果影响太大因此进行归一化操作；

S3、根据三个输入，机器人和终点的夹角heading、和最近障碍物之间的距离dis和速度vel进行计算并归一化得到目标函数，代入S1和S2计算得到的γ和C_sim得到新的目标函数；

S4、通过执行S3给所有采样得到的轨迹进行打分，选择得分最高的轨迹为最优轨迹，其对应的线速度和角速度为当前时刻运动指令；

S5、计算当前位置与目标点之间的距离Dg，判断Dg和Dgt的关系，若Dg＜Dgt，则结束；否则继续执行S1。

综上所述，本发明的有益效果为：移动机器人在狭窄通道环境下可以通过参数自适应机制以较低的速度前进，但低速会导致轨迹不平滑，而曲率约束因子可使机器人在低速穿越狭窄通道时不会出现角度较大的转向动作，保证了轨迹的曲率一致性。当远离狭窄通道后又可恢复高速运动，增加行驶效率。因此，本发明结合权值动态调节和曲率相似度因子机制，以提高狭窄通道环境下轨迹规划的合理性和一致性。

附图说明

图1本发明的流程图；

图2应用原始DWA算法通过狭窄通道时的实验图；

图3应用自适应DWA算法通过狭窄通道时的实验图；

图4应用本发明中的改进DWA算法通过狭窄通道时的实验图。

具体实施方式