机器人学代数基础——对偶四元数与速度旋量(或运动螺旋)之间的转换

最新推荐文章于 2024-08-26 19:48:52 发布

YukinoSiro

最新推荐文章于 2024-08-26 19:48:52 发布

阅读量7.5k

点赞数 12

分类专栏： ●机器人学(Robotics) ●代数(Algebra) 文章标签：对偶四元数旋量螺旋理论

本文链接：https://blog.csdn.net/yukinoai/article/details/84982033

版权

●机器人学(Robotics) 同时被 2 个专栏收录

13 篇文章 35 订阅

订阅专栏

●代数(Algebra)

8 篇文章 2 订阅

订阅专栏

对偶四元数与旋量(或螺旋理论)之间的转换

坐标系O1绕轴s旋转θ，并移动d即对偶角 $\mathsf{\hat{\theta}}=\theta +\text{ }\!\!\varepsilon\!\!\text{ }d$ 。用对偶矢量表示为：

$\mathsf{\hat{s}}={{\mathsf{s}}_{0}}+\text{ }\!\!\varepsilon\!\!\text{ }{{\mathsf{s}}_{1}}={{\mathsf{s}}_{0}}+\text{ }\!\!\varepsilon\!\!\text{ }\mathsf{r}\times {{\mathsf{s}}_{0}}$

假定坐标系O与坐标系O之间的一般刚体运动为先转动四元数q接着再平移2t，或先平移1t再转动四元数q，且

${}^{2}\mathsf{t}={{\mathsf{q}}^{*}}\circ {}^{1}\mathsf{t}\circ \mathsf{q}$

四元数用于矢量的转动变换，则对偶四元数表示对偶矢量的位姿变换，如果令

$\mathsf{{q}'}=\frac{1}{2}\mathsf{q}\circ {}^{2}\mathsf{t}\text{=}\frac{1}{2}{}^{1}\mathsf{t}\circ \mathsf{q}$

则对于O1中的一般对偶矢量 ${}^{1}\mathsf{\hat{v}}={}^{1}{{\mathsf{v}}_{0}}+\text{ }\!\!\varepsilon\!\!\text{ }{}^{1}{{\mathsf{v}}_{1}}$ 有如下变换

${}^{2}{{\mathsf{v}}_{0}}+\text{ }\!\!\varepsilon\!\!\text{ }{}^{2}{{\mathsf{v}}_{1}}\text{=}{{\left( \mathsf{q}+\text{ }\!\!\varepsilon\!\!\text{ }\mathsf{{q}'} \right)}^{*}}\circ \left( {}^{1}{{\mathsf{v}}_{0}}+\text{ }\!\!\varepsilon\!\!\text{ }{}^{1}{{\mathsf{v}}_{1}} \right)\circ \left( \mathsf{q}+\text{ }\!\!\varepsilon\!\!\text{ }\mathsf{{q}'} \right)$

即 ${}^{2}\mathsf{\hat{v}}={{\mathsf{\hat{q}}}^{*}}\circ {}^{1}\mathsf{\hat{v}}\circ \mathsf{\hat{q}}$

其中

$\mathsf{\hat{q}}=\mathsf{q}+\text{ }\!\!\varepsilon\!\!\text{ }\mathsf{{q}'}=\mathsf{q}+\text{ }\!\!\varepsilon\!\!\text{ }\frac{1}{2}{}^{1}\mathbf{t}\circ \mathsf{q}$

如果用螺旋运动表示刚体运动可以表示为坐标系O1绕轴s旋转θ后沿轴s平移d得到坐标系O2。

用对偶四元数变换

$\mathsf{\hat{q}}=\mathsf{q}+\text{ }\!\!\varepsilon\!\!\text{ }\mathsf{{q}'}=\mathsf{q}+\text{ }\!\!\varepsilon\!\!\text{ }\frac{1}{2}{}^{1}\mathbf{t}\circ \mathsf{q}$

推导螺旋变换。坐标系O1的原点在螺旋轴s上的投影点为R，则

$\mathsf{r}=\frac{1}{2}\left( \mathsf{t}-d\mathsf{s}+\cot \left( \frac{\theta }{2} \right)\mathsf{s}\times \mathsf{t} \right)$

位移 $d=\mathsf{t}\cdot \mathsf{s}$ ，又

$\mathsf{q}=\cos \frac{\theta }{2}+\sin \frac{\theta }{2}\mathsf{s}$

可得到

$\mathsf{\hat{q}}\text{=}\mathsf{q}+\text{ }\!\!\varepsilon\!\!\text{ }\frac{1}{2}{}^{1}\mathsf{t}\circ \mathsf{q}$

$=\left( \cos \frac{\theta }{2}+\sin \frac{\theta }{2}\mathsf{s} \right)+\text{ }\!\!\varepsilon\!\!\text{ }\frac{1}{2}\left( -d\sin \frac{\theta }{2}+\left( \cos \frac{\theta }{2}\mathsf{t}+\sin \frac{\theta }{2}\mathsf{t}\times \mathsf{s} \right) \right)$

$=\left( \cos \frac{\theta }{2}-\text{ }\!\!\varepsilon\!\!\text{ }\frac{d}{2}\sin \frac{\theta }{2} \right)+\left( \sin \frac{\theta }{2}\mathsf{s}+\text{ }\!\!\varepsilon\!\!\text{ }\left( \frac{d}{2}\cos \frac{\theta }{2}\mathsf{s}+\sin \frac{\theta }{2}\mathsf{r}\times \mathsf{s} \right) \right)$

又

$\sin \left( a+\text{ }\!\!\varepsilon\!\!\text{ }{{a}_{0}} \right)=\sin a+\text{ }\!\!\varepsilon\!\!\text{ }{{a}_{0}}\sin a$

$\cos \left( a+\text{ }\!\!\varepsilon\!\!\text{ }{{a}_{0}} \right)=\cos a-\text{ }\!\!\varepsilon\!\!\text{ }{{a}_{0}}\cos a$

则

$\mathsf{\hat{q}}=\cos \frac{{\mathsf{\hat{\theta }}}}{2}+\sin \frac{{\mathsf{\hat{\theta }}}}{2}\mathsf{\hat{s}}$

其中

螺旋轴为

$\mathsf{\hat{s}}=\mathsf{s}+\text{ }\!\!\varepsilon\!\!\text{ }\mathsf{{s}'}=\mathsf{s}+\text{ }\!\!\varepsilon\!\!\text{ }\mathsf{r}\times \mathsf{s}$

对偶角为

$\mathsf{\hat{\theta }}=\theta +\text{ }\!\!\varepsilon\!\!\text{ }d$ , $d=\mathsf{t}\cdot \mathsf{s}$

假设对偶四元数表示为

$\mathsf{\hat{q}}=\mathsf{q}+\text{ }\!\!\varepsilon\!\!\text{ }\mathsf{{q}'}={{x}_{0}}+{{x}_{1}}\mathbf{i}+{{x}_{2}}\mathbf{j}+{{x}_{3}}\mathbf{k}+\text{ }\!\!\varepsilon\!\!\text{ }\left( {{y}_{0}}+{{y}_{1}}\mathbf{i}+{{y}_{2}}\mathbf{j}+{{y}_{3}}\mathbf{k} \right)$

则螺旋角θ为

$\theta \text{=}2\arctan 2\left( \sqrt{{{\left( {{x}_{1}} \right)}^{2}}+{{\left( {{x}_{2}} \right)}^{2}}+{{\left( {{x}_{3}} \right)}^{2}}},{{x}_{0}} \right)$

螺旋轴s为

$\left[ \begin{matrix} {{s}_{x}} \\ {{s}_{y}} \\ {{s}_{z}} \\ \end{matrix} \right]=\frac{1}{\sin \frac{\theta }{2}}\left[ \begin{matrix} {{x}_{1}} \\ {{x}_{2}} \\ {{x}_{3}} \\ \end{matrix} \right]$

平移向量t为

$\left[ \begin{matrix} {{t}_{x}} \\ {{t}_{y}} \\ {{t}_{z}} \\ \end{matrix} \right]=2\left[ \begin{matrix} {{y}_{0}} \\ {{y}_{1}} \\ {{y}_{2}} \\ {{y}_{3}} \\ \end{matrix} \right]\circ \left[ \begin{matrix} {{x}_{0}} \\ -{{x}_{1}} \\ -{{x}_{2}} \\ -{{x}_{3}} \\ \end{matrix} \right]$ $=\left[ \begin{matrix} \begin{matrix} -2{{x}_{1}} & 2{{x}_{0}} & -2{{x}_{3}} & 2{{x}_{2}} \\ \end{matrix} \\ \begin{matrix} -2{{x}_{2}} & 2{{x}_{3}} & 2{{x}_{0}} & -2{{x}_{1}} \\ \end{matrix} \\ \begin{matrix} -2{{x}_{3}} & -2{{x}_{2}} & 2{{x}_{1}} & 2{{x}_{0}} \\ \end{matrix} \\ \end{matrix} \right]\left[ \begin{matrix} {{y}_{0}} \\ {{y}_{1}} \\ {{y}_{2}} \\ {{y}_{3}} \\ \end{matrix} \right]$

$=\left[ \begin{matrix} 2\left( -{{x}_{0}}{{y}_{1}}+{{x}_{1}}{{y}_{0}}-{{x}_{2}}{{y}_{3}}+{{x}_{3}}{{y}_{2}} \right) \\ 2\left( -{{x}_{0}}{{y}_{2}}+{{x}_{1}}{{y}_{3}}+{{x}_{2}}{{y}_{0}}-{{x}_{3}}{{y}_{1}} \right) \\ 2\left( -{{x}_{0}}{{y}_{3}}-{{x}_{1}}{{y}_{2}}+{{x}_{2}}{{y}_{1}}+{{x}_{3}}{{y}_{0}} \right) \\ \end{matrix} \right]$

直线距r为

$\mathsf{r}=\frac{1}{2}\left( \mathsf{t}-d\mathsf{s}+\cot \left( \frac{\theta }{2} \right)\mathsf{s}\times \mathsf{t} \right)$

$r=\frac{1}{2}\left[ \begin{matrix} {{t}_{x}}-d{{s}_{x}}+\cot \frac{\theta }{2}\left( {{t}_{y}}{{s}_{z}}-{{t}_{z}}{{s}_{y}} \right) \\ {{t}_{y}}-d{{s}_{y}}+\cot \frac{\theta }{2}\left( {{t}_{z}}{{s}_{x}}-{{t}_{x}}{{s}_{z}} \right) \\ {{t}_{z}}-d{{s}_{z}}+\cot \frac{\theta }{2}\left( {{t}_{x}}{{s}_{y}}-{{t}_{y}}{{s}_{x}} \right) \\ \end{matrix} \right]$

螺距h为

$h=\frac{2\pi }{\theta }d$

所以可以得到转换螺旋

$\$=\left({{s}^{T}};{{\left(r\timess+hs\right)}^{T}}\right)$

$\text{=}\left({{\mathbf{s}}^{T}};{{\left(\frac{1}{2}\left(\mathbf{t}\times\mathbf{s}+\cot\left(\frac{\theta}{2}\right)\mathbf{s}\times\mathbf{t}\times\mathbf{s}\right)+\frac{2\pi}{\theta}\left(\mathbf{t}\cdot\mathbf{s}\right)\mathbf{s}\right)}^{T}}\right)$

下面求运动螺旋

$\mathsf{\dot{\hat{q}}}=\mathsf{\dot{q}}+\text{ }\!\!\varepsilon\!\!\text{ }\frac{1}{2}\left( {}^{1}\mathsf{\dot{t}}\circ \mathsf{q}+{}^{1}\mathsf{t}\circ \mathsf{\dot{q}} \right)$

对于四元数表示的转动有

$\mathsf{\dot{q}}=\frac{1}{2}\mathsf{\omega }\circ \mathsf{q}$ ，故 $\mathsf{\omega }=2\mathsf{\dot{q}}\circ {{\mathsf{q}}^{*}}$ ，

角速度为

$\left[ \begin{matrix} {{\omega }_{x}} \\ {{\omega }_{y}} \\ {{\omega }_{z}} \\ \end{matrix} \right]=2\left[ \begin{matrix} {{{\dot{x}}}_{0}} \\ {{{\dot{x}}}_{1}} \\ {{{\dot{x}}}_{2}} \\ {{{\dot{x}}}_{3}} \\ \end{matrix} \right]\circ \left[ \begin{matrix} {{x}_{0}} \\ -{{x}_{1}} \\ -{{x}_{2}} \\ -{{x}_{3}} \\ \end{matrix} \right]$ $=\left[ \begin{matrix} \begin{matrix} -2{{x}_{1}} & 2{{x}_{0}} & -2{{x}_{3}} & 2{{x}_{2}} \\ \end{matrix} \\ \begin{matrix} -2{{x}_{2}} & 2{{x}_{3}} & 2{{x}_{0}} & -2{{x}_{1}} \\ \end{matrix} \\ \begin{matrix} -2{{x}_{3}} & -2{{x}_{2}} & 2{{x}_{1}} & 2{{x}_{0}} \\ \end{matrix} \\ \end{matrix} \right]\left[ \begin{matrix} {{{\dot{x}}}_{0}} \\ {{{\dot{x}}}_{1}} \\ {{{\dot{x}}}_{2}} \\ {{{\dot{x}}}_{3}} \\ \end{matrix} \right]$

带入上式有

$\mathsf{\dot{\hat{q}}}=\frac{1}{2}{}^{1}\mathsf{\hat{\omega }}\circ \mathsf{\hat{q}}$

故对偶角 $\mathsf{\hat{\theta }}=\theta +\text{ }\!\!\varepsilon\!\!\text{ }p$ 导数为

${}^{1}\mathsf{\hat{\omega }}\text{=}{}^{1}\mathsf{\omega }+\text{ }\!\!\varepsilon\!\!\text{ }\left( {}^{1}\mathsf{\dot{t}}+{}^{1}\mathsf{t}\times {}^{1}\mathsf{\omega } \right)$