如何用四元数表示姿态差

在机器人控制中,经常需要控制末端工具的姿态,如果用欧拉角表示姿态,会出现万向锁的问题,而使用四元数就不会有这个问题,此外四元数求出的姿态差为一个标量,更容易在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_1q_2的映射。由于一个四元数表示三维空间中的一个刚体的姿态,那么认为q_2*q_1^{-1}是从q_1q_2的旋转。

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

如果当前姿态就是期望姿态,那么旋转角度2\Omega =0Q=[\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

你可能感兴趣的:(机器人控制,强化学习,人工智能,机器人,姿态控制,四元数)