第二讲三维空间的刚体运动课后题

作者: 8416ac9040d9 | 来源:发表于2020-04-18 21:44 被阅读0次

[toc]

第二讲三维空间的刚体运动

1、熟悉 Eigen 矩阵运算

设线性⽅程 $Ax = b$ ，在 A 为⽅阵的前提下，请回答以下问题：

1.1 在什么条件下，x 有解且唯⼀？

答案：非齐次线性方程组有唯一解的充要条件是 rank(A)=n。

1.2 ⾼斯消元法的原理是什么？

答案：最基本的那种求方程解的方法，就是对矩阵进行行变换。

1.3 QR 分解的原理是什么？

答案：在了解 $QR$ 分解之前，先了解一下Gram-Schmidt正交化：

存在可逆矩阵 $A$ 的列向量组 $(\alpha_1,\alpha_2,\alpha_3,...,\alpha_n)$ 经过正交化之后可以得到：
$\varepsilon_1 =\frac{\beta_1} {\begin{Vmatrix} \beta_1 \end{Vmatrix}} = t_{11}\alpha_1 \\ \varepsilon_2 =\frac{\beta_2} {\begin{Vmatrix} \beta_2\end{Vmatrix}} = t_{11}\alpha_1 + t_{22}\alpha_2 \\ \vdots \\ \varepsilon_1 =\frac{\beta_{j}} {\begin{Vmatrix} \beta_j\end{Vmatrix}} = t_{1j}\alpha_1 + t_{2j}\alpha_2 + \dots + t_{jj}\alpha_j$
把上述式子使用矩阵表示就可以得到下面的定义

对于物理意义，有时候不是那么重要，知道是这种数学表达就可以了。

定义：对于n阶方阵A，若存在正交矩阵Q和上三角矩阵R，使得 $A = QR$ ，则该式称为矩阵A的完全QR分解或正交三角分解。(对于可逆矩阵A存在完全QR分解)。
$(\varepsilon_1,\varepsilon_2,\varepsilon_3,...,\varepsilon_j) = (\alpha_1,\alpha_2,\alpha_3,...,\alpha_n) \begin{pmatrix} t_{11}&\dots&t_{1n}\\ &\ddots&\vdots\\ 0&&t_{nn}\\ \end{pmatrix}$
$QR$ 基于我们熟悉的Gram-Schmidt正交化，令:

$A=(\alpha_1,\alpha_2,\alpha_3,...,\alpha_n)\\ T = (t_{ij})\\ Q = (\varepsilon_1,\varepsilon_2,\varepsilon_3,...,\varepsilon_j)$

由此有 $Q=AT$ 若记 $R=T^{-1}$ ，则此时 $A=QR$ 。其中 $Q$ 是由Gram-Schmidt正交化得到的标准的正交基。

1.4 Cholesky 分解的原理是什么？

可以阅读Cholesky分解维基百科获取更加详细的解释，下面只是为了便于理解而简单的进行说明。

直接先说Cholesky 分解是用于求解 $Ax=b$ 这个方程，然后具有等式：

$A = LL^T$

从式子可以看到就和代数的平方一样的效果，常用在优化里面作为误差；另外这个分解方法的优点是提高代数运算效率（矩阵求逆）、蒙特卡罗方法等场合中十分有用。

其中 $A$ 是一个 $n$ 阶厄米特正定矩阵(Hermitian positive-definite matrix)， $L$ 是下三角矩阵。下面介绍推导过程：

所以我们的目的是为了求 $L$ 矩阵，算法由 $i:=1$ 开始，令：

$A^{(1)}:=A$

在步骤 $i$ 中，矩阵 $A^{(i)}$ 的形式如下：

$A^{(i)} = \begin{pmatrix} I_{i-1}&0&0\\ 0&\alpha_{i,j}&b_{i}^*\\ 0&b_i&B^{i}\\ \end{pmatrix}$

其中 $b_i^{*}$ 表示 $b_i$ 的共轭转置，若 $b$ 是实数矩阵， $b_i^{*}$ 就是转置； $I_{i-1}$ 代表 $i-1$ 维的单位矩阵。此时 $L_i$ 定义为：
$L_i := \begin{pmatrix} I_{i-1}&0&0\\ 0&\sqrt{\alpha_{i,j}}&0\\ 0&\frac{1}{\sqrt{a_{i,j}}}b_i&I_{n-1}\\ \end{pmatrix}$
只有可以将矩阵 $A^{(i)}$ 改写为：

$A^{(i)} = L_{i}A^{(i+1)}L_i^{*}$

我们发现这个和我们熟悉的的特征值分解结构相似 $Q \Lambda Q$ ,接下来可以得到 $A^{(i+1)}$ 的形式：
$A^{(i+1)} = \begin{pmatrix} I_{i-1}&0&0\\ 0&1&0\\ 0&0&B^{(i)}-\frac{1}{a_{i,j}}b_ib_i^{*} \end{pmatrix}$

需要注意的是这里的 $b_{i}b_{i}^{*}$ 是一个外积。

重复此步骤，直到 $i$ 从1到 $n$ 。 $n$ 步之后，我们可以得到 $A^{(n+1)} = I$ 。因此下三角矩阵 $L$ 为：

$L := L_1L_2\dots L_n$

1.5 编程实现 A 为 100×100 随机矩阵时，⽤ QR 和 Cholesky 分解求 x 的程序。你可以参考本次课⽤到的 useEigen 例程。

#include <iostream>

using namespace std;

#include <ctime>

// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

#define MATRIX_SIZE 100


int main( int argc, char** argv )

{

    // 解方程
    // 我们求解 matrix_NN * x = v_Nd 这个方程
    // N的大小在前边的宏里定义，它由随机数生成
    // 直接求逆自然是最直接的，但是求逆运算量大
    
    cout <<"---解方程---"<<endl;
    clock_t time_stt = clock(); // 计时
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_Dy;
    Eigen::Matrix< double, Eigen::Dynamic,  1> v_Nd;
    Eigen::Matrix< double, Eigen::Dynamic,  1> x;
    matrix_Dy = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
    matrix_Dy=matrix_Dy.transpose()*matrix_Dy;       //乔利斯基分解需要正定矩阵
    
    // 求逆
    
    v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE, 1 );
    x = matrix_Dy.inverse()*v_Nd;
    cout<<x[0]<<endl;
    cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;


    // Cholesky 

    time_stt = clock();
    x = matrix_Dy.ldlt().solve(v_Nd);
    cout << x[0] << endl;
    cout <<"time use in Cholesky decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    // QR/Lu
    
    time_stt = clock();
    x = matrix_Dy.colPivHouseholderQr().solve(v_Nd);
    cout << x[0] << endl;
    x = matrix_Dy.fullPivLu().solve(v_Nd);
    cout << x[0] << endl;
    cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    return 0;
}

2、几何运算练习

设有⼩萝⼘1⼀号和⼩萝⼘⼆号位于世界坐标系中。⼩萝⼘⼀号的位姿为： $q1 = [0.55,0.3,0.2,0.2]$ , $t1 = [0.7,1.1,0.2]^T$ （ $q$ 的第⼀项为实部）。这⾥的 $q$ 和 $t$ 表达的是 $T_{cw}$ ，也就是世界到相机的变换关系。⼩萝⼘⼆号的位姿为 $q_2 = [−0.1,0.3,−0.7,0.2]$ , $t_2 = [−0.1,0.4,0.8]^T$ 。现在，⼩萝⼘⼀号看到某个点在⾃⾝的坐标系下，坐标为 $p_1 = [0.5,−0.1,0.2]^T$ ，求该向量在⼩萝⼘⼆号坐标系下的坐标。请编程实现此事，并提交你的程序。

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

int main ( int argc, char** argv )

{
    Eigen::Isometry3d Tcw1= Eigen::Isometry3d::Identity();                
    Eigen::Isometry3d Tcw2= Eigen::Isometry3d::Identity();              

    Eigen::Quaterniond q1(0.55,0.3,0.2,0.2);//定义四元数Q1
    Eigen::Quaterniond q2(-0.1,0.3,-0.7,0.2);

    cout<<"quaternion q1 = \n"<<q1.coeffs() <<endl;  // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部

    cout<<"quaternion q2 = \n"<<q2.coeffs() <<endl;   

    Eigen::Matrix<double, 3, 1> t1(0.7, 1.1, 0.2 );
    Eigen::Matrix<double, 3, 1> t2(-0.1, 0.4, 0.8 );

    Eigen::Matrix< double, 3, 1 > p1c(0.5, -0.1, 0.2);
    Eigen::Matrix< double, 3, 1 > p1w;
    Eigen::Matrix< double, 3, 1 > p2c;


    q1 = q1.normalized();
    q2 = q2.normalized();

    Tcw1.rotate ( q1.toRotationMatrix() );       // 按照rotation_vector进行旋转
    Tcw1.pretranslate (t1);                     // 平移向量
    cout << " Tcw1 Transform matrix = \n" << Tcw1.matrix() <<endl;

    Tcw2.rotate ( q2.toRotationMatrix() );     // 按照rotation_vector进行旋转
    Tcw2.pretranslate (t2);                     // 平移向量
    cout << "Tcw2 Transform matrix = \n" << Tcw2.matrix() <<endl;

    p1w = Tcw1.inverse()*p1c;
    p2c = Tcw2*p1w;
    cout << "p2c = \n" << p2c.matrix() <<endl;

    return 0;
}

3、旋转的表达

课程中提到了旋转可以⽤旋转矩阵、旋转向量与四元数表达，其中旋转矩阵与四元数是⽇常应⽤中常见的表达⽅式。请根据课件知识，完成下述内容的证明。

3.1 设有旋转矩阵 $R$ ，证明 $R^TR = I$ 且 $det R = +1^2$

3.1.1 正交矩阵性质：

正交矩阵的每一个列向量都是单位向量，且向量之间两两正交。
正交矩阵的行列式为1或者-1.
$A^-1 = A^T$ (充要条件)

3.1.2 证明旋转矩阵是正交矩阵且行列式为1

首先我们令空间的一个位置 $x_i$ 经过旋转 $R$ 和一次平移 $t$ 之后得到新的位置 $x_i^\prime$ :
$x_i\prime = Rx_i+t$
对于每一个 $i=1,\dots,n$ 有：
$x_i = \begin{pmatrix} x_i\\ y_i\\ z_i \end{pmatrix}$
需要注意的是旋转之后平移与平移之后旋转是不同的例如：
$x\prime = R(x+s) = Rx+Rs = Rx + t\\ t = Rs$
由于旋转矩阵可以对任意形式的（列）向量组和进行变换，这里我们考虑沿着 $x，y和z$ 轴变换的情况，也就是笛卡尔坐标系上进行变换：
$R\begin{pmatrix} x&y&z \end{pmatrix} = \begin{pmatrix} R_{11}&R_{12}&R_{13}\\ R_{21}&R_{22}&R_{23}\\ R_{31}&R_{32}&R_{33} \end{pmatrix} \begin{pmatrix} 1&0&0\\ 0&1&0\\ 0&0&1 \end{pmatrix} = \begin{pmatrix} r_1&r_2&r_3 \end{pmatrix}$
因为一开始的（列）向量组 $\begin{pmatrix}x&y&z\end{pmatrix}$ 是正交的，并且是单位向量；所以最后的结果 $\begin{pmatrix}r_1&r_2&r_3\end{pmatrix}$ 一定也是正交的单位向量组，然后一起组成了旋转向量 $R$ 。

之后考虑 $R$ 的转置 $R^T$ :
$R^TR = \begin{pmatrix} r_1^T\\ r_2^T\\ r_3^T \end{pmatrix} \begin{pmatrix} r_1 r_2 r_3 \end{pmatrix}$
因为 $r_1,r_2和r_3$ 都是单位向量并且相互正交，也就是 $r_1\cdot r_1 = 1, r_1 \cdot r_2 = 0$ ,因此：
$R^TR = \begin{pmatrix} 1&0&0\\ 0&1&0\\ 0&0&1 \end{pmatrix}$
由此可以得到 $R^T = R^{-1}$ ，即证旋转矩阵是正交的。

对于行列式，我们知道矩阵的行列式是向量的混合积: $r_1 \cdot (r_2 \times r_3)$ 。这也表示以这些向量为边构成的平行六面体的体积。

由前面的结果可以得到 $R$ 是正交矩阵，因此行列式为 +1或者-1；

emmm 看了很多解释，我觉得都不是很清除[Rotations and rotation matrices][ 2 ]，所以还是来算一下吧，具体看下面三种形式的旋转矩阵：

image-20200403185756164

简单的计算一下，例如 $R_x(a)$ 按照第一行展开：
$det(R_x(a)) = 1 * \big((cos \alpha) ^2 + (sin \alpha)^2 \big) == 1$

3.2 设有四元数 $q$ ，我们把虚部记为 $\varepsilon$ ，实部记为 $\eta$ ，那么 $q = (\varepsilon,\eta)$ 。请说明 $\varepsilon$ 和 $\eta$ 的维度[gaoxiang-cnblogs][3]

$q = [\varepsilon, \eta], \ \ \varepsilon = q_o \in R, \ \ \eta=\begin{pmatrix} q_1,q_2,q_3\end{pmatrix} \in R^3$

3.3 四元数运算总结：

这里可以简单的扩展一下四元数相关的知识:

一个四元数可以写成如下形式：
$q = \begin{pmatrix} a+id&-b-ic\\ b-ic&a-id \end{pmatrix} = aI +\ bi + \ cj + \ dk$
其中：
$I = \begin{pmatrix} 1&0\\ 0&1 \end{pmatrix}, \ i = \begin{pmatrix} 0&-1\\ 1&0 \end{pmatrix}, \ j = \begin{pmatrix} 0&-i\\ -i&0 \end{pmatrix}, \ k = \begin{pmatrix} i&0\\ 0&i \end{pmatrix}$

这样子我们就可以得到四元数的一些性质：
$\begin{cases} \begin{equation} \begin{aligned} &j^2 = k^2 = -1 \\ \\&ij = k, jk = i, ki = j\\ \\&ji = -k, kj = -i, ik = -j \end{aligned} \end{equation} \end{cases}$
对于四元数的运算（加法、乘法、共轭、模）有：
$\begin{cases} \begin{equation} \begin{aligned} & q = [\varepsilon, \eta], \ \ \varepsilon = q_o \in R, \ \ \eta=\begin{pmatrix} q_1,q_2,q_3\end{pmatrix} \in R^3 \\ \\& p + q = [\varepsilon_p + \varepsilon_q, \eta_p + \eta_q] \\ \\& pq = [\varepsilon_p, \eta_p][\varepsilon_q, \eta_q] = [\varepsilon_p\varepsilon_q - \eta_p \cdot \eta_q , \ \varepsilon_p \eta_q + \varepsilon_q \eta_p + \eta_p \times \eta_q] \\ \\&p \cdot q = p_0q_0 + p_1q_1i + p_2q_2j + p_3q_3k \\ \\& \overline p =[\varepsilon_p, -\eta_p] \\ \\ & \mid q \mid = \mid [s, v] \ \mid = \sqrt{s^2 + \mid v \ \mid ^2} \end{aligned} \end{equation} \end{cases}$

所以对于 $p q$ 是指四元数每个位置上的数值分别相乘经过一顿操作可以写成上面的式子的简洁形式注意与点乘区分。

其中：
$\eta_p \times \eta_q = \begin{vmatrix} i&j&k\\ p_1&p_2&p_3\\ q_1&q_2&q_3 \end{vmatrix}$
$\eta_p \cdot \eta_q = p_1q_1 + p_2q_2 + p_3q_3$

4、罗德里格斯公式的证明

参考： https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula

罗德里格斯公式提供了一种算法，可以计算从 $so(3)$ （ $SO(3)$ 的李代数）到 $SO(3)$ 的指数映射，而无需实际计算完整矩阵指数。令 $v\in R^3$ 然后 $k$ 是一个单位向量，描述了根据右手定则围绕其旋转角度θ的旋转轴，例如 $\begin{bmatrix}1 \ 0 \ 0\end{bmatrix}^T \ \begin{bmatrix}0 \ 1 \ 0\end{bmatrix}^T \ \begin{bmatrix}0\ 0\ 1 \end{bmatrix}^T$ ，那么旋转向量 $v_{rot}$ 的罗德里格斯公式的形式如下：
$v_{rot} = v cos\theta + (k \times v)sin\theta + k(k\cdot v)(1-cos\theta)$
首先，使用点积和叉积，向量 $v$ 可以分为与轴 $k$ 平行和垂直的分量：

$v = v_{\parallel } + v_{\perp}$

其中与 $k$ 平行的分量为： $v_{\parallel} = (v \cdot k)k$

垂直于 $k$ 的分量是 $v$ 在 $k$ 上的向量投影： $v_{\perp} = v - v_{\parallel} = v - (k \cdot v)k = -k \times (k \times v)$

向量 $k \times v$ 可以看作是 $v_{\perp}$ 的副本，逆时针绕 $k$ 旋转了 $90°$ ，因此它们的大小相等，但方向垂直。

同理可得，向量 $k \times（k \times v）$ 是将 $v$ 的副本绕 $k$ 逆时针旋转到180°，因此 $k \times（k \times v）$ 和 $v_⊥$ 的大小相等，但方向相反，所以是负号。

展开向量三乘积（ vector triple product ）可建立平行分量和垂直分量之间的连接，给定任何三个向量 $a，b，c$ 公式的公式为 $a\times(b \times c)=(a \cdot c)b −(a \cdot b)c$ 。

平行于轴的分量在旋转下不会改变大小或方向: $v_{\parallel rot} = v_{\parallel}$

只有垂直分量会改变方向，但保持其大小:
$\begin{cases} \mid v_{\perp rot} \mid = \mid v_{\perp} \mid, \\ \\ v_{\perp rot} = cos \theta v_{\perp} + sin \theta k \ \times \ v_{\perp} \end{cases}$
由于 $k \ \text{and} \ v_{\parallel}$ 是平行的，因此他们的叉积为0：
$\begin{cases} k \ \times \ v_{\perp} = k \ \times \ (v - v_{\parallel}) = k \ \times \ v - k \ \times v_{\parallel} = k \times v \\ \\ v_{\perp rot} = cos \theta v_{\perp} + sin \theta k \times v \end{cases}$
旋转分量的形式类似于笛卡尔基础上二维平面极坐标 $（r，θ）$ 中的径向矢量:
$r = rcos\theta e_x + rsin\theta e_y$
其中 $ex，ey$ 是其指示方向上的单位向量。

现在完整的旋转向量为：
$v_{rot} = v_{\parallel rot} + v_{\perp rot}$
通过将 $v_{∥rot}$ 和 $v_{⊥rot}$ 的定义代入等式，得出:
$\begin{equation} \begin{aligned} v_rot &= v_{\parallel} + cos \theta v_{\perp} + sin\theta \ (k \times v) \\ &= v_{\parallel} + cos\theta ({v - v_{\parallel}}) + sin\theta \ (k \times v ) \\ &= cos \theta v + (1-cos \theta)v_{\parallel} + sin \theta \ (k \times v) \\ &= cos \theta v + (1-cos \theta)(k \cdot v)k + sin \theta \ (k \times v) \end{aligned} \end{equation}$

参考链接：

https://www.cnblogs.com/indulge-code/p/10492209.html "东电逸仙-cnblog"
https://onlinelibrary.wiley.com/doi/pdf/10.1107/S0907444901012410 "Rotations and rotation matrices"
https://www.cnblogs.com/gaoxiang12/p/5120175.html " gaoxiang-cnblogs"

第二讲三维空间的刚体运动课后题