机器人避障问题
摘要
针对机器人避障问题,本文分别建立了机器人从区域中一点到达另一点的避障的最
短路径、最短时间路径的非线性 0-1 整数规划模型。同时,本文为求带有 NP 属性的非
线性 0-1 整数规划模型,构建了有效启发式算法,利用 MATLAB 软件编程,求得了 O→A、
O→B、O→C、O→A→B→A→C 的最短路径,同时得到了 O→A 的最短时间路径,求得的各
类最短路径均是全局最优。
针对区域中一点到达另一点的避障的最短路径问题,首先,本文证明了圆弧位置设
定在需要绕过障碍物的顶角上,且圆弧半径为 10 个单位时,能够使得机器人从区域中
一点到达另一点的行进路径最短;其次,本文将最短路径选择问题转化成了最短路径的
优选问题,根据避障条件,建立了具有较高普适性的避障最短路径的优化模型。为便于
求解,本文巧妙地将此优化模型转化成了以可行路径不与障碍物边界相交、不与圆弧相
交为约束条件,以机器人从区域中一点达到另一点避障路径最短为目标的 0-1 规划模型;
再次,本文构建了两种有效的启发式算法,利用 MATLAB 软件编程求得了 O→A、O→B、O
→C、O→A→B→A→C 的最短路径,最短路径长分别为 471.0372、853.7001、1088.1952、
2725.1596,其中 O-->A 的最短路径为(0,0)→(70.5063,213.1405) →(75.975,219.1542)
→(300,300),对应圆弧的圆心坐标为(80,210),O→B 的最短路径,对应圆弧的圆心坐
标:(60,300)、(150,435)、(220、470)、(220,530)、(150,600), O→C 经过的圆心:
(410,100)、(230,60)、(720,520),(720,600),(500,200), O→A→B→C→O 经过的圆
心:(410,100),(230,60), (80,210),(220,530),(150,600),(270,680),(370,680),
(430,680),(670,730),(540,730),(720,520),(720,600),(500,200)。
针对最短时间路径问题,我们建立了从 o 点出发到任意目标点的 0-1 非线性整数规
划模型,同时针对题意要求,具体构建了从 o 点出发到 A 的最短时间路径的 0-1 非线性
整数规划模型,利用 LINGO 软件求解,获得了机器人从 o 点出发,到达 A 的最短时间路
径,求得最短时间路径下转弯半径为 12.9885 ,同时最短时间路径时间长为 94.2283
个单位。相应圆弧的圆心坐标为(82.1414,207.9153),两切点坐标分别为(69.8045,
211.9779)、(77.7492,220.1387)。
关键字:机器人避障 启发式算法 0-1 规划模型
一、问题重述
在一个800×800的平面场景图,在原点O(0, 0)点处有一个机器人,它只能在该平面
场景范围内活动。图中有12个不同形状的区域是机器人不能与之发生碰撞的障碍物,障
碍物的数学描述如下表:
编号 障碍物名称 左下顶点坐标
其它特性描述
(300, 400)
边长200
圆心坐标(550, 450),半径70
底边长140,左上顶点坐标(400, 330)
上顶点坐标(345, 210),右下顶点坐标(410, 100)
边长150
上顶点坐标(150, 435),右下顶点坐标(235, 300)
长220,宽60
底边长90,左上顶点坐标(180, 680)
长60,宽120
边长130
边长80
长300,宽60
正方形
1
圆形
2
平行四边形 (360, 240)
3
(280, 100)
三角形
4
(80, 60)
正方形
5
三角形
(60, 300)
6
长方形
(0, 470)
7
平行四边形 (150, 600)
8
长方形
(370, 680)
9
(540, 600)
10 正方形
(640, 520)
11 正方形
12 长方形
(500, 140)
在图1的平面场景中,障碍物外指定一点为机器人要到达的目标点(要求目标点与
障碍物的距离至少超过10个单位)。规定机器人的行走路径由直线段和圆弧组成,其中
圆弧是机器人转弯路径。机器人不能折线转弯,转弯路径由与直线路径相切的一段圆弧
组成,也可以由两个或多个相切的圆弧路径组成,但每个圆弧的半径最小为10个单位。
为了不与障碍物发生碰撞,同时要求机器人行走线路与障碍物间的最近距离为10个单
位,否则将发生碰撞,若碰撞发生,则机器人无法完成行走。
机器人直线行走的最大速度为 5
0 v
(
)
v
v
完成行走。
v
0
10
e1
21.0
个单位/秒。机器人转弯时,最大转弯速度为
,其中是转弯半径。如果超过该速度,机器人将发生侧翻,无法
请建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模
型。对场景图中4个点O(0, 0),A(300, 300),B(100, 700),C(700, 640),具体计算:
(1) 机器人从O(0, 0)出发,O→A、O→B、O→C和O→A→B→C→O的最短路径。
(2) 机器人从O (0, 0)出发,到达A的最短时间路径。
注:要给出路径中每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标以及机器
人行走的总距离和总时间。
2.1 求取最短路径的分析
二、问题分析
本问题要求机器人从区域中一点到达另一点的避障最短路径。机器人只要做到转弯
时的圆弧半径最小为 10 个单位、与障碍物最近距离单时刻保持大于 10 个单位,那么可
行走的路径就有无数条,若想求得机器人从区域中一点到达另一点的避障最短路径,则
需要建立避障的最短路径模型,而建立避障的最短路径模型有一定难度。根据对问题的
分析,我们认为可以从简单做起,先确定小范围内最短路径条件,如圆弧位置的影响,
圆弧半径的大小,避免与障碍物碰撞条件等,通过确定最短路径条件来建立避障的最短
路径模型。对于最短路径的求取,我们可以通过确定穷举原则,利用穷举法来求解,当
然也可以通过构建启发式算法的进行求解。
2.2 最短时间路径的分析
对于要建立最短时间路径模型来说,我们容易知道影响的因素有直线行走速度、转
弯速度,同时还需要考虑使得最短时间路径条件,如圆弧位置(坐标)的影响,圆弧半
径的大小,避免与障碍物碰撞条件等。对于直线行进,我们希望行进速度越大越好,对
于机器人转弯时,转弯速度要有约束,要保证机器人不能发生侧翻。我们发现圆弧半径
的大小与转弯速度紧密相连,从转弯速度公式来分析,当转弯半径增大时,最大转弯速
度也增大,为在更短时间内行进到目标点,我们希望转弯速度为机器人的最大转弯速度
较好,但有很大的可能是行进的路径不是最短的,即行进路径有很大可能在增加。于是,
我们需要做的工作是,在满足最短时间路径条件时,找到一个圆弧的坐标位置,同时确
定半径的大小,以求得最短时间路径。
三、模型假设
1.将机器人看成一个质点;
2.半径不变时,机器人在行进、转弯过程中能一直保持最大的速度;
3.
四、符号说明
i
,
j
;
j
i
,36
...1
,
nn
Z :避障最短路径;
ija :圆弧i 切点到圆弧 j 切点的直线距离,即机器人从圆弧i 切点到圆弧 j 切点的直线路
径,
ijb :机器人从圆弧i 行进至圆弧 j 切点时的转弯路径,
ix :圆弧i 的横坐标,
iy :圆弧i 的纵坐标,
:圆弧对应圆心角;
:圆弧的半径;
36..1i
36..1j
...1
,
nn
;
,36
i
i
,
j
j
;
五、最短路径模型建立与求解
5.1 模型准备
5.1.1 确定圆弧位置与转弯半径
在建立机器人从区域中一点到达另一点的避障最短路径数学模型之前,我们需要考
虑两个问题:
问题一:机器人从区域中一点到达另一点过程中,若中间有障碍物,则需要通过转
弯来绕过障碍物,那么,在转弯半径一定的情况下,怎样设定最佳圆弧位置,使得绕行
路径最短?
问题二:绕行路径是最短时,转弯半径的大小为多少?
针对考虑的问题一,我们取机器人从O 到 A 点的行走过程来说明问题。在行走过程
中要求机器人行走线路与障碍物的最短距离为 10 个单位,圆弧(转弯)半径最小为 10
个单位。我们先令机器人转弯半径为 10 个单位,根据机器人行走过程中的要求,我们
易得两条极端的行走路径,如图 1。将路线 II 中圆弧 3 两切点线延长,两延长交路线 I,
两交点处分别作半径为 10 个单位的圆弧,由此我们可得机器人从O 到 A 点的行走时转
弯中心坐标的范围,如图 2 中四边形 abcd 。
图 1 两条极端路径
图 2 转弯中心坐标的范围
800
图 1 中路线 I 是理想化路线,机器人不能沿
800
平面区域边界也可以看成是一个障碍物,且有要求机器人行走线路与障碍物的最短距离
为 10 个单位,实际上作这样的处理并不会影响我们说明问题。
800 平面区域边界行走,
800
我们假设在平面中有
),0( aA
和
( AO 两点,中间有一正方形的障碍物,将图 2 进
)0,
行转化,如图 3.
图 3 最短路径证明图
,
ICB ...
,
为切点,
为圆弧圆心,四边形 abcd 为圆弧中心点的范围。
图 3 中,
对于最佳圆弧位置确定,我们采用“覆盖法”。我们容易知道,若路线 II 与OA 构
成的区域 II 能够完整覆盖线 I 与OA 构成的区域 I,即区域 I 属于区域 II,那么区域
II 的周长一定大于区域 I,否则。
,
,
dcba
图 3 中路线 I 与OA 构成的区域 I 周长为直线段
OB 长度、圆弧 BC 长、OA 长
CA
之和,区域 I 周长 1l 为
机器人沿路线 I 的路径长 1c 可表示为
OB
CA
路线 II 与 OA 构成的区域 II 周长为直线段
c
1
l
1
OB
CA
BC
OA
区域 II 周长 2l 为
l
2
OF
GA
FG
OA
机器人沿路线 II 的路径长 2c 可表示为
BC
OH 长度、圆弧 FG 长、OA 长之和,
FA
FG
显然我们知道区域 II 能够覆盖区域 I,即可得
GA
OF
c
2
l ,进而可得到
2
l
1
c
1
c
2
c 1
ic
同理,在圆弧中心点的范围任意取一点作为机器人转弯圆弧中点,并作路线i ,再
将路线i 与路线 I 做比较,可得到
由此,我们可得出结论:机器人从区域中一点到达另一点过程中,当圆弧位置设定
障碍物顶角上时,绕行路径最短,此时圆弧中心点坐标为障碍物顶角坐标。
针对考虑的问题二,为了更清晰说明绕行路径是最短时,转弯半径的大小为多少,
我们基于最小圆弧半径条件下使圆弧半径增大。为了保证机器人与障碍物不发生碰撞,
所以,需要保证大圆弧能够覆盖小圆弧对应圆的 1/4 圆弧。在设定好最佳圆弧位置情况
下,增加圆弧半径,比较最短路径的变化。假设圆弧半径为 R ( 10R
),对应最短路
线如图 4。
(1)
(2)
图 4 圆弧半径为 R 最短路径
图 4(1)中 B 为两圆弧公共切点,C 为小圆弧切点( 10r
图 4(2)中 A 、 D 为大圆弧切点, B 、C 为小圆弧切点。
), A 为大圆弧切点。
针对图 4(1)(2),根据“覆盖”思想与得出的结论,我们容易发现,当圆弧半径为
R( 10R
)时,行进路线与OA 构成的区域显然是能够完全覆盖圆弧半径为 r 时构成的
区域,由此,可说明在圆弧位置设定为最佳的条件下,圆弧半径越小行进路径越短,而
圆弧半径最小为 10 个单位,进而说明圆弧的半径为 10 个单位时,绕行路径最短。
根据考虑的两个问题与证明结果,我们可得出结论:要使得机器人从区域中一点到
达另一点的行进路径最短,应使圆弧位置设定在需要绕过障碍物的顶角上最佳,此时圆
弧中心点坐标为障碍物顶角坐标,并且此时圆弧的半径为 10 个单位。
5.1.2 问题的转化
在模型准备中我们已得出要使得机器人从区域中一点到达另一点的行进路径最短,
应使圆弧位置设定在需要绕过障碍物的顶角上最佳,此时圆弧中心点坐标为障碍物顶角
800 的平面场景图进行处
坐标,并且此时圆弧的半径为 10 个单位。因此,我们将
理,处理原则有:
800
1.每个障碍物的顶角都设定一个圆弧;
2.圆弧坐标为障碍物顶点坐标;
3.圆弧的半径设定为 10 个单位;
4.给每一段圆弧从 2... n 标号,O 点标记为 1、36。
根据处理原则,得图 5。
图 5 处理后
800 的平面场景图
800
在原问题中,若没进行确定圆弧位置与转弯半径以及平面场景的处理,原问题求解
将会很难,并且所有的转弯点均是未知,经处理后,我们将问题转化为在已知转弯点,
寻找合适的转弯点,使得路径最短,即我们将问题转化为了最短路径的优化问题。
5.2 避障最短路径模型的建立
问题转化为最短路径的优化问题后,我们易知优化的目标函数为机器人在行进过程
中最短路径,通过 0-1 变量来选取经过的转弯点,因此可建立 0-1 规划模型。
5.2.1 目标函数
目标函数为机器人出区域中一点到达另一点的避障最短路径,避障最短路径 Z 为行
进过程中直线路径与转弯路径之和,于是有
min
Z
n
n
i
1
j
1
(
aX
ij
ij
b
ij
)
(1)
其中, ija 为圆弧i 切点到圆弧 j 切点的直线距离,即机器人从圆弧i 切点到圆弧 j 切
点的直线路径; ijb 为机器人从圆弧i 行进至圆弧 j 切点时的转弯路径; ijX 为 0-1 变量,
表示若选择行走圆弧i 后行走圆弧 j , ijX 为 1,否则为 0,
...1
,36
,
nn
j
...1i
36
, k 为切点的次
为圆弧 i 的切点坐标, i 为各圆弧的编码,
。
j
i
i
,
序,
)
,
(
ik y
x
ik
,
(
i y
x
,如
1
我们设
2,1k
机器人从圆弧i 切点到圆弧 j 切点的直线路径 ija 为
2
表示为圆弧i 的第一个切点坐标。
)
1
i
2
a
ij
(
x
i
2
x
)
j
2
(
y
i
2
y
)
j
1
i
,
j
,36..1
i
j
(2)
机器人从圆弧i 行进至圆弧 j 切点时的 ijb 为
i
,
j
bij
,36..1
i
j
其中
1
i
为转弯半径, 10 。
5.2.2 约束条件
x
(
x
i
2
2
)
(
y
1
i
y
i
2
2
)
2
cos
2
2
2
(3)
1.避障条件的约束
在本问题中,障碍物边界可分为直线段与圆弧两种情况,针对障碍物边界不同的两
种情况,我们列出避障条件的约束:
(1)避障约束条件 1
任意一条可行路径与所有障碍物的边界线段间的最短距离大于 10 个单位.
(
设平面内有两条线段 AB 和CD ,A 点的坐标为
x
,
,其中 A 、 B 可视为圆弧的切点坐标,C 、
C 点的坐标为
D 为圆弧的切点坐标 AB 线段两圆弧的切点的连线,CD 视为障碍物的某一条边界线段。
, D 点的坐标为
, B 点的坐标为
,
j y
1
,
q y
,
p y
x
i
y
x
x
)
(
)
(
)
)
(
,
2
2
1
p
q
i
j
设 m
pqP 是直线 AB 上的一点, m
pqP 点的坐标 (
x
(
xs
X
(
Y
ys
x
i
y
2
2
j
i
j
1
1
m
,
pq
m
)
y 可以表示为
pq
)
x
i
)
y
2
i
2
当参数
0
s 时, m
pqP 是线段 AB 上的点;当参数 0s 时, m
pqP 是 BA 延长线上的
1
点;当参数 1s 时, m
pqP 是 AB 延长线上的点。
设 ijQ 是直线CD 上的一点, ijQ 点的坐标 (
(
xt
q
(
ys
xU
y
Y
P
p
q
)U V 可以表示为
,
x
p
y
)
)
p
0
当参数
当参数 1t
1
t 时, ijQ 是线段CD 上的点;当参数 0t 时, ijQ 是 DC 延长线上的点;
时, ijQ 是CD 延长线上的点。
m
pqP , ijQ 两点之间的距离为
距离的平方为
m
P Q
pq
ij
(
X U
)
2
(
Y V
)
2
(
)
Y V
(
)
ys
要求直线 AB ,CD 之间的最短距离,也就是要求函数
( , )
f s t
(
)
x
xt
i
(
X U
[(
y
m
P Q
pq
ij
x
2
)]
y
(
xs
)
[(
x
i
x
)
2
1
2
2
q
p
p
p
i
j
2
2
2
y
1
j
),(
tsf
(
yt
)
)]
2
i
的最小值。对
y
p
q
2
),(
tsf
分
别求关于 s ,t 的偏导数,并令偏导数为零:
),(
tsf
s
),(
tsf
t
展开并整理后,得到下列方程组:
0
0
[(
(
[(
[(
(
2
1
[(
x
1
j
1
j
x
i
x
x
i
x
p
x
1
2
2
2
(
)
x
2
i
)(
x
q
)(
x
i
)(
x
2
q
2
(
)
y
)(
x
q
x
p
x
x
p
)
y
y
1
i
j
2
(
)
y
(
)
y
i
p
(
)
y
1
j
2
) ]
y
t
p
(
y
i
q
x
2
p
2
j
x
x
i
x
x
q
x
i
j
j
2
2
) ]
s
y
1
i
y
y
i
2
j
q
)(
y
2
)(
y
1
i
)(
y
y
y
2
y
p
)]
t
)
p
)]
t
q
p
如果从这个方程组求出的参数 s ,t 的值满足
2
p
y
0
q
y
)(
y
p
p
s ,
1
)
0
t ,说明 m
1
pqP 点落在线
段 AB 上, ijQ 点落在线段CD 上,这时 m
P Q 的长度为
pq
(
X U
ij
此时 m
P Q 就是线段 AB 与CD 的最短距离。
pq
如果从方程组求出的参数 s ,t 的值不满足
m
P Q
pq
ij
0
)
ij
2
(
Y V
2
)
0
t ,说明不可能在线段
AB 内部找到一点 m
pqP ,在线段 CD 内部找到一点 ijQ ,使得 m
P Q 的长度就是线段 AB 与
pq
CD 的最短距离。这时,还需要进行考虑的是:平面中一个点到一条线段的最短距离。
m
y ,A
pq
pqP 点的坐标为 (
s ,
m
pq
1
1
x
)
,
ij
x
点的坐标为 2
i
,
(
)
(
pqP 和一条线段 AB ,设 m
设编号为 m 的障碍物的圆心坐标 m
,
y , B 点的坐标为 1
x
2
j
i
此时,直线 AB 的参数形式的方程为
x
y
1
直线上的点 ( ,
x y ,当参数
(
t x
j
(
t y
y 。
j
x
2
i
y
x
i
y
)
)
0
)
)
1
2
1
2
1
2
i
j
i
时,是 AB 延长线上的点。
长线上的点;当参数 1t
通过 m
pqP 点,与直线 AB 垂直的平面方程为
(
)(
y
下面求这个平面与直线 AB 的交点 ijQ 。显然, m
P Q
pq
ij
)(
y
i
m
pq
x
i
x
y
x
x
(
)
1
2
1
2
j
j
向直线 AB 作垂线的垂足点。
将
x
y
x
i
y
2
i
2
(
t x
j
(
t y
1
j
1
x
2
i
y
i
)
)
2
2
t
(
x
i
m
pq
代入平面方程,化简后解得
x
m
y
pq
y
然后,将上面得到的t 的值代入直线方程,得到
x
2
x
(
t x
(
t y
)(
x
i
(
x
i
X x
i
Y
y
y
i
y
i
x
i
y
(
(
)
2
1
)
2
2
1
2
2
1
2
1
1
j
j
j
j
i
j
i
)
)
2
2
y
i
2
y
)
j
1
)(
2
)
其中, (
)X Y 为垂足点 ijQ 的坐标。
,
P Q 的长度,也就是 m
此时线段 m
pq
(
m
P Q
pq
ij
ij
pqP 点到直线 AB 的垂直距离为
X x
m
pq
2
)
(
Y y
m
pq
2
)
t 时,是线段 AB 上的点;当参数 0t 时,是 BA 延
y
) 0
m
pq
AB ,所以 ijQ 点也是从 m
pqP 点