【计算几何 第十三章】机器人运动规划：随心所欲

本文涉及知识点

数学 几何

预备知识

伪圆盘，一个简单连通区域，和其它伪圆盘顶多两个交点。凸多边形属于伪圆盘。凸曲线都属于伪圆。 简单多边形是二维流形：边界上的点具有明确的内外两侧；沿内法线方向移动足够小，必进入内部；沿相反方向移动，必进入外部。这一结论在边和顶点上均成立。 排除三点共线，凸多边形不会有两条边共线。不会有三条边平行。 假定三条边平行，中间的那条是B，另外两条边上是AC。AC处于B两则，和凸多边形所有端点都在任意边一侧矛盾。 凸多边形的支撑函数（Support Function）：设

P

⊂

R

2

P \\sub R^2

P⊂R2是一个凸多边形(有界闭凸集)。对于任意方向向量

d

⃗

≠

0

\\vec d \\neq 0

d

=0,定义的支持函数为：

h

p

(

d

)

=

max

⁡

x

∈

p

<

x

,

d

>

h_p(d)=\\max\\limits_{x\\in p}<x,d>

hp​(d)=x∈pmax​<x,d>x,d>是点积。 将凸多边形沿d投影到一条直线上，得到的投影区间的最大值。 支撑函数的连续性：

h

p

(

d

⃗

)

h_p(\\vec d)

hp​(d

)是关于方向

d

⃗

\\vec d

d

的连续函数。 差值函数：

f

(

d

⃗

)

=

h

p

1

(

d

⃗

)

−

h

p

2

(

d

⃗

)

f(\\vec d)=hp_1(\\vec d)-hp_2(\\vec d)

f(d

)=hp1​(d

)−hp2​(d

),它也是连续的。

两个内部不相交的凸多边形最多两个方向支撑函数相同（预备知识一）

令两个凸多边形在方向d，支撑函数值相同，为x1，则过x1垂线p1。两个凸多边形所有的顶点都p1的某个半平面，且至少各有一个顶点在p1上。 假定两个多边形有三个方向支撑函数值相同，则两个凸多边形都在p1,p2,p3的相同的半平面，且各有点在p1,p2,p3上。由于p1,p2,p3方向不相同，有三种情况： 一，三条直线构成一个三角形。两个凸多边形在三角内。令a,b,c是第一个多边形的顶点，则第一个多边形一定包括abc。令def是第二多边形在p1,p2,p3的顶点，则def一定会分割abc，故内部相交，与假设矛盾。 二，三条直线构成三角形，但两个凸多边形都在无界梯形。令d,e,f是第二个凸多边形在直线a,b,c的顶点。如果e放到ab之间，e和f的折线会穿过abc；如果e在dc之间，ed之间的折线会穿过abc。 三，三条直线交于一点。

e=d，如果d在ab外，则db间的折线必定闯过abc，f在bc外同理。如果df在ab,ac内，则

b

d

f

⊂

a

b

c

bdf \\sub abc

bdf⊂abc

前言

所谓的机械手或称作多关节型机器人，由若干段杠件(link)通过关节(joint)联接而成。机械手的一端固定在工作平面上–称为底座(base)；另一端则装有手柄(hand)或某种工具。杆件的数目从三至六段不等。关节两种：旋转式关节和柱状关节。前者可以任意转动，后者只能滑进滑去。 简化：一，只讨论二维的运动规划问题，运动的环境是平面的一个区域，障碍物和机器人的外形都是多边形。二，环境是静态的，即机器人运动的沿途不会遇到行人。三，机器人对环境也知。 本章主要研究哪些只能平移的机器人。

1 工作空间与C-空间

设R为在二维环境中移动的一个机器人。机器人所处的环境也称为工作空间(work space)，它由一组障碍物S={P_1,\\cdots P_t}组成。我们假定，R本身是一个简单多边形,机器人所处的每个位置都可以表示为一个平移向量。如果机器人沿向量(x,y)做了一次平移，就记作R(x,y)。假设某个机器人所对应多边形的顶点为(1,-1)、(1,1)、(0,3)、(-1,1)和(-1,-1)，则R(6,4)所对应的顶点就是(7,3),(7,5),(6,7)和(5,3)。采用这种记号，可以用R(0,0)的所有顶点来表示一个机器人。 也可以按照参考点(reference point)的概念来理解。当R(0,0)的内部包含原点(0,0)时，这是一种直观的理解方式。R(x,y)的含义就是按机器人当前的位置，其参考点位于(x,y)。一般而言，参考点不一定落在机器人内部。 假设机器人能够通过旋转(比如绕着它的参考点旋转)改变方向。需要一个新的参数

ϕ

\\phi

ϕ来描述机器人的方西。如果机器人的参考点在(x,y)，已经逆时针旋转了

ϕ

\\phi

ϕ，就可以用R(x,y,

ϕ

\\phi

ϕ）。 描述机器人位置的一组参数，分别对应于机器人的几个自由度(degree of frerdom,DOF)。对于只能在平面上移动的机器人来说，自由度为二；如果机器人既能平移也能旋转，其自由度是三。三维空间中，只能平移的机器人自由度是三，能平移旋转的机器人度数是六。 机器人的参数空间，通常被称为C-空间，记作C( R )。C-空间中的每个点p分别对应于工作空间中的某一位置R§。对于平面上，可平移旋转的机器人来说，C-空间是三维的，并不是三维欧氏空间，其拓扑与圆柱面类似。 如果C空间的某个点指示的位置的机器人会和S中的障碍物相交，则这个点应该被禁止，这些点构成禁止空间(forbidden space)记作

C

f

o

r

b

(

R

,

S

)

C_{forb}(R,S)

Cforb​(R,S)。C空间的其余部分，每个点都对应于一个自由位置(free placement)，这些点称为自由空间(free space)，记作

C

f

r

e

e

(

R

,

S

)

C_{free}(R,S)

Cfree​(R,S)。

机器人的每条运动路径，都可以被映射为C-空间中的一条曲线。反之亦然—路径上的每一点都可以相应地映射为C-空间的某一点。每条无碰撞的路径都可以映射为自由空间的某条曲线。 可以将障碍无映射到C-空间中。任一障碍物 P都可以映射为C-空间中的某一点集，该点集由所有满足R§与p相交的点p组成。这个几何称作与P对应的C-空间的障碍物(configuration space obstacle)，或简称C-障碍物。 即使在工作空间中障碍物没有相交，它们在C-空间中对应的C-障碍物也可能会相交。如图13-6所示，如果机器人在某一位置同时与至少两个障碍物相交，就会发生这种情况。 如果机器人恰好与某个障碍物相切，是容许的。障碍物定义为开集。如果不容许相切，可以将障碍物外扩一定点。

2 点机器人

对于点机器人，工作空间和C-空间是完全相同的。 我了简化描述，我们将机器人的运动范围限制在一个包围框B中。这个包围框应该足够大，以容纳所有的多边形。自由C-空间就是B中没有被障碍物覆盖的那些区域。

引理13.1：对于一个点机器人，若它运动的环境中包含一组互不相交的多边形障碍物，且障碍物总共包含n条边，则可以借助随机算法，在O(nlogn)期望运行时间内构造出一幅描述其自由C-空间的梯形图。我们把描述自由空间的梯形图记作T(

C

f

r

e

e

C_{free}

Cfree​)。 如果起点和终点在同一个梯形图，则直接线段连接。否则依靠路线图。

在每个梯形的中心、每条垂直边的中点各放置一个节点。两个节点之间有一条弧相联，当且仅当其中的一个节点处于某个梯形的中心，而另一个处于该梯形的边界上。起点到当前梯形路线图的任意一点，沿路线图到中点所在梯形，线段直达终点。 不会碰撞证明：梯形都在自由空间，所以任意一点都在自由空间。梯形是凸多边形，故任意线段都在梯形内，且自由空间内。 如果连通则一定能够找到连通路径：两个相邻梯形的中心都会连接公共边的中心。 梯形间的路径可以用广度用广度优先搜索(BFS)，复杂度：O(边数) 定理13.2： 设点机器人R运动于一组多边形障碍物S之间，各障碍物所含边数的总数为n。可以在O(nlogn)期望运行时间内对S进行预处理，使得我们总可以在O(n)时间内，在任何起点与终点之间为R规划出一条无碰撞的路径。（如果的确存在这样一条路径的话）。

3 Minkowski 和(闵可夫斯基和)

假定机器人R是凸多边形，障碍物也是凸的。如果将P的C-障碍物记作CP，就有

C

P

:

=

{

(

x

,

y

)

∣

R

(

x

,

y

)

∩

≠

∅

}

CP:=\\{(x,y)|R(x,y)\\cap \\neq \\empty\\}

CP:={(x,y)∣R(x,y)∩=∅} 为了画出CP的形状，如图13-14所示，可以让R沿着P的边界滑行一圈。—-R的参考点经过的轨迹曲线，就是CP的边界。

闵可夫斯基和

采用Minkowski和,对于任何两个集合

S

1

⊂

R

2

和

S

2

⊂

R

2

S_1 \\sub R^2和S_2 \\sub R^2

S1​⊂R2和S2​⊂R2，其闵可夫斯基和(

S

1

⊕

S

2

S_1 \\oplus S_2

S1​⊕S2​)为：

S

1

⊕

S

2

:

=

{

p

+

q

∣

p

∈

S

1

,

q

∈

S

2

}

S_1 \\oplus S_2 :=\\{p+q|p\\in S_1,q \\in S_2\\}

S1​⊕S2​:={p+q∣p∈S1​,q∈S2​} 其中：p+q表示两个向量p和q的向量和。

p

+

q

:

=

(

p

x

+

q

x

,

p

y

+

q

y

)

p+q := (p_x+q_x,p_y+q_y)

p+q:=(px​+qx​,py​+qy​) 多边形是平面点集，故当然也可以定义它们之间的闵可夫斯基和。 对于任意点p=(

p

x

,

p

y

p_x,p_y

px​,py​)，定义-p :=(

−

p

x

,

−

p

y

-p_x,-p_y

−px​,−py​)；对于任何集合S，定义-S:={-p|p

∈

\\in

∈ S} 也就是说，-S是S相对于坐标原点对称镜像。

凸多边形P、Q的闵可夫斯基和R是凸的

设

r

1

=

p

1

+

q

1

,

r

2

=

p

2

+

q

2

r_1=p_1+q_1,r_2=p_2+q_2

r1​=p1​+q1​,r2​=p2​+q2​是R的任两点，则

r

3

=

r

1

+

λ

r

2

=

p

1

+

q

1

+

λ

(

p

2

+

q

2

)

=

(

p

1

+

λ

p

2

)

+

(

q

1

+

λ

q

2

)

r3=r1+\\lambda r_2=p_1+q_1+\\lambda(p_2+q_2)=(p_1+\\lambda p_2)+(q_1 + \\lambda q_2)

r3=r1+λr2​=p1​+q1​+λ(p2​+q2​)=(p1​+λp2​)+(q1​+λq2​)

p

1

+

λ

p

2

∈

P

,

q

1

+

λ

q

2

∈

Q

，故

r

3

∈

R

p_1+\\lambda p_2 \\in P,q_1+\\lambda q_2 \\in Q，故r_3 \\in R

p1​+λp2​∈P,q1​+λq2​∈Q，故r3​∈R

投影变换后求闵可夫斯基和$⟺$

旋转也投影变换的一种。令矩阵是m， 先变换后求和：pm+qm。 先求和再变换：(p+q)m。 根据矩阵分配律，两者相等。

R的边数

任意凸曲线

⟺
  

\\iff

⟺可能有无穷短边的凸多边形。 观察结论13.4：设P和R为平面上的两个物体，设CP:=P

⊕

\\oplus

⊕ R。则CP中沿着

d

⃗

\\vec d

d

方向的极点就是P和R各自沿

d

⃗

\\vec d

d

方向的极点之和。 定理13.5：设P和R为两个凸多边形，分别含有n和m条表。则闵可夫斯基和

P

⊕

R

P \\oplus R

P⊕R是一个由不超过n+m条边组成的凸多边形。

任取

P

⊕

R

P \\oplus R

P⊕R的一条边e。旋转P、Q、R直e水平，且是最右边。 假定此时P,R的包围盒的右边界和P,R交于一点，而不是一条线段。则此两点分别是(x1,y1),(x2,y2)。除这两点相加横坐标为x1+x2外，其它点的横坐标皆小于此值，故e是点不是边。与e是边矛盾。故P,R至少有一条边和e平行。故R最多m+n条边。 进一步：e的长度=P的最右边长度+Q的最右边边长度。点认为长度0。

路径规划

定理13.3：设R为沿平面做平移运动的一个机器人，P为任意一障碍物。则P所对应的C-障碍物为P

⊕

\\oplus

⊕-(R(0,0))。 只需要证明：R(x,y)与P相交当且仅当(x,y)

∈

P

⊕

(

−

R

(

0

,

0

)

)

\\in P \\oplus (-R(0,0))

∈P⊕(−R(0,0))。 首先，假设R(x,y)与P相交。并取q=(

q

x

,

q

y

)

q_x,q_y)

qx​,qy​)为它们的任一交点。由q

∈

R

(

x

,

y

)

可知

,

(

q

x

−

x

,

q

y

−

y

)

∈

R

(

0

,

0

)

成立。即

(

−

q

x

+

x

,

−

q

y

+

y

)

∈

−

R

(

0

,

0

)

\\in R(x,y)可知,(q_x-x,q_y-y)\\in R(0,0)成立。即(-q_x+x,-q_y+y)\\in -R(0,0)

∈R(x,y)可知,(qx​−x,qy​−y)∈R(0,0)成立。即(−qx​+x,−qy​+y)∈−R(0,0)成立。因为

q

∈

P

q \\in P

q∈P也同时成立，故有(x,y)

∈

P

⊕

(

−

R

(

0

,

0

)

)

\\in P \\oplus (-R(0,0))

∈P⊕(−R(0,0))。 设(x,y)

∈

P

⊕

(

−

R

(

0

,

0

)

)

\\in P \\oplus (-R(0,0))

∈P⊕(−R(0,0))。于是，必然存在点(

r

x

,

r

y

)

∈

R

(

0

,

0

)

和点

(

p

x

,

p

y

)

∈

P

，使得

(

x

,

y

)

=

(

p

x

−

r

x

,

p

y

−

r

y

)

成立。即，有

p

x

=

r

x

+

x

和

p

y

=

r

x

+

y

成立。由此可知，

R

(

x

,

y

)

必与

P

相交

r_x,r_y)\\in R(0,0)和点(p_x,p_y)\\in P，使得(x,y)=(p_x-r_x,p_y-r_y)成立。即，有p_x=r_x+x和p_y=r_x+y成立。由此可知，R(x,y)必与P相交

rx​,ry​)∈R(0,0)和点(px​,py​)∈P，使得(x,y)=(px​−rx​,py​−ry​)成立。即，有px​=rx​+x和py​=rx​+y成立。由此可知，R(x,y)必与P相交。 有时，P ⊕ (-R(0, 0))也被称作 Minkowski 差（Minkowski difference）

考虑一对平面物体o1和o2，其边界是

σ

(

o

1

)

σ

(

o

2

)

\\sigma(o1) \\sigma(o2)

σ(o1)σ(o2)，其内部是int(o1),int(o2)。物体o1和o2是一对伪圆盘，如果以下条件满足：

σ

(

o

1

)

∩

i

n

t

(

o

2

)

和

σ

(

o

2

)

∩

i

n

t

(

o

1

)

各自都是连通的

\\sigma(o1)\\cap int(o2)和\\sigma(o2) \\cap int(o1)各自都是连通的

σ(o1)∩int(o2)和σ(o2)∩int(o1)各自都是连通的。

考虑一对多边形P和P’。交点p

∈

σ

P

∩

σ

P

′

\\in \\sigma P \\cap \\sigma P'

∈σP∩σP′被称为是一个边界穿越点(boundary crossing)。如果

σ

P

在

p

除从

P

′

的内部转到

P

′

的外部

\\sigma P在p除从P'的内部转到P'的外部

σP在p除从P′的内部转到P′的外部。多边形伪圆盘具有如下重要性质： 观察结论13.6：任何一对多边形伪圆盘P和P’，最多有两个边界穿越点。 在任一方向

d

⃗

\\vec d

d

上，若一个多边形的极点相对于另一个的极点更远，就说“沿着

d

⃗

\\vec d

d

方向，前者比后者更极端”。

观察结论13.7：设p1和p2为内部互不相交的两个凸多边形，且在方向

d

1

⃗

和

d

2

⃗

\\vec{d1}和\\vec{d2}

d1

和d2

上p1都比p2更极端，则在从

d

⃗

1

→

d

⃗

2

\\vec d1 \\to \\vec d2

d

1→d

2,从

d

⃗

2

→

d

⃗

1

\\vec d2 \\to \\vec d1

d

2→d

1的两个区间中，必有一个区间的任一方向，p1都比p2更加极端。 不考虑退化情况，两个内部互不相交的凸多边形分离轴方向d，差值函数不为0。如果差值函数>0，则将d反向，则差值函数<0。不失一般性，令d在d1到d2之间。由于差值函数是连续函数，故d1到d，d到d2各有一个方向差值函数为0。根据预备知识一，两个内部相交的凸多边形最多两个方向差值函数为0，故d2到d1不存在差值为0的方向。由于差值函数是连续函数，故差值不会从正数越过0,称为负数。

定理13.8：设P1和P2为内部互不相交的两个凸多边形，R为另一个凸多边形。在闵可夫斯基和

P

1

⊕

R

与

P

2

⊕

R

P1 \\oplus R与P_2 \\oplus R

P1⊕R与P2​⊕R必定是一对伪圆盘。 证明：定义

C

P

1

:

=

P

1

⊕

R

,

C

P

2

:

=

P

2

⊕

R

CP_1:=P_1 \\oplus R,CP_2:=P_2 \\oplus R

CP1​:=P1​⊕R,CP2​:=P2​⊕R。根据对称性，只需证明

σ

(

C

P

1

)

∩

i

n

t

(

C

P

2

)

\\sigma(CP_1) \\cap int(CP_2)

σ(CP1​)∩int(CP2​)是连通的。

假设

σ

(

C

P

1

)

∩

i

n

t

(

C

P

2

)

不是连通的

\\sigma(CP_1) \\cap int(CP_2)不是连通的

σ(CP1​)∩int(CP2​)不是连通的。如图13-21所示，沿着

σ

(

C

P

1

)

\\sigma(CP_1)

σ(CP1​)依次取四次p、q、r和s，使得p,r

∈

i

n

t

(

C

P

2

)

\\in int(CP_2)

∈int(CP2​)，而q,s

∉

i

n

t

(

C

P

2

)

\\notin int(CP_2)

∈/int(CP2​)。考察这四个点的外发矢

d

⃗

p

,

d

⃗

q

,

d

⃗

r

,

d

⃗

s

\\vec d_p,\\vec d_q ,\\vec d_r,\\vec d_s

d

p​,d

q​,d

r​,d

s​。于是

d

⃗

p

,

d

⃗

r

\\vec d_p,\\vec d_r

d

p​,d

r​方向，

C

P

2

比

C

P

1

更极端

CP_2比CP_1更极端

CP2​比CP1​更极端，而沿

v

e

c

d

q

和

d

⃗

s

方向则不然

vec d_q和\\vec d_s方向则不然

vecdq​和d

s​方向则不然。与13.4和13.7矛盾。 注意：如果pqrs刚好是端点，逆时针移动无穷小。p,r同时CP1的边界和CP2的内部，故CP2更极端。 极端情况下，pqrs共线，则AB的外法线顺时钟旋转无穷小，此方向CP1更极端。 定理13.9：设S是一组多边形的伪圆盘，其中边的总数为n。则它们并集的负责度不超过2n。 平面上的任意n个圆盘的并集，复杂度必为O(n)，证明复杂得多。

算法 MinkowskiSum(P,R) 输入：由顶点v1,vn组成的凸多边形P，由顶点w1,wn组成的凸多边形R。约定，每个多边形各自的顶点都按逆时针方向排列,v1和w1分别是y-坐标最小的顶点(如果y-相同，取x-坐标更小) 输出：闵可夫斯基和

P

⊕

R

P \\oplus R

P⊕R

定理13.10：任意两个凸多边形的闵可夫斯基和，都可以在O(n+m)的时间内构造出来。其中n和m分别为这两个多边形各自所含的顶点数。 非凸多边形先三角剖分，再求并集。 定理13.11：设P和R为两个多边形，它们分别含有n和m个顶点。闵可夫斯基和

P

⊕

R

P \\oplus R

P⊕R的负责的上界分别为： 一，两个多边形都是凸的，则上届为O(n+m)； 二，若一个为凸，另一个非凸，则上界为O(nm)。 三，若两个多边形同时非凸，则上届为O

(

n

2

m

2

)

(n^2m^2)

(n2m2). 在最坏情况下，上面的每一上界都是紧的。

4 平移式运动规划

定理13.12：设R为一个具有常熟复杂度的机器人，它可以在一组互不相交的多边形障碍物S之间做平移运动。则自由C-空间

C

f

r

e

e

(

R

,

S

)

C_{free}(R,S)

Cfree​(R,S)的复杂度为o(n)，其中n为所有障碍物所含的变数。

证明：首先对每个障碍物多边形做三角剖分。这样就得到了一组共O(n)个三角形障碍物，这些障碍物互不相交。此时的自由C-空间，也就是这些三角形对应的C-障碍物的并集和补集。因为机器人本身是具有常数复杂度，所以每个C-障碍物的负责度也是常数。根据定理13.8这些障碍物必然构成一组伪圆盘。于是根据13.9它们具有线性负责度。 构造禁止空间，禁止空间的补集就是自由空间。 引理13.13：对于一个具有常数复杂度的机器人，若它在一组多边形障碍物之间做平移运动，则其对应的自由C-空间

C

f

r

e

e

可以在

O

（

n

l

o

g

2

n

C_{free}可以在O（nlog^2n

Cfree​可以在O（nlog2n时间内构造出来，其中n所有障碍物所含边的总数。 13.12讲的是复杂度，13.13讲的是构造时间。

定理13.14：设R为一个具有常数复杂度的机器人，它可以在一组互不相交的多边形障碍物S之间做平移运动。在经过

O

(

n

l

o

g

2

n

)

O(nlog^2n)

O(nlog2n)期望运行时间内对S进行预处理，对于任意指定的起始和终止位置，我们都可以在O(n)时间内，在这两个位置之间为R规划出一条无碰撞的路径(如果的确存在这样条路径的话)，其中n为所有障碍物所含边的总数。