基于图的理论解决机器人避障最短路径选择问题

张 琪，王一帆

（承德医学院，河北承德 067000）

随着智能机器人的广泛应用，人们对机器人避障问题从各个方面、以多种方法进行研究探讨。本文就机器人避障最短路径选择问题，用数学方法采集数据并进行计算，以有向图的理论筛选数据并建立数学模型，再用C语言按Dijkstra算法编写代码，从而实现求最短行走和最短时间路径。

1 问题提出

假设一个800×800的场景图（见图1）。机器人只能在该场景内活动。场景内有12个各种形状的障碍物，机器人不能与它们发生碰撞，障碍物的描述见附表。机器人从指定一点到达另一点，行走路径由直线段和圆弧组成。转弯的路径是由一段圆弧组成(与直线路径相切)，也可以由多于一个相切的圆弧组成，圆弧的半径最小为10个单位。要求行走的线路距离障碍物最少为10个单位，机器人行走时，直线最大速度为v0=5个单位/秒，转弯最大速度为，其中ρ是转弯行走时的半径。

要求建立机器人在场景从某点到达其它点行走路线的最短路径和最短时间路径的数学模型，并对场景图中的4个点O(0， 0)、A(300， 300)、B(100， 700)、C(700，640)进行计算：⑴机器人从O(0，0)出发，O→A、O→B、O→C和O→A→B→C→O的最短行走路径；⑵机器人从O(0，0)出发，O→A的最短时间路径。

图1 场景图

附表 障碍物描述

2 解决方法

2.1 采集数据 找出机器人从任意点出发，经最短路径到达其它各点可能经过点，给出点的坐标、弧的圆心，并求出线段长和弧长。在这里用到了初等数学中的两点间距离公式、点到直线距离公式、圆的内公切线方程和外公切线方程、求弧长公式、圆方程等。计算方法：以11个非圆型障碍物的各顶点为圆心，10为半径构造圆的方程。以2号障碍物圆心为圆心，80为半径构造圆方程。共34个圆，可能的最短路径经过22个圆。各直线段的计算方法是，从点O、A、B、C出发的直线段，先求过圆外一点到已知圆的切线，再求切点；其它位置的各线段，用与两已知圆的内公切线方程或外公切线方程，再求这条公切线与两已知圆切点；最后，以两点间距离公式求线段长度。弧长的求法：根据上面求直线段的方法，每条弧都与两条直线段相切，通过两个切点连接的一条弦，求出半径为r(r=10或r=80)所对应的弧长。这里我们经过计算，得到了满足条件的所有线段和弧长，这些线段和圆弧构成了图1中的有向网。愈求得最短路径和最短时间路径，机器人可能经过的点得到如下数据(为了计算方便，交点坐标取整数)：

（1）O到B所有可能经过的线段：

线段O(0，0)到D(70，211)，长度222.31，弧D的圆心(80，210)，弧长=9.2。

线段D(79，219)到F(235，290)，长度171.4，弧F的圆心(245，290)，弧长=15.7。

线段F (245，300)到K(230，530)，长度230.48，弧K的圆心(220，530)，弧长=13.9。

线段K(221，539)到L(151，591)，长度87.2；弧L的圆心(150，600)，弧长=12.8。

线段L到B(100，700)，长度107.71。

线段F(245，300)到G(280，680)，长度380，弧G的圆心(270，680)，弧长=15.7。

线段G(270，690)到B(100，700)，长度170.3。

线段O(0，0)到E(231，50)，长度为236.35，弧E的圆心(230，60)，弧长=13.76。

线段E(240，59)到G(280，680)，长度为622.29。

线段O(0，0)到H(40，300)，长度302.65，弧H的圆心(50，300)，弧长=12。

线段H(48，308)到I(140，435)，长度156.82，弧I的圆心(150，435)，弧长=15.7。

线段I(150，445)到J(220，460)，长度71.58；弧J的圆心(220，470)，弧长=15.7。

线段J(230，470)到K(230，530)，长度60。

（2）O到C所有可能经过的线段：

线段O(0，0)到M(412，90)，长度421.7，弧M的圆心(410，100)，弧长=11.64。

线段M(419，99)到N(489，201)，长度123.71，弧N的圆心(500，200)，弧长=12.92。

线段N(498，209)到R(721，511)，长度375.4，弧R圆心(720，520)，弧长=13.8。

线段R(730，520)到S(730，600)，长度80，弧S的圆心(720，600)，弧长=15.7。

线段S(719，610)到C(700，640)，长度31.32。

线段E(240，60)到P(391，331)，长度308.87，弧P的圆心(400，330)，弧长=12.08。

线段P(399，339)到Q(590，370)，长度193.5，弧Q的圆心(590，380)，弧长=13.8。

线段Q(599，379)到R(720，510)，长度178.33。

线段D(79，219)到P(391，331)，长度331.49。

（3）O到A所有可能经过的线段：

线段O(0，0)到E(231，50)，长度236.35，弧E的圆心(230，60)，弧长=13.76。

线段E(240，59)到A(300，300)，长度248.36。

线段O(0，0)到D(70，211)，长度222.31，弧D的圆心(80，210)，弧长=10.2。

线段D(79，220)到A(300，300)，长度235（其中O到v3、O到v1与上面重复）。

（4）A到B所有可能经过的线段：

线段A(300，300)到K(230，530)，长度264.2.(K到B以上线段已计算)。

线段A(300，300)到T(300，390)，长度90，弧T的圆心(300，400)，弧长=15.7。

线段T(290，400)到G(280，680)，长度280.18 (G到B以上线段已计算)。

（5）B到C所有可能经过的线段：

线段A(300，300)到T(300，390)，长度90，弧T的圆心(300，400)，弧长=15.7。

线段T(290，400)到G(280，680)，长度280.18 (G到B以上线段已计算)。

（6）B到C所有可能经过的线段：

线段B到G(280，680)的所有可能线段已经计算，总长为170.3+15.7，弧G的圆心(280，690)，弧长=13。

线段G(270，695)到U(360，680)，长度91.24，弧U的圆心(430，680)，弧长=13.76。

线段U(439，679)到V(531，729)，长度104.71，弧V的圆心(540，730)，弧长=9.34。

线段V(540，740)到W(670，740)，长度130，弧W的圆心(670，730)，弧长=15.7。

线段W(680，730)到C(733，400)，长度92.2。

线段G(280，680)到X(501，609)，长度232.12，弧X的圆心(500，600)，弧长=11.88。

线段X(509，601)到Y(631，519)，长度147，弧Y的圆心(640，520)，弧长=13.72。

线段Y(640，510)到R(720，510)，长度80(R到C上面已计算)。

2.2 筛选数据—建立数学模型 根据问题找出满足条件的所有线段和弧，构成一有向图，将入度和出度均为1的连续线段的长度相加，归并为一条弧（图论中将有向段称为弧）。可以由以上有向图中的各点找出其入度或出度不是1的点，构成以下（拓扑）有向图（网）[1]，即图2（根据问题，如需要，弧均为双向，图中粗线是双向）。

图2 有向图

根据图2得到如下数据：

线段点O到点K，长度568.11。

线段点O到点D，长度232.51。

线段点O到点E，长度250.11。

线段点O到点R，长度1085.9。

线段点D到点F，长度187.1。

线段点D到点P，长度331.49。

线段点D到点A，长度235。

线段点E到点A，长度248.36。

线段点E到点G(260，700)，长度620.2。

线段点E到点P，长度308.87。

线段点F到点K，长度230.48。

线段点F到点G(260，700)，长度377。

线段点G(280，680)到点B，长度185.12。

线段点G(260，700)到点R，长度498.72。

线段点G(270，690)到点C，长度521.41。

线段点B到点G(280，680)，长度185.29。

线段点G(260，700)到点C，长度525.96。

线段点R到点C，长度140.82。

线段点K到点B，长度221.61。

线段点P到点R长度383.91。

线段点A到点K长度264.2。

2.3 选择最短行走路径 根据图论的Dijkstra算法[2]，采用C语言编程[3]（邻接矩阵存储）。由于完整程序已经正确运行，由于篇幅有限，仅给出求最小路径函数代码。主要代码：

作为本程序的一个例子，运行程序，将所得数据按要求输入，给出各点间的最短路径。程序运行结果：

O-＞A 最短行走路径为：O-＞D-＞A，长度467.51。

O-＞C 最短行走路径为：O-＞R-＞C，长度1085.9。

O-＞B 最短行走路径为：O-＞K-＞B，长度789.71(O-＞K 568.11，K-＞B 221.61)。

A-＞B 最短行走路径为：A-＞K-＞B，长度485.81(A-＞K 264.2，K-＞B 221.61)。

B-＞C 最短行走路径为：B-＞G-＞C，长度691.7(B-＞G 170，G-＞C 521.41)。

2.4 计算最短时间路径 根据公式：距离=时间×速度（S=T×V）。在直线段上，T=S/V0 (V0=5)， 每段线段长除以V0即为最短时间。在弧上，按速度公式，求得V(ρ)=V0/2 (ρ=10)，每段弧长除以V(ρ)即为最短时间。

数学模型仍是图2，以O到A最短时间路径为例，求得数据为：

线段O(0，0)到E(231，50)，时间47.27；弧E的圆心(230，60)，时间=5.504。

线段E(240，59)到A(300，300)，时间49.672。

O-＞E-＞A，时间102.446。

线段O(0，0)到D(70，210)，时间44.46；弧D的圆心(80，210)，时间=4.008。

线段D(79，220)到A(300，300)，时间47。其中O到E(231，50)，O到D(70，210)同上面。

O-＞D-＞A，时间95.468。

得O-＞A的最短时间路径为：O-＞D-＞A，最短时间为95.468。

若想求得所有最短时间路径，如上方法计算，运行程序便得到结果。

3 结论

在解决机器人避障这个问题时，人们常用拟合的方法建立数学模型，利用穷举法或者启发式算法求解。这里用了精确的数学方法建立数学模型，并且应用图的最短路径方法，以C语言编程，将这个问题作为一个例子来处理，给出了解决整个问题的完整而又精确的数学方法，希望对相关业者有所帮助。

【参考文献】

[1]张琪，高红亚.与障碍问题有关的一些正则性结果[J].应用数学，2017，30(3)：644-651.

[2]严蔚敏.数据结构[M].北京：清华大学出版社，2007.187-189.

[3]谭浩强.C语言程序设计[M].北京：清华大学出版社，2008.110-112.