机器人避障问题
一、摘要
本文讨论了机器人在平面场景中避障行走的问题，已知机器人的行走模式（直线与相切圆弧）以及场景障碍物的分布,计算出到平面各个给定点的最短路径，以及到A 点的最短时间.
文中,首先,考虑到机器人与障碍物之间有10个单位的碰撞距离，故用C ＡＤ软件将平面场景图进行改进，再用ＣAD 设计可能的最短路径。接着，对每条具体路径进行分解,得到三种基本线圆形模型（点圆模型,双圆异侧模型，双圆同侧模型），对这三种模型进行求解，得到各个模型直线长度以及转弯圆弧圆形角的表达公式.之后,参照具体的行走路径,构造合适的行走矩阵，用以判断每段路径所属的基本模型。路径总的长度可用如下公式表达：
12
,1,1,2
1
1
N N i i i i i i i s m r θ--+++===+?∑∑
最后，通过计算设计的集中可能的最短路径，我们得到每段的最短路径的长度分别为：
Ｏ——A 路段:４７1。０3７２(单位）； O ——B 路段： 8５3。70０1（单位）； O ——Ｃ路段： 3100915.1?(单位);
O-—A —-B-—C ——Ｏ路段: 3
2.677810?（单位）.
对于问题二，我们在问题一的基础上分别利用直线最大速度和转弯最大速度计算出时间的表达式。为了方便计算，我们将转弯圆弧的圆心定在P （８0,210）（场景中正方形5的左上角），这样得到时间Ｔ与转弯半径ρ的函数关系式:
2
100.10
(1)(2arccos
arccos
)
e
a
b
T v ρρ
ρ
πα-?+?---=
通过M ＡT ＬAB 编程，画出其图像，求解得出：当半径ρ=1１.435时，时间T 最小，其大小为9４.5649（秒）。
关键词：最短路径 线圆模型 行走矩阵 ＭATLAB
二、问题重述
在一个800×800的平面场景图（见附录一),在原点Ｏ（0， 0）点处有一个机器人，它只能在
该平面场景范围内活动。图中有12个不同形状的区域是机器人不能与之发生碰撞的障碍物，障碍物的数学描述如下表：
在图中的平面场景中,障碍物外指定一点为机器人要到达的目标点（要求目标点与障碍物的距离至少超过10个单位）。规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径。机器人不能折线转弯，转弯路径由与直线路径相切的一段圆弧组成，也可以由两个或多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位。为了不与障碍物发生碰撞，同时要求机器人行走线路与障碍物间的最近距离为10个单位,否则将发生碰撞，若碰撞发生，则机器人无法完成行走。 机器人直线行走的最大速度为 50=v 个单位/秒。机器人转弯时，最大转弯速度为
2
1.0100
e
1)(ρρ-+=
=v v v ，其中 是转弯半径.如果超过该速度,机器人将发生侧翻,无法完成行走。
现需建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模型.对场景图中4个点O （0， 0），A （30０, 3０0），Ｂ(1０0， ７0０），C （7０0, ６４0）,具体计算：
(1） 机器人从O(0， 0)出发，O →Ａ、O →B 、O →C 和O →A →B →Ｃ→O 的最短路径。 （2) 机器人从O (０， 0)出发，到达A 的最短时间路径。
并要求给出路径中每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标以及机器人行走的总
距离和总时间。
三、模型假设
1、假设机器人可看做一个质点，不考虑其实际大小;
2、假设机器人能够准确的按照设计的路线行走行走,在其行走中不发生任何突发事故； ３、机器人以最大速度行驶，在转弯过程中没有发生侧翻，速度发生突变，不考虑加速减速。
四、符号说明
ij
l :圆i O 到圆j O 圆心的距离；
,1,2i i i θ++：机器人经过的第i 个圆弧的圆心角；
,1i i ?+：第i 段直线多对应的圆心角
r :圆的半径；
,1i i m +：机器人经过的第ｉ段直线的长度;
S :行走路径的总的长度; T ：机器人行走的总的时间
N ：机器人行走经过的总点数（包括起点终点以及转弯圆弧的圆心）； A ：行走矩阵。
五、模型建立
对于该题建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模型的研究,主要是用尽可能短的路径和时间避开障碍物到达目标点。根据题目中的要求可知,机器人行走线路与障碍物间的最近距离为10个单位，否则将发生碰撞,若碰撞发生,则机器人无法完成行走，故可把各障碍物的边界扩大1０单位。利用CAD 软件制图可在距每个障碍物的边缘１0个单位处添加外边框，形成新的屏障，特别注意的是在障碍物顶点处使用圆弧。机器人就可在新屏障外的范围内随意活动，不用担心发生碰撞,根据原图修改后的图形见下图１。
机器人行驶路线是由直线段和圆弧组成。机器人从Ｏ（0, ０)出发,建立数学模型求得Ｏ→A 、O →B 、Ｏ→Ｃ和Ｏ→A →B →C →Ｏ的最短路径.现将各个路段的情况进行综合分析，根据每个路段所遇到的情况,从起始点到目标点的最短距离应该是直线段与圆弧组成，由已知的数学知识,两点之间线段最短,故机器人走的直线越多,路径越短，也就是说当机器人绕过障碍物的时候,半径越小,路径越短，根据题意,转弯半径可按最小半径１0来计算，经过分析，可建立如下三个线圆模型。
模型一：（点圆模型)
该模型主要求解1O -—2O 的路径长度,该路径只有一个转弯,由图2易知，
12CO DC D O S ++=，C 、Ｄ均为直线与圆的切点，CD 为圆弧，θ为圆弧对应的圆心角的大小，
根据已积累的知识,圆弧的长度为圆弧对应的圆心角与圆半径的乘积,即r ?θ,同时利用余弦定理
ab
c b a C 2cos 222-+=，即可求得总距离为：
B(100, 700)
C(700, 640)
图1
A （300,300）
Ｓ＝r r l r l ?+-+-θ2
2122213；
其中 θ=-π2???
? ??-+++12132
2321221312132arccos arccos arccos l l l l l l r
l r
模型二：（双圆异侧模型)
该模型是为了计算1O ——4O 的距离,从起始点到目标点经过圆弧异侧拐弯(如图
3），根据已知点1O 、2O 、3O 、4O 可求得ij l 的长度。41DO CD BC AB A O S ++++=，AB 、CD 为两段圆弧，1θ、2θ为其对应的圆心角，BC 与2O 3O 的交点E 是这两条线段的中点，根据两个全等三角形以及勾股定理，可求得BC 长度.
r r S r l r l r l ?+
-+-+
?+-=
??
? ??θθ2
2
2
2
23412
2122232
其中,
222
122313
11223122322arccos arccos arccos 2r r l l l l l l l πθ
????+-=-++?? ? ???????
???
?????+++???? ??+-=l l l l l l l r r 34232
24234223233422arccos 2arccos arccos 2πθ
1O
2O
C
D
图2
3O
模型三：（双圆同侧模型）
该模型是为了计算1O -—4O 的距离，从起始点到目标点经过圆弧同侧拐弯（如图
4），添
加辅助线，连接1O 3O 、2O 4O ，运用几何知识轻易能够证明32//O O EF 。由图４易知
41CO FC EF DE D O S ++++=，可以根据已知点的坐标求出所需线段的长度，进而求得起始点
到目标点的总距离。
22
3422312212r l r l r r l S -+?++?+-=θθ
其中，)2arccos 2arccos arccos 2(232
342
242322342312213223212121l l l l l l l l l l l r
-++-+++-=π
πθ
)2arccos 2arccos arccos
2
(213
232
12
213223233422422323434
2l l l l l l l l l l l r
-++-+++-=ππθ
图3
1O
4O
2O
3O
A
B
C
D
E
综合模型：
在实际情况中,机器人所走的路线是以上三种模型的结合。设计好机器人的行走方案，可根据设计好的方案构建行走矩阵，构建方法如下：
123i A ??=???
则ｉ段的直线长度为：
22
,12
,12,1
,1
2i i i i i i i i l r
l r m l ++++?-???????- ? ?=???
??????
其中ｉ＝1，2…Ｎ—1；
1O
2O
3O 4O
C
F E
D
图4
i 段为模型1 i 段为模型2 i 段为模型3
i 段为模型1
i 段为模型2
i 段为模型3
第i 段与i+1段之间的转弯圆弧所对应的的圆心角为：
其中i=1,２…N －2；
i i i i ,11,++=??
机器人行走的总的长度为:
12
,1,1,2
1
1
N N i i i i i i i s m r θ--+++===+?∑∑
六、模型求解
问题（1）：
根据所建立的数学模型，用C ＡD 画出可能的最短路径,构建每条路线的行走矩阵，通过ＭATL ＡB 编程计算出每天路线的实际长度，从而得到最短路径.
1、O ——A 路段，这是四个路段中最简单的情况,从O 到Ａ经过了一个转弯，从图5中易看出有两种方案，虚线与实线各代表一个方案。
2
,1,12
2
,22,12,12
,1,12,1,i 2arccos
2++++++++++++-+---=i i i i i i i i i i i i i i i i l l l l l ??πθ?
??
?
?
?
?
??
=+++22arccos
arccos 1,1,1
,π?l l i i i i i i r r i 段为模型1
i 段为模型2
i 段为模型3
利用M ＡT ＬＡB 编程求解,计算结果如下: 机器人从O 到A 的行走路线长度为47１。0372；
同理，O 从下面绕到目标点A 的总的路线长度为49８。42５9；
通过比较两种方案的结果易知,机器人在点O 从上面绕到目标点A 的距离最短,期最短路线长度为471。０３7２ 。
2、Ｏ——B 路段,经过分析与整理,我们得到四种方案,如图6所示①②③④，在这四种方案中，三种模型全部都要用到，模型一在O-—Ａ路段已详细说明,模型二就是从起始点到目标点经过的圆弧在所走路径的异侧，而模型三就是从起始点到目标点经过的圆弧在所走路径的同侧，从Ｏ-—B 路段，有多次转弯，具体见图6。
A （300,300）
图5
1O （80,60）
O （0,0）
就①路线而言，机器人经过了五次转弯，根据三种模型中的理论公式,需要把各个圆弧与直线长度求得,可利用ＭＡT ＬAB 软件对其进行编程.计算结果如下:
①路线机器人行走的总距离为８53。７001; ②路线机器人行走的总距离为８７7。3841; ③路线机器人行走的总距离为990.1６08; ④路线机器人行走的总距离为3
100584.1 ；
经过比较可得①路线为最短路径，即机器人在点Ｏ从上面绕到目标点B 的距离最短，期最短路线长度为853。７001 。
3、O ——C 路段，经过整理分析，我们得到四种方案，具体见图7，
图6
B （100,700）
①
②
③
④
O （0,0）
每条路线都是由圆弧与直线段，但是该路段与其他路段相比,又有其不同之处,在③路线中有一部分是在两个不同大小的圆的异侧和同侧（如图8，图９所示）
就图8容易看出， 22
1222
12R r R Rl r r R rl M AB
-??
?
??++-??? ??+= 图7
C
（700,640）
①
②
③
④
O （0,0）
1O
2O
A
B
E
图8