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

权利要求书

1.一种狭窄通道环境下基于自适应动态窗口法的曲率一致性路径规划算法，其特征在于，基于权值动态调节和曲率相似度因子机制以选取最优轨迹，提高狭窄通道环境下轨迹规划的合理性和曲率一致性。

2.根据权利要求1所述的基于权值动态调节和曲率相似度因子机制的轨迹规划方法，其特征在于，包括以下步骤：

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。