如何用四元数表示姿态差

文章列表

如何用四元数表示姿态差

在机器人控制中，经常需要控制末端工具的姿态，如果用欧拉角表示姿态，会出现万向锁的问题，而使用四元数就不会有这个问题，此外四元数求出的姿态差为一个标量，更容易在RL算法中使用。

首先，假设末端工具的姿态为 $q_1 = [x_1,y_1,z_1,w_1]=[\\boldsymbol{v}_1sin\\alpha_1,cos\\alpha_1 ]$ ，期望姿态为 $q_2 = [x_2,y_2,z_2,w_2]=[\\boldsymbol{v}_2sin\\alpha_2,cos\\alpha_2 ]$ 。

其中， $q_1,q_2$ 均为单位四元数，即 $q.norm = x^2+y^2+z^2+w^2=1$ ，单位四元数还具有以下特性，其中 $q^{-1}$ 为单位四元数的逆， $q^*$ 为单位四元数的共轭。

$q^{-1}q = 1$

$q^{-1} = q^* = [-x, -y, -z, w]$

$\\boldsymbol{v}_1,\\boldsymbol{v}_2$ 均为三维单位向量，即 $\\boldsymbol{v}.norm=1$ .

为求末端当前姿态到期望姿态的差距distance，使用 $q_2*q_1^{-1}$ 。解释如下：

$\\because q_1^{-1}q_1 = 1 \\newline \\therefore q_2=q_2*(q_1^{-1}q_1) \\newline \\therefore q_2 = (q_2*q_1^{-1})q_1$

可见， $q_2*q_1^{-1}$ 构建了从 $q_1$ 到 $q_2$ 的映射。由于一个四元数表示三维空间中的一个刚体的姿态，那么认为 $q_2*q_1^{-1}$ 是从 $q_1$ 到 $q_2$ 的旋转。

令 $Q=q_2*q_1^{-1}=[\\boldsymbol{v}sin\\Omega , cos\\Omega]$ ，认为从 $q_1$ 绕着向量 $\\boldsymbol{v}$ 转动了 $2\\Omega$ 角度到达了 $q_2$ 。

如果当前姿态就是期望姿态，那么旋转角度 $2\\Omega =0$ ， $Q=[\\boldsymbol{v}sin\\Omega,cos\\Omega]=[0,0,0,1]$ .

令 $Q = [0,0,0,1]=[v,w],v=[0,0,0]$ ， $q_1=[x_1,y_1,z_1,w_1]=[v_1,w_1],v_1=[x_1,y_1,z_1]$ .

$q_2=Q*q_1 \\newline =[v,w]*[v_1,w_1] \\newline \\overset{def}{=}[v\\times v_1+wv_1+w_1v,ww_1-vv_1] \\newline =[v_1,w_1]=q_1$

将Q转换为轴角形式：

$Q_{aa} = \\{\\boldsymbol{v},2\\Omega\\}=[v_x*2\\Omega,v_y*2\\Omega,v_z*2\\Omega]$

求 $Q_{aa}.norm=2\\Omega$ .

总结以上，为求实际姿态 $q_1$ 与期望姿态 $q_2$ 的差距，分为以下步骤：

一、求距离四元数： $Q=q_2*q_1^{-1}$

二、将其转换为轴角形式： $Q$ $\\Rightarrow$ $Q_{aa}$ ，

三、求 $Q_{aa}.norm$ ，这个标量即可代表实际姿态 $q_1$ 与期望姿态 $q_2$ 的差距。

参考文献：

https://web.archive.org/web/20200503045740if_/http://www.cs.ucr.edu/~vbz/resources/quatut.pdf

https://www.astro.rug.nl/software/kapteyn-beta/_downloads/attitude.pdf

心经全文

如何用四元数表示姿态差

公告

标签

如何用四元数表示姿态差

相关问题

公告

标签