南科大高等机器人控制课程总结与project

July 29, 2024

现代机器人学: 旋量法推导机器人运动学与动力学方程

本文公式可能有误,欢迎评论指正

刚体构型描述,旋量方法

斜对称矩阵(Skew symmetric)

$a\times b=[a]b$

$[a]\triangleq\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{bmatrix}$

特殊正交群(Special Orthogonal Group)

$\begin{aligned}&SO(n)=\{R\in\mathbb{R}^{n\times n}:R^TR=I,\det(R)=1\}\\&^AR_B\text{: orientation of \{B\} relative to \{A\} expressed in \{A\} frame}\end{aligned}$

对于绕轴旋转运动, 设单位转轴ω, 旋转矩阵R可以通过罗德里格斯公式计算：

由微分方程 $\dot{q}(t)=\omega\times q(t)=[\omega]q(t)$

可得 $q(t)=e^{[\omega]t}q(0)$

因而 $Rot(\omega,\theta)=e^{[\omega]\theta}==I+[\omega]\sin\theta+[\omega]^2(1-\cos\theta)$

$[\omega_s(t)]=\dot{R}_s(t)R_s^{-1}(t)$

特殊欧几里得群(Special Euclidean Group)

$se(3)=\{([\omega],v):[\omega]\in so(3),v\in\mathbb{R}^3\}$

Homogeneous Transformation Matrix: $^AT_B= (R, p)=\begin{bmatrix}^AR_B&^Ap_B\\0&1\end{bmatrix}$

$T^{-1}=\begin{bmatrix}R&p\\0&1\end{bmatrix}^{-1}=\begin{bmatrix}R^T&-R^Tp\\0&1\end{bmatrix}$

$R[w]R^T=[Rw]$

$[\mathcal{S}]=(\omega,v)=\begin{bmatrix}[\omega]&v\\0&0\end{bmatrix}$

$\tilde{p}(t)=e^{[\mathcal{S}]t}\tilde{p}(0) ,e^{[\mathcal{S}]t}\in SE(3)$

正向运动学

运动旋量Twist

$^AV=^AX_{B}^{B}V$

$^AX_B=[Ad_T]\triangleq\begin{bmatrix}R&0\\{[p]}R&R\end{bmatrix}$

$[V_b]=T^{-1}\dot{T} , [V_s]=\dot{T}T^{-1}$

如果坐标系不动,则先求导再坐标变换与先坐标变换再求导等价: $^o[\frac{d}{dt}(r)]=\frac{d}{dt}(^or)$ , 也即 $^o\dot{r}=^o\mathring{r}$

如果坐标系移动,则有: $^o[\frac{d}{dt}(r)]=\frac{d}{dt}(^or)+^B\omega_B\times^or$

PoE

PoE需要从end effector的位置和姿态开始，向base的位置和姿态推导，这样可以避免靠近base的关节的运动影响到靠近end effector的轴的位置和姿态。

当然,前向和后向最后可以证明是一样的,没有区别,但是前向的PoE需要考虑到上面的问题,导致求解比较麻烦.

雅可比矩阵

Geometric Jacobian

对于n轴robot，令V为end-effector的twist, J为雅可比矩阵, J_i为第i个关节的雅可比矩阵, coordinate free形式下有:

$\mathcal{V}=J(\theta)\dot{\theta}=J_1(\theta)\dot{\theta_1}+\cdots+J_n(\theta)\dot{\theta}_n$

因此有 $J_i(\theta)=\mathcal{S}_i(\theta)$

考虑到坐标变换有 $J(\theta)=[S_1,[Ad_T(\theta_1)]\bar{S}_2]$

也即 $^0J_i(\theta)=^0X_i(\theta)^iS_i$

Analytical Jacobian

定义：对于一个机器人，其末端执行器的位置和姿态可以用一个向量表示，这个向量的每个元素是末端执行器的位置和姿态的一个分量。这个向量的导数就是机器人的雅可比矩阵。

注意：x可能为6维向量，分别为位置和姿态的三个分量,也可能仅仅是位置的三维向量。

对于末端执行器位置与姿态向量p(θ), $J=\left[\begin{matrix}{\frac{\partial p_{x}}{\partial\theta_{A}}}&{\frac{\partial p_{x}}{\partial\theta_{B}}}&{\frac{\partial p_{x}}{\partial\theta_{C}}}\\{\frac{\partial p_{y}}{\partial\theta_{A}}}&{\frac{\partial p_{y}}{\partial\theta_{B}}}&{\frac{\partial p_{y}}{\partial\theta_{C}}}\\{\frac{\partial p_{z}}{\partial\theta_{A}}}&{\frac{\partial p_{z}}{\partial\theta_{B}}}&{\frac{\partial p_{z}}{\partial\theta_{C}}}\\\end{matrix}\right]$

动力学

加速度的旋量表示

对于 $\ddot{q}(t)\neq\dot{v}_o(t)$ 的证明:

由 $\dot{q}(t)=v_0(t)+w(t)\times\overrightarrow{q}(t)$

求导可得, $\ddot{q}(t)=\dot{v}_{0}(t)+\dot{w}(t)\times\vec{oq}(t)+w(t)\times\dot{q}(t)$ ,证得结论.

由 $^AX_B=[Ad_T]\triangleq\begin{bmatrix}R&0\\{[p]}R&R\end{bmatrix}$ ,

可得 $^A\dot{X_B}=\frac{d}{dt}(\begin{bmatrix}R&0\\{[p]}R&R\end{bmatrix})$

其中, $([p]R)^{\prime}=[\dot{p}]R+[p]\dot{R}$

$\begin{aligned}[\dot{p}]&=[v_{0}+w\times p]\\&=[v_{0}]+[{w\times p}]\\&=[v_{0}]+[w][p]-[p][w]\end{aligned}$

其中,对于, $[w\times p]=[w][p]-[p][w]$ 部分, 李括号性质的分析与证明中有详细证明

因此有 $^A\dot{X_B} = [\begin{matrix}[w]R&0\\{[v_0]}R+[w][p]R&[w]R\end{matrix}] = \begin{bmatrix}[w]&0\\{[v_{0}]}&[w]\end{bmatrix}{}^A{X_B}$

对于 $\mathcal{V}_1\times\mathcal{V}_2$ ,令其等于 $[\mathcal{V}_1\times]\mathcal{V}_2$ (一种李括号)

则有 $[V_{1}\times] \triangleq [\begin{matrix}{[w]}&0\\{[v]}&[w]\end{matrix}]$

代入之前的式子,有 $^A\dot{X_B} = [V_{B}\times]{}^A{X_B}$

空间加速度(spatial acceleration): $A=\dot{\mathcal{V}}=\left[\begin{array}{c}\dot{\omega}\\\dot{v}_o\end{array}\right]$

根据定义 $^{o}(A_{body})=\frac{d}{dt}(^{o}V_{body})=\frac{d}{dt}(^{o}X_{B}^{o}V_{body})={}^o\dot{X}_{B}{}^BV_{body}+^{o}X_{B}{}^{B}\mathring{V}_{body}$

因此 $^{o}(A_{body})=[^oV_{B}\times]{}^o{X_B}{}^BV_{body}+^{o}X_{B}{}^{B}\mathring{V}_{body}$

加速度坐标变换: ${}^{B}A={}^{B}X_{0}^{\circ}A$

力的旋量表示

对于O点力矩 $n_O$ , 有 $n_{0}=\sum_{i}\vec{op_{i}}\times f_{i}$

空间力(Spatial Force),也称力旋量(Wrench): $\mathcal{F}=\left[\begin{array}{c}n_O\\f\end{array}\right]$

frame A下有 ${}^A\mathcal{F}=\left[\begin{array}{c}{}^An_{o_A}\\{}^Af\end{array}\right]$

空间力坐标变换： ${}^A\mathcal{F}={}^AX_B^*{}^B\mathcal{F}$ ，其中 $^AX_B^*={}^BX_A{}^T$ 的证明:

又易证 $\dot{X}_{B}^{*}=[\mathcal{V}\times^{*}]X_{B}^{*}$ ,其中 $[\mathcal{V}\times^{*}]=[\begin{matrix}[w]&[v]\\0&[w]\end{matrix}]$

可推导空间力的导数: $^A\left[\frac d{dt}\mathcal{F}\right]=\frac d{dt}\left(^A\mathcal{F}\right)+[^A\mathcal{V}\times^*]{}^{A}\mathcal{F}$

$\left[\begin{matrix}^A{n_{OA}}\\^A{f}\end{matrix}\right]=[\begin{matrix}{^AR_{B}} & {-^AR_{B}[^BP_{A}]}\\ {0} & {^AR_{B}}\end{matrix}]\left[\begin{matrix}^B{n_{OB}}\\^B{f}\end{matrix}\right]$

$[\begin{matrix}{^AR_{B}} & {-^AR_{B}[^BP_{A}]}\\ {0} & {^AR_{B}}\end{matrix}] = [\begin{matrix}{^BR_{A}} & {0}\\ {[^BP_{A}]{}^BR_{A}} & {^BR_{A}}\end{matrix}]^T = {}^BX_A{}^T$

power: $P=\left({}^A\mathcal{V}\right)^T{}^A\mathcal{F}$

由 $\mathcal{V}={\hat{\mathcal{S}}}\dot{\theta}$ ( $\hat{\mathcal{S}}$ 为screw axis of the joint)

并定义torque, $\tau=\hat{\mathcal{S}}^T\mathcal{F}=\mathcal{F}^T\hat{\mathcal{S}}$ (由几何雅可比 $J(\theta)=\mathcal{S}(\theta)$ 可得 $\tau=\hat{J(\theta)}^T\mathcal{F}=\mathcal{F}^T\hat{J(\theta)}$ )

可得， $P=\mathcal{V}^T\mathcal{F}=(\hat{\mathcal{S}}^T\mathcal{F})\dot{\theta}\triangleq\tau\dot{\theta}$

转动惯量 $\bar{I}=\int_V\rho(r)[r][r]^Tdr$ ,特别地,对于原点恰在质心的情况，转动惯量为常量

对于线性动量(linear momentum)， $L=mv$

可推得角动量(angluar momentum)： $\phi=r\times L=r\times(m\omega \times r)=m[r][-r]\omega =\bar{I}\omega$

空间动量(spatial momentum): $h\triangleq\left[\begin{array}{c}\phi_n\\L\end{array}\right]$

空间动量坐标变换: ${}^Ah={}^AX_{{B}}^*{}^{{B}}h$

同空间力的推导一样,也可以推导空间动量的导数: $^A\left[\frac d{dt}h\right]=\frac d{dt}\left({}^Ah\right)+[^A\mathcal{V}\times^*]h$

令 ${h}={\mathcal{I}\mathcal{V}}$ ,其中 $\mathcal{I}$ 为空间惯量矩阵

对于frame C,如果原点恰为质心,则空间惯量 ${^C\mathcal{I}}=\left[\begin{array}{cc}{^c\bar{I}_c}&0\\0&{mI_3}\end{array}\right]$

空间惯量坐标变换: ${}^A\mathcal{I}={}^AX_C^*{}^C\mathcal{I}^CX_A$

newton-euler方程

newton-euler方程: $\mathcal{F}=\frac d{dt}h=\mathcal{I}\mathcal{A}+\mathcal{V}\times^*\mathcal{I}\mathcal{V}$

证明: $^o\mathcal{F}=^o(\frac d{dt}h)=\frac d{dt}(^oh)=\frac d{dt}(^o\mathcal{I}{}^o\mathcal{V})$ $=^o{\dot{\mathcal{I}}}^{\circ}V+{}^o\mathcal{I}^{o}A$ $=\frac{d}{dt}(^oX_{B}^{*B}\mathcal{I}^{B}X_{0})^{o}V+^{o}I^{o}A$

其中, $\frac{d}{dt}(^oX_{B}^{*B}\mathcal{I}^{B}X_{0})^{o}V$ $=^o\dot{X_{B}}^{*}{}^B \mathcal{I}{}^BX_o{}^{o}\mathcal{V}+^o{X_{B}}^{*}{}^B \mathcal{I}{}^B\dot{X_o}{}^{o}\mathcal{V}$

由于 $^oX_{B}{}^{B}X_{o}=I$ (I为单位矩阵)

有 $^o\dot{X_{B}}{}^{B}X_{o}+^oX_{B}{}^{B}\dot{X_{o}}=0$

因此有 $^o\dot{X_{B}}=-{}^oX_{B}{}^{B}\dot{X_{o}}{}^oX_{B}{}^{B}$

因此 $\frac{d}{dt}(^oX_{B}^{*B}\mathcal{I}^{B}X_{0})^{o}V$

$=\left[^oV_{B}\times^{*}\right]^oX_{B}^{*}{}^B \mathcal{I}{}^BX_o{}^{o}\mathcal{V}-{}^oX_{B}^{*}{}^B\mathcal{I} {}^BX_{o}[^o\mathcal{V}_{B}\times]^{0}\mathcal{V}$