如何用四元数表示姿态差

news2025/1/19 11:29:57

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

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/434903.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

如何用四元数表示姿态差

相关文章

初始化一个Vue3+TS项目

第1章、C++基础

MIPI D-PHYv2.5笔记（19） -- Fault Detection

10.1 二重积分的概念与性质

Window10 源码编译UE4-27

编写高质量代码:改善Java程序的151个建议(数组和集合）

SRv6项目实践（三）：P4Runtime基础以及交换机控制

24.Spring练习（spring、springMVC）

新建论文三线表模板，一键格式刷

linux speedtest-cli工具测试网络带宽、测试网速（下载速度、上传速度和延迟等信息）

cookies 和 Session

verdaccio搭建npm私服

【花雕学AI】真是太多了：汇总国内免费在线使用ChatGPT的完整镜像站列表大全（1000+）

[Android+JetPack] (Java实现) Retrofit2+RxJava3+Paging3+RecyclerView 实现加载网络数据例子记录

2023年mathorcup杯A题代码小技巧总结

SAP MDG —— 使用DIF导入物料主数据 Part4 开发篇

计算机基础--Redis

使用assembly插件对boot项目打zip和war部署包

MATLAB应用笔记

[ 云原生 | Docker ] 构建高可用性的 SQL Server：Docker 容器下的主从同步实现指南