《Planning Algorithm》 第三章：几何表示与变换
在连续空间内的规划
第三章：几何表示与变换
- 3.1 几何模型
- 3.1.1 多边形和多面体模型
- 3.1.2 半代数模型
- 3.1.3 其他模型
- 3.2 刚体变换
- 3.2.1 基本概念
- 3.2.2 二维变换
- 3.2.3 三维变换
- 3.3 物体运动链的变换
- 3.3.1 二维运动链
- 3.4 运动树变换
- 3.5 非刚性变换
构建和解决运动规划问题需要在空间中定义复杂的几何模型并对其进行操作，3.1节介绍了几何模型，其关注点在于半代数模型构建，这也是第五章和第六章的基础。3.2节和3.3节分别介绍了如何变换一个机器人和一条机器人链，这也是运动的基础。
3.1 几何模型
几何模型的替代表示方法有很多，主要有两种：1）边界表示（boundary representation） ，例如一个行星的表面；2）实心表示(solid representation)，将包含在球面内的所有点均被描述在模型内。
第一步，定义世界（world，W\mathcal{W}W）,二维世界的 W=R2\mathcal{W}=\mathbb{R}^{2}W=R2和三维世界的W=R3\mathcal{W}=\mathbb{R}^{3}W=R3，亦或是更高维度的空间。世界中通常含有两类实体：障碍物O\mathcal{O}O和机器人R\mathcal{R}R，他们都是世界集的子集。接下来就需要定义这两个集合，使他们便于计算。
3.1.1 多边形和多面体模型
本节将使用障碍物O\mathcal{O}O的一种实心表示：即一系列基集合（primitive） HiH_iHi的组合，每个基集合都便于表示和计算。这里可以表示成m个半平面的交。
凸多边形（Convex Polygons）:首先考虑障碍物范围是2D世界的一个凸多边形子集，W=R2\mathcal{W}=\mathbb{R}^{2}W=R2。一个子集X⊂RnX \subset \mathbb{R}^{n}X⊂Rn被称为凸集当且仅当： ∀x1,x2∈X,λ∈[0,1],λx1+(1−λ)x2∈X\forall x_{1}, x_{2} \in X, \lambda \in[0,1] ,\lambda x_{1}+(1-\lambda) x_{2} \in X∀x1,x2∈X,λ∈[0,1],λx1+(1−λ)x2∈X。不满足此的称为非凸。
半平面的表示：首先一条通过(x1,y1)\left(x_{1}, y_{1}\right)(x1,y1)和(x2,y2)\left(x_{2}, y_{2}\right)(x2,y2)的直线方程可以写成ax+by+c=0a x+b y+c=0ax+by+c=0，其中参数都是由两个点确定的。令f:R2→Rf: \mathbb{R}^{2} \rightarrow \mathbb{R}f:R2→R，f(x,y)=ax+by+c.f(x, y)=a x+b y+c .f(x,y)=ax+by+c.那么，令f(x,y)<0f(x, y)<0f(x,y)<0代表了直线的左侧。令fi(x,y)f_{i}(x, y)fi(x,y)表示凸边形的第i条边从(xi,yi)\left(x_{i}, y_{i}\right)(xi,yi)到(xi+1,yi+1),1≤i<m,\left(x_{i+1}, y_{i+1}\right), 1 \leq i<m,(xi+1,yi+1),1≤i<m, 令fm(x,y)f_{m}(x, y)fm(x,y)表示(xm,ym)\left(x_{m}, y_{m}\right)(xm,ym) to (x1,y1)\left(x_{1}, y_{1}\right)(x1,y1)。因此半平面可以表示为W\mathcal{W}W的一个子集：
Hi={(x,y)∈W∣fi(x,y)≤0}H_{i}=\left\{(x, y) \in \mathcal{W} \mid f_{i}(x, y) \leq 0\right\} Hi={(x,y)∈W∣fi(x,y)≤0}一个m条边的凸多边形障碍区域可以表示为式（3.3）：
O=H1∩H2∩⋯∩Hm\mathcal{O}=H_{1} \cap H_{2} \cap \cdots \cap H_{m} O=H1∩H2∩⋯∩Hm
凹多边形（Nonconvex Polygons）：若O\mathcal{O}O是一个非凸多边形，可以表示为式（3.4）
O=O1∪O2∪⋯∪On\mathcal{O}=\mathcal{O}_{1} \cup \mathcal{O}_{2} \cup \cdots \cup \mathcal{O}_{n} O=O1∪O2∪⋯∪On其中Oi\mathcal{O_i}Oi是凸多边形，彼此之间可以相交。而表示非凸多边形的方式不唯一。一种高效的方法是将O\mathcal{O}O进行正交分解，会在第6章讲到。
定义逻辑符（Defining a logical predicate）：我们可以将定义逻辑符视为一个碰撞检测器。令ϕ:W→{\phi: \mathcal{W} \rightarrow\{ϕ:W→{ TRUE, FALSE }\}}，若返回true，则该世界中的点位于障碍物范围内。对于一条线f(x,y)=0f(x, y)=0f(x,y)=0，令e(x,y)=0e(x, y)=0e(x,y)=0代表一个逻辑判断，如果返回TRUE，则代表f(x,y)≤0f(x, y)\leq 0f(x,y)≤0.
关于凸多边形区域的逻辑判断可以表示为式（3.5）:
α(x,y)=e1(x,y)∧e2(x,y)∧⋯∧em(x,y)\alpha(x, y)=e_{1}(x, y) \wedge e_{2}(x, y) \wedge \cdots \wedge e_{m}(x, y) α(x,y)=e1(x,y)∧e2(x,y)∧⋯∧em(x,y)式中∧\wedge∧表示逻辑与,当上式返回TRUE，则表示点(x,y)(x,y)(x,y)在凸多边形内。
一个包含n个凸多边形的障碍物区域可以表示为式（3.6）：
ϕ(x,y)=α1(x,y)∨α2(x,y)∨⋯∨αn(x,y)\phi(x, y)=\alpha_{1}(x, y) \vee \alpha_{2}(x, y) \vee \cdots \vee \alpha_{n}(x, y) ϕ(x,y)=α1(x,y)∨α2(x,y)∨⋯∨αn(x,y)即使有更加快速有效的方法，但是式（3.6）可以检验点(x,y)(x,y)(x,y)是否在O(n)O(n)O(n)时刻在O\mathcal{O}O内。
多面体模型（Polyhedral models）：多面体模型可以由点、线和面组成，每个面都是平整的，双连通边列表作为一种数据结构包含了三种元素：面、半边（有向边）和点。每个顶点记录保存点坐标和指向与顶点接触的任意半边的指针。每个面记录包含指向其边界上任意半边的指针。多边形的每一条边都由一对半边记录。每一条半边包含了5个指向其他元素的指针：1）半边的起始点；2）另一半的半边（相邻面的边界，方向相反）；3）以该边为边界的面；4）在循环边列表里面的下一条半边；5）在循环边列表里面的上一条半边
现在考虑多边形的实心表示（solid representation），假设O\mathcal{O}O是一个凸多边形，如图3.3所示。每一个面至少由三个点构成，假设三个点不共线，则任意一个通过该直线的面可以表示为式（3.7）：
ax+by+cz+d=0,a,b,c,d∈Rax+by+cz+d=0,a,b,c,d \in \mathbb{R} ax+by+cz+d=0,a,b,c,d∈R令f:R3→Rf: \mathbb{R}^{3} \rightarrow \mathbb{R}f:R3→R，则如式（3.8）
f(x,y,z)=ax+by+cz+d.f(x,y,z)=ax+by+cz+d. f(x,y,z)=ax+by+cz+d.对于有mmm个面的凸多面体，每个半平面HiH_{i}Hi都是世界W\mathcal{W}W的子集
Hi={(x,y,z)∈W∣fi(x,y,z)≤0}H_{i}=\left\{(x, y, z) \in \mathcal{W} \mid f_{i}(x, y, z) \leq 0\right\} Hi={(x,y,z)∈W∣fi(x,y,z)≤0}凸多面体可以视为是一系列半平面的交集，非凸多面体可以视为是一系列半平面的并集。ϕ(x,y,z)\phi(x, y, z)ϕ(x,y,z)可以用类似的方式定义，当返回TRUE时(x,y,z)∈O(x, y, z) \in \mathcal{O}(x,y,z)∈O.
3.1.2 半代数模型
在多边形和多面体模型中，fff是一个多项式。在2D环境中，fff是关于x,yx,yx,y的多项式；在3D环境中，fff是关于x,y,zx,y,zx,y,z的多项式。半代数模型包含多边形和多面体模型，其中fff是一次多项式。由单个多项式元确定的点集称为代数集。由有限个代数集的并集和交集得到的点集称为半代数集。
考虑2D世界，一个实心表示可以通过代数单元集(algebraic primitives)表示：
H={(x,y)∈W∣f(x,y)≤0}H=\{(x, y) \in \mathcal{W} \mid f(x, y) \leq 0\} H={(x,y)∈W∣f(x,y)≤0}其中若f=x2+y2−4f=x^{2}+y^{2}-4f=x2+y2−4，则表示了一个圆盘，如图3.4(a)所示。
笑脸集（图3.4 (b)）:
f1=x2+y2−r12f2=−((x−x2)2+(y−y2)2−r22)f3=−((x−x3)2+(y−y3)2−r32)f4=−(x2/a2+(y−y4)2/b2−1)\begin{aligned} &f_{1}=x^{2}+y^{2}-r_{1}^{2} \\ &f_{2}=-\left(\left(x-x_{2}\right)^{2}+\left(y-y_{2}\right)^{2}-r_{2}^{2}\right) \\ &f_{3}=-\left(\left(x-x_{3}\right)^{2}+\left(y-y_{3}\right)^{2}-r_{3}^{2}\right) \\ &f_{4}=-\left(x^{2} / a^{2}+\left(y-y_{4}\right)^{2} / b^{2}-1\right) \end{aligned} f1=x2+y2−r12f2=−((x−x2)2+(y−y2)2−r22)f3=−((x−x3)2+(y−y3)2−r32)f4=−(x2/a2+(y−y4)2/b2−1)O=H1∩H2∩H3∩H4\mathcal{O}=H_{1} \cap H_{2} \cap H_{3} \cap H_{4} O=H1∩H2∩H3∩H4可以发现，一系列障碍物区域的交集不一定是凸集，因此逻辑运算符ϕ(x,y)\phi(x,y)ϕ(x,y)将继续起到作用。
在三维空间内，半代数方式表示的结果为(3.13)：
H={(x,y,z)∈W∣f(x,y,z)≤0}H=\{(x, y, z) \in \mathcal{W} \mid f(x, y, z) \leq 0\} H={(x,y,z)∈W∣f(x,y,z)≤0}
3.1.3 其他模型
模型类别的选择通常要依据规划算法所采取的操作。
非凸多边形和多面体(Nonconvex polygons and polyhedra): 3.1.1节采用图多边形的并集来表示非凸多边形。与此不同的是，使用边界表示方法来描述非凸多边形时，通过具有特殊排列顺序的点列表示。如图3.5所示，保证多边形在边的左侧。对于3d空间而言，则需要制定面、边、点的序列，而洞需要使用相反的方向重新建模。
3D三角形：令W=R3\mathcal{W}=\mathbb{R}^3W=R3，最简便的几何模型表示方法是一个三角形集合，每个三角形可以表示为一组点集(x1,y1,z1),(x2,y2,z2),(x3,y3,z3)(x_1,y_1,z_1),(x_2,y_2,z_2),(x_3,y_3,z_3)(x1,y1,z1),(x2,y2,z2),(x3,y3,z3)，如图3.6所示。没有可利用的一致性来简单地声明一个点是在 3D 障碍物的“内部”还是“外部” 。那么有时最好减少三角形坐标规范中的冗余（许多三角形将共享相同的角）。 采用了三角带（triangle strip），即是一系列三角形，且每对邻居有一个共同边。或者三角扇（triangle fan），即每个三角形共用一个点。
非均匀有理 B 样条(Nonuniform rational B-splines, NURBS)：这些被用于许多工程设计系统，以便在飞机或汽车车身设计等应用中方便地设计和调整曲面。 与作为隐式方程的半代数模型相比，NURBS 和其他样条是参数方程。 这使得渲染等计算更容易； 然而，其他的，如碰撞检测，变得更加困难。 这些模型可以在任何维度上定义。 这里给出了一个简短的 2D 公式。
C(u)=∑i=0nwiPiNi,k(u)∑i=0nwiNi,k(u)C(u)=\frac{\sum_{i=0}^{n} w_{i} P_{i} N_{i, k}(u)}{\sum_{i=0}^{n} w_{i} N_{i, k}(u)} C(u)=∑i=0nwiNi,k(u)∑i=0nwiPiNi,k(u)其中wi∈Rw_i \in \mathbb{R}wi∈R是权重，PiP_{i}Pi是控制点，Ni,kN_{i, k}Ni,k是 k 阶的归一化基函数（normalized basis functions of degree k），可以通过递归地(recursively)表示：
Ni,k(u)=(u−titi+k−ti)Ni,k−1(u)+(ti+k+1−uti+k+1−ti+1)Ni+1,k−1(u)N_{i, k}(u)=\left(\frac{u-t_{i}}{t_{i+k}-t_{i}}\right) N_{i, k-1}(u)+\left(\frac{t_{i+k+1}-u}{t_{i+k+1}-t_{i+1}}\right) N_{i+1, k-1}(u) Ni,k(u)=(ti+k−tiu−ti)Ni,k−1(u)+(ti+k+1−ti+1ti+k+1−u)Ni+1,k−1(u)递归的起点是
Ni,0(u)=1if ti≤u<ti+1,and Ni,0(u)=0otherwise N_{i, 0}(u)=1 \text { if } t_{i} \leq u<t_{i+1}, \text { and } N_{i, 0}(u)=0 \text { otherwise } Ni,0(u)=1 if ti≤u<ti+1, and Ni,0(u)=0 otherwise 结向量（knot vector）是实数值的非递减序列{t0,t1,...,tm}\left \{t_0, t_1,...,t_m \right \}{t0,t1,...,tm}，控制某些基本功能生效的时间间隔。
位图(Bitmaps):典型的位图是灰度图（gray-scale map 或占用网格（occupancy grid）。
超二次曲面 （Superquadrics）：除了使用多项式定义fif_ifi，还可以使用很多其他的函数类型。比如用超二次曲面来构建二次曲面。例如超椭球体：对W=R3\mathcal{W} =\mathbb{R}^3W=R3
(∣x/a∣n1+∣y/b∣n2)n1/n2+∣z/c∣n1−1≤0\left(|x / a|^{n_{1}}+|y / b|^{n_{2}}\right)^{n_{1} / n_{2}}+|z / c|^{n_{1}}-1 \leq 0 (∣x/a∣n1+∣y/b∣n2)n1/n2+∣z/c∣n1−1≤0 n1≥2,n2≥2n_1 \geq 2, n_2 \geq 2n1≥2,n2≥2, 如果 n1=n2=2n_1=n_2=2n1=n2=2,则产生椭圆。随着n1，n2n_1，n_2n1，n2增加，超椭球变成一个圆角的盒子（b站小电视）。
广义圆柱体（Generalized cylinders）：广义圆柱体的中心轴线是一条曲线(x(s),y(s),z(s)),s∈[0,1](x(s), y(s), z(s)),s \in [0,1](x(s),y(s),z(s)),s∈[0,1]，而圆柱的半径也是一个沿着轴变化的变量r(s)r(s)r(s).
3.2 刚体变换
3.2.1 基本概念
令O\mathcal{O}O代表障碍物区域，令A\mathcal{A}A代表机器人区域,它是“移动”的 ，可能是R2\mathbb{R^2}R2或者R3\mathbb{R^3}R3，这与W\mathcal{W}W一致。刚体变换是函数h:A→Wh:\mathcal{A} \to\mathcal{W}h:A→W，同时必须保证1）A\mathcal{A}A中的点与点之间的距离不变；2）A\mathcal{A}A的方向保留（不出现镜像）。h(a)h(a)h(a)表示映射到W\mathcal{W}W中的点，
h(A)={h(a)∈W∣a∈A}h(\mathcal{A})=\{h(a) \in \mathcal{W} \mid a \in \mathcal{A}\} h(A)={h(a)∈W∣a∈A}这是 hhh 的映射，表示变换后的机器人在 W\mathcal{W}W中占据的所有点。
机器人模型变换:若A\mathcal{A}A表示R2\mathbb{R^2}R2内的指定点，若采用边界表示方法，则很容易的采用h函数映射到世界中。若采用实心表示中的半平面集合模型，则需要先定义w∈Ww \in \mathcal{W}w∈W,作为h(a)∈Wh(a) \in \mathcal{W}h(a)∈W的逆函数，a=h−1(w)a=h^{-1}(w)a=h−1(w).
h(Hi)={w∈W∣fi(h−1(w))≤0}h\left(H_{i}\right)=\left\{w \in \mathcal{W} \mid f_{i}\left(h^{-1}(w)\right) \leq 0\right\} h(Hi)={w∈W∣fi(h−1(w))≤0}
一个参数化的变换族 （A parameterized family of transformation）：定义参数向量q∈Rnq \in \mathbb{R}^nq∈Rn，变换后的点为h(q,a)h(q,a)h(q,a)，机器人集合变换后为h(q,A)⊂Wh(q,\mathcal{A}) \subset \mathcal{W}h(q,A)⊂W. h(q,a)h(q,a)h(q,a)采用简记形式a(q)a(q)a(q)，h(q,A)h(q,\mathcal{A})h(q,A)采用简记形式A(q)\mathcal{A}(q)A(q)。这个符号表明，通过调整参数 qqq，机器人A\mathcal{A}A在W\mathcal{W}W中四处移动，因为从预定的族中选择了不同的变换。 A(q)\mathcal{A}(q)A(q)可以被考虑为一个设定值函数，当A\mathcal{A}A被 qqq 变换时，产生W\mathcal{W}W中被A\mathcal{A}A占据的点集。
定义框架（Defining frames）：A\mathcal{A}A（body frame）和W\mathcal{W}W (world frame)属于不同的坐标系，变换可以视为是一种坐标变换。
3.2.2 二维变换
平移：刚体机器人A⊂R2\mathcal{A} \subset \mathbb{R}^2A⊂R2的平移使用了q=(xt,yt),xt,yt∈Rq=(x_t,y_t),x_t,y_t \in \mathbb{R}q=(xt,yt),xt,yt∈R. 边界表示法表示的A\mathcal{A}A可以用下式依序移动多边形的边界点实现
h(x,y)=(x+xt,y+yt)h(x,y)=(x+x_t,y+y_t) h(x,y)=(x+xt,y+yt)对于实心表示法表示的A\mathcal{A}A，定义一些列的元集（primitives）：
Hi={(x,y)∈W∣f(x,y)≤0}H_{i}=\left\{(x, y) \in \mathcal{W} \mid f(x, y) \leq 0\right\} Hi={(x,y)∈W∣f(x,y)≤0}变换为h(Hi)={(x,y)∈W∣f(x−xt,y−yt)≤0}.h\left(H_{i}\right)=\left\{(x, y) \in \mathcal{W} \mid f\left(x-x_{t}, y-y_{t}\right) \leq 0\right\} . h(Hi)={(x,y)∈W∣f(x−xt,y−yt)≤0}.变换的两种方式：1）世界坐标系固定，移动机器人；2）机器人固定，移动坐标系。本书采用第一种方式。
旋转：(x,y)∈A(x,y) \in \mathcal{A}(x,y)∈A通过逆时针旋转θ∈[0,2π]\theta \in [0,2\pi]θ∈[0,2π].
(x,y)↦(xcosθ−ysinθ,xsinθ+ycosθ)(x, y) \mapsto(x \cos \theta-y \sin \theta, x \sin \theta+y \cos \theta) (x,y)↦(xcosθ−ysinθ,xsinθ+ycosθ)其中2x2的旋转矩阵R如下所示
R(θ)=(cosθ−sinθsinθcosθ)R(\theta)=\left(\begin{array}{cc} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right) R(θ)=(cosθsinθ−sinθcosθ)因此旋转可以表示为矩阵形式
(xcosθ−ysinθxsinθ+ycosθ)=R(θ)(xy)\left(\begin{array}{l} x \cos \theta-y \sin \theta \\ x \sin \theta+y \cos \theta \end{array}\right)=R(\theta)\left(\begin{array}{l} x \\ y \end{array}\right) (xcosθ−ysinθxsinθ+ycosθ)=R(θ)(xy)使用3.2.1里的概念，R(θ)R(\theta)R(θ)就是h(q)h(q)h(q),q=θq=\thetaq=θ.
平移和旋转
(cosθ−sinθxtsinθcosθyt001)(xy1)=(xcosθ−ysinθ+xtxsinθ+ycosθ+yt1)\left(\begin{array}{ccc} \cos \theta & -\sin \theta & x_{t} \\ \sin \theta & \cos \theta & y_{t} \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} x \\ y \\ 1 \end{array}\right)=\left(\begin{array}{c} x \cos \theta-y \sin \theta+x_{t} \\ x \sin \theta+y \cos \theta+y_{t} \\ 1 \end{array}\right) ⎝⎛cosθsinθ0−sinθcosθ0xtyt1⎠⎞⎝⎛xy1⎠⎞=⎝⎛xcosθ−ysinθ+xtxsinθ+ycosθ+yt1⎠⎞定义3x3的齐次变换矩阵TTT:
T=(cosθ−sinθxtsinθcosθyt001)T=\left(\begin{array}{ccc} \cos \theta & -\sin \theta & x_{t} \\ \sin \theta & \cos \theta & y_{t} \\ 0 & 0 & 1 \end{array}\right) T=⎝⎛cosθsinθ0−sinθcosθ0xtyt1⎠⎞
3.2.3 三维变换
3D平移：(x,y,z)↦(x+xt,y+yt,z+zt)(x, y, z) \mapsto\left(x+x_{t}, y+y_{t}, z+z_{t}\right)(x,y,z)↦(x+xt,y+yt,z+zt)，对于Hi={(x,y,z)∈W∣fi(x,y,z)≤0}H_{i}=\left\{(x, y, z) \in \mathcal{W} \mid f_{i}(x, y, z) \leq 0\right\} Hi={(x,y,z)∈W∣fi(x,y,z)≤0}变换为{(x,y,z)∈W∣fi(x−xt,y−yt,z−zt)≤0}\left\{(x, y, z) \in \mathcal{W} \mid f_{i}\left(x-x_{t}, y-y_{t}, z-z_{t}\right) \leq 0\right\} {(x,y,z)∈W∣fi(x−xt,y−yt,z−zt)≤0}
偏航、俯仰和横滚旋转
偏航（yaw）：z轴旋转：Rz(α)=(cosα−sinα0sinαcosα0001)R_{z}(\alpha)=\left(\begin{array}{ccc} \cos \alpha & -\sin \alpha & 0 \\ \sin \alpha & \cos \alpha & 0 \\ 0 & 0 & 1 \end{array}\right) Rz(α)=⎝⎛cosαsinα0−sinαcosα0001⎠⎞
俯仰（pitch）：y轴旋转：Ry(β)=(cosβ0sinβ010−sinβ0cosβ)R_{y}(\beta)=\left(\begin{array}{ccc} \cos \beta & 0 & \sin \beta \\ 0 & 1 & 0 \\ -\sin \beta & 0 & \cos \beta \end{array}\right) Ry(β)=⎝⎛cosβ0−sinβ010sinβ0cosβ⎠⎞
横滚（roll）：x轴旋转：Rx(γ)=(1000cosγ−sinγ0sinγcosγ)R_{x}(\gamma)=\left(\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \gamma & -\sin \gamma \\ 0 & \sin \gamma & \cos \gamma \end{array}\right) Rx(γ)=⎝⎛1000cosγsinγ0−sinγcosγ⎠⎞得到角度旋转合成的旋转矩阵：R(α,β,γ)=Rz(α)Ry(β)Rx(γ)=(cosαcosβcosαsinβsinγ−sinαcosγcosαsinβcosγ+sinαsinγsinαcosβsinαsinβsinγ+cosαcosγsinαsinβcosγ−cosαsinγ−sinβcosβsinγcosβcosγ).\begin{aligned} &R(\alpha, \beta, \gamma)=R_{z}(\alpha) R_{y}(\beta) R_{x}(\gamma)= \\ &\qquad\left(\begin{array}{ccc} \cos \alpha \cos \beta & \cos \alpha \sin \beta \sin \gamma-\sin \alpha \cos \gamma & \cos \alpha \sin \beta \cos \gamma+\sin \alpha \sin \gamma \\ \sin \alpha \cos \beta & \sin \alpha \sin \beta \sin \gamma+\cos \alpha \cos \gamma & \sin \alpha \sin \beta \cos \gamma-\cos \alpha \sin \gamma \\ -\sin \beta & \cos \beta \sin \gamma & \cos \beta \cos \gamma \end{array}\right) . \end{aligned} R(α,β,γ)=Rz(α)Ry(β)Rx(γ)=⎝⎛cosαcosβsinαcosβ−sinβcosαsinβsinγ−sinαcosγsinαsinβsinγ+cosαcosγcosβsinγcosαsinβcosγ+sinαsinγsinαsinβcosγ−cosαsinγcosβcosγ⎠⎞.
通过旋转矩阵得到rpy角度：对于(r11r12r13r21r22r23r31r32r33)\left(\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right) ⎝⎛r11r21r31r12r22r32r13r23r33⎠⎞得到α,β,γ\alpha,\beta,\gammaα,β,γ,其中tan−1\tan ^{-1}tan−1可以用C语言中的atan2替代： α=tan−1(r21/r11)β=tan−1(−r31/r322+r332)γ=tan−1(r32/r33)\begin{gathered} \alpha=\tan ^{-1}\left(r_{21} / r_{11}\right) \\ \beta=\tan ^{-1}\left(-r_{31} / \sqrt{r_{32}^{2}+r_{33}^{2}}\right)\\ \gamma=\tan ^{-1}\left(r_{32} / r_{33}\right) \end{gathered} α=tan−1(r21/r11)β=tan−1(−r31/r322+r332)γ=tan−1(r32/r33)其中r11≠0,r33≠0r_{11} \neq 0,r_{33} \neq 0r11=0,r33=0
3D齐次变换矩阵：顺序rpy，平移T=(cosαcosβcosαsinβsinγ−sinαcosγcosαsinβcosγ+sinαsinγxtsinαcosβsinαsinβsinγ+cosαcosγsinαsinβcosγ−cosαsinγyt−sinβcosβsinγcosβcosγzt0001).T=\left(\begin{array}{cccc} \cos \alpha \cos \beta & \cos \alpha \sin \beta \sin \gamma-\sin \alpha \cos \gamma & \cos \alpha \sin \beta \cos \gamma+\sin \alpha \sin \gamma & x_{t} \\ \sin \alpha \cos \beta & \sin \alpha \sin \beta \sin \gamma+\cos \alpha \cos \gamma & \sin \alpha \sin \beta \cos \gamma-\cos \alpha \sin \gamma & y_{t} \\ -\sin \beta & \cos \beta \sin \gamma & \cos \beta \cos \gamma & z_{t} \\ 0 & 0 & 0 & 1 \end{array}\right) . T=⎝⎜⎜⎛cosαcosβsinαcosβ−sinβ0cosαsinβsinγ−sinαcosγsinαsinβsinγ+cosαcosγcosβsinγ0cosαsinβcosγ+sinαsinγsinαsinβcosγ−cosαsinγcosβcosγ0xtytzt1⎠⎟⎟⎞.使用3.2.1节概念，A(xt,yt,zt,α,β,γ)\mathcal{A}(x_t,y_t,z_t,\alpha,\beta,\gamma)A(xt,yt,zt,α,β,γ)
3.3 物体运动链的变换
对于一组链式链接的刚体机器人，变换变得更加复杂。为了简便处理，将机器人视为连杆(link)，令A1,A2,...,Am\mathcal{A}_1,\mathcal{A}_2,...,\mathcal{A}_mA1,A2,...,Am表示一组连杆，对于1≤i≤m1 \leq i \leq m1≤i≤m连杆Ai\mathcal{A}_iAi与Ai+1\mathcal{A}_{i+1}Ai+1以某种方式连接，使得这允许Ai+1\mathcal{A}_{i+1}Ai+1 相对于Ai\mathcal{A}_{i}Ai 进行一些受约束的运动。 运动约束必须显示的给出。本节考虑在单个链中连接的刚体，并将这特定链接称为运动链。
3.3.1 二维运动链
假设两个不相交的机器人A1,A2⊂W\mathcal{A}_1,\mathcal{A}_2 \sub \mathcal{W}A1,A2⊂W，每个拥有3自由度，q=(x1,y1,z1,x2,y2,z2)q=(x_1,y_1,z_1,x_2,y_2,z_2)q=(x1,y1,z1,x2,y2,z2)
对于运动链，如下图所示，T1T2⋯Tm(xy1)T_{1} T_{2} \cdots T_{m} \left( \begin{array}{l}x \\ y \\ 1\end{array} \right)T1T2⋯Tm⎝⎛xy1⎠⎞
其中Ti=(cosθi−sinθiai−1sinθicosθi0001)T_{i}=\left(\begin{array}{ccc} \cos \theta_{i} & -\sin \theta_{i} & a_{i-1} \\ \sin \theta_{i} & \cos \theta_{i} & 0 \\ 0 & 0 & 1 \end{array}\right) Ti=⎝⎛cosθisinθi0−sinθicosθi0ai−101⎠⎞
三维运动链：运动副约束如下所示。
引入DH参数，如下图：
- 沿 ziz_izi 轴平移 did_idi 。
- 绕 ziz_izi 轴逆时针旋转 θi\theta_iθi 。
- 沿 xi−1x_{i-1}xi−1 轴平移 ai−1a_{i-1}ai−1 。
- 绕 xi−1x_{i-1}xi−1 轴逆时针旋转αi−1\alpha_{i-1}αi−1 。
Ri=(cosθi−sinθi00sinθicosθi00001di0001)R_{i}=\left(\begin{array}{cccc} \cos \theta_{i} & -\sin \theta_{i} & 0 & 0 \\ \sin \theta_{i} & \cos \theta_{i} & 0 & 0 \\ 0 & 0 & 1 & d_{i} \\ 0 & 0 & 0 & 1 \end{array}\right) Ri=⎝⎜⎜⎛cosθisinθi00−sinθicosθi00001000di1⎠⎟⎟⎞
Qi−1=(100ai−10cosαi−1−sinαi−100sinαi−1cosαi−100001)Q_{i-1}=\left(\begin{array}{cccc} 1 & 0 & 0 & a_{i-1} \\ 0 & \cos \alpha_{i-1} & -\sin \alpha_{i-1} & 0 \\ 0 & \sin \alpha_{i-1} & \cos \alpha_{i-1} & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) Qi−1=⎝⎜⎜⎛10000cosαi−1sinαi−100−sinαi−1cosαi−10ai−1001⎠⎟⎟⎞
Ti=Qi−1Ri=(cosθi−sinθi0ai−1sinθicosαi−1cosθicosαi−1−sinαi−1−sinαi−1disinθisinαi−1cosθisinαi−1cosαi−1cosαi−1di0001).T_{i}=Q_{i-1} R_{i}=\left(\begin{array}{cccc}\cos \theta_{i} & -\sin \theta_{i} & 0 & a_{i-1} \\ \sin \theta_{i} \cos \alpha_{i-1} & \cos \theta_{i} \cos \alpha_{i-1} & -\sin \alpha_{i-1} & -\sin \alpha_{i-1} d_{i} \\ \sin \theta_{i} \sin \alpha_{i-1} & \cos \theta_{i} \sin \alpha_{i-1} & \cos \alpha_{i-1} & \cos \alpha_{i-1} d_{i} \\ 0 & 0 & 0 & 1\end{array}\right) . Ti=Qi−1Ri=⎝⎜⎜⎛cosθisinθicosαi−1sinθisinαi−10−sinθicosθicosαi−1cosθisinαi−100−sinαi−1cosαi−10ai−1−sinαi−1dicosαi−1di1⎠⎟⎟⎞.
3.4 运动树变换
在多机器人搬运问题中，每个机器人与物体均有接触，即形成一条运动链，而多机搬运则形成了运动树，
3.5 非刚性变换
以可变形物体为例，需要涉及到非刚性变换。
线性变换：旋转时线性变换的特殊例子，通过矩阵Mn×nM_{n \times n}Mn×n，
机器人的缩放矩阵，通过m11m_{11}m11在x轴上缩放，通过m22m_{22}m22在y轴上缩放，若其中有一个是负数，则会产生镜像。
(m1100m22)(xy)\left(\begin{array}{cc} m_{11} & 0 \\ 0 & m_{22} \end{array}\right)\left(\begin{array}{l} x \\ y \end{array}\right) (m1100m22)(xy)
m12=1m_{12}=1m12=1如图3.28所示
(1m1201)(xy)\left(\begin{array}{cc} 1 & m_{12} \\ 0 & 1 \end{array}\right)\left(\begin{array}{l} x \\ y \end{array}\right) (10m121)(xy)
可变形材料：在某些应用中，存在超越线性变换的情况。 想象一下，试图扭曲一种柔性材料使其通过一个走廊，例如床垫。 床垫可以用二维链接阵列来近似； 然而，复杂性和自由度会太麻烦。 再举一个例子，假设一个蛇形机器人是通过将 100 个旋转关节连接在一起而设计的。 3.3 节中的工具可用于将其转换为 100 个旋转参数，1，… . ., 100，但这对于在规划算法中使用可能变得笨拙。 另一种方法是用可变形的曲线或形状来近似蛇。
对于此类问题，最好使用参数化的曲线或曲面族。 样条模型通常是最合适的，因为它们旨在通过调整少量参数来轻松控制曲线的形状。 其他可能性包括第 3.1.3 节中提到的广义圆柱和超二次曲面模型。
一种复杂情况是可能对允许参数的空间施加复杂的约束。 例如，蛇形机器人的每个关节都可以有很小的旋转范围。 这将很容易使用运动链进行建模； 然而，确定样条族中的哪些样条满足这个额外的约束可能很困难。 同样，对于操纵柔性材料，通常存在基于材料弹性的复杂约束。 即使在某些力的作用下确定其正确的形状，也需要整合一个材料上的弹性能量函数。
《Planning Algorithm》 第三章：几何表示与变换相关推荐
- OpenGL学习——入门篇 第三章 四个变换及模拟地球公转
一.四个变换 1.1 视图变换:不同位置观察它: 涉及函数: glMatrixMode(GL_MODELVIEW);//设置当前操作的矩阵为"模型视图矩阵" glLoadIdent ...
- dx12 龙书第三章学习笔记 -- 变换
1.线性变换: 函数的输入和输出都是3D向量,我们称为线性变换 矩阵表示法: ⭐所以已知一个线性变换,只要将i,j,z也就是标准基向量代入线性变换,就能构造一个变换矩阵 A:线性变换的矩阵表示法 ...
- WebGL学习笔记 第三章绘制和变换三角形
- 交互式计算机图形学总结：第三章 几何对象和变换
第三章 几何对象和变换 基本概念 –零向量:长度为零,方向没有定义 –向量空间:包含向量和标量 –仿射空间:包含向量.标量.点 –计算机科学的观点:把向量.标量.点看作抽象数据类型(ADT) –凸性: ...
- 计算机控制z反变换公式,第三章 计算机控制系统的数学描述(修正Z变换).ppt
第三章 计算机控制系统的数学描述(修正Z变换) * §3.4 修改Z变换 1．具有多采样频率系统 在某些控制系统中,存在着不同采样频率的采样开关,如图3.10所示. 图3.10 具有不同采样频率系统结 ...
- 第三章，矩阵，06-初等变换与初等矩阵
第三章,矩阵,06-初等变换与初等矩阵 标准形 初等行变换 初等列变换 初等变换 行阶梯形矩阵 行最简形 标准形 初等矩阵 定义 性质 可逆 等价矩阵 相关定义 行等价 列等价 等价 性质 定理1 定 ...
- CV：翻译并解读2019《A Survey of the Recent Architectures of Deep Convolutional Neural Networks》第一章~第三章
CV:翻译并解读2019<A Survey of the Recent Architectures of Deep Convolutional Neural Networks>第一章~第三 ...
- 信息安全工程师学习笔记《第三章》
第三章 密码学基本理论 本章讲述了密码学的基本概念以及常见的密码体制.密码算法,分析了杂凑函数.数字签名.国产密码算法.安全协议等的工作原理:本章还分析了密码在网络安全方面的应用场景类型. 3.1密码 ...
- [翻译] 神经网络与深度学习 第三章 提升神经网络学习的效果 - Chapter 3 Improving the way neural networks learn
目录: 首页 译序 关于本书 关于习题和难题 第一章 利用神经网络识别手写数字 第二章 反向传播算法是如何工作的 > 第三章 提升神经网络学习的效果 第四章 可视化地证明神经网络可以计算任何函数 ...
最新文章
- androidstudio判断手指滑动方向_方向盘的黑科技有多“黑”
- php变量定义的位置,php变量一般放在哪个位置
- OpenCV之imgproc 模块. 图像处理（1）图像平滑处理 腐蚀与膨胀(Eroding and Dilating) 更多形态学变换 图像金字塔 基本的阈值操作
- re-for-50-plz-50 寒假逆向生涯（6/100）
- 记载下这个题中的语法（对这些语法的使用不熟悉）
- 如何理解 Graph Convolutional Network (GCN)？
- 深度学习在美团点评推荐平台排序中的应用 widedeep推荐系统模型--学习笔记
- Linux定时检测内存，若使用率超过指标，重启Tomcat并清空内存
- SQL on and 和 on where 的区别
- 洛谷P1420 最长连号
- windows mysql 备份_Windows下MySQL数据库备份脚本（二） | 系统运维
- PostgreSQL Json字段作为查询条件的解决方案
- 字符串的一般性和特殊性
- Ubuntu 18.04安装c++版OpenCV4
- lisp 车位块自动编号_CAD如何生成自动编号
- 锐捷（三）清除交换机的虚拟化(VSU)配置
- JRE和JDK的区别（笔记）
- Laravel+layui后台开发框架
- DNSPod十问李亚飞：小公司如何破解程序员的招聘迷局？
- linux运维培训大纲,Linux运维体系大纲