【机器人学】微分运动

【机器人学】微分运动

一、预备知识

1.1 正逆运动学

• 【机器人学】正运动学详解
• 【机器人学】逆运动学

1.2 齐次变换矩阵的逆

不妨假设已知坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的齐次变换矩阵为${}_B^{A}T$，求坐标系 { A } \{A\} {A}相对于坐标系 { B } \{B\} {B}的齐次变换矩阵，那么我们知道：
$$\begin{gathered} {}_B^{A}T=\left[\begin{array}{cc}{}_B^{A}R& {}^A{\vec{p}}_{Bo}\\ \vec{0}& 1\end{array}\right]=\left[\begin{array}{cccc}\mathbf{n}& \mathbf{o}& \mathbf{a}& \mathbf{p}\\ 0& 0& 0& 1\end{array}\right]，{ \\ }_A^{B}T=\left[\begin{array}{cc}{}_A^{B}R& {}^B{\vec{p}}_{Ao}\\ \vec{0}& 1\end{array}\right] \end{gathered}$$
利用旋转矩阵正交性，我们可以得到：${}_B^A{R}^{-1}{=}_B^A{R}^T{=}_A^{B}R$

对于${}^B{\vec{p}}_{Ao}$，我们可以有如下推导：
$${}^B\left({}^A{\vec{p}}_{Bo}\right)=\vec{0}{=}_A^{B}R{}^A{\vec{p}}_{Bo}{+}^B{\vec{p}}_{Ao}$$
因此，
$${}^B{\vec{p}}_{Ao}={-}_A^{B}R{}^A{\vec{p}}_{Bo}={-}_B^A{R}^T{}^A{\vec{p}}_{Bo}=-\left[\begin{array}{c}{\mathbf{n}}^T\\ {\mathbf{o}}^T\\ {\mathbf{a}}^T\end{array}\right]\mathbf{p}=\left[\begin{array}{c}-{\mathbf{n}}^T\cdot \mathbf{p}\\ -{\mathbf{o}}^T\cdot \mathbf{p}\\ -{\mathbf{a}}^T\cdot \mathbf{p}\end{array}\right]$$
综上所述，我们可以得到：
$${}_A^{B}T{=}_B^A{T}^{-1}=\left[\begin{array}{cc}{}_B^A{R}^T& {-}_B^A{R}^T{}^A{\vec{p}}_{Bo}\\ \vec{0}& 1\end{array}\right]=\left[\begin{array}{cc}{\mathbf{n}}^T& -{\mathbf{n}}^T\cdot \mathbf{p}\\ {\mathbf{o}}^T& -{\mathbf{o}}^T\cdot \mathbf{p}\\ {\mathbf{a}}^T& -{\mathbf{a}}^T\cdot \mathbf{p}\\ \vec{0}& 1\end{array}\right]$$

二、坐标变换的微分

2.1 简单的微分引例

先看上图，经过简单推导不难得到：

$$\begin{array}{rl}{\mathbf{v}}_B& ={\mathbf{v}}_A+{\mathbf{v}}_{B/A}\\ & =l_1{\dot{\theta }}_1+l_2({\dot{\theta }}_1+{\dot{\theta }}_2)\\ & =-l_1\sin {\dot{\theta }}_1\mathbf{i}+l_1{\dot{\theta }}_1\cos {\theta }_1\mathbf{j}-l_2({\dot{\theta }}_1+{\dot{\theta }}_2)\sin ({\theta }_1+{\theta }_2)\mathbf{i}+l_2({\dot{\theta }}_1+{\dot{\theta }}_2)\cos ({\theta }_1+{\theta }_2)\mathbf{j}\end{array}$$
以矩阵形式示之：
$$\underset{B点位置微分}{\underbrace{\left[\begin{array}{c}d{x}_B\\ d{y}_B\end{array}\right]}}=\underset{雅可比矩阵}{\underbrace{\left[\begin{array}{cc}-l_1\sin {\theta }_1-l_2\sin ({\theta }_1+{\theta }_2)& -l_2\sin ({\theta }_1+{\theta }_2)\\ l_1\cos {\theta }_1+l_2\cos ({\theta }_1+{\theta }_2)& l_2\cos ({\theta }_1+{\theta }_2)\end{array}\right]}}\underset{关节微分}{\underbrace{\left[\begin{array}{c}d{\theta }_1\\ d{\theta }_2\end{array}\right]}}$$

2.2 雅可比矩阵

2.2.1 定义

假设我们有一系列关于变量$x_j(j=1,2,\dots ,m)$的方程$y_i(i=1,2,\dots ,n)$如下：
$$y_i=f_i\left(x_1,x_2,\dots ,x_m\right)$$
对每个$y_i$求其微分：
$$\{\begin{array}{l}\delta y_1=\frac{\partial f_1}{\partial x_1}\delta x_1+\frac{\partial f_1}{\partial x_2}\delta x_2+\cdots +\frac{\partial f_1}{\partial x_m}\delta x_m\\ \\ \delta y_2=\frac{\partial f_2}{\partial x_1}\delta x_1+\frac{\partial f_2}{\partial x_2}\delta x_2+\cdots +\frac{\partial f_2}{\partial x_m}\delta x_m\\ ⋮\\ \delta y_n=\frac{\partial f_n}{\partial x_1}\delta x_1+\frac{\partial f_n}{\partial x_2}\delta x_2+\cdots +\frac{\partial f_n}{\partial x_m}\delta x_m\end{array}$$
那么，可以得到：
$$\begin{array}{r}\left[\begin{array}{c}\delta y_1\\ \delta y_2\\ ⋮\\ \delta y_n\end{array}\right]\end{array}=\underset{雅可比矩阵}{\underbrace{\begin{array}{r}\left[\begin{array}{cccc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}& \cdots & \frac{\partial f_1}{\partial x_m}\\ & & & \\ \frac{\partial f_2}{\partial x_1}& \frac{\partial f_2}{\partial x_2}& \cdots & \frac{\partial f_2}{\partial x_m}\\ ⋮& ⋮& \cdots & ⋮\\ \frac{\partial f_n}{\partial x_1}& \frac{\partial f_n}{\partial x_2}& \cdots & \frac{\partial f_n}{\partial x_m}\end{array}\right]\end{array}}}\begin{array}{r}\left[\begin{array}{c}\partial x_1\\ \partial x_2\\ ⋮\\ \partial x_m\end{array}\right]\end{array}$$
简单表达为：
$$[D]=[J][D_{\theta }]$$

2.2.2 计算

对于机器人末端执行器中常常使用的变量而言，一般含有六个变量，三个位置变量和三个方向变量，因此，可以表达如下：

以下计算不提供证明

• 矩阵元素几何计算

一般认为，讨论雅可比矩阵时关注的是机械臂的末端执行器主要是位置与方向的微分：
$$\begin{array}{r}\left[\begin{array}{c}{}^O\delta \vec{p}\\ {}^O\delta \vec{o}\end{array}\right]={\left[\begin{array}{cccc}{}^O{\vec{J}}_{p_1}& {}^O{\vec{J}}_{p_2}& \cdots & {}^O{\vec{J}}_{p_n}\\ {}^O{\vec{J}}_{o_1}& {}^O{\vec{J}}_{o_2}& \cdots & {}^O{\vec{J}}_{o_n}\end{array}\right]}_{6\times n}{\left[\begin{array}{c}{}^O\delta {\theta }_1\\ {}^O\delta {\theta }_2\\ ⋮\\ {}^O\delta {\theta }_n\end{array}\right]}_{n\times 1}\end{array}$$
其中，雅可比矩阵各矢量的计算方法如下：
$$\begin{array}{r}\left[\begin{array}{c}{}^O{\vec{J}}_{p_i}\\ {}^O{\vec{J}}_{o_i}\end{array}\right]\end{array}=\{\begin{array}{lr}\left[\begin{array}{c}{}^O{\mathbf{z}}_i\\ 0\end{array}\right]& 移动关节\\ \left[\begin{array}{c}{}^O{\mathbf{z}}_i\times {(}^O{\vec{p}}_n{-}^O{\vec{p}}_i)\\ {}^O{\mathbf{z}}_i\end{array}\right]& 转动关节\end{array}$$
举个小栗子：不想画图，就拿出某课堂教师笔记的简图来充数看一下。

其雅可比矩阵就可以表示为：
$$J=\begin{array}{r}\left[\begin{array}{cccc}{}^O{a}_1\times {(}^O{P}_N{-}^O{P}_1)& \cdots & {}^O{a}_{N-1}\times {(}^O{P}_N{-}^O{P}_{N-1})& \vec{0}\\ {}^O{a}_1& \cdots & {}^O{a}_{N-1}& {}^O{a}_N\end{array}\right]\end{array}$$

2.3 位置和方向微分

• 位置微分

假设位置矢量的微分为$d\vec{p}=[dx,dy,dz{]}^T$，那么对应的其次变换矩阵为$\begin{array}{r}\left[\begin{array}{cc}I& d\vec{p}\\ \vec{0}& 1\end{array}\right]\end{array}$，多次位置微分的齐次变换矩阵为$\begin{array}{r}\left[\begin{array}{cc}I& \sum d{\vec{p}}_i\\ \vec{0}& 1\end{array}\right]\end{array}$。

• 方向微分

假设方向矢量的微分为$d\vec{o}=[\delta n,\delta o,\delta a{]}^T$，由于对于无穷小量而言有：$\underset{\delta t\to 0}{\lim }\sin \delta t=\delta t,\cos \delta t=1$，因此：
$$Rot(x,\delta n)=\begin{array}{r}\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& -\delta n& 0\\ 0& \delta n& 1& 0\\ 0& 0& 0& 1\end{array}\right]\end{array},Rot(y,\delta o)=\begin{array}{r}\left[\begin{array}{cccc}1& 0& \delta o& 0\\ 0& 1& 0& 0\\ -\delta o& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\end{array},Rot(z,\delta a)=\begin{array}{r}\left[\begin{array}{cccc}1& -\delta a& 0& 0\\ \delta a& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}$$
接下来，我们验证一件事：微分运动下，矩阵乘法满足交换律：

微分运动下，忽略高阶无穷小，那么上图两者的乘积结果相同，因此，对于微分$d\vec{o}=[\delta n,\delta o,\delta a{]}^T$，我们能够得到方向微分的齐次变换矩阵为：
$$T_{Rotation}=\begin{array}{r}\left[\begin{array}{cccc}1& -\delta a& \delta o& 0\\ \delta a& 1& -\delta n& 0\\ -\delta o& \delta n& 1& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}$$

2.4 微分算子

一个坐标系的微分变换是位置微分和方向微分以任何次序的结合：
$$[T+dT]=\begin{array}{r}\left[\begin{array}{c}Trans(dx,dy,dz)Rot(q,d\vec{o})]\end{array}\right]\end{array}[T]$$
因此变换矩阵的微分为：
$$[dT]=[Trans(dx,dy,dz)Rot(q,d\vec{o})-I][T]=[\Delta ][T]$$
则相对于固定坐标系的微分算子$[\Delta ]$满足：
$$\begin{array}{rl}\left[\Delta \right]& =Trans(dx,dy,dz)\times Rot(q,d\vec{o})-I_4\\ & =\left[\begin{array}{cccc}1& 0& 0& dx\\ 0& 1& 0& dy\\ 0& 0& 1& dz\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& -\delta a& \delta o& 0\\ \delta a& 1& -\delta n& 0\\ -\delta o& \delta n& 1& 0\\ 0& 0& 0& 1\end{array}\right]-I_4\\ & =\left[\begin{array}{cccc}0& \delta a& \delta o& dx\\ \delta a& 0& -\delta n& dy\\ \delta o& -\delta n& 0& dz\\ 0& 0& 0& 1\end{array}\right]\end{array}$$
结合之前所学，我们知道，坐标系的变化有相对于固定坐标系和相对于自身坐标系两种，上述得到的是相对于固定坐标系的微分算子，那么，我们假设相对于自身坐标系的微分算子为${}^T\Delta$。
$$\begin{array}{rl}& \left[dT\right]=[\Delta ]T=T{[}^T\Delta ]\\ ⟶& {[}^T\Delta ]=T^{-1}[\Delta ]T\\ ⟶& [\Delta ]=T{[}^T\Delta ]T^{-1}\end{array}$$
其中，$T^{-1}=\left[\begin{array}{cc}{\mathbf{n}}^T& -{\mathbf{n}}^T\cdot \mathbf{p}\\ {\mathbf{o}}^T& -{\mathbf{o}}^T\cdot \mathbf{p}\\ {\mathbf{a}}^T& -{\mathbf{a}}^T\cdot \mathbf{p}\\ \vec{0}& 1\end{array}\right]$

三、机器人的微分运动

机器人的运动是由两种关节驱动的，一种是旋转关节，一种是移动关节，我们都定义为$\delta {\theta }_i$，那么机器人的变换后的其次变换矩阵可以表达为：
$$T_{nex}=T_{old}+dT=T_{old}+[\Delta ]T_{old}=T_{old}+T_{old}{[}^T\Delta ]$$
其雅可比矩阵可以表示为：
$$\begin{array}{r}\left[\begin{array}{c}{}^O\delta \vec{p}\\ {}^O\delta \vec{o}\end{array}\right]={\left[\begin{array}{cccc}{}^O{\vec{J}}_{p_1}& {}^O{\vec{J}}_{p_2}& \cdots & {}^O{\vec{J}}_{p_n}\\ {}^O{\vec{J}}_{o_1}& {}^O{\vec{J}}_{o_2}& \cdots & {}^O{\vec{J}}_{o_n}\end{array}\right]}_{6\times n}{\left[\begin{array}{c}{}^O\delta {\theta }_1\\ {}^O\delta {\theta }_2\\ ⋮\\ {}^O\delta {\theta }_n\end{array}\right]}_{n\times 1}\end{array}$$

$$\begin{array}{r}\left[\begin{array}{c}{}^O{\vec{J}}_{p_i}\\ {}^O{\vec{J}}_{o_i}\end{array}\right]\end{array}=\{\begin{array}{lr}\left[\begin{array}{c}{}^O{\mathbf{z}}_i\\ 0\end{array}\right]& 移动关节\\ \left[\begin{array}{c}{}^O{\mathbf{z}}_i\times {(}^O{\vec{p}}_n{-}^O{\vec{p}}_i)\\ {}^O{\mathbf{z}}_i\end{array}\right]& 转动关节\end{array}$$