CN103576680B - 一种机器人路径规划方法及装置 - Google Patents

Abstract

本发明公开了一种基于改进觅食行为的人工鱼群算法的机器人路径规划方法，基于自由空间法的环境建模具有建模简单、易于实现的优点，由自由空间法建立自由链接线，连接自由链接线上的中点而构成机器人自由移动网络；通过Dijkstra算法搜索自由移动网络得到局部最短路径，再利用改进觅食行为的人工鱼群算法对搜索到的算法进一步优化，最终得到全局最短路径。该发明提供了搜索效率高的机器人路径规划方法及装置。

Description

一种机器人路径规划方法及装置

技术领域

本发明涉及计算机技术领域，特别涉及一种机器人规划路径的方法及装置。

背景技术

随着工业化的发展，机器人不在局限于制造业领域，在服务业、农业也有所应用。路经规划是移动机器人系统的一个重要内容，并直接影响到机器人完成任务的质量。人工鱼群算法由李晓磊等人于2002年基于鱼群行为而提出的新型群体智能优化算法，本发明通过改进人工鱼群算法的觅食行为，改进后的算法在收敛速度和搜索结果的满意解域精确度上都得到明显提高。目前机器人路径规划分为两种：基于传感器信息的局部路径规划和基于环境信息完全已知的全局路径规划。本发明涉及的机器人路径规划是基于环境信息完全已知的全局路径规划，传统全局路径规划方法主要有：栅格法、可视图法、拓扑法、自由空间法、神经网络法等。机器人路径规划的基本步骤是：1）建立环境模型，即建立基于现实环境的相关抽象模型。2）搜索无碰撞路径，利用搜索算法搜索符合条件的最短路径。自由空间法具有建模简单，易于实现的优点。目前基于自由空间法的机器人路径规划方法有很多种，例如基于自由空间法的Dijkstra算法路径优化方法。Dijkstra算法优化的路径只是局部最短路径，并非是全局最短路径。现有搜索算法有收敛过快，无法搜索到全局最有值，或由于搜索效率低，收敛速度慢。

发明内容

本发明克服了现有技术中的缺点，基于改进人工鱼群算法搜索效率高，收敛速度快的特点，提出了基于改进觅食行为的人工鱼群算法搜索全局最短路径方法，搜索效率高的机器人路径规划方法及装置。

为了解决上述技术问题，本发明是通过以下技术方案实现的：

一种机器人路径规划方法，它包括如下步骤：

（1）机器人路径规划基于环境信息完全已知的全局路径规划；在机器人路径中把机器人简化成一个点，并在二维空间内运动，机器人通过双目视觉传感器和测距传感器感知自身当前的位姿和障碍物的位置；

（2）将双目视觉传感器和测距传感器采集的环境信息中的障碍物处理成凸多边形；

（3）构建基于自由空间法的机器人移动链接图模型；

设机器人的起始位置用start表示，终点位置用goal表示；用黑色多边形代替障碍物,以多边形顶点为端点的连线表示链接线，机器人自由移动路线由链接线中点的连线构成，连接各链接线中点组成机器人自由移动网络；

（4）通过Dijkstra算法求出机器人自由移动网络最短路径；

设链接线的中点i表示该链接线的路径点i，则取链接线上的路径点标识序列数作为路径编码，如果起始点或终点不在链接线上，则连接该点（始点或终点）与最近链接线中点连线构成加入起始点或终点的新机器人自由移动网络；设路径点i与路径j的权值为W_ij,则对应权值的表达式为：

$W_{\mathrm{ij}}=\left\{\begin{array}{c}{W}_{\mathrm{ij}},i\≠j\\ 0,i=j\end{array}\right.;---1)$

其中V_ij代表路径点i、j在自由移动网络中的对应线段，如果在自由移动网络中不存在对于线段，则V_ij=∞；通过用Dijkstra算法搜寻起点到终点的最优有效权值组合，获得最短机器人移动路径；

（5）建立机器人路径编码数学模型；

设路径编码P_i（i＝1，2，...，n）表示路径点i所在自由链接线上任意一点，则P_i的编码模型如下所示：

$\left\{\begin{array}{c}{P}_i=P_{i1}+(P_{i2}-P_{i1})t_i,\\ t_i\∈\left[\mathrm{0,1}\right],\\ i=\mathrm{1,2},...,n\end{array}\right.---2)$

其中P_i1、P_i2表示链接线两端点，线段P_i1P_i2表示链接线；对其它路径点进行同样处理，得到对应路径点表达形式；路径编码P对应的路径编码序列为：P=t1，t2，...，ti，...，tn（t_i∈[0,1]），其中每一个编码系列只有对应一个机器人路径；通过搜索ti值，可以找到最优的路径编码；

（6）基于改进觅食行为的人工鱼群算法机器人路径优化模型；

（6.1）人工鱼模型：

利用路径编码方法生成m条人工鱼，人工鱼X_q表示为：

$\left\{\begin{array}{c}{X}_q=(t_{q1},t_{q2},...,t_{\mathrm{qi}},...,t_{\mathrm{qn}}),q\∈(\mathrm{1,2},...,m)\\ t_{\mathrm{qi}}\∈\left[\mathrm{0,1}\right],i\∈(\mathrm{1,2},...,n)\end{array}\right.---3)$

每条人工鱼有n个可变分量，且每个分量表示链接线上一点；

（6.2）适应值函数（食物浓度函数）：

由于Pi=P_i1+(P_i2-P_i1)t_i，则Pi点坐标表示成：

$\left\{\begin{array}{c}{x}_i=x_{i1}+(x_{i2}-x_{i1})t_i\\ y_i=y_{i1}+(y_{i2}-y_{i1})t_i\end{array}\right.---4)$

因此描述路径长度的适应函数为：

其中，||P_iP_i-1||表示P_i和P_i-1两点的距离；

（6-3）定义人工鱼X₁与人工鱼X₂的距离：

设人工鱼X₁为：X₁=(t₁₁,t₁₂,...,t_1i)，人工鱼X₂为：X₂=(t₂₁,t₂₂,...,t_2i)，则人工鱼X₁与X₂的距离d(X₁,X₂)由机器人的双目视觉传感器、测距传感器和位移传感器信息融合得到，表示为：

（6.4）人工鱼X₁，X₂，...，X_q，...，X_m的中心位置（即聚群的鱼群中心位置）定义：

$\mathrm{Center}(X_1,X_2,...,X_q,...,X_m)=(\frac{\underset{q=1}{\overset{m}{\mathrm{\Σ}}}{x}_{q1}}{m},\frac{\underset{q=1}{\overset{m}{\mathrm{\Σ}}}{x}_{q2}}{m},...,\frac{\underset{q=1}{\overset{m}{\mathrm{\Σ}}}{x}_{\mathrm{qj}}}{m},...,\frac{\underset{q=1}{\overset{m}{\mathrm{\Σ}}}{x}_{\mathrm{qn}}}{m})---7)$

(i＝1,2,...,m;j=1,2,...,n)

其中，Center(X₁,X₂,...,X_q,...,X_m)表示取人工鱼X₁，X₂，...，X_q，...，X_m的中心位置，选取m条人工鱼的每个分量平均值；

（6.5）改进的觅食行为：

设Xinext表示人工鱼下一个状态，Xnext表示人工鱼经审视环节得到的最优状态，Xi表示人工鱼当前状态,visual表示感知距离，step表示移动最大步长；

定义审视环节：maxfind(Xi,visual，k)表示人工鱼Xi在通过双目视觉传感器和测距传感器感知距离visual范围内通过k次审视而获得k个状态，并从k个状态中找出最优状态；

则，改进的觅食行为即：

X_next=max find(X_i,visual,k)； 8）

$X_{\mathrm{inext}}=X_i+\mathrm{rand}\left(\right)*\mathrm{step}*\frac{X_{\mathrm{next}}-X_i}{\left|\right|X_{\mathrm{next}}-X_i\left|\right|},\mathrm{rand}\left(\right)\∈\left(\mathrm{0,1}\right);---9);$

（7）通过运用改进觅食行为的人工鱼群算法对Dijkstra算法求得的最短机器人移动路径进一步的优化，获得全局最短路径；优化步骤如下所述：

（7.1）人工鱼的数量一般在10至100条之间，对每条人工鱼做随机初始赋值，每条人工鱼的每个变量取值范围是[0，1]；

（7.2）评估每条人工鱼三种行为（即觅食行为、聚群行为、追尾行为）的优劣，选择三种行为中最优行为，再用执行这一行为后的状态与当前状态对比；如果优于当前状态则执行此行为，否则执行随机行为（即随机移动）；

（7.3）检验终止条件，如果迭代次数达到规定次数，或收敛精度达到规定精度，则搜索终止。

根据机器人路径规划方法设计的专用装置，它包括一个能执行人工鱼群算法的机器人、机器人视觉系统组成；其中机器人视觉系统由一个双目视觉传感器、一个位移传感器、一个测距传感器组成；

双目视觉传感器用于获取前方图像和利用图像中物体的形状变化情况估计机器人与障碍物的距离；

位移传感器用于测量机器人移动位移；

测距传感器用于测量机器人与障碍物的距离。

本发明还可以：

所述的适应函数全局优化搜索，再对聚群行为、追尾行为做具体描述，设在人工鱼i通过双目视觉传感器和测距传感器感知距离visual范围内的人工鱼数量为m，则用X′_inext表示执行聚群行为后的状态，用X″_inext表示执行追尾行为后的状态；具体公式表示如下：

$X_{\mathrm{inext}}^{\′}=X_i+\mathrm{rand}\left(\right)*\mathrm{step}*\frac{\mathrm{Center}(X_1,X_2,...,X_m)-X_i}{\left|\right|\mathrm{Center}(X_1,X_2,...,X_m)-X_i\left|\right|};---10)$

$X_{\mathrm{inext}}^{\′\′}=X_i+\mathrm{rand}\left(\right)*\mathrm{step}*\frac{\mathrm{Best}(X_1,X_2,...,X_m)-X_i}{\left|\right|\mathrm{Best}(X_1,X_2,...,X_m)-X_i\left|\right|};---11)$

其中，Best(X₁,X₂,...,X_m)表示搜寻感知范围内的最优个体。

与现有技术相比，本发明的有益效果是：

本发明基于环境信息完全已知的全局路径规划，利用相关的算法求解最短机器人路径。首先运用自由空间法建立机器人移动路径链接图模型，构建的链接图模型有多条机器人移动网络构成。然后再通过Dijkstra算法搜寻移动网络的机器人移动最短路径，最后用改进觅食行为的人工鱼群算法对Dijkstra算法搜寻到的最短路径进一步优化。

原人工鱼群算的觅食行为只对周围环境随机搜索一次，然后用搜索到状态与当前状态对比，再判断是执行觅食行为，还是执行随机行为。这样设计的觅食行为随机性强，不能有效反映周围环境状态。为了能部分反映周围的环境状态本发明对觅食行为做一些改进，改进方法是在觅食行为中增加审视环节。将随机搜索一次改为随机搜索k次，然后对比这k次环境状态，找出最优的状态。通过对比最优状态与当前状态，再判断是否执行觅食行为。本发明将改进的随机搜索环节称为审视环节，审视环节搜索次数与适应函数有关。而适应函数的复杂度与输入变量的维数有关，当输入变量维数高于10维时适应函数为复杂函数，适应函数越复杂随机搜索次数k就越大，但k越大算法运行速度也就越慢。一般输入变量低于10维时，随机搜索次数k在5至50次，此时算法整体搜索效率高。

附图说明

图1表示基于自由空间法建立的环境模型

其中黑色多边形为障碍物，点线为自由链接线，机器人自由运动路线由各自由链接线上点的连线构成。如图所示虚线表示机器人自由运动网络。

图2表示路径编码

图3表示Dijkstra算法求得的最短机器人运动路径和通过改进觅食行为的人工鱼群算法优化的机器人最短运动路径。

其中黑色连线代表Dijkstra算法求得的最短机器人运动路径，红色连线代表通过改进觅食行为的人工鱼群算法优化的机器人最短运动路径。

图4表示原鱼群算法收敛性图

图5表示改进觅食行为的鱼群算法收敛性图

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

一种机器人路径规划装置，它包括一个能执行人工鱼群算法的机器人、机器人视觉系统组成；其中机器人视觉系统由一个双目视觉传感器、一个位移传感器、一个测距传感器组成；双目视觉传感器用于获取前方图像和利用图像中物体的形状变化情况估计机器人与障碍物的距离；位移传感器用于测量机器人移动位移；测距传感器用于测量机器人与障碍物的距离。

该装置按照以下方法完成路径规划，具体方法如下：

步骤一：机器人路径规划基于环境信息完全已知的全局路径规划；在机器人路径中把机器人简化成一个点，并在二维空间内运动，机器人通过双目视觉传感器和测距传感器感知自身当前的位姿和障碍物的位置；

步骤二：将双目视觉传感器和测距传感器采集的环境信息中的障碍物处理成凸多边形。

步骤三：构建基于自由空间法的机器人移动链接图模型。如图1所示机器人的起始位置：start（0,0），终点位置：goal（5.5，4.5）。其中黑色多边形为障碍物,以障碍物顶点为端点的点线表示自由链接线，机器人自由移动路线则由各自由链接线中点的连线构成。如图1所示连接各自由链接线中点的虚线表示机器人自由移动网络，链接线中点代表该链接线的路径点；

步骤四：通过Dijkstra算法求出机器人自由移动网络的最短路径。首先通过Dijkstra算法求得最短机器人移动路径：P0,P1,…,Pn,Pn+1。如图1所示路径：start—>2—>3—>4—>5—>21—>goal为Dijkstra算法求得的最短路径，其对应的路径编码为：P0=start，P1=2，P2=3，P3=4，P5=5，P6=21，P7=goal；

步骤五：结合图2建立机器人路径编码数学模型。如图2所示编码Pi（i=1,2，...，n）表示自由链接线上的点，其中P_i1、P_i2为链接线的两端点，线段P_i1P_i2表示链接线。则路径编码模型为：

$\left\{\begin{array}{c}{P}_i=P_{i1}+(P_{i2}-P_{i1})t_i,\\ t_i\∈\left[\mathrm{0,1}\right],\\ i=\mathrm{1,2},...,n\end{array}\right.---2)$

最短路径编码对应的链接线端点分别为：P11=（0.80,1.00），P12=（0.50,2.00）,P21=（2.00,3.20），P22=（1.10,3.50）,P31=（2.60,4.00），P32=（2.30,5.00），P41=（3.40,4.20），P42=（4.10，5.10），P51=（4.60,4.90），P52=（5.00,4.50）；

步骤六：该最短路径对应的路径点编号：[P1,P2,P3,P4,P5]=[2,3,4,5,21]。用（2）式对最短路径做成编码序列，则对于的编码序列:[P1,P2,P3,P4,P5]=[t1,t2,t3,t4,t5],t_i∈[0,1],i＝1,2,3,4,5。通过双目视觉传感器和测距传感器搜索t_i的值，达到优化路径的目的。

步骤七：利用该觅食行为的人工鱼群算法对Dijkstra算法求出的最短路径进一步优化。

（7.1）人工鱼个体表示为：X=[t1,t2,t3,t4,t5]；

（7.2）人工鱼个数为50条，表示为m=50。最大移动步长step=0.2，感知距离visual=0.5。鱼群拥挤度delta=9，审视次数k=10，迭代次数gen=30，人工鱼的初值在[0,1]的随机实数，公告板的适应值为正无穷大。

（7.3）利用（9）式确定执行觅食行为的下一状态X_inext，利用(10)式确定执行聚群行为的下一状态X′_inext，利用（11）式确定执行追尾行为的下一状态X″_inext。从X_inext、X′_inext、X″_inext三个状态中选出最优状态与当前状态对比。若最优状态优于当前状态，则执行对于的行为，否则执行随机行为。

（7.4）利用（5）式计算每条人工鱼的适应值。并对比公告板的适应值，若优于公告板的适应值，则更新公告板的适应值与路径编码序列。

（7.5）判断迭代次数是否达到规定值，或是否达到规定的精度。若是则终止迭代输出公告板适应值与路径编码序列，若否则回到（7.3）继续进行搜索。

步骤八：根据优化输出优化后的路径编码序列，利用（2）式将路径编码序列转化为路径点。机器人根据路径点的描述，利用自身的视觉系统完成起点到终点最短路径的移动。

如图3所示虚线连接线表示Dijkstra算法搜寻到的最短机器人移动路径，该路径总长为8.5122。如图3所示实线连接线是改进觅食行为的人工鱼群算法优化后的机器人移动路径，该路径总长为7.966764。从图3中可知改进觅食行为的人工鱼群算法的优化效果很明显。

从图4中可知原人工鱼群算法达到收敛值需要迭代20次。从图5中可知改进觅食行为的人工鱼群算法达到收敛值需要迭代9次。改进觅食行为的人工鱼群算法的收敛速度明显提高，搜索效率得到明显提高。

Claims (2)

1.一种机器人路径规划方法，其特征在于，它包括如下步骤：

(1)机器人路径规划基于环境信息完全已知的全局路径规划；在机器人路径中把机器人简化成一个点，并在二维空间内运动，机器人通过双目视觉传感器和测距传感器感知自身当前的位姿和障碍物的位置；

(2)将双目视觉传感器和测距传感器采集的环境信息中的障碍物处理成凸多边形；

(3)构建基于自由空间法的机器人移动链接图模型；

设机器人的起始位置用start表示，终点位置用goal表示；用黑色多边形代替障碍物,以多边形顶点为端点的连线表示链接线，机器人自由移动路线由链接线中点的连线构成，连接各链接线中点组成机器人自由移动网络；

(4)通过Dijkstra算法求出机器人自由移动网络最短路径；

设链接线的中点i表示该链接线的路径点i，则取链接线上的路径点标识序列数作为路径编码，如果起始点或终点不在链接线上，则连接起始点或终点与最近链接线中点连线构成加入起始点或终点的新机器人自由移动网络；设路径点i与路径点j的权值为W_ij,则对应权值的表达式为：

$W_{ij}=\left\{\begin{array}{c}{V}_{ij},i\≠j\\ 0,i=j\end{array}\right.;---1)$

其中V_ij代表路径点i、j在自由移动网络中的对应线段，如果在自由移动网络中不存在对应线段，则V_ij＝∞；通过用Dijkstra算法搜寻起始点到终点的最优有效权值组合，获得最短机器人移动路径；

(5)建立机器人路径编码数学模型；

设路径编码P_i表示路径点i所在链接线上任意一点，其中，i＝1,2，...，n；则P_i的编码模型如下所示：

$\left\{\begin{array}{c}{P}_i-P_{i1}+(P_{i2}-P_{i1})t_i,\\ t_i\∈\[0,1\],\\ i=1,2,...,n\end{array}\right.---2)$

其中P_i1、P_i2表示链接线两端点，线段P_i1P_i2表示链接线；对其它路径点进行同样处理，得到对应路径点表达形式；路径编码P_i对应的路径编码序列为：P＝t₁,t₂,…,t_i,…,t_n，其中t_i∈[0,1]，每一个编码系列只有对应一个机器人路径；通过搜索t_i值，可以找到最优的路径编码；

(6)基于改进觅食行为的人工鱼群算法机器人路径优化模型；

(6.1)人工鱼模型：

利用路径编码方法生成m条人工鱼，人工鱼X_q表示为：

$\left\{\begin{array}{c}{X}_q=(t_{q1},t_{q2},...,i_{qi},...,t_{qn}),q\∈(1,2,\mathrm{...},m)\\ t_{qi}\∈\[0,1\],i\∈(1,2,...,n)\end{array}\right.---3)$

每条人工鱼有n个可变分量，且每个分量表示链接线上一点；

(6.2)适应值函数：

由于P_i＝P_i1+(P_i2-P_i1)t_i，则P_i点坐标表示成：

$\left\{\begin{array}{c}{x}_i=x_{i1}+(x_{i2}-x_{i1})t_i\\ y_i=y_{i1}+(y_{i2}-y_{i1})t_i\end{array}\right.---4)$

因此描述路径长度的适应函数为：

其中，x_i1表示第i条鱼在第一个点的x位置，y_i1表示第i条鱼在第一个点的y位置，x_i2表示第i条鱼在第二个点的x位置，y_i2表示第i条鱼在第二个点的y位置；||P_iP_i-1||表示P_i和P_i-1两点的距离；

(6.3)定义人工鱼X₁与人工鱼X₂的距离：

设人工鱼X₁为：X₁＝(t₁₁,t₁₂,...,t_1i)，人工鱼X₂为：X₂＝(t₂₁,t₂₂,...,t_2i)，则人工鱼X₁与X₂的距离d由机器人的双目视觉传感器、测距传感器和位移传感器信息融合得到，表示为：

(6.4)人工鱼X₁,X₂,…,X_q,…,X_m的聚群的鱼群中心位置定义：

$Center(X_1,X_2,\mathrm{...},X_q,\mathrm{...},X_m)=\left(\frac{\underset{q=1}{\overset{m}{\Σ}}{x}_{q1}}{m},\frac{\underset{q=1}{\overset{m}{\Σ}}{x}_{q2}}{m},\mathrm{...},\frac{\underset{q=1}{\overset{m}{\Σ}}{x}_{qj}}{m},\mathrm{...},\frac{\underset{q=1}{\overset{m}{\Σ}}{x}_{qn}}{m}\right)---7)$

其中，i＝1,2，...，m；j＝i＝1,2，...，n；Center(X₁,X₂,...,X_q,...,X_m)表示取人工鱼X₁,X₂,…,X_q,…,X_m的鱼群中心位置，选取m条人工鱼的每个分量平均值；x_q1表示第q条鱼的第1个元素，x_q2表示第q条鱼的第2个元素，x_qj表示第q条鱼的第j个元素，x_qn表示第q条鱼的第n个元素；

(6.5)改进的觅食行为：

设X_inext表示人工鱼下一个状态，X_next表示人工鱼经审视环节得到的最优状态，X_i表示人工鱼当前状态,visual表示感知距离，step表示移动最大步长；

定义审视环节：maxfind(X_i,visual,k)表示人工鱼X_i在通过双目视觉传感器和测距传感器感知距离visual范围内通过k次审视而获得k个状态，并从k个状态中找出最优状态；

则，改进的觅食行为即：

X_next＝maxfind(X_i,visual,k)； 8)

$X_{inext}=X_i+rand\left(\right)*step*\frac{X_{next}-X_i}{\left|\right|X_{next}-X_i\left|\right|},rand\left(\right)\∈(0,1);---9);$

设在人工鱼X_i通过双目视觉传感器和测距传感器感知距离visual范围内的人工鱼数量为m，则用X′_inext表示执行聚群行为后的状态，用X"_inext表示执行追尾行为后的状态；具体公式表示如下：

$X_{inext}^{\′}=X_i+rand\left(\right)*step*\frac{Center(X_1,X_2,...,X_m)-X_i}{\left|\right|Center(X_1,X_2,...,X_m)-X_i\left|\right|};---10)$

$X_{inext}^{\′\′}=X_i+rand\left(\right)*step*\frac{Bext(X_1,X_2,...,X_m)-X_i}{\left|\right|Bext(X_1,X_2,...,X_m)-X_i\left|\right|};---11)$

其中，Best(X₁,X₂,...,X_m)表示搜寻感知范围内的最优个体；

(7)通过运用改进觅食行为的人工鱼群算法对Dijkstra算法求得的最短机器人移动路径进一步的优化，获得全局最短路径；优化步骤如下所述：

(7.1)人工鱼的数量在10至100条之间，对每条人工鱼做随机初始赋值，每条人工鱼的每个变量取值范围是[0,1]；

(7.2)评估每条人工鱼三种行为的优劣，其中，三种行为包括觅食行为、聚群行为、追尾行为；选择三种行为中最优行为，再用执行这一行为后的状态与当前状态对比；如果优于当前状态则执行此行为，否则执行随机行为；

(7.3)检验终止条件，如果迭代次数达到规定次数，或收敛精度达到规定精度，则搜索终止。

2.一种用于实现如权利要求1中所述的机器人路径规划方法的装置，其特征在于，所述装置包括一个能执行人工鱼群算法的机器人、机器人视觉系统；其中机器人视觉系统由一个双目视觉传感器、一个位移传感器、一个测距传感器组成；

双目视觉传感器用于获取前方图像和利用图像中物体的形状变化情况估计机器人与障碍物的距离；

位移传感器用于测量机器人移动位移；

测距传感器用于测量机器人与障碍物的距离。