CN112378408B - 一种实现轮式移动机器人实时避障的路径规划方法 - Google Patents

Abstract

本发明属于机器人技术领域，主要涉及一种实现轮式移动机器人实时避障的路径规划方法，包括如下步骤：S1、利用车载传感器获取环境信息，实现移动机器人的自身定位，并构建栅格地图，建立TF坐标转换；S2、利用改进的A*算法完成移动机器人的全局路径规划；S3、利用改进A*算法采用队列数据格式的特性，按照权值将点由小到大依次放入队列；S4、针对车载传感器获取未知障碍物信息，根据初始参数、最大速度Vm、线加速度v、角加速度ω，利用DWA算法结合全局路径规划信息做出路径评价，选择局部最优的路径。本发明方法采用改进A*算法与DWA算法相结合的混合算法对移动机器人全局路径进行规划，该混合算法可以实现在未知环境中的实时避障，提高路径规划的鲁棒性。

Description

一种实现轮式移动机器人实时避障的路径规划方法

技术领域

本发明属于机器人技术领域，具体涉及一种实现轮式移动机器人实时避障的路径规划方法。

背景技术

随着计算机科学技术的快速发展，轮式移动机器人自主导航的研究已是智能车交通系统的研究热点之一，其中，路径规划是智能车研究领域的重要技术，旨在障碍物环境中寻找从起始点到目标点的无碰撞路径。

目前，国内外学者已经相继开发了几种解决路径规划问题的算法，已知的路径规划分为全局规划与局部规划。全局路径规划算法：Dijkstra算法、A*算法、RRT算法等从环境中找到一条从起始点到目标点的最佳路径，其中A*算法是一种智能优化算法，鲁棒性好、全局优化性能好；局部路径规划方法有：人工势场法、模糊逻辑法、动态窗口法等利用车载传感器获取实时环境信息，规划出一条无碰撞局部路径，其中动态窗口法可以根据环境信息实时路径规划，具有较好的避障能力，可以很好的满足智能车的自主导航能力。

全局规划算法一般采用A*,不过在动态的或即使是仅部分不变的状况中A*算法就显得力不从心。A*算法生成的轨迹十分不自然，且在实际的小车运动中，难以满足直行限制，并且有小车最小转弯半径等动力学约束的局限性。局部规划算法一般采用DWA,但是，在某些场景中，DWA较高的灵活性会极大的降低行驶的平稳性。

发明内容

本发明的目的是：旨在提供一种实现轮式移动机器人实时避障的路径规划方法，用于解决移动机器人全局、局部路径优化及实现实时避障的问题。

为实现上述技术目的，本发明采用的技术方案如下：

一种实现轮式移动机器人实时避障的路径规划方法，包括如下步骤：

S1、利用车载传感器获取环境信息，实现移动机器人的自身定位，并构建栅格地图，建立TF坐标转换；

S2、利用改进的A*算法完成移动机器人的全局路径规划；

S3、利用改进A*算法采用队列数据格式的特性，按照权值将点由小到大依次放入队列；

S4、针对车载传感器获取未知障碍物信息，根据初始参数、最大速度Vm、线加速度v、角加速度ω，利用DWA算法结合全局路径规划信息做出路径评价，选择局部最优的路径。

进一步，S2包括以下步骤：

S201、将输入的栅格地图，经过map_sever节点处理，将处理过后的map的数据存入二维数组中，数组中的0和1分别代表可通行和不可通行；

S202、设定数组中的每一个数代表地图上的一个栅格，每一个栅格有G(n)、H(n)两个代价，其中，G(n)代表的是从初始位置A到节点n的移动代价，H(n)指定n到目标节点的估计代价。

进一步，S3包括如下步骤：

S301、所有节点移动评估函数为：F(n)＝(a)G(n)+(b)H(n)，其中，a，b为权值，当a＝0，b＝1，此时，是贪心算法，当a＝1,b＝0,是迪杰斯特拉，当a＝1,b＝1,是A*算法；

S302、H(n)为启发函数并采用传统的欧式距离；

S303、在实际工程中，改进A*算法采用Tie Breaker方法，路径中的点都有相同的f值，通过改变h值使得f有细微的差别；

其中，h＝h(1.0+p)，P<每一步的最代价/栅格总数的期望最大路径代价；

S304、通过加入可视函数，当更新节点时，实现扩展点与该点的父节点之间是否可视进行判定；

S305、改进A*算法打开一个节点s，并更新周围的节点s'时，会检查s'与s父节点的可见性，若可见，则把s'的父节点设置成s的父节点，实现只保留必要的拐点，当搜索到目标点后，调用回溯函数，找到一条无冗余点的直线段集合。

进一步，S4包括如下步骤：

S401、机器人运动模型的建立：机器人相邻时刻内，运动时间很短，运动距离也短，将两相邻点之间的运动轨迹看成直线，即沿机器人坐标系x轴移动了υ_t*Δt，将该段距离分别投影在世界坐标系x轴和y轴上得到t+1时刻相对于t时刻机器人在世界坐标系中坐标移动的位移Δx和Δy：

Δx＝υΔtcos(θ_t)

Δy＝υΔtsin(θ_t)

一定时间内位移增量，如下：

x＝x+υΔtcos(θ_t)

y＝y+υΔtsin(θ_t)

θ_t＝θ_t+ωΔt

若机器人是全向运动的，即机器人坐标系的y轴有速度表达，将机器人在机器人坐标y轴移动的距离投影到世界坐标系即可：

最终运动轨迹模型如下：

x＝x+υΔtcos(θ_t)-υ_yΔtsin(θ_t)

y＝y+υΔtsin(θ_t)+υ_yΔtcos(θ_t)

θ_t＝θ_t+ωΔt；

S402、速度采样模型的建立：移动机器人的移动受自身最大速度和最小速度的限制：

υ_m＝{υ∈[υ_min,υ_max],ω∈[ω_min,ω_max]}

其中，υ_m(υ,ω)为组合速度，υ表示线速度，ω表示角速度；

移动机器人轨迹前向模拟的周期内，存在一个动态窗口，在该窗口内的速度是机器人能够实际达到的速度：

其中，υ_d为可达速度，υ_curr为当前线速度，ω_curr为当前角速度；

整个机器人轨迹假设是由n个时段的n个小圆弧组成，从安全性考虑，根据简单的运动学推算，在某一时刻，不发生碰撞的允许速度υ_a为：

其中，dist(υ,ω)为速度为(υ,ω)时对应轨迹上离最近障碍物的距；

最终速度采样模型为：υ_r＝υ_m∩υ_a∩υ_d；

S403、预测轨迹评价模型的建立：采用评价函数的方式为每条轨迹进行评价，从中选择满足约束条件的最优速度，评价函数综合考虑了机器人运动速度、航向和避障，其定义如下：

G(υ,ω)＝α·to_goal_cost(υ,ω)+β·obstacle_cost(υ,ω)+γ·speed_cost(υ,ω)

其中，to_goal_cost表示机器人航向和到目标点的距离的得分；

obstacle表示预测轨迹中的点距离障碍物的最小距离；

speed_cost表示圆弧轨迹中t+1时刻的线速度，该项将选择动态窗口中线速度大的值；

α、β、γ为3个权值参数，为避免其中一项值占比过高，在总的评价前，将评价函数的3项参数归一化处理为[0,1]之间的参数；

goal_cost(υ,ω)是用于评价机器人在当前设定的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差距以及达到模拟轨迹末端后到目标的距离，角度差与距离越小，得分越低；

obstacle_cost(υ,ω)是用于规避障碍物的评价项，计算预测轨迹上每个点到附近每个障碍物的距离，比较其中最小距离与机器人与障碍物之间的容忍距离，若小于容忍距离则舍弃该预测轨迹，通过对机器人到障碍物的最小距离取反，距离越大，则得分越低；

speed_cost(υ,ω)是用于评估机器人移动速度的评价项，在速度窗口内，预测轨迹所对应的线速度与期望速度的差值越小，则得分越低；

通过归一化处理，给每一个评价项分配不同的权重，得到每一条预测轨迹的最终得分，选择其中得分最低的预测轨迹，并将其对应的速度发送给移动机器人地盘。

进一步，S4的具体实现，还包括建立一个ROS的循环体，该循环体的输入为局部目标点的位姿信息、局部地图信息，输出为发布给底盘使其运动至目标点的速度。

进一步，ROS循环体的具体步骤如下：

判断局部目标点是否发布，若已发布则接受并将其转换至机器人坐标系下，同时触发goal回调函数；

判断scan或者local_map是否更新，若已更新则触发scan回调函数或者触发local_map回调函数，同时将输入更新标志input_updated更新；

获取目标点相对机器人的位姿，求取机器人与目标点的距离，判断该距离是否小于容许值，若小于，则调整机器人姿态，并将速度降为0，该判断为DWA规划的结束标志，并在其后判断控制模式并发布对应的控制状态；

建立速度的动态窗口，由当前速度结合机器人的速度限制和动力学限制，列出机器人在下一时刻所有的可能的速度，形成速度窗口，其中包括线速度范围[min_velocity,max_velocity]和角速度范围[min_yawrate,max_yawrate]，该速度窗口会作为后面选择最佳运动轨迹模块的输入参数使用；

生成障碍物位置信息列表，首先判断采用激光扫描周围障碍生成障碍物列表还是采用将局部地图的障碍物信息转换到机器人坐标系下生成障碍物列表，得到的障碍物位置信息存放在数组中，作为选择最佳运动轨迹模块的输入参数使用；

选择最佳运动轨迹，该模块以速度动态窗口、障碍物位置信息列表和目标点作为输入参数，在速度窗口内，以速度分辨率将线速度和角速度分别划分成n个线速度和m个角速度，在它们的相互作用下，会生成n*m条预测轨迹，由于模拟时间为PREDICT_TIME,时间分辨率为DT，所以每一条预测轨迹实际上是由PREDICT_TIME/DT个点组成，将组成预测轨迹的点的信息保存进traj中，并将traj保存进trajectories中，然后用与目标点位姿的差值、与期望速度的差值和与障碍物的距离三个评价函数对预测轨迹进行打分，并将不同项的分值进行归一化处理，给不同项配上不同的权重系数，最后选择得分最优的预测轨迹，将其对应的线速度和角速度输出。

进一步，利用三个评价函数对预测轨迹进行打分的具体步骤如下：

to_goal_cost：求预测轨迹末端点相对于目标点的位姿差[x,y,yaw]的范数作为得分，轨迹越好，得分越低；

speed_cost：求预测轨迹末端点的线速度与期望速度的差值作为得分，轨迹越好，得分越低；

obstacle_cost：求预测轨迹的每个点与每个障碍物的距离，并选取其中的最小距离取反，并将取反后的值作为得分，离最近的障碍物的距离越大，轨迹越好得分越低，若预测轨迹中有一个点离障碍物的最小距离低于容许值，则抛弃该预测轨迹。

进一步，归一化处理具体步骤如下：

用某条轨迹的每一项得分除以所有轨迹的该项得分的总和，即为：

经过归一化处理后，评价函数中各项得分均限制于[0,1]以内，使得各项的占比由权重系数来控制。

采用上述技术方案的发明，具有如下优点：

1、本发明方法采用改进A*算法与DWA算法相结合的混合算法对移动机器人全局路径进行规划，该混合算法可以实现在未知环境中的实时避障，提高路径规划的鲁棒性，实现差速移动机器人的自动反馈控制；

2、由于实际工程中，直接订阅底盘odom信息非常不准确，本发明通过cartographer维护的TF树，转换得到机器人在地图上的位置信息，在栅格地图上，通过订阅TF转换后的odom信息，定位机器人在栅格地图上的位置和姿态，并设置目标点；

3、本发明方法通过改进A*算法规划局部移动的目标点，DWA算法对局部目标点进行实时追踪并下发线速度和角速度，下层底盘接收速度信息，驱动电机，完成差速移动机器人的局部实时避障；

4、本发明的混合算法，去除了路径上的冗余点，使得路径更加合理，在DWA算法中添加原地转向的功能，实现了移动机器人的平稳运行。

附图说明

本发明可以通过附图给出的非限定性实施例进一步说明；

图1为本发明改进A*算法流程图；

图2为传统A*算法最短路径与实际最短路径对比图；

图3为本发明改进A*算法路径节点更新示意图；

图4为本发明改进A*算法h优化的寻找区域对比图；

图5为本发明相邻时刻机器人运动的模型图；

图6为本发明速度对应轨迹上距离最近的障碍物的距离图；

图7为本发明动态窗口采样轨迹图；

图8为本发明评价项heading示意图；

图9为本发明ROS主循环模块流程框图；

图10为本发明轨迹的生成与选择流程框图。

具体实施方式

以下将结合附图和具体实施例对本发明进行详细说明，需要说明的是，在附图或说明书描述中，相似或相同的部分都使用相同的图号，附图中未绘示或描述的实现方式，为所属技术领域中普通技术人员所知的形式。另外，实施例中提到的方向用语，例如“上”、“下”、“顶”、“底”、“左”、“右”、“前”、“后”等，仅是参考附图的方向，并非用于限制本发明的保护范围。

如图1所示，一种实现轮式移动机器人实时避障的路径规划方法，包括如下步骤：

S1、利用车载传感器获取环境信息，实现移动机器人的自身定位，并构建栅格地图，建立TF坐标转换；

S2、利用改进的A*算法完成移动机器人的全局路径规划；

S3、利用改进A*算法采用队列数据格式的特性，按照权值将点由小到大依次放入队列；

S4、针对车载传感器获取未知障碍物信息，根据初始参数、最大速度Vm、线加速度v、角加速度ω，利用DWA算法结合全局路径规划信息做出路径评价，选择局部最优的路径。

实施例1：传统A*算法

A*算法是经典的搜索算法，A*搜索的估价函数f(n)、g(n)、h(n)，其中g(n)指从起始点到点n的最小路径损失，h(n)表示从点n到目标的最佳路径的估计损失。A*算法是在静止不变的环境中解决最佳路径的有用方法，很适用在所有数据不变的状况中，在动态的或即使是仅部分不变的状况中A*算法具有较大局限性。

A*算法的数据结构非常简单，描述环境的数组或图Graph、描述数组或图的节点Node、开启列表Open List，以及关闭列表Closed List。每一节点拥有前文讲到的实际代价g(n)、启发函数h(n)、与之相连的孩子节点数组Children。开启列表OpenList的每个节点都要被检测，并按照估价值f(n)的大小进行排序。关闭列表ClosedList存放被访问过的节点，放入关闭列表的节点的父节点都会被遍历且当目标点被加入到关闭列表时算法结束。A*算法的策略是由启发式函数：F(n)＝(a)G(n)+(b)H(n)决定的：其中a,b为权值，当a＝0，b＝1,此时，是贪心算法，当a＝1,b＝0,是迪杰斯特拉，当a＝1,b＝1,是A*算法。

在极其极端的状况中，假设b为零，那么对算法起到帮助的仅仅是g(n)，这时就A*算法演变为Dijkstra算法，虽然在这种情况下依然还是能够找出最短路径，但是性能却下降了很多。A*算法同样能找到一条最短最优的路径，假设h(n)一直比n移动到目标的实际消耗代价小或者是一样的。h(n)越小，A*散出的点就越繁，效率就越低。假如h(n)绝对即是从n至目标点的代价，那么A*就只找寻最优路径且不产生其他新点，效率就提高了。假如h(n)存在相对从n至目标的现实付出多，则不能保证A*会寻出最近路径，不过效率却是越高。假如h(n)的影响比g(n)重要很多，那么显然A*算法就会演变为较差的BFS算法。

实施例2：传统A*算法局限性

如图2所示，由于A*算法是在网格上运行，只能沿着网格的边、对角线或者顶点。这样会造成两大缺点：1)真正的最短路径与实际最短路径有偏差；2)会产生较多不必要的拐点。A*算法无法解决动态障碍物的问题，但是其效率比较高，工程实践也比较简单。

实施例3：改进A*算法

如图1、3所示，改进A*算法与A*算法关键的不同是改进A*算法允许一个顶点的父节点可以是任意顶点，而A*算法只允许顶点的父节点必须是邻居节点。改进A*算法与A*算法流程基本是一致的，除了改进A*算法会通过考虑如图3所示两种路径来更新顶点s未展开的邻接节点的g值。

实施例4：改进A*算法的优化

如图4所示，在实际工程中，改进A*算法采用Tie Breaker方法，路径中的点都有相同的f值，通过改变h值使得f有细微的差别；其中，h＝h(1.0+p)，P<每一步的最代价/栅格总数的期望最大路径代价。

实施例5：移动机器人运动模型建立

如图5所示，机器人运动模型的建立：机器人相邻时刻内，运动时间很短，运动距离也短，将两相邻点之间的运动轨迹看成直线，即沿机器人坐标系x轴移动了υ_t*Δt，将该段距离分别投影在世界坐标系x轴和y轴上得到t+1时刻相对于t时刻机器人在世界坐标系中坐标移动的位移Δx和Δy：

Δx＝υΔtcos(θ_t)

Δy＝υΔtsin(θ_t)

一定时间内位移增量，如下：

x＝x+υΔtcos(θ_t)

y＝y+υΔtsin(θ_t)

θ_t＝θ_t+ωΔt

若机器人是全向运动的，即机器人坐标系的y轴有速度表达，将机器人在机器人坐标y轴移动的距离投影到世界坐标系即可：

最终运动轨迹模型如下：

x＝x+υΔtcos(θ_t)-υ_yΔtsin(θ_t)

y＝y+υΔtsin(θ_t)+υ_yΔtcos(θ_t)

θ_t＝θ_t+ωΔt。

实施例6：速度采样模型的建立

如图6-7所示，移动机器人的移动受自身最大速度和最小速度的限制：

υ_m＝{υ∈[υ_min,υ_max],ω∈[ω_min,ω_max]}

其中，υ_m(υ,ω)为组合速度，υ表示线速度，ω表示角速度；

移动机器人轨迹前向模拟的周期内，存在一个动态窗口，在该窗口内的速度是机器人能够实际达到的速度：

其中，υ_d为可达速度，υ_curr为当前线速度，ω_curr为当前角速度；

整个机器人轨迹假设是由n个时段的n个小圆弧组成，从安全性考虑，根据简单的运动学推算，在某一时刻，不发生碰撞的允许速度υ_a为：

其中，dist(υ,ω)为速度为(υ,ω)时对应轨迹上离最近障碍物的距；

最终速度采样模型为：υ_r＝υ_m∩υ_a∩υ_d。

实施例7：预测轨迹评价模型的建立

如图8所示，采用评价函数的方式为每条轨迹进行评价，从中选择满足约束条件的最优速度，评价函数综合考虑了机器人运动速度、航向和避障，其定义如下：

G(υ,ω)＝α·to_goal_cost(υ,ω)+β·obstacle_cost(υ,ω)+γ·speed_cost(υ,ω)

其中，to_goal_cost表示机器人航向和到目标点的距离的得分；

obstacle表示预测轨迹中的点距离障碍物的最小距离；

speed_cost表示圆弧轨迹中t+1时刻的线速度，该项将选择动态窗口中线速度大的值；

α、β、γ为3个权值参数，为避免其中一项值占比过高，在总的评价前，将评价函数的3项参数归一化处理为[0,1]之间的参数；

goal_cost(υ,ω)是用于评价机器人在当前设定的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差距以及达到模拟轨迹末端后到目标的距离，角度差与距离越小，得分越低；

obstacle_cost(υ,ω)是用于规避障碍物的评价项，计算预测轨迹上每个点到附近每个障碍物的距离，比较其中最小距离与机器人与障碍物之间的容忍距离，若小于容忍距离则舍弃该预测轨迹，通过对机器人到障碍物的最小距离取反，距离越大，则得分越低；

speed_cost(υ,ω)是用于评估机器人移动速度的评价项，在速度窗口内，预测轨迹所对应的线速度与期望速度的差值越小，则得分越低；

实施例9：归一化处理

通过归一化处理，给每一个评价项分配不同的权重，得到每一条预测轨迹的最终得分，选择其中得分最低的预测轨迹，并将其对应的速度发送给移动机器人地盘。

实施例10：ROS的循环体的建立

如图9-10所示，判断局部目标点是否发布，若已发布则接受并将其转换至机器人坐标系下，同时触发goal回调函数；

判断scan或者local_map是否更新，若已更新则触发scan回调函数或者触发local_map回调函数，同时将输入更新标志input_updated更新；

获取目标点相对机器人的位姿，求取机器人与目标点的距离，判断该距离是否小于容许值，若小于，则调整机器人姿态，并将速度降为0，该判断为DWA规划的结束标志，并在其后判断控制模式并发布对应的控制状态；

建立速度的动态窗口，由当前速度结合机器人的速度限制和动力学限制，列出机器人在下一时刻所有的可能的速度，形成速度窗口，其中包括线速度范围[min_velocity,max_velocity]和角速度范围[min_yawrate,max_yawrate]，该速度窗口会作为后面选择最佳运动轨迹模块的输入参数使用；

生成障碍物位置信息列表，首先判断采用激光扫描周围障碍生成障碍物列表还是采用将局部地图的障碍物信息转换到机器人坐标系下生成障碍物列表，得到的障碍物位置信息存放在数组中，作为选择最佳运动轨迹模块的输入参数使用；

选择最佳运动轨迹，该模块以速度动态窗口、障碍物位置信息列表和目标点作为输入参数，在速度窗口内，以速度分辨率将线速度和角速度分别划分成n个线速度和m个角速度，在它们的相互作用下，会生成n*m条预测轨迹，由于模拟时间为PREDICT_TIME,时间分辨率为DT，所以每一条预测轨迹实际上是由PREDICT_TIME/DT个点组成，将组成预测轨迹的点的信息保存进traj中，并将traj保存进trajectories中，然后用与目标点位姿的差值、与期望速度的差值和与障碍物的距离三个评价函数对预测轨迹进行打分，并将不同项的分值进行归一化处理，给不同项配上不同的权重系数，最后选择得分最优的预测轨迹，将其对应的线速度和角速度输出。

利用三个评价函数对预测轨迹进行打分的具体步骤如下：

to_goal_cost：求预测轨迹末端点相对于目标点的位姿差[x,y,yaw]的范数作为得分，轨迹越好，得分越低；

speed_cost：求预测轨迹末端点的线速度与期望速度的差值作为得分，轨迹越好，得分越低；

obstacle_cost：求预测轨迹的每个点与每个障碍物的距离，并选取其中的最小距离取反，并将取反后的值作为得分，离最近的障碍物的距离越大，轨迹越好得分越低，若预测轨迹中有一个点离障碍物的最小距离低于容许值，则抛弃该预测轨迹。

归一化处理具体步骤如下：

用某条轨迹的每一项得分除以所有轨迹的该项得分的总和，即为：

经过归一化处理后，评价函数中各项得分均限制于[0,1]以内，使得各项的占比由权重系数来控制。

本发明对实现轮式移动机器人实时避障的路径规划方法进行了详细介绍。具体实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，改进和修饰也落入本发明权利要求的保护范围内。

Claims (5)

1.一种实现轮式移动机器人实时避障的路径规划方法，其特征在于，包括如下步骤：

S1、利用车载传感器获取环境信息，实现移动机器人的自身定位，并构建栅格地图，建立TF坐标转换；

S2、利用改进的A*算法完成移动机器人的全局路径规划；

S3、利用改进A*算法采用队列数据格式的特性，按照权值将点由小到大依次放入队列；

S4、针对车载传感器获取未知障碍物信息，根据初始参数、最大速度Vm、线加速度v和角加速度ω，利用DWA算法结合全局路径规划信息做出路径评价，选择局部最优的路径；

其中，S2包括以下步骤：

S201、将输入的栅格地图，经过map_sever节点处理，将处理过后的map的数据存入二维数组中，数组中的0和1分别代表可通行和不可通行；

S202、设定数组中的每一个数代表地图上的一个栅格，每一个栅格有G(n)、H(n)两个代价，其中，G(n)代表的是从初始位置A到节点n的移动代价，H(n)指定n到目标节点的估计代价；

S3包括如下步骤：

S301、所有节点移动评估函数为：F(n)＝(a)G(n)+(b)H(n)，其中，a，b为权值，当a＝0，b＝1，此时，是贪心算法，当a＝1,b＝0,是迪杰斯特拉，当a＝1,b＝1,是A*算法；

S302、H(n)为启发函数并采用传统的欧式距离；

S303、在实际工程中，改进A*算法采用Tie Breaker方法，路径中的点都有相同的f值，通过改变h值使得f有细微的差别；

其中，h＝h(1.0+p)，P<每一步的最代价/栅格总数的期望最大路径代价；

S304、通过加入可视函数，当更新节点时，实现扩展点与该点的父节点之间是否可视进行判定；

S305、改进A*算法打开一个节点s，并更新周围的节点s'时，会检查s'与s父节点的可见性，若可见，则把s'的父节点设置成s的父节点，实现只保留必要的拐点，当搜索到目标点后，调用回溯函数，找到一条无冗余点的直线段集合；

S4包括如下步骤：

S401、机器人运动模型的建立：机器人相邻时刻内，运动时间很短，运动距离也短，将两相邻点之间的运动轨迹看成直线，即沿机器人坐标系x轴移动了υ_t*Δt，将该段距离分别投影在世界坐标系x轴和y轴上得到t+1时刻相对于t时刻机器人在世界坐标系中坐标移动的位移Δx和Δy：

Δx＝υΔtcos(θ_t)

Δy＝υΔtsin(θ_t)

一定时间内位移增量，如下：

x＝x+υΔtcos(θ_t)

y＝y+υΔt sin(θ_t)

θ_t＝θ_t+ωΔt

若机器人是全向运动的，即机器人坐标系的y轴有速度表达，将机器人在机器人坐标y轴移动的距离投影到世界坐标系即可：

最终运动轨迹模型如下：

x＝x+υΔtcos(θ_t)-υ_yΔtsin(θ_t)

y＝y+υΔtsin(θ_t)+υ_yΔtcos(θ_t)

θ_t＝θ_t+ωΔt；

S402、速度采样模型的建立：移动机器人的移动受自身最大速度和最小速度的限制：

υ_m＝{υ∈[υ_min,υ_max],ω∈[ω_min,ω_max]}

其中，υ_m(υ,ω)为组合速度，υ表示线速度，ω表示角速度；

移动机器人轨迹前向模拟的周期内，存在一个动态窗口，在该窗口内的速度是机器人能够实际达到的速度：

其中，υ_d为可达速度，υ_curr为当前线速度，ω_curr为当前角速度；

整个机器人轨迹假设是由n个时段的n个小圆弧组成，从安全性考虑，根据简单的运动学推算，在某一时刻，不发生碰撞的允许速度υ_a为：

其中，dist(υ,ω)为速度为(υ,ω)时对应轨迹上离最近障碍物的距；

最终速度采样模型为：υ_r＝υ_m∩υ_a∩υ_d；

S403、预测轨迹评价模型的建立：采用评价函数的方式为每条轨迹进行评价，从中选择满足约束条件的最优速度，评价函数综合考虑了机器人运动速度、航向和避障，其定义如下：

G(υ,ω)＝α·to_goal_cost(υ,ω)+β·obstacle_cost(υ,ω)+γ·speed_cost(υ,ω)

其中，to_goal_cost表示机器人航向和到目标点的距离的得分；

obstacle表示预测轨迹中的点距离障碍物的最小距离；

speed_cost表示圆弧轨迹中t+1时刻的线速度，该项将选择动态窗口中线速度大的值；

α、β、γ为3个权值参数，为避免其中一项值占比过高，在总的评价前，将评价函数的3项参数归一化处理为[0,1]之间的参数；

goal_cost(υ,ω)是用于评价机器人在当前设定的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差距以及达到模拟轨迹末端后到目标的距离，角度差与距离越小，得分越低；

obstacle_cost(υ,ω)是用于规避障碍物的评价项，计算预测轨迹上每个点到附近每个障碍物的距离，比较其中最小距离与机器人与障碍物之间的容忍距离，若小于容忍距离则舍弃该预测轨迹，通过对机器人到障碍物的最小距离取反，距离越大，则得分越低；

speed_cost(υ,ω)是用于评估机器人移动速度的评价项，在速度窗口内，预测轨迹所对应的线速度与期望速度的差值越小，则得分越低；

通过归一化处理，给每一个评价项分配不同的权重，得到每一条预测轨迹的最终得分，选择其中得分最低的预测轨迹，并将其对应的速度发送给移动机器人地盘。

2.根据权利要求1所述的一种实现轮式移动机器人实时避障的路径规划方法，其特征在于，所述S4的具体实现，还包括建立一个ROS的循环体，该循环体的输入为局部目标点的位姿信息、局部地图信息，输出为发布给底盘使其运动至目标点的速度。

3.根据权利要求2所述的一种实现轮式移动机器人实时避障的路径规划方法，其特征在于，所述ROS循环体的具体步骤如下：

判断局部目标点是否发布，若已发布则接受并将其转换至机器人坐标系下，同时触发goal回调函数；

判断scan或者local_map是否更新，若已更新则触发scan回调函数或者触发local_map回调函数，同时将输入更新标志input_updated更新；

获取目标点相对机器人的位姿，求取机器人与目标点的距离，判断该距离是否小于容许值，若小于，则调整机器人姿态，并将速度降为0，该判断为DWA规划的结束标志，并在其后判断控制模式并发布对应的控制状态；

建立速度的动态窗口，由当前速度结合机器人的速度限制和动力学限制，列出机器人在下一时刻所有的可能的速度，形成速度窗口，其中包括线速度范围[min_velocity,max_velocity]和角速度范围[min_yawrate,max_yawrate]，该速度窗口会作为后面选择最佳运动轨迹模块的输入参数使用；

生成障碍物位置信息列表，首先判断采用激光扫描周围障碍生成障碍物列表还是采用将局部地图的障碍物信息转换到机器人坐标系下生成障碍物列表，得到的障碍物位置信息存放在数组中，作为选择最佳运动轨迹模块的输入参数使用；

选择最佳运动轨迹，该模块以速度动态窗口、障碍物位置信息列表和目标点作为输入参数，在速度窗口内，以速度分辨率将线速度和角速度分别划分成n个线速度和m个角速度，在它们的相互作用下，会生成n*m条预测轨迹，由于模拟时间为PREDICT_TIME,时间分辨率为DT，所以每一条预测轨迹实际上是由PREDICT_TIME/DT个点组成，将组成预测轨迹的点的信息保存进traj中，并将traj保存进trajectories中，然后用与目标点位姿的差值、与期望速度的差值和与障碍物的距离三个评价函数对预测轨迹进行打分，并将不同项的分值进行归一化处理，给不同项配上不同的权重系数，最后选择得分最优的预测轨迹，将其对应的线速度和角速度输出。

4.根据权利要求3所述的一种实现轮式移动机器人实时避障的路径规划方法，其特征在于，所述利用三个评价函数对预测轨迹进行打分的具体步骤如下：

to_goal_cost：求预测轨迹末端点相对于目标点的位姿差[x,y,yaw]的范数作为得分，轨迹越好，得分越低；

speed_cost：求预测轨迹末端点的线速度与期望速度的差值作为得分，轨迹越好，得分越低；

obstacle_cost：求预测轨迹的每个点与每个障碍物的距离，并选取其中的最小距离取反，并将取反后的值作为得分，离最近的障碍物的距离越大，轨迹越好得分越低，若预测轨迹中有一个点离障碍物的最小距离低于容许值，则抛弃该预测轨迹。

5.根据权利要求3所述的一种实现轮式移动机器人实时避障的路径规划方法，其特征在于，所述归一化处理具体步骤如下：

用某条轨迹的每一项得分除以所有轨迹的该项得分的总和，即为：

经过归一化处理后，评价函数中各项得分均限制于[0,1]以内，使得各项的占比由权重系数来控制。