本文涉及知识点

数学 几何

预备知识

伪圆盘，一个简单连通区域，和其它伪圆盘顶多两个交点。凸多边形属于伪圆盘。凸曲线都属于伪圆。
简单多边形是二维流形：边界上的点具有明确的内外两侧；沿内法线方向移动足够小，必进入内部；沿相反方向移动，必进入外部。这一结论在边和顶点上均成立。
排除三点共线，凸多边形不会有两条边共线。不会有三条边平行。
假定三条边平行，中间的那条是B，另外两条边上是AC。AC处于B两则，和凸多边形所有端点都在任意边一侧矛盾。
凸多边形的支撑函数（Support Function）：设$P\subset R^2$是一个凸多边形(有界闭凸集)。对于任意方向向量$\vec{d}\ne 0$,定义的支持函数为：
$$h_p(d)=\underset{x\in p}{\max }<x,d>$$x,d>是点积。
将凸多边形沿d投影到一条直线上，得到的投影区间的最大值。
支撑函数的连续性：$h_p(\vec{d})$是关于方向$\vec{d}$的连续函数。
差值函数：$f(\vec{d})=h{p}_1(\vec{d})-h{p}_2(\vec{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内，则$bdf\subset 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)。一般而言，参考点不一定落在机器人内部。
假设机器人能够通过旋转(比如绕着它的参考点旋转)改变方向。需要一个新的参数$\varphi$来描述机器人的方西。如果机器人的参考点在(x,y)，已经逆时针旋转了$\varphi$，就可以用R(x,y,$\varphi$）。

描述机器人位置的一组参数，分别对应于机器人的几个自由度(degree of frerdom,DOF)。对于只能在平面上移动的机器人来说，自由度为二；如果机器人既能平移也能旋转，其自由度是三。三维空间中，只能平移的机器人自由度是三，能平移旋转的机器人度数是六。
机器人的参数空间，通常被称为C-空间，记作C( R )。C-空间中的每个点p分别对应于工作空间中的某一位置R§。对于平面上，可平移旋转的机器人来说，C-空间是三维的，并不是三维欧氏空间，其拓扑与圆柱面类似。
如果C空间的某个点指示的位置的机器人会和S中的障碍物相交，则这个点应该被禁止，这些点构成禁止空间(forbidden space)记作$C_{forb}(R,S)$。C空间的其余部分，每个点都对应于一个自由位置(free placement)，这些点称为自由空间(free space)，记作$C_{free}(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_{free}$)。
如果起点和终点在同一个梯形图，则直接线段连接。否则依靠路线图。

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

3 Minkowski 和(闵可夫斯基和)

假定机器人R是凸多边形，障碍物也是凸的。如果将P的C-障碍物记作CP，就有
$$CP:=\{(x,y)|R(x,y)\cap \ne \varnothing \}$$
为了画出CP的形状，如图13-14所示，可以让R沿着P的边界滑行一圈。----R的参考点经过的轨迹曲线，就是CP的边界。

闵可夫斯基和

采用Minkowski和,对于任何两个集合$S_1\subset R^2和S_2\subset R^2$，其闵可夫斯基和($S_1\oplus S_2$)为：
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=($p_x,p_y$)，定义-p :=($-p_x,-p_y$)；对于任何集合S，定义-S:={-p|p $\in$ S} 也就是说，-S是S相对于坐标原点对称镜像。

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

设$r_1=p_1+q_1,r_2=p_2+q_2$是R的任两点，则$r3=r1+\lambda r_2=p_1+q_1+\lambda (p_2+q_2)=(p_1+\lambda p_2)+(q_1+\lambda q_2)$
$p_1+\lambda p_2\in P,q_1+\lambda q_2\in Q，故r_3\in R$

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

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

R的边数

任意凸曲线$⟺$可能有无穷短边的凸多边形。
观察结论13.4：设P和R为平面上的两个物体，设CP:=P$\oplus$ R。则CP中沿着$\vec{d}$方向的极点就是P和R各自沿$\vec{d}$方向的极点之和。
定理13.5：设P和R为两个凸多边形，分别含有n和m条表。则闵可夫斯基和$P\oplus R$是一个由不超过n+m条边组成的凸多边形。

任取$P\oplus 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) $\in P\oplus (-R(0,0))$。
首先，假设R(x,y)与P相交。并取q=($q_x,q_y)$为它们的任一交点。由q $\in R(x,y)可知,(q_x-x,q_y-y)\in R(0,0)成立。即(-q_x+x,-q_y+y)\in -R(0,0)$成立。因为$q\in P$也同时成立，故有(x,y) $\in P\oplus (-R(0,0))$。
设(x,y)$\in P\oplus (-R(0,0))$。于是，必然存在点($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相交$。
有时，P ⊕ (-R(0, 0))也被称作 Minkowski 差（Minkowski difference）

考虑一对平面物体o1和o2，其边界是$\sigma (o1)\sigma (o2)$，其内部是int(o1),int(o2)。物体o1和o2是一对伪圆盘，如果以下条件满足：
$\sigma (o1)\cap int(o2)和\sigma (o2)\cap int(o1)各自都是连通的$。

考虑一对多边形P和P’。交点p$\in \sigma P\cap \sigma P^{\prime }$被称为是一个边界穿越点(boundary crossing)。如果$\sigma P在p除从P^{\prime }的内部转到P^{\prime }的外部$。多边形伪圆盘具有如下重要性质：
观察结论13.6：任何一对多边形伪圆盘P和P’，最多有两个边界穿越点。
在任一方向$\vec{d}$上，若一个多边形的极点相对于另一个的极点更远，就说“沿着$\vec{d}$方向，前者比后者更极端”。

观察结论13.7：设p1和p2为内部互不相交的两个凸多边形，且在方向$\overrightarrow{d1}和\overrightarrow{d2}$上p1都比p2更极端，则在从$\vec{d}1\to \vec{d}2$,从$\vec{d}2\to \vec{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为另一个凸多边形。在闵可夫斯基和$P1\oplus R与P_2\oplus R$必定是一对伪圆盘。
证明：定义$C{P}_1:=P_1\oplus R,C{P}_2:=P_2\oplus R$。根据对称性，只需证明$\sigma (C{P}_1)\cap int(C{P}_2)$是连通的。

假设$\sigma (C{P}_1)\cap int(C{P}_2)不是连通的$。如图13-21所示，沿着$\sigma (C{P}_1)$依次取四次p、q、r和s，使得p,r$\in int(C{P}_2)$，而q,s $\notin int(C{P}_2)$。考察这四个点的外发矢${\vec{d}}_p,{\vec{d}}_q,{\vec{d}}_r,{\vec{d}}_s$。于是${\vec{d}}_p,{\vec{d}}_r$方向，$C{P}_2比C{P}_1更极端$，而沿$vec{d}_q和{\vec{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\oplus R$

1. $i\leftarrow 1,j\leftarrow 1$
2. $v_{n+1}\leftarrow v_1;w_{m+1}\leftarrow w_1$
3. repeat
4. 将$v_i+w_j作为顶点加入P\oplus R$
5. if(angle(v_iv_{i+1}) < angle(w_jw_{j+1}))

6.  then $i \leftarow i+1$
   

7.  else if(anlge(v_iv_{i+1}) > angle(w_jw_{j+1}))
   

8.  	then $j \leftarrow j+1$
   

9.  	else $i \leftarrow i+1 j \leftarrow j+1$ 
   

10. until (i=n+1 and j=m+1)

定理13.10：任意两个凸多边形的闵可夫斯基和，都可以在O(n+m)的时间内构造出来。其中n和m分别为这两个多边形各自所含的顶点数。
非凸多边形先三角剖分，再求并集。
定理13.11：设P和R为两个多边形，它们分别含有n和m个顶点。闵可夫斯基和$P\oplus R$的负责的上界分别为：
一，两个多边形都是凸的，则上届为O(n+m)；
二，若一个为凸，另一个非凸，则上界为O(nm)。
三，若两个多边形同时非凸，则上届为O$(n^2{m}^2)$.
在最坏情况下，上面的每一上界都是紧的。

4 平移式运动规划

定理13.12：设R为一个具有常熟复杂度的机器人，它可以在一组互不相交的多边形障碍物S之间做平移运动。则自由C-空间$C_{free}(R,S)$的复杂度为o(n)，其中n为所有障碍物所含的变数。

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

定理13.14：设R为一个具有常数复杂度的机器人，它可以在一组互不相交的多边形障碍物S之间做平移运动。在经过$O(nlo{g}^{2}n)$期望运行时间内对S进行预处理，对于任意指定的起始和终止位置，我们都可以在O(n)时间内，在这两个位置之间为R规划出一条无碰撞的路径(如果的确存在这样条路径的话)，其中n为所有障碍物所含边的总数。