一种基于混合算法的移动机器人路径规划方法

一种基于混合算法的移动机器人路径规划方法
(一)技术领域：
1.本发明属于移动机器人领域，具体涉及一种基于混合算法的移动机器人路径规划方法。
(二)

背景技术：
：
2.目前，移动机器人因其具备环境适应性强、运动高度灵活等优势而被广泛应用，包括多目标点巡航、搜寻救援、勘测探索、管道检查、资源开发等方面。移动机器人的路径规划研究已经成为当前移动机器人技术中较为重要的研究课题之一。而路径规划的发展也为移动机器人走向实用、更安全更可靠地执行任务提供了安全保障。目前大多数的移动机器人只能对单一的目标点进行路径规划，且不能够实时的进行避障，已无法满足现在社会的需求，如果能找到一条多个目标点的路径，并且能实时的避障，显得尤为重要，本方法针对以上问题，首先规划一条遍历所有目标点的全局最优路径，然后融合动态窗口算法进行实时的避障，最终完成移动机器人整体的路径规划。
(三)

技术实现要素：
：
3.本发明的目的在于提供一种基于混合算法的移动机器人路径规划方法，既解决了现有技术只能对单一目标点路径规划的问题，又解决了实时避障的问题，是一种操作简单容易实现的路径规划方法。
4.本发明的技术方案：一种基于混合算法的移动机器人路径规划方法，其特征在于它具体包括如下步骤：
5.(1)通过移动机器人配置传感器对全局环境信息进行采集，得到能够描述作业环境信息的环境图像信号；
6.所述步骤(1)中的移动机器人可以是陆地机器人、水下机器人或空中机器人；所述通过移动机器人配置传感器对全局环境信息进行采集是指利用移动机器人自身携带的环境信息采集装置及数据采集方法获得，用于构建能够描述作业环境信息的环境图像信号。
7.(2)利用栅格法，根据步骤(1)采集到的环境图像信号创建移动机器人的作业环境模型；
8.(3)基于步骤(2)创建的移动机器人的作业环境模型，利用a
‑
star算法规划出每两个目标点的路径；
9.所述步骤(3)中采用a
‑
star算法规划每两个目标点间的规划路径，由以下步骤构成：
10.(3
‑
1)选择待规划路径的起点，由a
‑
star算法原理确定待考察节点，并建立存放待考察节点的open列表；并从open列表中选出“考察到的且评价函数最低”的节点放入到closed列表；
11.所述步骤(3
‑
1)中的待考察节点包括两种情况，一是从起点开始，以起点为中心，周围与之相邻的节点，二是从待考察节点中选出的其评价函数最低的节点，再以评价函数
最低的节点为中心，周围与之相邻的节点；待考察节点包括已经考察过的节点或者含有障碍物的节点。
12.(3
‑
2)确定起始栅格节点s，把起始栅格节点s归入open列表中；
13.(3
‑
3)在open列表中进行选择，根据式(1)所示的评价函数，计算open列表中的每个待考察节点的评价函数，并从中选择评价函数最低的节点放入到closed列表中；
14.f(i)＝g(i)+h(i)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
15.其中，i表示第i个节点，g(i)表示从起始栅格节点s到第i个节点的付出代价值，h(i)表示从第i个节点到目标点t的预测代价值；
16.(3
‑
4)若此时open列表为空，也就是指以当前节点为中心，open列表中的待考查的节点分为两大类，一类是障碍物栅格不能通过，另一类则是已经放入到了closed列表中，以导致待考察的节点都不能选择，此时，两种情况都会使open列表表现为空，将意味着路径规划失败；若此时open列表为非空，则查找open列表中评价函数f(i)最小的节点，将其作为当前节点a，并将该节点a放入到closed列表中；
17.(3
‑
5)判断步骤(3
‑
4)中选取的节点a是否为目标点，如果是，则流程结束；反之，则以该节点a为父节点在其周围进行子节点扩展，并进行扩展后处理；
18.所述步骤(3
‑
5)中对子节点进行扩展后处理具体是指：
19.(3
‑5‑
1)若待扩展的子节点即不在closed列表中，也不在open列表中，同时也不在障碍栅格中，则把该节点加入到open表中；
20.(3
‑5‑
2)若待扩展的子节点在closed列表中或者是障碍栅格中，则跳过该子节点；
21.(3
‑5‑
3)若在open列表中已经有该子节点，判断该子节点的评价函数f(i)值是否小于open列表中的评价函数f(i)值，如果是，则更新open列表中的评价函数f(i)值，依此将待扩展的子节点遍历，并找到评价函数f(i)值最小的子节点。
22.(3
‑
6)根据(3
‑
5)步骤继续进行子节点扩展，直到当目标节点被放入到closed列表时，则停止搜索，表明找到从起点到目标点通行的路径；当open列表为空且目标节点未被找到时，也要停止搜索，标明此时没有能通行的路径。
23.(4)在步骤(3)的基础上，利用融合蚁群算法确定所有目标点的遍历顺序，由此遍历顺序构成的路径是一条能遍历所有目标点的最短距离：
24.所述步骤(4)中利用融合蚁群算法确定所有目标点的遍历顺序，由此遍历顺序构成的路径是一条能遍历所有目标点的最短距离，其具体包括以下步骤：
25.(4
‑
1)初始化蚁群参数各路径的信息素，蚂蚁数量为m，n为目标点的数量，每一只蚂蚁随机选择n个目标点的某一个目标点为出发点；
26.(4
‑
2)每只蚂蚁按照如式(4)所示的状态转移概率，计算下一个待访问的目标点，即从第i个目标点移到下一个顶点j，直到所有蚂蚁完成一次周游，即所有目标点被所有蚂蚁遍历；
[0027][0028]
式中，s为起始栅格节点；allowed
k
为蚂蚁下一步可走的目标点集合；为t时刻路
径上的信息素，为路径长度的倒数，α为信息素重要程度因子，β为启发函数重要程度因子；
[0029]
(4
‑
3)当所有蚂蚁执行完一次循环以后，记录每只蚂蚁经过的路径总长度l以及当前迭代的最短路径，并按式(5)
‑
(7)的信息素更新公式，更新目标点之间的信息素浓度：
[0030][0031][0032][0033]
其中，ρ0为信息素挥发系数；n
c
为当前蚂蚁迭代次数；n
max
为蚂蚁最大的迭代次数，δτ
i，j
为目标点i，j之间来自所有蚂蚁的信息素浓度值和，l为第k只蚂蚁在当前搜索中行走距离之和，q表示信息素的强度；
[0034]
(4
‑
4)若当迭代次数达到最大，则输出最优遍历顺序，将其作为移动机器人在路径规划过程中所遍历目标点的顺序，此遍历目标点的顺序与原点构成的路径则为最短距离，否则，迭代次数加1，并清空蚂蚁路径记录表，返回步骤(4
‑
2)继续执行，直到完成移动机器人的多目标点全局静态路径规划。
[0035]
(5)移动机器人从起点开始沿着由步骤(4)找到的能够遍历所有目标点的最短距离的路径移动，并利用基于融合动态窗口算法的局部动态规划预测移动机器人的运动轨迹，对行进过程中遇到的移动障碍物进行实时的避障，选择最优路径，使移动机器人能够避开障碍物，并在避障后仍按着原先的路径移动，最终返回起点。
[0036]
所述步骤(5)中利用基于融合动态窗口算法的局部动态规划预测移动机器人的运动轨迹具体由以下步骤构成：
[0037]
(5
‑
1)当移动机器人得到开始移动的指令，以步骤(4)全局规划好的路径为基础，将移动机器人放置原点，并将初始位置到第一个目标点的中间转折点设置为第一个动态的子目标点，若没有转折点则将第一个目标点设置为第一个动态的子目标点；
[0038]
(5
‑
2)建立移动机器人的运动模型：设移动机器人的线速度为v
t
和角速度为w
t
；当前移动机器人的位姿表示为(x0,y0,θ0)，记下一时刻位姿为(x
t
,y
t
,θ
t
)，则其运动模型如式(8)所示：
[0039][0040]
其中，x0代表移动机器人当前位置的横坐标，y0代表移动机器人当前位置的纵坐标，θ0代表移动机器人当前位置的航向角，x
t
为移动机器人下一时刻的横坐标，y
t
为移动机器人下一时刻的纵坐标，θ
t
代表移动机器人下一时刻的航向角；
[0041]
(5
‑
3)在如式(8)所示的移动机器人的运动模型的基础上，对移动机器人的运行速度进行采样，根据采样速度推算出该移动机器人的运动轨迹，并对该轨迹进行评价；
[0042]
所述步骤(5
‑
3)中对移动机器人的运行速度进行采样受移动机器人自身条件和环境因素影响的限制：
[0043]
(5
‑3‑
1)移动机器人受自身最大最小速度的限制如式(9)所示：
[0044]
v
m
∈{v∈[v
min
,v
max
],w∈[w
min
,w
max
]}
ꢀꢀ
(9)
[0045]
式中，v
min
为最小线速度，v
max
为最大线速度，w
min
为最小旋转角速度，w
max
为最大旋转角速度；
[0046]
(5
‑3‑
2)移动机器人受自身的电机性能的影响：
[0047]
由于电机力矩有限，存在最大的加减速限制，因此移动机器人轨迹前向模拟的周期内，存在一个动态窗口，在该窗口内的速度是移动机器人能够实际达到的速度，即：
[0048][0049]
式中，v
c
,w
c
是移动机器人的当前速度和旋转角速度，表示瞬时减速度，表示瞬时旋转减速度，表示瞬时加速度，表示瞬时旋转加速度；
[0050]
(5
‑3‑
3)从移动机器人安全的考虑，为了使移动机器人能够在碰到障碍物前停下来，则在其最大减速度的条件下，移动机器人的行驶应满足式(11)所示的速度范围：
[0051][0052]
其中，dist(v,w)为速度(v,w)对应轨迹上离障碍物最近的距离。
[0053]
(5
‑
4)将步骤(5
‑
3)中根据式(9)
‑
(11)得到的每一种条件下的采样速度取交集，即可得到至少一组速度；并将其代入式(8)所示的运动学模型中，从而得到该移动机器人的运动轨迹；且不同的速度将得到不同的运动轨迹；
[0054]
(5
‑
5)由于在步骤(5
‑
4)中根据采样速度组，能够得到不少于1条的可行运动轨迹，因此采用如式(12)所示评价函数的方式对每条轨迹进行评价：
[0055]
g(v,w)＝σ(α
·
heading(v,w)+β
·
dist(v,w)+γ
·
velocity(v,w))
ꢀꢀ
(12)
[0056]
其中，heading(v,w)是用来评价移动机器人在当前设定的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差距；dist(v,w)代表移动机器人在当前轨迹上与最近的障碍物之间的距离；如果在这条轨迹上没有障碍物，那就将dist(v,w)设定为一个常数；velocity(v,w)用来评价当前轨迹的速度大小；
[0057]
(5
‑
6)将式(12)中的heading(v,w)、dist(v,w)及velocity(v,w)三个部分分别按照式(13)
‑
(15)进行归一化处理：
[0058][0059][0060][0061]
(5
‑
7)将步骤(5
‑
6)中归一化处理后的heading(v,w)，dist(v,w)，dist(v,w)带入式(12)中，选取评价函数最高的轨迹为最终的机器人的运动轨迹，并按照该条轨迹所对应的速度来驱动机器人运动；
[0062]
(5
‑
8)移动机器人按照步骤(5
‑
7)中得到的最终运动轨迹所对应的速度前行，直到遇到目标点，并判断当前目标点的位置是否为起点的位置，若是则为最终目标点，运动结束，否则要准备移动到下一个目标点，重复步骤(5
‑
1)
‑
步骤(5
‑
8)，直到机器人运动到起点则结束。
[0063]
本发明的优越性：通过结合传统a
‑
star算法搜索策略，融合蚁群算法解决了多个目标点的遍历顺序问题,其次融合动态窗口算法解决了移动机器人实时避障的问题。本发明主要是解决了实时避障问题，由于目标点较多，移动机器人采集完环境后，会在系统中规划好一条路径，此时移动机器人还在原点，当移动机器人移动的过程中，可能碰到移动的障碍物占据了原来的路径，导致移动机器人不能完成任务，而增加了动态避障以后，可以绕过移动的障碍物，并且继续沿着原来的路径行走，从而更好的完成任务。
附图说明：
[0064]
图1为本发明所涉一种基于混合算法的移动机器人路径规划方法的算法流程图。
[0065]
图2为本发明所涉一种基于混合算法的移动机器人路径规划方法的融合蚁群算法解决多目标点间路径规划对比图(其中，图2
‑
a为传统的a*算法，图2
‑
b为融合蚁群算法)。
具体实施方式：
[0066]
实施例：一种基于混合算法的移动机器人路径规划方法，如图1所示，其特征在于它具体包括如下步骤：
[0067]
(1)通过移动机器人配置传感器对全局环境信息进行采集，得到能够描述作业环境信息的环境图像信号；
[0068]
其中，移动机器人是陆地机器人、水下机器人或空中机器人中的一种；且移动机器人自身携带的环境信息采集装置及数据采集方法获得全局环境信息，用于构建能够描述作业环境信息的环境图像信号。本实施例中是球形两栖移动机器人。
[0069]
(2)利用栅格法，根据步骤(1)采集到的环境图像信号创建移动机器人的作业环境模型；
[0070]
(3)基于步骤(2)创建的移动机器人的作业环境模型，利用a
‑
star算法规划出每两个目标点的路径；
[0071]
所述步骤(3)中采用a
‑
star算法规划每两个目标点间的规划路径，由以下步骤构成：
[0072]
(3
‑
1)选择待规划路径的起点，由a
‑
star算法原理确定待考察节点，并建立存放待考察节点的open列表；并从open列表中选出“考察到的且评价函数最低”的节点放入到closed列表；
[0073]
待考察节点包括两种情况，一是从起点开始，以起点为中心，周围与之相邻的节点，二是从待考察节点中选出的其评价函数最低的节点，再以评价函数最低的节点为中心，周围与之相邻的节点；待考察节点包括已经考察过的节点或者含有障碍物的节点。
[0074]
(3
‑
2)确定起始栅格节点s，把起始栅格节点s归入open列表中；
[0075]
(3
‑
3)在open列表中进行选择，根据式(1)所示的评价函数，计算open列表中的每个待考察节点的评价函数，并从中选择评价函数最低的节点放入到closed列表中；
[0076]
f(i)＝g(i)+h(i)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0077]
其中，i表示第i个节点，g(i)表示从起始栅格节点s到第i个节点的付出代价值，h(i)表示从第i个节点到目标点t的预测代价值；
[0078]
(3
‑
4)若此时open列表为空，也就是指以当前节点为中心，open列表中的待考查的节点分为两大类，一类是障碍物栅格不能通过，另一类则是已经放入到了closed列表中，以导致待考察的节点都不能选择，此时，两种情况都会使open列表表现为空，将意味着路径规划失败；若此时open列表为非空，则查找open列表中评价函数f(i)最小的节点，将其作为当前节点a，并将该节点a放入到closed列表中；
[0079]
(3
‑
5)判断步骤(3
‑
4)中选取的节点a是否为目标点，如果是，则流程结束；反之，则以该节点a为父节点在其周围进行子节点扩展，并进行扩展后处理；
[0080]
所述步骤(3
‑
5)中对子节点进行扩展后处理具体是指：
[0081]
(3
‑5‑
1)若待扩展的子节点即不在closed列表中，也不在open列表中，同时也不在障碍栅格中，则把该节点加入到open表中；
[0082]
(3
‑5‑
2)若待扩展的子节点在closed列表中或者是障碍栅格中，则跳过该子节点；
[0083]
(3
‑5‑
3)若在open列表中已经有该子节点，判断该子节点的评价函数f(i)值是否小于open列表中的评价函数f(i)值，如果是，则更新open列表中的评价函数f(i)值，依此将待扩展的子节点遍历，并找到评价函数f(i)值最小的子节点。
[0084]
(3
‑
6)根据(3
‑
5)步骤继续进行子节点扩展，直到当目标节点被放入到closed列表时，则停止搜索，表明找到从起点到目标点通行的路径；当open列表为空且目标节点未被找到时，也要停止搜索，标明此时没有能通行的路径。
[0085]
(4)在步骤(3)的基础上，利用融合蚁群算法确定所有目标点的遍历顺序，由此遍历顺序构成的路径是一条能遍历所有目标点的最短距离：
[0086]
蚁群算法是一种模拟蚂蚁利用信息素确定觅食路径的方法；在觅食过程中优先选取信息素浓度较高的路径，同时也释放定量的信息素，随着信息素的浓度越来越高，形成正反馈，通过遍历所有路径，选择信息素浓度最高的路径作为最优路径。将蚁群算法应用在解决tsp问题上，蚁群算法可描述为已知所有目标点的距离，在每个目标点只经过一次的前提下，求解一条最短的遍历n个目标点的路径。即：
[0087]
由于蚁群算法可描述为：已知所有目标点之间的距离，可以在每个目标点只经过一次的前提下，求解一条最短的遍历n个目标点的路径，即：
[0088]
p＝(c1,c2,c3,...c
n
)
ꢀꢀ
(2)
[0089][0090]
式中，p为目标点的遍历顺序表示，c
i
为目标点编号，i＝1,2,
……
,n，d(c
i
,c
i+1
)表示相邻编号目标点之间的距离，f(p)遍历n个目标点的路径。
[0091]
利用融合蚁群算法确定所有目标点的遍历顺序，由此遍历顺序构成的路径是一条能遍历所有目标点的最短距离，其具体包括以下步骤：
[0092]
(4
‑
1)初始化蚁群参数各路径的信息素，蚂蚁数量为m，n为目标点的数量，每一只蚂蚁随机选择n个目标点的某一个目标点为出发点；
[0093]
(4
‑
2)每只蚂蚁按照如式(4)所示的状态转移概率，计算下一个待访问的目标点，
即从第i个目标点移到下一个顶点j，直到所有蚂蚁完成一次周游，即所有目标点被所有蚂蚁遍历；
[0094][0095]
式中，s为起始栅格节点；allowed
k
为蚂蚁下一步可走的目标点集合；为t时刻路径上的信息素，为路径长度的倒数，α为信息素重要程度因子，β为启发函数重要程度因子；
[0096]
(4
‑
3)当所有蚂蚁执行完一次循环以后，记录每只蚂蚁经过的路径总长度l以及当前迭代的最短路径，并按式(5)
‑
(7)的信息素更新公式，更新目标点之间的信息素浓度：
[0097][0098][0099][0100]
其中，ρ0为信息素挥发系数；n
c
为当前蚂蚁迭代次数；n
max
为蚂蚁最大的迭代次数，δτ
i，j
为目标点i，j之间来自所有蚂蚁的信息素浓度值和，l为第k只蚂蚁在当前搜索中行走距离之和，q表示信息素的强度；
[0101]
(4
‑
4)若当迭代次数达到最大，则输出最优遍历顺序，将其作为移动机器人在路径规划过程中所遍历目标点的顺序，此遍历目标点的顺序与原点构成的路径则为最短距离，否则，迭代次数加1，并清空蚂蚁路径记录表，返回步骤(4
‑
2)继续执行，直到完成移动机器人的多目标点全局静态路径规划。
[0102]
步骤(3)是找到了每两个目标点之间的最短距离，步骤(4)是考虑到所有点包括起点和目标点在内，找一个最好的遍历顺序，将所有点连接起来，不同的遍历顺序，最终的路径长度不同。利用蚁群算法遍历的目标点顺序，则为最短距离，并且这个距离是连接所有目标点和起点的最短距离。
[0103]
步骤(4)完成后，指的是移动机器人在已知环境的情况下，找到了一条遍历所有目标点的最短距离，但是移动机器人还没开始移动，步骤(5)则是沿着步骤(4)规划好的路径移动，在移动的过程中会发现由于有了移动障碍物占据了已经规划好的路径，导致原来规划好的路径不能行的通，需要躲避开移动障碍物，并继续沿着原来规划好的路径走。
[0104]
(5)移动机器人从起点开始沿着由步骤(4)找到的能够遍历所有目标点的最短距离的路径移动，并利用基于融合动态窗口算法的局部动态规划预测移动机器人的运动轨迹，对行进过程中遇到的移动障碍物进行实时的避障，选择最优路径，使移动机器人能够避开障碍物，并在避障后仍按着原先的路径移动，最终返回起点。
[0105]
利用基于融合动态窗口算法的局部动态规划预测移动机器人的运动轨迹具体由
以下步骤构成：
[0106]
(5
‑
1)当移动机器人得到开始移动的指令，以步骤(4)全局规划好的路径为基础，将移动机器人放置原点，并将初始位置到第一个目标点的中间转折点设置为第一个动态的子目标点，若没有转折点则将第一个目标点设置为第一个动态的子目标点；
[0107]
(5
‑
2)建立移动机器人的运动模型：设移动机器人的线速度为v
t
和角速度为w
t
；当前移动机器人的位姿表示为(x0,y0,θ0)，记下一时刻位姿为(x
t
,y
t
,θ
t
)，则其运动模型如式(8)所示：
[0108][0109]
其中，x0代表移动机器人当前位置的横坐标，y0代表移动机器人当前位置的纵坐标，θ0代表移动机器人当前位置的航向角，x
t
为移动机器人下一时刻的横坐标，y
t
为移动机器人下一时刻的纵坐标，θ
t
代表移动机器人下一时刻的航向角；
[0110]
(5
‑
3)在如式(8)所示的移动机器人的运动模型的基础上，对移动机器人的运行速度进行采样，根据采样速度推算出该移动机器人的运动轨迹，并对该轨迹进行评价；
[0111]
对移动机器人的运行速度进行采样受移动机器人自身条件和环境因素影响的限制：
[0112]
(5
‑3‑
1)移动机器人受自身最大最小速度的限制如式(9)所示：
[0113]
v
m
∈{v∈[v
min
,v
max
],w∈[w
min
,w
max
]}
ꢀꢀ
(9)
[0114]
式中，v
min
为最小线速度，v
max
为最大线速度，w
min
为最小旋转角速度，w
max
为最大旋转角速度；
[0115]
(5
‑3‑
2)移动机器人受自身的电机性能的影响：
[0116]
由于电机力矩有限，存在最大的加减速限制，因此移动机器人轨迹前向模拟的周期内，存在一个动态窗口，在该窗口内的速度是移动机器人能够实际达到的速度，即：
[0117][0118]
式中，v
c
,w
c
是移动机器人的当前速度和旋转角速度，表示瞬时减速度，表示瞬时旋转减速度，表示瞬时加速度，表示瞬时旋转加速度；
[0119]
(5
‑3‑
3)从移动机器人安全的考虑，为了使移动机器人能够在碰到障碍物前停下来，则在其最大减速度的条件下，移动机器人的行驶应满足式(11)所示的速度范围：
[0120][0121]
其中，dist(v,w)为速度(v,w)对应轨迹上离障碍物最近的距离。
[0122]
(5
‑
4)将步骤(5
‑
3)中根据式(9)
‑
(11)得到的每一种条件下的采样速度取交集，即可得到至少一组速度；并将其代入式(8)所示的运动学模型中，从而得到该移动机器人的运动轨迹；且不同的速度将得到不同的运动轨迹；
[0123]
取交集后的速度通常是多组，因为式(9)
‑
(11)中的速度都在一个区间内，而取交集以后仍在一个区间内，只不过区间变小；
[0124]
(5
‑
5)由于在步骤(5
‑
4)中根据采样速度组，能够得到不少于1条的可行运动轨迹，
因此采用如式(12)所示评价函数的方式对每条轨迹进行评价：
[0125]
g(v,w)＝σ(α
·
heading(v,w)+β
·
dist(v,w)+γ
·
velocity(v,w))
ꢀꢀꢀ
(12)
[0126]
其中，heading(v,w)是用来评价移动机器人在当前设定的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差距；dist(v,w)代表移动机器人在当前轨迹上与最近的障碍物之间的距离；如果在这条轨迹上没有障碍物，那就将dist(v,w)设定为一个常数；velocity(v,w)用来评价当前轨迹的速度大小；
[0127]
(5
‑
6)将式(12)中的heading(v,w)、dist(v,w)及velocity(v,w)三个部分分别按照式(13)
‑
(15)进行归一化处理：
[0128][0129][0130][0131]
(5
‑
7)将步骤(5
‑
6)中归一化处理后的heading(v,w)，dist(v,w)，dist(v,w)带入式(12)中，选取评价函数最高的轨迹为最终的机器人的运动轨迹，并按照该条轨迹所对应的速度来驱动机器人运动；
[0132]
归一化的目的是平滑：比如障碍物距离，移动机器人传感器检测到的最小障碍物距离在二维空间中是不连续的，这条轨迹能够遇到障碍，旁边那条不一定能遇到，并且这条轨迹最小的障碍物距离是1m，旁边那条是10m，那么障碍物距离的这种评价标准导致评价函数不连续，也会导致某个项在评价函数中太占优势，所以将它们归一化处理，这样都变成同一的百分比，每个障碍物的最小距离都是100份中的一份。
[0133]
(5
‑
8)移动机器人按照步骤(5
‑
7)中得到的最终运动轨迹所对应的速度前行，直到遇到目标点，并判断当前目标点的位置是否为起点的位置，若是则为最终目标点，运动结束，否则要准备移动到下一个目标点，重复步骤(5
‑
1)
‑
步骤(5
‑
8)，直到机器人运动到起点则结束。
[0134]
在附图2中，用仿真软件进行了仿真，图(a)与图(b)选择了相同的背景环境进行路径规划，图(a)中选择了七个目标点分别进行单独的路径规划，如果想要完成每个目标点的任务，则总的路径长度为单个目标点到起点的距离的二倍，然后求和，而图(b)是在相同的背景下，且目标点相同的情况下，完成所有目标点的遍历，只需遍历一次起点的路径，由以上可以得知，路径的长度大大的被缩短。