機器人建模與控制複習總結

數學補充

符號約定

(1) 一般大寫字母的變數表示向量或矩陣，小寫字母的變數表示標量

(2) 左上標和左下標表示變數所在的座標系

如：\(^AP\)表示座標系\(\{A\}\)中的位置向量；\(_B^AR\) 是確定座標系\(\{A\}\)和座標系\(\{B\}\)相對關係的矩陣；無左上、下標的位置向量一般是世界座標系中的。

(3) 某些三角函式有時候會被簡化表示，如：

• \(\cos\theta_3=c\theta_3=c_3\)
• \(\sin\theta_1=s\theta_1=s_1\)
• \(c_{23}=\cos(\theta_2+\theta_3)\)
• \(s_{13}=\sin(\theta_1+\theta_3)\)

雙引數反正切函式

\[\theta=\mathrm{Atan2}(y,x) \]

定義域 \(\mathbb{R}^2\backslash(0,0)\) ，值域 \((-\pi,\pi]\) 。

三維向量的外積

\[^AP\times{}^AQ=\begin{vmatrix}i&j&k\\\\p_x&p_y&p_z\\\\q_x&q_y&q_z\end{vmatrix}=\begin{bmatrix}0&-p_z&p_y\\p_z&0&-p_x\\-p_y&p_x&0\end{bmatrix}\begin{bmatrix}q_x\\q_y\\q_z\end{bmatrix} \]

線性方程組解與零空間

若 \(A\) 為 \(m \times n\) 維矩陣，且 \(A\) 為滿秩，則線性方程組 \(A x=b\) 的解

(1) \(m>n\) 時，方程組是過定的，通常方程組無解。此時，使得 \(\|A x-b\|^{2}\) 最小的 \(x\) 為方程的最小二乘解，由左偽逆計算

\[x^{*}=A^{+} b=A_{\text {left}}^{-1} b=\left(A^{T} A\right)^{-1} A^{T} b \]

(2) \(m<n\) 時，方程組是欠定的，通常方程組可能存在無數個解。此時，所有解中使得 \(x\) 範數最小的 \(x\) 為方程的最小范數解，由右偽逆計算

\[x^{*}=A^{+} b=A_{\text{right}}^{-1} b=A^{T}\left(A A^{T}\right)^{-1} b \]

零空間(Null Space)：若 \(A\) 為 \(m \times n\) 維矩陣，則 \(A\) 的零空間為線性方程組 \(A x=0\) 的所有解集合，記為 \(N(A)=\left\{x \in \mathbb{R}^{n}: A x=0\right\}\)

(1) \(m \geq n\) 時，若 \(A\) 為列滿秩， \(A\) 的零空間只有零向量；

(2) \(m<n\) 時，若 \(A\) 為行滿秩， \(A\) 的零空間中的向量為

\[\tilde{x}=\left(I-A^{+} A\right) x=\left(I-A_{\text {right }}^{-1} A\right) x=\left(I-A^{T}\left(A A^{T}\right)^{-1} A\right) x \]

其中， \(x\) 為任意 \(n\) 維向量。

矩陣求導

\[\dot{V} = \frac{\mathrm{d}V}{\mathrm{d}t} \]

設有三個矩陣 \(A, B, C\)

\[\dot{C} = \dot{A} B + A \dot{B} \]

雅可比 Jacobian

\[Y=F(X) \]

\[\delta Y=\frac{\partial F}{\partial X}\delta X=J(X)\delta X \]

偏導數矩陣 \(J(X)\) 稱作雅可比矩陣，是 \(x_i\) 的函式。雅可比矩陣可看成是 \(X\) 中的速度向\(Y\)中速度的對映。\(J(X)\) 是一個時變的線性變換。

李雅普諾夫穩定性

李雅普諾夫方法用於確定下列微分方程的穩定性

\[\dot{X}=f(X) \]

式中，\(X\) 為 \(m\times1\) 向量，\(f(\cdot)\) 可以是非線性函式。注意，高階微分方程總是可以被寫成一組形式如上的一階微分方程。為了用李雅普諾夫方法證明一個系統是否穩定，必須構造一個具有如下性質的廣義能量函式 \(V(X)\)

(1) \(V(X)\) 具有連續的一階偏導數，除 \(V(0)=0\)，對於任意 \(X\) 有 \(V(X)>0\)；

(2) \(\dot{V}(X)\leqslant0\) ，\(\dot{V}(X)\) 指 \(V(X)\)在系統所有軌跡上的變化率。

若這些性質僅在特定區域成立，則相應的系統為弱穩定的；若這些性質在全域性成立，則相應的系統為強穩定的。

空間描述與變換

位姿

空間中任何一點可用一個位置向量描述，該向量還需附加座標系的資訊。位置向量的每個分量為向量在相應座標軸上的投影。

在物體上固定一個座標系，並給出此座標系相對於參考座標系的表達，可稱為物體的姿態描述。

點的位置描述

\(O_{A}\) 表示{A}的原點。\(\hat{X}_{A}\)、\(\hat{Y}_{A}\) 和\(\hat{Z}_{A}\) 分別表示{A}的 \(x\) 軸向、\(y\) 軸向和 \(z\) 軸向的單位向量。

\(\overrightarrow{O_AP}=\begin{bmatrix}\hat{X}_A&\hat{Y}_A&\hat{Z}_A\end{bmatrix}{}^AP\)，\({}^AP=\begin{bmatrix}p_x\\p_y\\p_z\end{bmatrix}\)

物體的位置和姿態描述

設{B}是某物體的一個聯體座標系，即該物體上的任何一個點在{B}中的位置已知且始終不變。

在{A}中表示出{B}的位置

\[\overrightarrow{O_AO_B}=\begin{bmatrix}\hat{X}_A&\hat{Y}_A&\hat{Z}_A\end{bmatrix}{}^AO_B \]

在{A}中表示出{B}的姿態

\[\begin{bmatrix}\hat{X}_B&\hat{Y}_B&\hat{Z}_B\end{bmatrix}=\begin{bmatrix}\hat{X}_A&\hat{Y}_A&\hat{Z}_A\end{bmatrix}{}_B^AR \]

旋轉矩陣 \(_B^AR=\begin{bmatrix}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\end{bmatrix}\)

任何一個旋轉矩陣 \(R\)（一一對應於剛體的一個姿態）都屬於集合 \(SO(3)\)

1. 每個列向量是單位向量。即 \(||\vec{r}_1|| = ||\vec{r}_2|| = ||\vec{r}_3|| = 1\)，其中 \(\vec{r}_1, \vec{r}_2, \vec{r}_3\) 分別是 \(R\) 的第一、第二、第三列。
2. 任意兩個列向量都是正交的。即 \(\vec{r}_1 \cdot \vec{r}_2 = \vec{r}_2 \cdot \vec{r}_3 = \vec{r}_1 \cdot \vec{r}_3 = 0\)。
3. 第三個列向量是前兩個列向量的向量積。即 \(\vec{r}_3 = \vec{r}_1 \times \vec{r}_2\)。

• \(R\) 的行列式等於1
• \(R\) 可逆且 \(R^{-1}=R^{\mathrm{T}}\)

在{A}中表示出{B}的位姿(描述物體在{A}中的位姿)：

齊次變換矩陣 \(\displaystyle {}_{B}^AT=\left.\left[\begin{array}{ccc|c}&{}_B^AR&&{}^AO_B\\\hline0&0&0&1\end{array}\right.\right]\in\mathbb{R}^{4\times4}\)

在座標系{A}中描述座標系{B}中的位置向量 \(\boldsymbol{P}\)

\[{}^AP={}_B^AR {}^BP+{}^AO_B \]

\[\begin{bmatrix}{}^AP\\1\end{bmatrix}={}^A_BT \begin{bmatrix}{}^BP\\1\end{bmatrix} \]

座標系幾何關係

\[_A^BR={}_B^AR^{-1}={}_B^AR^{\mathrm{T}} \]

\[{}^BO_A=-{}_A^BR \,{}^AO_B \]

\[_A^BT={}_B^AT^{-1} \]

\[T^{-1}=\left.\left[\begin{array}{ccc|c}&R^{\mathrm{T}}&&{}-R^{\mathrm{T}}O\\\hline0&0&0&1\end{array}\right.\right] \]

旋轉矩陣的鏈乘法則

\[{}^A_CR={}^A_BR \,{}^B_CR \]

齊次變換矩陣的鏈乘法則

\[{}^A_CT={}^A_BT \,{}^B_CT \]

右乘聯體左乘基。

尤拉角、固定角

基本旋轉矩陣

\[R_z(\theta)=\begin{bmatrix}\cos\theta&-\sin\theta&0\\[0.3em]\sin\theta&\cos\theta&0\\[0.3em]0&0&1\end{bmatrix}=\begin{bmatrix}c\theta&-s\theta&0\\[0.3em]s\theta&c\theta&0\\[0.3em]0&0&1\end{bmatrix} \]

\[R_y(\theta)=\begin{bmatrix}\cos\theta&0&\sin\theta\\0&1&0\\-\sin\theta&0&\cos\theta\end{bmatrix}=\begin{bmatrix}c\theta&0&s\theta\\0&1&0\\-s\theta&0&c\theta\end{bmatrix} \\ \]

\[R_x(\theta)=\begin{bmatrix}1&0&0\\0&\cos\theta&-\sin\theta\\0&\sin\theta&\cos\theta\end{bmatrix}=\begin{bmatrix}1&0&0\\0&c\theta&-s\theta\\0&s\theta&c\theta\end{bmatrix} \]

表示旋轉矩陣的方法：

尤拉角

\[R_{Z^{\prime}Y^{\prime}X^{\prime}}(\alpha,\beta,\gamma)=R_{z}(\alpha)R_{y}(\beta)R_{x}(\gamma) \]

ABC 型尤拉角，ABA 型尤拉角，各有6種。

固定角

\[R_{XYZ}(\gamma,\beta,\alpha)=R_z(\alpha)R_y(\beta)R_x(\gamma) \]

有12種。

由旋轉矩陣求尤拉角或固定角的解

已知 \(R\in SO(3)\) ，求 \((\alpha,\beta,\gamma)\in(-\pi,\pi]\times[-\pi/2,\pi/2]\times(-\pi,\pi]\) ，使得 \(R=R_{Z^{\prime}Y^{\prime}X^{\prime}}(\alpha,\beta,\gamma)\)

\[R_{Z^{\prime}Y^{\prime}X^{\prime}}(\alpha,\beta,\gamma)=\begin{bmatrix}\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{bmatrix} \]

尤拉旋轉定理

定點轉動：在三維空間裡，假設一個剛體在運動過程中，剛體內部至少有一點固定不動，稱此運動為定點轉動。將聯體座標系原點設在此固定點，剛體姿態變、位置不變。

尤拉旋轉定理：若剛體從初姿態作任意定點轉動後呈終姿態，則必可找到一個過原點的軸 \(K\) 及角度 \(\theta\) ，剛體從初姿態繞 \(K\) 作定軸轉動 \(\theta\) 後呈終姿態。

以單位向量 \(^AK=[k_x,k_y,k_z]^\mathrm{T}\) 表示等效軸，記等效軸角為 \(\theta\) ，等效旋轉矩陣為

\[R_K(\theta)=\begin{bmatrix}k_x^2\nu\theta+c\theta&k_xk_y\nu\theta-k_zs\theta&k_xk_z\nu\theta+k_ys\theta\\k_xk_y\nu\theta+k_zs\theta&k_y^2\nu\theta+c\theta&k_yk_z\nu\theta-k_xs\theta\\k_xk_z\nu\theta-k_ys\theta&k_yk_z\nu\theta+k_xs\theta&k_z^2\nu\theta+c\theta\end{bmatrix} \]

其中 \(\nu\theta=1-\cos\theta\)

\[R_K(\theta)=R_{-K}(-\theta) \]

等效軸角的計算

已知\(R\in SO(3)\)，求等效軸 \(\begin{bmatrix}k_x&k_y&k_z\end{bmatrix}^{\mathrm{T}}\) 和 \(\theta\in\left[0,\pi\right]\) 使得 \(R=R_K(\theta)\)

\[\theta=\text{Acos}\left(\frac{r_{11}+r_{22}+r_{33}-1}2\right) \]

若 \(\theta\in\left(0,\pi\right)\)，有唯一解

\[\left.\left[\begin{array}{c}k_x\\k_y\\k_z\end{array}\right.\right]=\dfrac{1}{2\sin\theta}\left[\begin{array}{c}r_{32}-r_{23}\\r_{13}-r_{31}\\r_{21}-r_{12}\end{array}\right] \]

單位四元數表示

四元數

四元數 \(q=\eta+i\varepsilon_1+j\varepsilon_2+k\varepsilon_3\)

四元數加法 \((\eta+i\varepsilon_1+j\varepsilon_2+k\varepsilon_3)+(\xi+i\delta_1+j\delta_2+k\delta_3)=(\eta+\xi)+i(\varepsilon_1+\delta_1)+j(\varepsilon_2+\delta_2)+k(\varepsilon_3+\delta_3)\)

四元數乘法

\[\begin{aligned} &(\eta+i\varepsilon_1+j\varepsilon_2+k\varepsilon_3)(\xi+i\delta_1+j\delta_2+k\delta_3) \\ =&(\eta\xi-\varepsilon_{1}\delta_{1}-\varepsilon_{2}\delta_{2}-\varepsilon_{3}\delta_{3})+i(\eta\delta_1+\varepsilon_1\xi+\varepsilon_2\delta_3-\varepsilon_3\delta_2) \\ &+j(\eta\delta_2-\varepsilon_1\delta_3+\varepsilon_2\xi+\varepsilon_3\delta_1)+k(\eta\delta_3+\varepsilon_1\delta_2-\varepsilon_2\delta_1+\varepsilon_3\xi) \\ =&\begin{bmatrix}\eta&-\varepsilon_1&-\varepsilon_2&-\varepsilon_3\\\varepsilon_1&\eta&-\varepsilon_3&\varepsilon_2\\\varepsilon_2&\varepsilon_3&\eta&-\varepsilon_1\\\varepsilon_3&-\varepsilon_2&\varepsilon_1&\eta\end{bmatrix}\begin{bmatrix}\xi\\\delta_1\\\delta_2\\\delta_3\end{bmatrix} \end{aligned} \]

四元數共軛 \((\eta+i\varepsilon_1+j\varepsilon_2+k\varepsilon_3)^*=\eta-i\varepsilon_1-j\varepsilon_2-k\varepsilon_3\)

四元數模長 \(|\eta+i\varepsilon_1+j\varepsilon_2+k\varepsilon_3|=\sqrt{\eta^2+\varepsilon_1^2+\varepsilon_2^2+\varepsilon_3^2}\)

純四元數：僅有虛部的四元數

四元數的逆 \(q^{-1}\) 滿足 \(qq^{-1}=q^{-1}q=1\)，\(q^{-1}=\dfrac{q^*}{\left\|q\right\|^2}\)

尤拉引數

在等效軸 \([k_xk_yk_z]^\mathrm{T}\) 和等效軸角 \(\theta{\in}\mathbb{R}\) 的基礎上，定義尤拉引數(單位四元數, unit quaternion) \(\begin{bmatrix}\eta&\varepsilon_1&\varepsilon_2&\varepsilon_3\end{bmatrix}^\mathrm{T}\)

\[\eta=\cos\frac\theta2 ,\qquad \left.\varepsilon=\left[\begin{array}{c}\varepsilon_1\\\\\varepsilon_2\\\\\varepsilon_3\end{array}\right.\right]=\left[\begin{aligned}k_x\sin\frac\theta2\\k_y\sin\frac\theta2\\k_z\sin\frac\theta2\end{aligned}\right] \]

記 \(U\) 為由全體尤拉引數構成的集合，\(U\) 是\(\mathbb{R}^4\)中的單位超球面。

任給一個姿態（或旋轉），必有兩組反號的尤拉引數與之對應。

Grassmann積

\[\begin{bmatrix}\eta\\\varepsilon\end{bmatrix}\oplus\begin{bmatrix}\xi\\\delta\end{bmatrix}=\begin{bmatrix}\eta\xi-\varepsilon^\mathrm{T}\delta\\\eta\delta+\xi\varepsilon+\varepsilon\times\delta\end{bmatrix}=\begin{bmatrix}\eta&-\varepsilon_1&-\varepsilon_2&-\varepsilon_3\\\varepsilon_1&\eta&-\varepsilon_3&\varepsilon_2\\\varepsilon_2&\varepsilon_3&\eta&-\varepsilon_1\\\varepsilon_3&-\varepsilon_2&\varepsilon_1&\eta\end{bmatrix}\begin{bmatrix}\xi\\\delta_1\\\delta_2\\\delta_3\end{bmatrix}=A\begin{bmatrix}\xi\\\delta_1\\\delta_2\\\delta_3\end{bmatrix} \]

單位四元數(unit quaternion)與尤拉引數一一對應。

3D 旋轉公式

任意向量 \(\mathbf{v}\) 沿著以單位向量定義的旋轉軸 \(\mathbf{u}\) 旋轉 \(\theta\) 之後的 \(\mathbf{v}^{\prime}\) 可由四元數乘法獲得。

令 \(\nu=\left[0,\mathbf{v}\right]\)，\(\nu^{\prime}=\left[0,\mathbf{v}^{\prime}\right]\)，\(q=\left[\cos\dfrac{\theta}{2},\mathbf{u}\sin\dfrac{\theta}{2}\right]\)

則 \(\nu^{\prime}=q\nu q^*=q\nu q^{-1}\)

旋轉矩陣基於尤拉參數列達為

\[_B^AR=R_\varepsilon(\eta)=\begin{bmatrix}2(\eta^2+\varepsilon_1^2)-1&2(\varepsilon_1\varepsilon_2-\eta\varepsilon_3)&2(\varepsilon_1\varepsilon_3+\eta\varepsilon_2)\\2(\varepsilon_1\varepsilon_2+\eta\varepsilon_3)&2(\eta^2+\varepsilon_2^2)-1&2(\varepsilon_2\varepsilon_3-\eta\varepsilon_1)\\2(\varepsilon_1\varepsilon_3-\eta\varepsilon_2)&2(\varepsilon_2\varepsilon_3+\eta\varepsilon_1)&2(\eta^2+\varepsilon_3^2)-1\end{bmatrix} \]

微分運動學

時變位姿的符號表示

位置向量的導數

向量 \(^{B}Q\) 的微分表示為如下同維向量

\[^BV_{Q}=\frac{\mathrm{d}}{\mathrm{d}t}{}^BQ=\lim_{\Delta t\to0}\frac{^BQ(t+\Delta t)-{}^BQ(t)}{\Delta t} \]

速度向量 \(^BV_{Q}\) 可在任意座標系中描述

\[{}^A({}^BV_Q)=\frac{^A\mathrm{d}}{\mathrm{d}t}{}^BQ={}_B^AR {}^BV_Q \]

\[{}^B({}^BV_Q)={}^BV_Q \]

經常討論的是一個座標系原點相對於世界座標系\(\{U\}\)的速度，對於這種情況，定義一個縮寫符號

\[v_{C}={}^UV_{\mathrm{CORG}} \]

\[{}^Av_C={}_U^AR v_C={}_U^AR {}^UV_{\mathrm{CORG}}\neq{}^AV_{\mathrm{CORG}} \]

角速度向量 \(^A\Omega_B\) 描述了座標系{B}相對於座標系{A}的旋轉。從物理意義上講， \(^A\Omega_B\) 的方向就是{B}相對於{A}的瞬時旋轉軸，\(^{A}\Omega_B\) 的大小表示旋轉速率。

\[{}^C({}^A\Omega_B)={}_A^CR^A\Omega_B \]

經常討論的是動座標系(比如{C}) 相對於世界座標系{U}的角速度，對於這種情況，定義一個縮寫符號

\[\omega_C={}^U\Omega_C \]

特別要注意下列符號的意思

\[^A\omega_C={}_U^AR\omega_C={}_U^AR {}^U\Omega_C\neq{}^A\Omega_C \]

\[^C\omega_C={}_U^CR\omega_C={}_U^CR {}^U\Omega_C \]

加速度

\[^B\dot{V}_Q=\frac{\mathrm{d}}{\mathrm{d}t}{}^BV_Q=\lim_{\Delta t\to0}\frac{^BV_Q(t+\Delta t)-^BV_Q(t)}{\Delta t} \]

\[^A\dot{\Omega}_B=\frac{\mathrm{d}}{\mathrm{d}t}{}^A\Omega_B=\lim_{\Delta t\to0}\frac{^A\Omega_B\left(t+\Delta t\right)-^A\Omega_B\left(t\right)}{\Delta t} \]

\[\dot{v}_{A}={}^{U}\dot{V}_{AORG} \]

\[\dot{\omega}_A={}^U\dot{\Omega}_A \]

剛體的速度

座標系間的速度和加速度變換

剛體上某點 \(Q\) 相對於座標系 \(A\) 的速度

\[^AV_Q={}^{A}V_{BORG}+{}_{B}^{A}R{}^{B}V_{Q}+{}^{A}\Omega_{B}\times{}_{B}^{A}R^{B}Q \]

• \(^{A}V_{BORG}\)：座標系 \(B\) 的原點相對於座標系 \(A\) 的速度。
• \(_{B}^{A}R\)：從座標系 \(B\) 到座標系 \(A\) 的旋轉矩陣，表示兩個座標系之間的空間方向關係。
• \(^{B}V_{Q}\)：點 \(Q\) 相對於座標系 \(B\) 的速度。
• \(^{A}\Omega_{B}\)：座標系 \(B\) 相對於座標系 \(A\) 的角速度。
• \(^{B}Q\)：點 \(Q\) 在座標系 \(B\) 中的位置向量。

剛體上某點 \(Q\) 相對於座標系 \(A\) 的加速度

\[^A\dot{V}_Q={}^A\dot{V}_{BORG}+{}_{B}^{A}R^{B}\dot{V}_{Q}+2{}^{A}\Omega_{B}\times{}_{B}^{A}R^{B}V_{Q}+^{A}\dot{\Omega}_{B}\times{}_{B}^{A}R^{B}Q+^{A}\Omega_{B}\times(^{A}\Omega_{B}\times{}_{B}^{A}R^{B}Q) \]

• \(^A\dot{V}_{BORG}\)：座標系 \(B\) 的原點相對於座標系 \(A\) 的加速度。
• \(^{B}\dot{V}_{Q}\)：點 \(Q\) 相對於座標系 \(B\) 的加速度。
• \(^{A}\dot{\Omega}_{B}\)：座標系 \(B\) 相對於座標系 \(A\) 的角加速度。

當點 \(Q\) 相對於座標系 \(B\) 保持不變（即位置向量 \(^BQ\) 是一個常數），可以簡化為：

\[^A\dot{V}_Q={}^A\dot{V}_{BORG}+{}^A\Omega_B\times(^A\Omega_B\times{}_B^AR^BQ)+{}^A\dot{\Omega}_B\times{}_B^AR^BQ \]

座標系間的角速度和角加速度變換

假設座標系 \(B\) 以角速度 \(^A\Omega_B\) 相對於座標系 \(A\) 轉動，同時座標系 \(C\) 以角速度 \(^B\Omega_C\) 相對於座標系 \(B\) 轉動。

座標系 \(C\) 相對於座標系 \(A\) 的角速度

\[^A\Omega_C={}^A\Omega_B+{}_B^AR {}^B\Omega_C \]

角加速度

\[^A\dot{\Omega}_C={}^A\dot{\Omega}_B+{}_B^AR {}^B\dot{\Omega}_C+{}^A\Omega_B\times{}_B^AR {}^B\Omega_C \]

剛體力學

慣性張量

座標系{A}中的慣性張量

\[^AI=\begin{bmatrix}I_{xx}&-I_{xy}&-I_{xz}\\-I_{xy}&I_{yy}&-I_{yz}\\-I_{xz}&-I_{yz}&I_{zz}\end{bmatrix} \]

\(\displaystyle I_{xx} =\iiint_V(y^2+z^2)\rho\mathrm{d}v\) ，
\(\displaystyle I_{yy} =\iiint_V(x^2+z^2)\rho\mathrm{d}v\) ，
\(\displaystyle I_{zz} =\iiint_V(x^2+y^2)\rho\mathrm{d}v\)

\(\displaystyle I_{xy} =\iiint_Vxy\rho\mathrm{d}v\) ，
\(\displaystyle I_{xz} =\iiint_Vxz\rho\mathrm{d}v\) ，
\(\displaystyle I_{yz} =\iiint_Vyz\rho\mathrm{d}v\)

\[I=\iiint_V(\boldsymbol{r}^\mathrm{T}\boldsymbol{r}I_3-\boldsymbol{r}\boldsymbol{r}^\mathrm{T})\mathrm{d}m \]

\(I_{xx}\)、\(I_{yy}\)和\(I_{zz}\)稱為慣性矩。其餘 3 個交叉項稱為慣量積。當任意選擇座標系的姿態時，可能會使剛體的慣量積為零。此時，座標系的軸被稱為主軸，而相應的慣量矩被稱為主慣性矩。

平行移軸定理

假設{C}是以剛體質心為原點的座標系，{A}為任意平移後的座標系，向量 \(P_c=(x_c,y_c,z_c)^T\) 表示剛體質心在座標系{A}中的位置。平行移軸定理可以表示成為向量-矩陣形式

\[^AI={}^CI+m(P_c^TP_cI_3-P_c P_c^T) \]

式中 \(I_{3}\) 是 3×3 單位矩陣。

靜力學

力的座標變換

\[{}^BF={}_B^AR{}^AF \]

\[{}^BN={}^BP_{AORG}\times{}^BF+{}_A^BR {}^AN \]

牛頓方程和尤拉方程

牛頓方程

作用在質心上的力 \(F\) 引起剛體的加速度為

\[F = m\dot{v}_C \]

式中 \(m\) 代表剛體的總質量。

尤拉方程

作用在剛體上的力矩 \(N\) 引起剛體的轉動為

\[{}^CN={}^CI {}^C\dot{\omega}+{}^C\omega\times{}^CI {}^C\omega \]

式中 \(^{C}I\) 是剛體在座標系{C}中的慣性張量。座標系{C}固連於連桿，原點在剛體的質心。

正運動學

串聯機構中的運動學參量

從串聯機器人進行編號

• 固定基座為連桿0
• 第一個可動連桿為連桿1
• 以此類推，機器人最末端的連桿為連桿\(N\)
• 連桿0 與連桿1 透過關節1 連線，… ，連桿\(N-1\) 與連桿\(N\) 透過關節\(N\) 連線

為了確定末端執行器在3維空間的位置和姿態，串聯機器人至少需要6個關節。

用空間中的直線“軸\(i\) ”表示關節\(i\) 的軸線，軸\(i\) 的正方向由設計者指定。

• 旋轉關節：旋轉軸中心線
• 滑動關節：滑動方向中心線

連桿\(i\) 繞（沿）軸\(i\) 相對於連桿\(i–1\) 運動。

D-H 引數：運動學參量

若軸\(i-1\) 和軸\(i\) 不平行，它們有唯一的公垂線段；若軸\(i-1\) 和軸\(i\) 平行，它們的公垂線段不唯一，可按需取一條公垂線段。公垂線段\(\overrightarrow{O_{i-1}P_i}\) 的正方向為軸\(i-1\) 指向軸\(i\)，稱\(\overrightarrow{O_{i-1}P_i}\) 為幾何連桿。

公垂線段\(\overrightarrow{O_{i-1}P_i}\) 的長度，表示為連桿長度 \(a_{i-1}\)。當\(a_{i-1}=0\) 時，我們並不將零長度的\(\overrightarrow{O_{i-1}P_i}\) 視為傳統的零向量，而是在與軸\(i-1\)和軸\(i\)同時垂直的方向中選一個作為\(\overrightarrow{O_{i-1}P_i}\) 的正方向。

過軸\(i-1\) 作一個平面垂直於\(\overrightarrow{O_{i-1}P_i}\)，然後將軸\(i\) 投影到該平面上，按照軸\(i-1\) 繞\(\overrightarrow{O_{i-1}P_i}\) 旋轉到軸\(i\) 投影的思路以右手螺旋法則確定軸\(i-1\) 與軸\(i\) 夾角的值，此夾角即為連桿轉角 \(\alpha_{i-1}\)。

從\(P_i\) 到\(O_i\) 的有向距離，記為連桿偏距 \(d_i\)

過\(\overrightarrow{O_{i-1}P_i}\) 作一個平面垂直於軸\(i\) ，然後將\(\overrightarrow{O_iP_{i+1}}\) 投影到該平面上，在平面內按照\(\overrightarrow{O_{i-1}P_i}\) 繞軸\(i\) 旋轉到\(\overrightarrow{O_iP_{i+1}}\) 投影的思路以右手螺旋法則確定\(\overrightarrow{O_{i-1}P_i}\) 與\(\overrightarrow{O_iP_{i+1}}\) 夾角的值，此旋轉角度即為關節角 \(\theta_i\)

關節1 的運動學參量（首關節約定）

設定一個虛擬的軸0與軸1重合，即取 \(a_0=0\) ，\(\alpha_0=0\) 。

若關節1是轉動關節，取\(d_1=0\)，而\(\overrightarrow{O_0P_1}\) 的方向則任取與軸1垂直的某個方向，取\(\overrightarrow{O_0P_1}\) 的方向就是決定\(\overrightarrow{O_1P_2}\) 的零位方向。

若關節1是滑動關節，取\(\theta_1{=}0\)，而\(\overrightarrow{O_0P_1}\) 的位置則任取軸1上的某個點，取\(\overrightarrow{O_0P_1}\) 的位置就是決定\(\overrightarrow{O_1P_2}\) 的零位位置。

\(\overrightarrow{O_0P_1}\) 是一個固定不動的幾何連桿。

關節N 的運動學參量（尾關節約定）

軸\(N-1\) 和軸\(N\) 存在，\(a_{N-1}\) 和\(\alpha_{N-1}\) 已知，需要選取長度任意的\(\overrightarrow{O_N P_{N+1}}\) 。

若關節\(N\) 是轉動關節，取\(d_{N}=0\)，而\(\overrightarrow{O_N P_{N+1}}\) 的方向則任取與連桿\(N\) 固連的某個方向，\(\overrightarrow{O_N P_{N+1}}\) 與 \(\overrightarrow{O_{N-1} P_{N}}\) 的夾角即是關節角 \(\theta_N\) 。

若關節\(N\) 是滑動關節，取\(\theta_{N}=0\)，而點\(O_N\) 則任取軸\(N\) 上與連桿\(N\) 固連的某個點，\(O_N\) 與\(P_N\) 的相對位移即決定了連桿偏距\(d_{N}\) 。

建立座標系的改進D-H方法

運動學參量表

\(a_{i-1}\) 和 \(\alpha_{i-1}\) 是固定不變的引數，不會隨著關節\(i\)的運動而變化。

若關節\(i\) 是轉動關節，則 \(d_i\) 是固定不變的引數，\(\theta_i\) 是會隨著關節\(i\)的運動而變化的關節變數，即：3個連桿引數 \(a_{i-1},\alpha_{i-1},d_i\) ，1個關節變數\(\theta_i\)

若關節\(i\) 是滑動關節，則 \(\theta_i\) 是固定不變的引數，\(d_i\) 是會隨著關節\(i\)的運動而變化的關節變數，即：3個連桿引數\(a_{i-1},\alpha_{i-1},\theta_i\) ，1個關節變數\(d_i\)

一個有\(N\)個關節的串聯機構，有\(4N\)個運動學參量，其中\(3N\) 個是連桿引數、\(N\)個是關節變數，它們包含了串聯機構的全部空間幾何資訊。

連桿聯體座標系的配置

串聯機器人是一個多體系統（\(N+1\) 個剛體），描述機器人的運動即是描述\(N+1\) 個剛體的運動，因此需要建立每個剛體（實物連桿）的聯體座標系。

幾何連桿 \(\overrightarrow{O_{i-1} P_i}\) 與連桿\(i-1\) 固連，\(\overrightarrow{O_{i-1}P_i}\) 的聯體座標系也是連桿\(i-1\) 的聯體座標系。

建立連桿\(i-1\) 的聯體座標系\(\{i-1\}\)

• \(O_{i-1}\)為\(\{i-1\}\)的原點
• 軸\(i-1\)為\(\{i-1\}\)的Z軸
• \(\overrightarrow{O_{i-1}P_i}\)為\(\{i-1\}\)的X軸
• 右手定則定\(\{i-1\}\)的Y軸

機器人的正運動學計算

相鄰連桿聯體座標系的變換

\[\begin{aligned} {}_{i}^{i-1}T &=\left[\begin{array}{c:c} R_{x}(\alpha_{i-1}) & 0\\ \hdashline 0 & 1 \end{array}\right] \left[\begin{array}{ccc:c} 1 & 0 & 0 & a_{i-1} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ \hdashline 0 & 0 & 0 & 1 \end{array}\right] \left[\begin{array}{c:c} R_{z}(\theta_{i}) & 0\\ \hdashline 0 & 1 \end{array}\right] \left[\begin{array}{ccc:c} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_i \\ \hdashline 0 & 0 & 0 & 1 \end{array}\right] \\ &=\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] \end{aligned} \]

對於具有N個關節的串聯機器人，其N個關節變數可形成一個N×1的關節向量。所有關節向量構成的空間稱為關節空間。關節空間中的每一個關節向量都確定了機器人的一個位形。

正運動學：關節空間 → 笛卡爾空間

逆運動學

工作空間是機器人末端工具聯體座標系原點所能到達的範圍。靈巧工作空間是機器人末端工具能夠以任何姿態到達的區域。可達工作空間是機器人末端工具以至少一種姿態到達的區域。靈巧工作空間是可達工作空間的子集。若目標位置在靈巧工作空間內，則逆運動學問題的解存在。若目標位置不在可達工作空間內，則逆運動學問題的解不存在。當操作臂少於6自由度時，它在三維空間內不能達到全部位姿。

封閉解法是指基於解析形式的演算法。將運動學的封閉解法分為兩類：代數解法和幾何解法。數值解法具有迭代性質，所以比封閉解法的求解速度慢得多。在多重解的情況下，數值解法不能求出全部的解。

所有包含轉動關節和移動關節的串聯型6自由度操作臂都是可解的，但這種解一般是數值解。對於6自由度操作臂來說，只有在特殊情況下才有解析解，這種存在解析解（封閉解）的操作臂存在幾個正交關節軸，或者有多個 \(\alpha_i\) 為0 或\(\pm90^\circ\) 。具有6個旋轉關節的操作臂存在封閉解的充分條件是相鄰的三個關節軸線相交於一點。

軌跡生成

路徑：機器人位形的一個特定序列，而不考慮機器人位形的時間因素。軌跡：與何時到達路徑中的每個部分有關，強調了時間性和連續性。

機械臂軌跡控制過程中要求以足夠的路徑更新率自動完成各關節位置、速度和加速度的軌跡。

關節空間規劃方法計算量小、沒有奇異位形問題，不過，末端在笛卡爾空間的路徑形狀較複雜，不是直線、圓或正弦等常見形狀。

四元數插補

對於單位四元數表示的兩個姿態 \(q_0=\left[\cos(\theta_0),\quad\sin(\theta_0)\mathbf{u}_0\right]\) ，\(q_1=\left[\cos(\theta_1),\quad\sin(\theta_1)\mathbf{u}_1\right]\)

四元數插補要求規劃出一些中間姿態\(q_t\)，使初始姿態\(q_{0}\)能夠平滑地過渡到最終變換姿態\(q_{1}\) 。

四元數球面線性插值Slerp

\[q_t=\mathrm{Slerp}(q_0,q_1,t)=\frac{\sin((1-t)\theta)}{\sin(\theta)}q_0+\frac{\sin(t\theta)}{\sin(\theta)}q_1 \]

\[\theta=\cos^{-1}(q_0\cdot q_1) \]

Slerp 的鈍角處理：

若 \(q_0\cdot q_1<0\) ，則將 \(q_1\) 改為 \(-q_1\) ，使用新的夾角進行插值，保證插值的路徑最短。

操作臂運動學

在考慮機器人連桿運動時，一般使用連桿座標系{0}作為參考座標系。因此，\(v_i\) 是連桿座標系{i}原點的線速度，\(\omega_i\) 是連桿座標系{i}的角速度。

\[\begin{aligned} ^iv_i&={}_U^iRv_i={}_U^iR \, {}^UV_{iORG}={}_0^iR^0V_{iORG} \\ ^i\omega_i&={}_U^iR\omega_i={}_U^iR \, {}^U\Omega_i={}_0^iR^0\Omega_i \\ ^{i+1}v_i&={}_U^{i+1}Rv_i={}_U^{i+1}R \, {}^UV_{iORG}={}_0^{i+1}R^0V_{iORG} \\ ^{i+1}\omega_i&={}_U^{i+1}R\omega_i={}_U^{i+1}R \, {}^U\Omega_i={}_0^{i+1}R{}^0\Omega_i \end{aligned} \]

連桿間的速度與加速度傳遞

連桿間的速度傳遞

若 \(i+1\) 關節為轉動關節

\[{}^{i+1}v_{i+1}={}^{i+1}_{i}R\left({}^{i}v_{i}+{}^{i}\omega_{i}\times{}^{i}P_{i+1}\right) \]

\[^{i+1}\omega_{i+1}={}^{i+1}_iR \, {}^i\omega_i+\dot{\theta}_{i+1} \, {}^{i+1}\hat{Z}_{i+1} \]

移動關節

\[{}^{i+1}v_{i+1}={}_i^{i+1}R({}^iv_i+{}^i\omega_i\times{}^iP_{i+1})+\dot{d}_{i+1}{}^{i+1}\dot{Z}_{i+1} \]

\[{}^{i+1}\omega_{i+1}={}_i^{i+1}R^i\omega_i \]

連桿間的加速度傳遞

\[^{i+1}\dot{\omega}_{i+1}={}_{i}^{i+1}R^{i}\dot{\omega}_{i}+{}_{i}^{i+1}R^{i}\omega_{i}\times\dot{\theta}_{i+1}{}^{i+1}\hat{Z}_{i+1}+\ddot{\theta}_{i+1}{}^{i+1}\hat{Z}_{i+1} \]

當第 \(i+1\) 個關節是移動關節時，上式可簡化為

\[{}^{i+1}\dot{\omega}_{i+1}={}_i^{i+1}R^i\dot{\omega}_i \]

連桿座標系原點的線加速度

\[^{i+1}\dot{v}_{i+1}={}^{i+1}_iR\left[{}^i\omega_i\times{}^iP_{i+1}+{}^i\omega_i\times\left({}^i\omega_i\times{}^iP_{i+1}\right)+{}^i\dot{v}_i\right] \]

當第 \(i+1\) 個關節是移動關節時，上式可簡化為

\[^{i+1}\dot{v}_{i+1}={}^{i+1}_iR\left[{}^i\omega_i\times{}^iP_{i+1}+{}^i\omega_i\times\left({}^i\omega_i\times{}^iP_{i+1}\right)+{}^i\dot{v}_i\right] \\ +2^{i+1}\omega_{i+1}\times\dot{d}_{i+1}{}^{i+1}\hat{Z}_{i+1}+\ddot{d}_{i+1}{}^{i+1}\hat{Z}_{i+1} \]

每個連桿質心的線加速度

\[^i\dot{v}_{C_i}={}^i\dot{\omega}_i\times{}^iP_{C_i}+{}^i\omega_i\times({}^i\omega_i\times{}^iP_{C_i})+{}^i\dot{v}_i \]

假定座標系{C}附加於連桿 \(i\) 上，座標系原點位於連桿質心，且各座標軸方向與原連桿座標系{i}方向相同。

雅可比

幾何雅可比矩陣：操作臂的關節速度 \(\Theta\) 與末端線速度和角速度 \(\begin{pmatrix}v&\omega\end{pmatrix}^T\) 之間的對映關係矩陣 \(J(\Theta)\) 。

\[\begin{bmatrix}v\\\omega\end{bmatrix}=J(\Theta)\dot{\Theta} \]

向量積構造法

若機械臂末端執行器固連在連桿\(N\)上，而且相對於世界座標系{U}，每個連桿的座標系原點 \(P_i\) 和轉軸單位向量 \(\hat{Z}_i\) 已由正運動學求得。

假設其他關節固定不動，只有第\(i\)個關節運動，則由此運動產生的連桿\(N\)的線速度和角速度如下：

若第\(i\)個關節為轉動關節

\[v_N^{(i)}=\dot{\theta}_i\hat{Z}_i\times(P_N-P_i) ,\quad \omega_N^{(i)}=\dot{\theta}_i\hat{Z}_i \]

若第\(i\)個關節為移動關節

\[v_N^{(i)}=\dot{d}_i\hat{Z}_i ,\quad \omega_N^{(i)}=0 \]

末端實際線速度和角速度

\[v_N=\sum_{i=1}^Nv_N^{(i)} ,\quad \omega_N=\sum_{i=1}^N\omega_N^{(i)} \]

奇異性

對於一般機械臂，奇異位形為令雅可比矩陣 \(J\) 不滿秩的 \(\Theta\) 值所構成的位形。不同情況下的奇異點的判斷條件為

(1) 無冗餘(\(m=n\))，在此 \(\Theta\) 時 \(J\) 不可逆，即 \(\det(J(\Theta))=0\) ；

(2) 冗餘(\(m<n\))，在此 \(\Theta\) 時 \(J\) 不行滿秩，即 \(rank\left(J(\Theta)\right)<m\) ；

(3) 欠驅動(\(m>n\))，在此 \(\Theta\) 時 \(J\) 不列滿秩，即 \(rank\left(J(\Theta)\right)<n\) 。

分析雅可比 \(J_{\alpha}(\Theta)\) ，基於對末端執行器姿態的最小表示。

令 \(X=\begin{bmatrix}d(\Theta)\\\alpha(\Theta)\end{bmatrix}\) 表示末端執行器的姿態，其中 \(d(\Theta)\) 為基座座標系原點到末端執行器座標系原點的一般向量，\(\alpha\) 為末端執行器座標系相對於基座座標系姿態的最小表示(例如固定角表示或尤拉角表示)。

\[\dot{X}=\begin{bmatrix}\dot{d}\\\dot{\alpha}\end{bmatrix}=J_a(\Theta)\dot{\Theta} \]

若剛體姿態矩陣為 \(R=\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]\) ，剛體角速度 \(\omega=\left[\begin{array}{c}\omega_{x} \\ \omega_{y} \\ \omega_{z}\end{array}\right]\) ，有

\[\dot{R} R^{\mathrm{T}}=S \]

\[{\left[\begin{array}{lll} \dot{r}_{11} & \dot{r}_{12} & \dot{r}_{13} \\ \dot{r}_{21} & \dot{r}_{22} & \dot{r}_{23} \\ \dot{r}_{31} & \dot{r}_{32} & \dot{r}_{33} \end{array}\right]\left[\begin{array}{lll} r_{11} & r_{21} & r_{31} \\ r_{12} & r_{22} & r_{32} \\ r_{13} & r_{23} & r_{33} \end{array}\right]=\left[\begin{array}{ccc} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0 \end{array}\right]} \]

\[\omega_{x}=\dot{r}_{31} r_{21}+\dot{r}_{32} r_{22}+\dot{r}_{33} r_{23} \]

\[\omega_{y}=\dot{r}_{11} r_{31}+\dot{r}_{12} r_{32}+\dot{r}_{13} r_{33} \]

\[\omega_{z}=\dot{r}_{21} r_{11}+\dot{r}_{22} r_{12}+\dot{r}_{23} r_{13} \]

角速度與尤拉角速率的關係

\[\omega=B(\phi)\dot{\phi} \]

幾何雅可比與分析雅可比矩陣的轉換

\[J_a(\Theta)=\begin{bmatrix}I&0\\0&B^{-1}(\alpha)\end{bmatrix}J(\Theta) \]

記 \(T_a=\begin{bmatrix}I&0\\0&B^{-1}(\alpha)\end{bmatrix}\)

操作臂動力學

靜力學

為相鄰杆件所施加的力和力矩定義以下特殊的符號：

• \(f_i\) 連桿 \(i-1\) 施加在連桿 \(i\) 上的力
• \(n_i\) 連桿 \(i-1\) 施加在連桿 \(i\) 上的力矩

靜力平衡，有

\[{}^if_i={}_{i+1}^iR{}^{i+1}f_{i+1} \]

\[{}^in_i={}_{i+1}^iR{}^{i+1}n_{i+1}+{}^iP_{i+1}\times{}^if_i \]

保持系統靜平衡的關節力矩

\[\tau_i={}^in_i^T \,{}^i\hat{Z}_i \]

對於關節 \(i\) 是移動關節的情況，可以算出關節驅動力為

\[\tau_i={}^if_i^T \,{}^i\hat{Z}_i \]

按照慣例，通常將使關節角增大的旋轉方向定義為關節力矩的正方向。

力域中的雅可比

在靜態下，關節力矩與末端力平衡

\[\tau=J(\Theta)^T F \]

雅可比的轉置將作用於操作臂的笛卡爾力對映成等效關節力矩。

動力學

由關節運動計算關節力矩的完整演算法由兩部分組成。第一部分是對每個連桿應用牛頓-尤拉方程，從連桿 1 到連桿 \(n\) 向外遞推計算連桿的速度和加速度的。第二部分是從連桿 \(n\) 到連桿 1 迭代計算連桿間的相互作用力和力矩以及關節驅動力矩。

動力學方程的結構

狀態空間方程

\[\tau=M(\Theta)\ddot{\Theta}+V(\Theta,\dot{\Theta})+G(\Theta) \]

式中 \(M(\Theta)\) 為操作臂的 \(n\times n\) 質量矩陣，\(V(\Theta,\dot{\Theta})\) 是 \(n\times1\) 的離心力和哥氏力向量，\(G(\Theta)\) 是 \(n\times1\) 重力向量。

拉格朗日方程

計算操作臂的第 \(i\) 根連桿的質心線速度和角速度：

\[v_{C_i}=J_{v_{C_i}}(\theta)\dot{\theta}\qquad{}^i\omega_i=J_{^i\omega_i}(\theta)\dot{\theta} \]

連桿 \(i\) 的動能表示式

\[k_i=\frac12m_iv_{C_i}^\mathrm{T}v_{C_i}+\frac12{}^i\omega_i^\mathrm{T~}{}^{C_i}I_i{}^i\omega_i \]

操作臂的動能可以描述為關節位置和速度的標量函式

\[k(\Theta,\dot{\Theta})=\frac12\dot{\Theta}^TM(\Theta)\dot{\Theta} \]

操作臂的質量矩陣一定是對稱且正定矩陣。

連桿 \(i\) 的勢能可表示為

\[u_i=-m_i{}^0g^{\mathrm{T}}{}^0P_{C_i}+u_{\mathrm{ref}_i} \]

\(^{0}g\) 是 3×1 的重力向量，\(^{0}P_{C_i}\) 是連桿 \(i\) 的質心的位置，\(u_{\mathrm{ref}_i}\) 是使 \(u_i\) 的最小值為零的常數。

操作臂的總勢能 \(u(\Theta)\) 可描述為關節位置的標量函式。

\[u=\sum_{i=1}^nu_i \]

拉格朗日函式

\[\mathcal{L}(\Theta,\dot{\Theta})=k(\Theta,\dot{\Theta})-u(\Theta) \]

則操作臂的運動方程為

\[\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial\mathcal{L}}{\partial\dot{\Theta}}-\frac{\partial\mathcal{L}}{\partial\Theta}=\tau \]

這裡 \(\tau\) 是 \(n\times1\) 的激勵力矩向量。

運動控制

機器人控制是透過反饋調節執行器的輸入，在保證閉環系統穩定和實現期望行為的同時，滿足既定的瞬態和穩態要求。機器人控制分為自由空間的運動控制和與環境互動控制。機器人運動控制要求實現對期望軌跡的跟蹤，達到穩、快、準。

逆動力學控制

α-β分解運動控制

二階線性系統的動力學模型

\[m\ddot{x}+b\dot{x}+kx=f \]

分解成模型控制+位置校正兩部分，先利用反饋將模型化為單位質量模型，再對單位質量模型設計控制律。

\[m\ddot{x}+b\dot{x}+kx=\alpha f^{\prime}+\beta \]

取

\[\alpha=m \qquad \beta=b\dot{x}+kx \]

得到單位質量運動模型

\[\ddot{x}=f^{\prime} \]

設計控制律

\[f^{\prime}=-k_v\dot{x}-k_px \]

軌跡跟蹤控制，跟蹤一條給定的軌跡 \(x_d(t)\)

定義跟蹤誤差，\(e=x_d-x\)，\(\dot{e}=\dot{x}_d-\dot{x}\)

設計PID伺服控制律 \(f^{\prime}=\ddot{x}_d+k_v\dot{e}+k_pe+k_i\int edt\)

非線性的基於模型的控制律(α-β分解運動控制)，用來“抵消”系統的非線性，並簡化為單位質量系統後，設計簡單的線性伺服控制律。這種方法稱為反饋線性化。

魯棒控制

動力學模型

\[W(\theta)\ddot{\theta}+C(\theta,\dot{\theta})\dot{\theta}+G(\theta)=\nu \]

內環控制律

\[\nu=\hat{W}(\theta)a_{q}+\hat{C}(\theta,\dot{\theta})\dot{\theta}+\hat{G}(\theta) \]

得

\[\ddot{\theta}=a_q-W^{-1}(\theta)(\tilde{W}(\theta)a_q+\tilde{C}(\theta,\dot{\theta})\dot{\theta}+\tilde{G}(\theta))=a_q-\eta(\theta,\dot{\theta},a_q) \]

\(\eta(\theta,\dot{\theta},a_q)\) 表示不確定性

外環控制律

\[a_{q}=\ddot{\theta}_{d}+F_{0}(\theta_{d}-\theta)+F_{1}(\dot{\theta}_{d}-\dot{\theta})+\delta a \]

由 \(e=\theta_d-\theta\)，有閉環系統方程

\[\ddot{e}=-F_0e-F_1\dot{e}+(\eta-\delta a) \]

令 \(h= \begin{bmatrix} e\\ \dot{e} \end{bmatrix}\) ，有閉環狀態方程

\[\dot{h} = \begin{bmatrix} 0& I\\ - F_0& - F_1\end{bmatrix} h+ \begin{bmatrix} 0\\ I\end{bmatrix} ( \eta- \delta a) = Fh+ \begin{bmatrix} 0\\ I\end{bmatrix} ( \eta- \delta) \]

首先設計 \(F_{0}\) 和 \(F_{1}\) 使得 \(F\) 的特徵值具有負實部，然後設計 \(\delta a=S(e)\) 應對 \(\eta\) 。

自適應控制

動力學模型

\[W(\theta)\ddot{\theta}+C(\theta,\dot{\theta})\dot{\theta}+G(\theta)=\nu \]

引數線性化：\(W(\theta)\ddot{\theta}+C(\theta,\theta)\dot{\theta}+G(\theta)\) 可以重寫為 \(Y(\theta,\dot{\theta},\ddot{\theta})\Theta\) 。

構造 \(\hat{W}(\theta) \ddot{\theta}+\hat{C}(\theta, \dot{\theta}) \dot{\theta}+\hat{G}(\theta)=Y(\theta, \dot{\theta}, \ddot{\theta}) \hat{\Theta}\) ，其中時變的 \(\hat{\Theta}\) 是 \(\Theta\) 的估計，時變的 \(\hat{W} 、 \hat{C}\) 和 \(\hat{G}\) 分別是 \(W\)、\(C\) 和 \(G\) 的估計。

內環控制律 \(\nu=\hat{W}(\theta) a_{q}+\hat{C}(\theta, \dot{\theta}) \dot{\theta}+\hat{G}(\theta)\)

外環控制律 \(a_{q}=\ddot{\theta}_{d}+F_{0}\left(\theta_{d}-\theta\right)+F_{1}\left(\dot{\theta}_{d}-\dot{\theta}\right)\)

\(F_{0}\) 和 \(F_{1}\) 為有正值增益的對角陣。

\[\ddot{e}=-F_{1} \dot{e}-F_{0} e+\hat{W}^{-1}(\theta) Y(\theta, \dot{\theta}, \ddot{\theta}) \tilde{\Theta} \]

\(e=\theta_{d}-\theta\) ，\(\tilde{\Theta}=\Theta-\hat{\Theta}\)

令 \(h=\left[\begin{array}{l}e \\ \dot{e}\end{array}\right]\) ，得狀態方程

\[\dot{h}=\left[\begin{array}{cc}0 & I \\ -F_{0} & -F_{1}\end{array}\right] h+\left[\begin{array}{l}0 \\ I\end{array}\right] \hat{W}^{-1}(\theta) Y(\theta, \dot{\theta}, \ddot{\theta}) \tilde{\Theta}=F h+\Phi \tilde{\Theta} \]

設計 \(F_{0}\) 和 \(F_{1}\) 使得 \(F\) 的特徵值具有負實部，給定 2 個正定矩陣 \(Q\) 和 \(\Gamma\) ，則存在正定矩陣 \(P\) ，滿足李雅普諾夫方程 \(F^{\mathrm{T}} P+P F=-Q\) 。

設計引數更新律 \(\dot{\hat{\Theta}}=\Gamma^{-1} \Phi^{\mathrm{T}} P h\) ，構造李雅普諾夫函式 \(V(h, \tilde{\Theta})=h^{\mathrm{T}} P h+\tilde{\Theta}^{\mathrm{T}} \Gamma \tilde{\Theta}\) ，可以計算得 \(\dot{V}(h, \tilde{\Theta})=-h^{\mathrm{T}} Q h\) 半負定。

位置跟蹤誤差 \(e\) 收斂到零，引數估計誤差 \(\tilde{\Theta}\) 有界。

基於無源性的自適應控制

\[W(\theta)a_q+C(\theta,\dot{\theta})v_q+G(\theta)=X(\theta,\dot{\theta},v_q,a_q)\Theta \]

\[\hat{W}(\theta)a_q+\hat{C}(\theta,\dot{\theta})v_q+\hat{G}(\theta)=X(\theta,\dot{\theta},v_q,a_q)\hat{\Theta} \]

內環控制律 \(\nu=\hat{W}(\theta)a_q+\hat{C}(\theta,\dot{\theta})v_q+\hat{G}(\theta)+Kr\)

外環控制律 \(a_q=\ddot{\theta}_d+\Lambda(\dot{\theta}_d-\dot{\theta})\) ，\(v_q=\dot{\theta}_d+\Lambda(\theta_d-\theta)\) ，\(r=(\dot{\theta}_d-\dot{\theta})+\Lambda(\theta_d-\theta)\)

\(K\) 和 \(\Lambda\) 為有正值增益的對角陣。

\[W(\theta)\dot{r}+C(\theta,\dot{\theta})r+Kr=X(\theta,\dot{\theta},\nu_q,a_q)\tilde{\Theta} \]

自治系統

\[\begin{cases}\dot{r}=-W^{-1}(C+K)r+W^{-1}X\tilde{\Theta}\\\dot{e}=r-\Lambda e\\\dot{\tilde{\Theta}}=-\Gamma^{-1}X^{\mathrm{T}}r\end{cases} \]

多變數控制演算法的比較

自適應逆動力學控制，要求測量\(\ddot{\theta}\)，要求\(\hat{W}(\theta)\) 可逆。

基於無源性的自適應控制，不要求測量\(\ddot{\theta}\)，不要求\(\hat{W}(\theta)\) 可逆。

魯棒控制的\(\hat{W}\)、\(\hat{C}\) 和\(\hat{G}\) 是不變的；自適應控制的\(\hat{W}\)、\(\hat{C}\) 和\(\hat{G}\) 是變化的。

魯棒控制以不變應對不確定性；自適應控制以變化應對不確定性。

力控制

約束座標系

為了描述機器人與環境之間的相互作用，令 \(\xi=(v^T,\omega^T)^T\) 表示末端執行器的瞬時線速度和瞬時加速度，令 \(F=(f^T,n^T)^T\) 表示作用在末端執行器上的瞬時力和瞬時力矩。向量 \(\xi\) 和 \(F\) 均為 6 維向量空間中的元素。

自然約束，特定接觸條件下自然形成，與期望運動無關，滿足對偶條件

\[\xi^{\mathrm{T}}F=0 \]

人工約束，用於運動和力控制任務的參考輸入。

裝配策略為完成任務而事先規劃好的人工約束序列。策略需包含檢測手段以檢測接觸狀態的變化，並跟蹤自然約束的變化。當自然約束變化時，要呼叫新的人工約束集並進行控制。

力位混合控制

力位混合控制器需要在存在自然力約束的方向進行操作臂的位置控制，在存在自然位置約束的方向進行操作臂的力控制。

質量-彈簧系統

\[f=mk_e^{-1}\ddot{f}_e+f_e+f_{dist} \]

令 \(e_f=f_d-f_e\) ，設計控制律為

\[f=mk_e^{-1}[\ddot{f}_d+k_{vf}\dot{e}_f+k_{pf}e_f]+f_e+f_{dist} \]

由此得到誤差線性系統

\[\ddot{e}_f+k_{vf}\dot{e}_f+k_{pf}e_f=0 \]

力檢測的噪聲很大，無法數值微分，可由 \(\dot{f}_e=k_e\dot{x}\) 代替，透過機械臂的速度測量代替力的微分。

阻抗控制

許多情況下，互動過程往往伴隨著能量的轉移，這時單一的位置、速度或力控制就不足以控制互動過程的能量流動。阻抗控制，設計控制器使互動力與機械臂位置之間呈現出期望的關係，從而實現柔順控制。

機械阻抗定義為複頻域內作用力與速度的比值 \(\dfrac{F(s)}{\dot{X}(s)}\)

理想的位置控制器對應高阻抗，要在外力干擾下維持運動狀態不變；理想的力控制器對應低阻抗，要保持期望的作用力，不受位置變化的干擾。

阻抗控制的目標不是力或位置的軌跡，而是物體對外界所表現出的動力學特性。

例：

質量塊的位移為 \(x\) ，質量為 \(m\) ，\(F_{ext}\) 和 \(F\) 分別為環境施加的外力和控制力。則運動方程為

\[m\ddot{x}=F+F_{ext} \]

質量塊的期望軌跡為 \(x_d(t)\)，令 \(\tilde{x}(t)=x(t)-x_d(t)\) 表示運動的跟蹤誤差，則阻抗控制的目標為

\[M_d\ddot{\tilde{x}}+B_d\dot{\tilde{x}}+K_d\tilde{x}=F_{ext} \]

其中 \(M_d,B_d,K_d\) 分別表示期望的慣量、阻尼和剛度。

感測器測量當前位置與目標位置的偏差，並調整控制力的大小來達到預期的阻抗關係。阻抗控制律可以表示為

\[F=m\ddot{x}_d+(m-M_d)\ddot{\tilde{x}}-\left(B_d\dot{\tilde{x}}+K_d\tilde{x}\right) \]

上式中出現了跟蹤誤差的二階導數，在實際系統中會引入嚴重的測量噪聲。透過測量外力，可以將控制律改為

\[F=m\ddot{x}_d-\frac m{M_d}\left(B_d\dot{\tilde{x}}+K_d\tilde{x}\right)+\left(\frac m{M_d}-1\right)F_{ext} \]

機械臂動力學

關節空間動力學

\[M(\Theta)\ddot{\Theta}+V(\Theta,\dot{\Theta})+G(\Theta)=\tau+J^T(\Theta)F \]

笛卡爾空間動力學

\[M_X(\Theta)\ddot{X}+V_X(\Theta,\dot{\Theta})+G_X(\Theta)=J_a^{-T}(\Theta)\tau+F_a \]

• \(F_a=T_a^{-T}(\alpha)F\)
• \(M_X(\Theta)=J_a^{-T}(\Theta)M(\Theta)J_a^{-1}(\Theta)=\left(J_a(\Theta)M^{-1}(\Theta)J_a^T(\Theta)\right)^{-1}\)
• \(V_X(\Theta,\dot{\Theta})=J_a^{-T}(\Theta)V(\Theta,\dot{\Theta})-M_X(\Theta)\dot{J}_a(\Theta)\dot{\Theta}\)
• \(G_X(\Theta)=J_a^{-T}(\Theta)G(\Theta)\)

反饋線性化，內環控制律，閉環系統線性化為單位質量系統

\[\tau=J_a^T(\Theta){\left[M_X(\Theta)a+V_X(\Theta,\dot{\Theta})+G_X(\Theta)-F_a\right]} \]

設計期望動態阻抗模型

\[M_m\left(\ddot{X}-\ddot{X}_d\right)+D_m\left(\dot{X}-\dot{X}_d\right)+K_m\left(X-X_d\right)=F_a \]

通常期望慣量、阻尼和剛度矩陣都取為對角陣，以將各個分量解耦。可透過設定外環控制律 \(a\) 實現。

如果在設計阻抗控制律時將期望的慣量矩陣取為機械臂的笛卡爾慣量矩陣，可以消除了接觸力反饋項（不再需要力/力矩感測器）。

阻抗控制基於測量的位置來控制外力，需要直接對關節力矩進行控制，但很多機械臂不開放底層力矩控制介面。此時可以使用導納控制來實現與環境接觸時的阻抗特性。導納控制基於測量的外力來控制位置，只需要傳送位置/速度指令。