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

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

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

坐标系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{ }dd=\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)

即该对偶四元数的运动螺旋表示为

                                                                               \left( {}^{1}\mathsf{\omega }\text{ }{}^{1}\mathsf{\dot{t}}+{}^{1}\mathsf{t}\times {}^{1}\mathsf{\omega } \right)

你可能感兴趣的:(●机器人学(Robotics),●代数(Algebra))