营销网站建设计划书,广州购物网站公司地址,wordpress去除版本号,做房产中介网站文章目录 02 三维空间的刚体运动2.0 机器人位姿表述2.1 点和坐标系2.1.1 三维坐标系有关表述2.1.2 坐标系变换 2.2 旋转向量和欧拉角2.2.1 旋转向量2.2.2 欧拉角 2.3 四元数2.3.1 四元数的定义2.3.2 四元数的计算2.3.3 四元数表示旋转2.3.4 四元数与其他旋转表示法的转换 2.4 相… 文章目录 02 三维空间的刚体运动2.0 机器人位姿表述2.1 点和坐标系2.1.1 三维坐标系有关表述2.1.2 坐标系变换 2.2 旋转向量和欧拉角2.2.1 旋转向量2.2.2 欧拉角 2.3 四元数2.3.1 四元数的定义2.3.2 四元数的计算2.3.3 四元数表示旋转2.3.4 四元数与其他旋转表示法的转换 2.4 相似、仿射、射影变换 02 三维空间的刚体运动
2.0 机器人位姿表述
1二维与三维空间中机器人位姿的表述 2D 的情况横纵坐标 旋转角机器人朝向即 [ x , y , θ ] [x, y, \theta] [x,y,θ] 3D 的情况三维空间中的旋转和平移
2.1 点和坐标系
2.1.1 三维坐标系有关表述
1基本运算 加减法 内积点乘 a ⋅ b a T b ∑ i 1 3 a i × b i ∣ a ∣ ∣ b ∣ c o s a , b \boldsymbol{a}\cdot\boldsymbol{b}\boldsymbol{a}^T\boldsymbol{b}\sum_{i1}^{3}a_{i}\times b_{i} |\boldsymbol{a}| |\boldsymbol{b}|cos\boldsymbol{a}, \boldsymbol{b} a⋅baTb∑i13ai×bi∣a∣∣b∣cosa,b且 a ⋅ b b ⋅ a \boldsymbol{a}\cdot\boldsymbol{b} \boldsymbol{b}\cdot\boldsymbol{a} a⋅bb⋅a 外积叉乘 a × b ∥ e 1 e 2 e 3 a 1 a 2 a 3 b 1 b 2 b 3 ∥ ( a 2 b 3 − a 3 b 2 ) e 1 ( a 3 b 1 − a 1 b 3 ) e 2 ( a 1 b 3 − a 3 b 1 ) e 3 [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b def a ∧ b (2-1) \begin{aligned} \boldsymbol{a} \times \boldsymbol{b}\left\|\begin{array}{ccc} \boldsymbol{e}_{1} \boldsymbol{e}_{2} \boldsymbol{e}_{3} \\ a_{1} a_{2} a_{3} \\ b_{1} b_{2} b_{3} \end{array}\right\| (a_2b_3-a_3b_2)\boldsymbol{e}_{1}(a_3b_1-a_1b_3)\boldsymbol{e}_{2}(a_1b_3-a_3b_1)\boldsymbol{e}_{3} \\ \left[\begin{array}{c} a_{2} b_{3}-a_{3} b_{2} \\ a_{3} b_{1}-a_{1} b_{3} \\ a_{1} b_{2}-a_{2} b_{1} \end{array}\right]\left[\begin{array}{ccc} 0 -a_{3} a_{2} \\ a_{3} 0 -a_{1} \\ -a_{2} a_{1} 0 \end{array}\right] \boldsymbol{b}\stackrel{\text { def }}{}\boldsymbol{a}^{\wedge} \boldsymbol{b} \end{aligned} \tag{2-1} a×b e1a1b1e2a2b2e3a3b3 (a2b3−a3b2)e1(a3b1−a1b3)e2(a1b3−a3b1)e3 a2b3−a3b2a3b1−a1b3a1b2−a2b1 0a3−a2−a30a1a2−a10 b def a∧b(2-1) 也即 a × b ∣ a ∣ ∣ b ∣ s i n a , b \boldsymbol{a} \times \boldsymbol{b} |\boldsymbol{a}| |\boldsymbol{b}|sin\boldsymbol{a}, \boldsymbol{b} a×b∣a∣∣b∣sina,b方向垂直于向量 a \boldsymbol{a} a、 b \boldsymbol{b} b 组成的平面遵循右手定则。
性质 a × b − b × a \boldsymbol{a} \times \boldsymbol{b} - \boldsymbol{b} \times \boldsymbol{a} a×b−b×a
为便于后续表达记 a ∧ [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] (2-2) \boldsymbol{a}^{\wedge}\left[\begin{array}{ccc} 0 -a_{3} a_{2} \\ a_{3} 0 -a_{1} \\ -a_{2} a_{1} 0 \end{array}\right] \tag{2-2} a∧ 0a3−a2−a30a1a2−a10 (2-2) 这是一个反对称矩阵满足 A T − A A^T-A AT−A。
2.1.2 坐标系变换
1三维空间向量表示 a [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] a 1 e 1 a 2 e 2 a 3 e 3 (2-3) \boldsymbol{a}\left[\boldsymbol{e_1},\boldsymbol{e_2},\boldsymbol{e_3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]a_1\boldsymbol{e_1}a_2\boldsymbol{e_2}a_3\boldsymbol{e_3} \tag{2-3} a[e1,e2,e3] a1a2a3 a1e1a2e2a3e3(2-3) 其中 e 1 、 e 2 、 e 3 \boldsymbol{e_1}、\boldsymbol{e_2}、\boldsymbol{e_3} e1、e2、e3为基向量。
2两个坐标系之间的运动由一个旋转加一个平移组成这种运动称为刚体运动。
3对于坐标系旋转变换假设原坐标系和现坐标系单位正交基底分别为 [ e 1 , e 2 , e 3 ] [\boldsymbol{e_1}, \boldsymbol{e_2}, \boldsymbol{e_3}] [e1,e2,e3]和 [ e 1 ′ , e 2 ′ , e 3 ′ ] [\boldsymbol{e_1}, \boldsymbol{e_2}, \boldsymbol{e_3}] [e1′,e2′,e3′]
坐标分别为 [ a 1 , a 2 , a 3 ] T [a_1, a_2, a_3]^T [a1,a2,a3]T和 [ a 1 ′ , a 2 ′ , a 3 ′ ] T [a_1, a_2, a_3]^T [a1′,a2′,a3′]T由于向量本身的绝对位置没有改变则 [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] (2-4) \left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]\left[\boldsymbol{e}_{1}^{\prime}, \boldsymbol{e}_{2}^{\prime}, \boldsymbol{e}_{3}^{\prime}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \tag{2-4} [e1,e2,e3] a1a2a3 [e1′,e2′,e3′] a1′a2′a3′ (2-4)
将上式两端分别左乘 [ e 1 T e 2 T e 3 T ] \left[\begin{array}{l} \boldsymbol{e_{1}^T} \\ \boldsymbol{e}_{2}^T \\ \boldsymbol{e}_{3}^T \end{array}\right] e1Te2Te3T 得到 [ a 1 a 2 a 3 ] [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] def R a ′ (2-5) \left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]\left[\begin{array}{lll} \boldsymbol{e}_{1}^T \boldsymbol{e}_{1}^{\prime} \boldsymbol{e}_{1}^T \boldsymbol{e}_{2}^{\prime} \boldsymbol{e}_{1}^T \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{2}^T \boldsymbol{e}_{1}^{\prime} \boldsymbol{e}_{2}^T \boldsymbol{e}_{2}^{\prime} \boldsymbol{e}_{2}^T \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{3}^T \boldsymbol{e}_{1}^{\prime} \boldsymbol{e}_{3}^T \boldsymbol{e}_{2}^{\prime} \boldsymbol{e}_{3}^T \boldsymbol{e}_{3}^{\prime} \end{array}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \stackrel{\text { def }}{} \boldsymbol{R} \boldsymbol{a}^{\prime} \tag{2-5} a1a2a3 e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′ a1′a2′a3′ def Ra′(2-5)
矩阵 R \boldsymbol{R} R即为旋转矩阵。它有如下性质 R \boldsymbol{R} R 行列式为 1; R \boldsymbol{R} R 是一个正交矩阵即满足 R T R I \boldsymbol{R}^T\boldsymbol{R}\boldsymbol{I} RTRI。坐标系 1 到坐标系 2 有 a 1 R 12 a 2 \boldsymbol{a_1}\boldsymbol{R_{12}}\boldsymbol{a_2} a1R12a2反之有 a 2 R 21 a 1 \boldsymbol{a_2}\boldsymbol{R_{21}}\boldsymbol{a_1} a2R21a1其中 R 12 R 21 − 1 R 21 T \boldsymbol{R_{12}}\boldsymbol{R_{21}}^{-1}\boldsymbol{R_{21}}^T R12R21−1R21T
将满足此性质的矩阵集合定义为特殊正交群 S O ( n ) { R ∈ R n × n ∣ R R T I , det ( R ) 1 } (2-6) \mathrm{SO}(n)\left\{\boldsymbol{R} \in \mathbb{R}^{n \times n} \mid \boldsymbol{R} \boldsymbol{R}^{\mathrm{T}}\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})1\right\} \tag{2-6} SO(n){R∈Rn×n∣RRTI,det(R)1}(2-6)
4旋转加平移
可以将三维空间的刚体运动分解为平移以及旋转运动满足 a ′ R a t (2-7) \boldsymbol{a^{\prime}}\boldsymbol{Rat} \tag{2-7} a′Rat(2-7)
5齐次坐标与旋转矩阵参考机器人学中的坐标变换
为便于表达写成齐次形式 [ a ′ 1 ] [ R t 0 T 1 ] [ a 1 ] def T [ a 1 ] (2-8) \left[\begin{array}{l} \boldsymbol{a}^{\prime} \\ 1 \end{array}\right]\left[\begin{array}{ll} \boldsymbol{R} \boldsymbol{t} \\ \mathbf{0}^{\mathrm{T}} 1 \end{array}\right]\left[\begin{array}{l} \boldsymbol{a} \\ 1 \end{array}\right] \stackrel{\text { def }}{} \boldsymbol{T}\left[\begin{array}{l} \boldsymbol{a} \\ 1 \end{array}\right] \tag{2-8} [a′1][R0Tt1][a1] def T[a1](2-8)
其中 T \boldsymbol{T} T即为变换矩阵注意左乘右乘区别。
绕静坐标系世界坐标系旋转即左乘绕自身坐标系旋转即右乘
这种形式的矩阵集合定义为特殊欧式群即 S E ( 3 ) { T [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } (2-9) \mathrm{SE}(3)\left\{\boldsymbol{T}\left[\begin{array}{ll} \boldsymbol{R} \boldsymbol{t} \\ \mathbf{0}^{\mathrm{T}} 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in \mathrm{SO}(3), \boldsymbol{t} \in \mathbb{R}^{3}\right\} \tag{2-9} SE(3){T[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}(2-9)
同样地逆方向的变换为 T − 1 [ R T − R T t 0 T 1 ] (2-10) \boldsymbol{T}^{-1}\left[\begin{array}{cc} \boldsymbol{R}^{\mathrm{T}} -\boldsymbol{R}^{\mathrm{T}} \boldsymbol{t} \\ \mathbf{0}^{\mathrm{T}} 1 \end{array}\right] \tag{2-10} T−1[RT0T−RTt1](2-10)
2.2 旋转向量和欧拉角
2.2.1 旋转向量
1三维空间中刚体运动有六个自由度旋转和平移各三个。显然无论是上述的旋转矩阵9个量还是变换矩阵16个量都有很大的冗余并且矩阵中的元素相互关联这不利于后续的非线性优化计算。因此我们希望找到一种更为紧凑的表达方式。
2任意一个旋转都可以用旋转轴和旋转角刻画因此可以用旋转向量也称为角轴或轴角来表达即向量方向为旋转轴其模长为旋转角。
3罗德里格斯公式描述了旋转矩阵和旋转向量之间的关系。 R cos ( θ I ) ( 1 − cos θ ) n n T sin ( θ n ∧ ) (2-11) \boldsymbol{R}\cos (\theta\boldsymbol{I}) \left( 1- \cos \theta\right) \boldsymbol{n} {\boldsymbol{n}}^T\sin (\theta\boldsymbol{n}^{\wedge}) \tag{2-11} Rcos(θI)(1−cosθ)nnTsin(θn∧)(2-11)
反之有 θ a r c c o s ( t r ( R ) − 1 2 ) (2-12) \thetaarccos(\frac{tr(\boldsymbol{R})-1}{2}) \tag{2-12} θarccos(2tr(R)−1)(2-12) t r ( R ) tr(\boldsymbol{R}) tr(R)表示求迹即矩阵对角线元素之和。
由于旋转轴在旋转过程中是不动的则有 R n n (2-13) \boldsymbol{R}\boldsymbol{n}\boldsymbol{n} \tag{2-13} Rnn(2-13) 这说明 n \boldsymbol{n} n是矩阵 R \boldsymbol{R} R特征值为 1 对应的特征向量由此可以求出向量 n \boldsymbol{n} n的值。
2.2.2 欧拉角
1将旋转分解为 X、Y、Z 三个方向上的转动常用的是 ZYX 顺序的旋转
绕 Z 轴旋转得到偏航角 yaw绕旋转后的 Y 轴旋转得到俯仰角 pitch绕旋转后的 X 轴旋转得到滚转角 roll。
此时使用 [ y , p , r ] T [y, p, r]^T [y,p,r]T这样一个三维向量便可以描述任意旋转。
2存在万向锁问题即当俯仰角为 ±90° 时第三次旋转轴和第一次旋转轴重合使系统丢失了一个自由度这被称为奇异性问题。因此欧拉角在 SLAM 中较少使用。
3旋转向量也存在奇异性当 θ \theta θ超过 2 π 2\pi 2π时会产生周期性这时将其限定在 2 π 2\pi 2π范围内便可以避免。
2.3 四元数
2.3.1 四元数的定义
12D 情况下可用单位复数表达旋转在复数平面中结合欧拉公式如下图向量 z \boldsymbol{z} z旋转 90° 相当于乘上 i i i。
2类似的在三维空间中采用四元数描述旋转。四元数有一个实部三个虚部分别对应 x、y、z轴。 q q 0 q 1 i q 2 j q 3 k (2-14) \boldsymbol{q} q_0q_1iq_2jq_3k \tag{2-14} qq0q1iq2jq3k(2-14)
写成向量形式 q [ s , v ] , s q 0 ∈ R , v [ q 1 , q 2 , q 3 ] T ∈ R 3 (2-15) \boldsymbol{q}[s, \boldsymbol{v}], sq_0\in \mathbb{R},\boldsymbol{v}[q_1,q_2,q_3]^T \in \mathbb{R}^3 \tag{2-15} q[s,v],sq0∈R,v[q1,q2,q3]T∈R3(2-15)
并且虚部之间满足 { i 2 j 2 k 2 − 1 i j k , j i − k j k i , k j − i k i j , i k − j (2-16) \left\{ \begin{matrix} i^2j^2k^2-1 \\ ijk, ji-k \\ jki, kj-i \\ kij, ik-j \end{matrix} \right. \tag{2-16} ⎩ ⎨ ⎧i2j2k2−1ijk,ji−kjki,kj−ikij,ik−j(2-16)
3当四元数的实部为 0 时称为虚四元数分别对应三维坐标此时可以用来表示三维空间中的点。
2.3.2 四元数的计算 加法对应部分相加 乘法每一项相乘最后相加 模长各项系数平方和再开方并且两个四元数乘积的模等于各自模的乘积即 ∣ ∣ q 1 q 2 ∣ ∣ ∣ ∣ q 1 ∣ ∣ ⋅ ∣ ∣ q 1 ∣ ∣ ||\boldsymbol{q_1}\boldsymbol{q_2}||||\boldsymbol{q_1}||\cdot||\boldsymbol{q_1}|| ∣∣q1q2∣∣∣∣q1∣∣⋅∣∣q1∣∣。 共轭实部相等虚部互为相反数 q a ∗ [ s a , − v a ] T \boldsymbol{q_a^*}[s_a, -\boldsymbol{v_a}]^T qa∗[sa,−va]T。并且 q ∗ q q q ∗ [ s 2 v T v , 0 ] T \boldsymbol{q^*q}\boldsymbol{qq^*}[s^2\boldsymbol{v^Tv}, \boldsymbol{0}]^T q∗qqq∗[s2vTv,0]T 逆 q − 1 q ∗ / ∣ ∣ q ∣ ∣ 2 \boldsymbol{q^{-1}}\boldsymbol{q^*}/{||\boldsymbol{q}||}^2 q−1q∗/∣∣q∣∣2则 q − 1 q q q − 1 1 \boldsymbol{q^{-1}}\boldsymbol{q}\boldsymbol{q}\boldsymbol{q^{-1}}1 q−1qqq−11同时 ( q a q b ) − 1 q a − 1 q b − 1 (\boldsymbol{q_a}\boldsymbol{q_b})^{-1}\boldsymbol{q_a}^{-1}\boldsymbol{q_b}^{-1} (qaqb)−1qa−1qb−1 数乘 k q [ k s , k v ] T k\boldsymbol{q}[ks, k\boldsymbol{v}]^T kq[ks,kv]T
2.3.3 四元数表示旋转
1三维空间中任意点均可用一个纯虚四元数表示即 p [ 0 , v ] T \boldsymbol{p}[0,\boldsymbol{v}]^T p[0,v]T经一个单位四元数 q \boldsymbol{q} q的旋转后得到 p ′ \boldsymbol{p} p′则 p ′ q p q − 1 (2-17) \boldsymbol{p}\boldsymbol{q}\boldsymbol{p}\boldsymbol{q^{-1}} \tag{2-17} p′qpq−1(2-17)
最终 p ′ \boldsymbol{p} p′的虚部即为旋转后点的坐标。
2.3.4 四元数与其他旋转表示法的转换
角轴到四元数 q [ c o s θ 2 , n x s i n θ 2 , n y s i n θ 2 , n z s i n θ 2 ] (2-18) \boldsymbol{q}[cos{\frac{\theta}{2}},n_xsin{\frac{\theta}{2}}, n_ysin{\frac{\theta}{2}},n_zsin{\frac{\theta}{2}}] \tag{2-18} q[cos2θ,nxsin2θ,nysin2θ,nzsin2θ](2-18) 四元数到角轴 { θ 2 a r c c o s q 0 [ n x , n y , n z ] T [ q 1 , q 2 , q 3 ] T / s i n θ 2 (2-19) \left\{ \begin{matrix} \theta2arccos{q_0} \\ \\ [n_x, n_y, n_z]^T[q_1, q_2, q_3]^T/sin{\frac{\theta}{2}} \end{matrix} \right. \tag{2-19} ⎩ ⎨ ⎧θ2arccosq0[nx,ny,nz]T[q1,q2,q3]T/sin2θ(2-19) 四元数到旋转矩阵 旋转矩阵到四元数
2.4 相似、仿射、射影变换
欧式变换不改变向量本身只是进行旋转或平移。
1相似变换
相似变换比欧式变换多了一个自由度即相当于在旋转或平移后各坐标再进行等比例缩放表达式为 T s [ s R t 0 T 1 ] (2-20) \boldsymbol{T}_s\left[\begin{array}{ll} s\boldsymbol{R} \boldsymbol{t} \\ \mathbf{0}^{\mathrm{T}} 1 \end{array}\right] \tag{2-20} Ts[sR0Tt1](2-20) s s s为缩放因子。
2仿射变换
表达式如下 T A [ A t 0 T 1 ] (2-21) \boldsymbol{T}_A\left[\begin{array}{ll} \boldsymbol{A} \boldsymbol{t} \\ \mathbf{0}^{\mathrm{T}} 1 \end{array}\right] \tag{2-21} TA[A0Tt1](2-21)
这里不要求 A \boldsymbol{A} A为正交矩阵因此变换后正方形就不是方的了但仍是平行四边形。
3射影变换 T P [ A t a T v ] (2-22) \boldsymbol{T}_P\left[\begin{array}{ll} \boldsymbol{A} \boldsymbol{t} \\ \mathbf{a}^{\mathrm{T}} v \end{array}\right] \tag{2-22} TP[AaTtv](2-22)
射影变换是最一般的变换左上角 A \boldsymbol{A} A为可逆矩阵右上角 t \boldsymbol{t} t为平移左下角为缩放 a T \boldsymbol{a^T} aT。从真实世界到相机照片的变换可以看做是射影变换原本正方形的地砖在照片中将不再是方形由于近大远小甚至可能是不规则的四边形。
