[发明专利]一种动态局部路径规划方法及系统

说明书

本发明涉及一种动态局部路径规划方法，包括以下步骤：获取运动体的起始位姿和终止位姿，并实时更新代价地图，判断规划路径为几阶路线，并通过数学表达式表示所述规划路径；离散化所述数学表达式表示的规划路径得到离散的路径点，将离散的路径点转化为栅格坐标，根据栅格坐标判断所述规划路径是否为不可行区域，若是不可行区域则舍弃所述规划路径，否则将所述规划路径作为可能的路径；通过评价函数评价所有可能的路径，并选出得分最高的一条路径进行平滑处理。本发明还涉及一种动态局部路径规划系统。本发明使得规划路径曲率连续，曲线光滑且符合机器人或AGV等可移动设备的运动特性。

技术领域

本发明涉及机器人技术领域，特别是涉及一种动态局部路径规划方法及系统。

背景技术

路径规划是指机器人或AGV等可移动设备按照某些性能指标(如，距离、时间等)搜索一条从起始状态到目标状态的最优或次优路径。根据对环境信息掌握的程度将其分为2种：(1)基于环境先验完全信息的全局路径规划，又称静态或离线规划；(2)基于传感器信息的局部路径规划，又称动态或在线路径规划。在自动泊车的应用场景中，一般的局部路径规划方法如动态窗口算法，模糊逻辑算法，并不能考虑规划路径末端汽车的姿态，只能到达动态避障的效果，而dubin算法和reeds-sheep算法虽然可以考虑末端位姿问题，但是只能在无障碍物情况下路径规划。又比如在物流堆场中，想要插取已知位姿的托盘，现有算法并不能已在考虑末端姿态的基础上动态避障。

目前比较成熟的算法有A*算法，其地图格式也是使用栅格化的代价地图，但是A*算法A*算法在节点选取时固定采用8邻域扩展方式，周围至多只能选择8个运动方向，且运动角度限制为π/4的整数倍，不利于机器人转向，导致产生更多冗余节点，且无对冗余节点进行优化，使最终路径转折点多、不平滑。

发明内容

本发明所要解决的技术问题是提供一种动态局部路径规划方法及系统，使得规划路径曲率连续，曲线光滑且符合机器人或AGV等可移动设备的运动特性。

本发明解决其技术问题所采用的技术方案是：提供一种动态局部路径规划方法，包括以下步骤：

(1)获取运动体的起始位姿和终止位姿，并实时更新代价地图，判断规划路径为几阶路线，并通过数学表达式表示所述规划路径；

(2)离散化所述数学表达式表示的规划路径得到离散的路径点，将离散的路径点转化为栅格坐标，根据栅格坐标判断所述规划路径是否为不可行区域，若是不可行区域则舍弃所述规划路径，否则将所述规划路径作为可能的路径；

(3)通过评价函数评价所有可能的路径，并选出得分最高的一条路径进行平滑处理。

所述步骤(1)中判断规划路径为几阶路线，并通过数学表达式表示所述规划路径前，需要将原坐标系下的全局地图转换为以终止位姿的位置为原点、姿态方向为Y轴的设定坐标系下的全局地图，转换方式为(x₁,y₁)＝([x·cos(θ)+y·sin(θ)+X],[y·cos(θ)-x·sin(θ)+Y])，其中，(x,y)为原坐标系下的坐标，θ为原坐标系X轴与设定坐标系X轴的夹角，(X,Y)为原坐标系原点与设定坐标系原点的差值。

所述步骤(1)中判断规划路径为几阶路线时需要对每个阶的定义，约束条件和相应的动作进行分析，其中，一阶分析如下：

一阶路径定义：运动体从当前位姿通过一段方向变化就能到终点位置；

一阶路径约束条件：

(a)d1＞R_min且d2＞R_min，其中，d1和d2分别为运动体到(R_min,0)和(-R_min,0)的距离，R_min为最小转弯半径；若不满足约束条件(a)则进行三阶分析，若满足则验证约束条件(b)；