【刚体姿态运动学】角速度和欧拉角速率的换算关系的详细推导

0 引言

本文以一种新的角度推导刚体姿态运动学,也即角速度和欧拉角速率之间的换算,不同于相似博文的地方在于,本文旨在从原理上给出直观清晰生动的解释。将详细过程记录于此,便于后续学习科研查找需要。

1 符号

符号含义
{ E } \{E\} {E}地面坐标系(惯性坐标系,牛顿运动定律严格成立)
{ B } \{B\} {B}随体坐标系(固连在刚体上,且原点位于质心)
Φ = [ ϕ , θ , ψ ] T \Phi=[\phi,\theta,\psi]^T Φ=[ϕ,θ,ψ]T姿态角,ZYX欧拉角,分别为roll, pitch, yaw
R X ( ϕ ) , R Y ( θ ) , R z ( ψ ) R_X(\phi),R_Y(\theta),R_z(\psi) RX(ϕ),RY(θ),Rz(ψ)随体坐标系绕地面坐标系X/Y/Z轴旋转 ϕ / θ / ψ \phi/\theta/\psi ϕ/θ/ψ角度得到的旋转矩阵
B E R ^E_BR BER旋转矩阵,随体坐标系姿态在地面坐标系下的表达
c , s c,s c,s c c c表示 c o s cos cos s s s表示 s i n sin sin
ω b = [ ω b x , ω b y , ω b z ] T {\omega_b}=[{\omega_{bx}},{\omega_{by}},{\omega_{bz}}]^T ωb=[ωbx,ωby,ωbz]T刚体相对地面坐标系转动的角速度在 { B } \{B\} {B}系下的表达

2 欧拉角与旋转矩阵

这里我们使用ZYX欧拉角来表达姿态,那么有:
B E R = R Z ( ψ ) R Y ( θ ) R X ( ϕ ) = [ c ψ − s ψ 0 s ψ c ψ 0 0 0 1 ] [ c θ 0 s θ 0 1 0 − s θ 0 c θ ] [ 1 0 0 0 c ϕ − s ϕ 0 s ϕ c ϕ ] = [ c ψ c θ s θ s ϕ c ψ − c ϕ s ψ s θ c ϕ c ψ + s ψ s ϕ s ψ c θ s θ s ϕ s ψ + c ϕ c ψ s θ c ϕ s ψ − c ψ s ϕ − s θ c θ s ϕ c θ c ϕ ] \begin{equation} \begin{split} ^E_BR&=R_Z(\psi)R_Y(\theta)R_X(\phi)\\ &= \begin{bmatrix} c\psi & -s\psi & 0\\ s\psi & c\psi & 0\\ 0&0&1 \end{bmatrix} \begin{bmatrix} c\theta & 0 & s\theta\\ 0 & 1 & 0\\ -s\theta&0&c\theta \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\\ 0 & c\phi & -s\phi \\ 0 & s\phi &c\phi \end{bmatrix}\\ &=\begin{bmatrix} c\psi c\theta & s\theta s\phi c\psi-c\phi s\psi & s\theta c\phi c\psi + s\psi s\phi\\ s\psi c\theta & s\theta s\phi s\psi+c\phi c\psi & s\theta c\phi s\psi - c\psi s\phi\\ -s\theta & c\theta s\phi & c\theta c\phi \end{bmatrix} \end{split} \end{equation} BER=RZ(ψ)RY(θ)RX(ϕ)= cψsψ0sψcψ0001 cθ0sθ010sθ0cθ 1000cϕsϕ0sϕcϕ = cψcθsψcθsθsθsϕcψcϕsψsθsϕsψ+cϕcψcθsϕsθcϕcψ+sψsϕsθcϕsψcψsϕcθcϕ
上述旋转矩阵用欧拉角给出,可以理解为:随体坐标系(1)先绕自身的 Z ^ B \hat Z_B Z^B轴旋转 ψ \psi ψ角度,(2)再绕 Y ^ B \hat Y_B Y^B轴旋转 θ \theta θ角度,(3)最后绕 X ^ B \hat X_B X^B轴旋转 ϕ \phi ϕ角度

欧拉角和固定角的区别为:

  • 欧拉角:刚体绕运动轴旋转的角度(内旋Intrinsic rotations)
  • 固定角:刚体绕固定轴旋转的角度(外旋 Extrinsic rotations)

3 机体下的角速度表达与欧拉角的关系

姿态角速率 Φ ˙ \dot \Phi Φ˙和机体角速度 ω b \omega_b ωb之间的转换关系为:
在这里插入图片描述

该公式不便于记忆,但是需要知道如何推导,并且最重要的是理解其原理,关键的时候查找即可。我几乎把高赞和高收藏的博客都看了一遍,但是都没能理解作者的意思,写的也有一定模糊性,后来还是自己琢磨才明白的,于是将自己能够理解的推导过程记录如下。

4 推导

假设当前姿态角为 Φ = [ ϕ , θ , ψ ] T \Phi=[\phi,\theta,\psi]^T Φ=[ϕ,θ,ψ]T,为了使角速度的表达更直观,这里用 ω b = [ ω b x , ω b y , ω b z ] T {\omega_b}=[{\omega_{bx}},{\omega_{by}},{\omega_{bz}}]^T ωb=[ωbx,ωby,ωbz]T代替上图所示的 ω b = [ p , q , r ] T {\omega_b}=[p,q,r]^T ωb=[p,q,r]T,那么:
由偏航角速率 ψ ˙ \dot \psi ψ˙引起的角速度在最终的 { B } \{B\} {B}系下可以表达为:
[ ω b x ω b y ω b z ] ψ ˙ = B z B z , y , x R [ 0 0 ψ ˙ ] \begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}_{\dot\psi}= {^{B_{z,y,x}}_{B_{z}}R}\begin{bmatrix}0\\0\\\dot\psi\end{bmatrix} ωbxωbyωbz ψ˙=BzBz,y,xR 00ψ˙
其中,
B z B z , y , x R = R X T ( ϕ ) R Y T ( θ ) B z , y B z , y , x R = R X T ( ϕ ) B z B z , y R = R Y T ( θ ) \begin{split} ^{B_{z,y,x}}_{B_{z}}R&=R^T_X(\phi)R^T_Y(\theta)\\ {{^{B_{z,y,x}}_{B_{z,y}}}R}&=R^T_X(\phi)\\ {{^{B_{z,y}}_{B_{z}}}R}&=R^T_Y(\theta)\\ \end{split} BzBz,y,xRBz,yBz,y,xRBzBz,yR=RXT(ϕ)RYT(θ)=RXT(ϕ)=RYT(θ)
在这里插入图片描述
为了便于理解,我特地画了一个示意图,如上图所示。
这里假设物体有一个预设的姿态角(本文与其他文章最大的不同):

  • 第一幅图只有绕绿色 z z z轴的 ψ \psi ψ运动,为了与后面的情况作区分,这里 { B z } \{B_z\} {Bz}用下标 z _z z 表示第一步绕机体 z z z轴的运动时的随体坐标系, ω b z = ψ ˙ \omega_{bz}=\dot \psi ωbz=ψ˙
  • 第二幅图,在第一幅图的基础上,俯仰角 θ \theta θ不为0,虽然引入了 θ \theta θ,但是却没有绕 y y y轴的运动,也即此时仍然只有 ψ \psi ψ变化,请大家想象第二幅图下方的圆柱,在倾斜的情况下,仍然绕“竖直”方向转动,那么显然,在这个时候的随体坐标系 { B z , y } \{B_{z,y}\} {Bz,y}下,出现了角速度的 x x x轴分量,而不仅仅有 z z z轴分量,也即角速度(注意橙色的 ϕ ˙ \dot\phi ϕ˙箭头)在新的坐标系下的表达。
  • 同理,第三幅图,引入了 ϕ \phi ϕ角,但没有绕 x x x轴的运动( ϕ ˙ = 0 \dot \phi=0 ϕ˙=0),此时的机体角速度就是 ψ ˙ \dot \psi ψ˙角速度(注意橙色的 ϕ ˙ \dot\phi ϕ˙箭头)在姿态角 Φ \Phi Φ表示的坐标系下的表达。

类似的,如果 θ ˙ \dot \theta θ˙不为0,则由俯仰角速率 θ ˙ \dot \theta θ˙引起的角速度在最终的 { B } \{B\} {B}系下可以表达为:
[ ω b x ω b y ω b z ] θ ˙ = B z , y B z , y , x R [ 0 θ ˙ 0 ] \begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}_{\dot\theta}= {^{B_{z,y,x}}_{B_{z,y}}R}\begin{bmatrix}0\\\dot\theta\\0\end{bmatrix} ωbxωbyωbz θ˙=Bz,yBz,y,xR 0θ˙0

由滚转角速率 ϕ ˙ \dot \phi ϕ˙引起的角速度在最终的 { B } \{B\} {B}系下就更直接了,就是:
[ ω b x ω b y ω b z ] ϕ ˙ = [ ϕ ˙ 0 0 ] \begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}_{\dot\phi}= \begin{bmatrix}\dot\phi\\0\\0\end{bmatrix} ωbxωbyωbz ϕ˙= ϕ˙00

以上三个成分相加:
[ ω b x ω b y ω b z ] = [ ω b x ω b y ω b z ] ψ ˙ + [ ω b x ω b y ω b z ] θ ˙ + [ ω b x ω b y ω b z ] ϕ ˙ = R X T ( ϕ ) R Y T ( θ ) [ 0 0 ψ ˙ ] + R X T ( ϕ ) [ 0 θ ˙ 0 ] + [ ϕ ˙ 0 0 ] = [ c θ 0 − s θ s θ s ϕ c ϕ c θ s ϕ s θ c ϕ − s ϕ c θ c ϕ ] [ 0 0 ψ ˙ ] + [ 1 0 0 0 c ϕ s ϕ 0 − s ϕ c ϕ ] [ 0 θ ˙ 0 ] + [ 1 0 0 0 1 0 0 0 1 ] [ ϕ ˙ 0 0 ] = [ 1 0 − s θ 0 c ϕ c θ s ϕ 0 − s ϕ c θ c ϕ ] [ ϕ ˙ θ ˙ ψ ˙ ] \begin{split} \begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}&= \begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}_{\dot\psi}+\begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}_{\dot\theta}+\begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}_{\dot\phi}\\ &= R^T_X(\phi)R^T_Y(\theta)\begin{bmatrix}0\\0\\\dot\psi\end{bmatrix} +R^T_X(\phi)\begin{bmatrix}0\\\dot \theta\\0\end{bmatrix} +\begin{bmatrix}\dot\phi\\0\\0\end{bmatrix}\\ &= \begin{bmatrix} c\theta & 0 & -s\theta\\ s\theta s\phi & c\phi & c\theta s\phi \\ s\theta c\phi & -s\phi & c\theta c\phi \end{bmatrix} \begin{bmatrix}0\\0\\\dot\psi\end{bmatrix} + \begin{bmatrix} 1 & 0 & 0\\ 0 & c\phi & s\phi\\ 0 & -s\phi & c\phi \end{bmatrix} \begin{bmatrix}0\\\dot \theta\\0\end{bmatrix} +\begin{bmatrix}1&0&0\\0&1&0\\0&0&1\end{bmatrix} \begin{bmatrix}\dot\phi\\0\\0\end{bmatrix}\\ &= \begin{bmatrix} 1& 0& -s\theta \\ 0& c\phi& c\theta s\phi \\ 0&-s\phi& c\theta c\phi \end{bmatrix} \begin{bmatrix} \dot\phi\\\dot\theta\\\dot\psi \end{bmatrix} \end{split} ωbxωbyωbz = ωbxωbyωbz ψ˙+ ωbxωbyωbz θ˙+ ωbxωbyωbz ϕ˙=RXT(ϕ)RYT(θ) 00ψ˙ +RXT(ϕ) 0θ˙0 + ϕ˙00 = cθsθsϕsθcϕ0cϕsϕsθcθsϕcθcϕ 00ψ˙ + 1000cϕsϕ0sϕcϕ 0θ˙0 + 100010001 ϕ˙00 = 1000cϕsϕsθcθsϕcθcϕ ϕ˙θ˙ψ˙
也即:
[ ω b x ω b y ω b z ] = [ 1 0 − sin θ 0 cos ϕ cos θ sin ϕ 0 − sin ϕ cos θ cos ϕ ] [ ϕ ˙ θ ˙ ψ ˙ ] \begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}= \begin{bmatrix} 1& 0& -\text{sin}\theta \\ 0& \text{cos}\phi& \text{cos}\theta \text{sin}\phi \\ 0&-\text{sin}\phi& \text{cos}\theta \text{cos}\phi \end{bmatrix} \begin{bmatrix} \dot\phi\\\dot\theta\\\dot\psi \end{bmatrix} ωbxωbyωbz = 1000cosϕsinϕsinθcosθsinϕcosθcosϕ ϕ˙θ˙ψ˙
上述矩阵的逆,由matlab代码求出:

syms theta phi real
A=[1,0,-sin(theta);0,cos(phi),cos(theta)*sin(phi);0,-sin(phi),cos(theta)*cos(phi)];
A_inv = simplify(inv(A))

在这里插入图片描述
也即:
[ ϕ ˙ θ ˙ ψ ˙ ] = [ 1 sin ϕ tan θ cos ϕ tan θ 0 cos ϕ − sin ϕ 0 sin ϕ / cos θ cos ϕ / cos θ ] [ ω b x ω b y ω b z ] \begin{bmatrix} \dot\phi\\\dot\theta\\\dot\psi \end{bmatrix}= \begin{bmatrix} 1 & \text{sin}\phi \text{tan}\theta & \text{cos}\phi \text{tan}\theta\\ 0 & \text{cos}\phi & -\text{sin}\phi \\ 0 & \text{sin}\phi/\text{cos}\theta & \text{cos}\phi/\text{cos}\theta \end{bmatrix} \begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix} ϕ˙θ˙ψ˙ = 100sinϕtanθcosϕsinϕ/cosθcosϕtanθsinϕcosϕ/cosθ ωbxωbyωbz

参考

刚体姿态运动学(二)旋转的微分形式——角速度、欧拉角速度、四元数导数、旋转矩阵导数
控制笔记
姿态角速度和机体角速度,横摆角速度(Yaw Rate)估算
欧拉角速度和机体角速度

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

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

爱上C语言:操作符详解(上)

🚀 作者:阿辉不一般 🚀 你说呢:生活本来沉闷,但跑起来就有风 🚀 专栏:爱上C语言 🚀作图工具:draw.io(免费开源的作图网站) 如果觉得文章对你有帮助的话,还请…

双通道 H 桥电机驱动芯片AT8833,软硬件兼容替代DRV8833,应用玩具、打印机等应用

上期小编给大家分享了单通道 H 桥电机驱动芯片,现在来讲一讲双通道的驱动芯片。 双通道 H 桥电机驱动芯片能通过控制电机的正反转、速度和停止等功能,实现对电机的精确控制。下面介绍双通道H桥电机驱动芯片的工作原理和特点。 一、工作原理 双通道 H 桥电…

毫米波雷达技术在自动驾驶中的关键作用:安全、精准、无可替代

自动驾驶技术正以前所未有的速度不断演进,而其中的关键之一就是毫米波雷达技术。作为自动驾驶系统中的核心感知器件之一,毫米波雷达在保障车辆安全、实现精准定位和应对复杂环境中发挥着不可替代的作用。本文将深入探讨毫米波雷达技术在自动驾驶中的关键…

基于Springboot的时装购物系统(有报告)。Javaee项目,springboot项目。

演示视频: 基于Springboot的时装购物系统(有报告)。Javaee项目,springboot项目。 前些天发现了一个巨牛的人工智能学习网站,通俗易懂,风趣幽默,忍不住分享一下给大家。点击跳转到网站。 项目介绍…

Ansible自动化部署工具-组件及语法介绍

大家好,我是蓝胖子,我认为自动化运维要做的事情就是把运维过程中的某些步骤流程化,代码化,这样在以后执行类似的操作的时候就可以解放双手了,让程序自动完成。避免出错,Ansible就是这方面非常好用的工具。它…

【性能测试】Linux下Docker安装与docker-compose管理容器(超细整理)

目录:导读 前言一、Python编程入门到精通二、接口自动化项目实战三、Web自动化项目实战四、App自动化项目实战五、一线大厂简历六、测试开发DevOps体系七、常用自动化测试工具八、JMeter性能测试九、总结(尾部小惊喜) 前言 1、Linux下Docker…

Python123:使用函数输出指定范围内的Fibonacci数、使用函数验证哥德巴赫猜想(C语言)

文章目录 1、使用函数输出指定范围内的Fibonacci数2、使用函数验证哥德巴赫猜想 1、使用函数输出指定范围内的Fibonacci数 题目&#xff1a; 本题要求实现一个计算Fibonacci数的简单函数&#xff0c;并利用其实现另一个函数&#xff0c;输出两正整数m和n&#xff08;0<m≤n…

SLAM_语义SLAM相关论文

目录 1. 综述 2. 相关文章 Probabilistic Data Association for Semantic SLAM VSO:Visual Semantic Odometry 语义信息分割运动物体

RuoYi-Vue 在Swagger和Postman中 上传文件测试方案

RequestPart是Spring框架中用于处理multipart/form-data请求中单个部分的注解。在Spring MVC中&#xff0c;当处理文件上传或其他类型的多部分请求时&#xff0c;可以使用RequestPart注解将请求的特定部分绑定到方法参数上。 使用RequestPart注解时&#xff0c;需要指定要绑定…

iOS使用NSURLSession实现后台上传下载

NSURLSession后台上传的基本逻辑是&#xff1a;首先创建一个后台模式的NSURLSessionConfiguration&#xff0c;然后通过这个configuration创建一个NSURLSession&#xff0c;接着是创建相关的NSURLSessionTask&#xff0c;最后就是处理相关的代理事件。 1、创建NSURLSession -…

asp.net水资源检测系统VS开发sqlserver数据库web结构c#编程Microsoft Visual Studio

一、源码特点 asp.net 水资源检测系统是一套完善的web设计管理系统&#xff0c;系统具有完整的源代码和数据库&#xff0c;系统主要采用B/S模式开发。开发环境为vs2010&#xff0c;数据库为sqlserver2008&#xff0c;使用c#语言开 发 ASP.NET水资源检测系统1 (1) 应用技…

易点易动固定资产管理系统:定制流程与用量控制的高效库存管理利器

对于企业来说&#xff0c;有效的库存管理是保证供应链运作顺畅、降低成本和提高客户满意度的关键要素。易点易动固定资产管理系统凭借其自定义库存管理流程和库存用量控制功能&#xff0c;成为了提升库存管理效率的利器。本文将详细介绍易点易动固定资产管理系统的自定义流程和…

基于SSM框架的共享单车管理系统小程序系统的设计和实现

基于SSM框架的共享单车管理系统小程序系统的设计和实现 源码传送入口前言主要技术系统设计功能截图Lun文目录订阅经典源码专栏Java项目精品实战案例《500套》 源码获取 源码传送入口 前言 随着科学技术的飞速发展&#xff0c;各行各业都在努力与现代先进技术接轨&#xff0c;…

腾讯云16核服务器配置有哪些?CPU型号处理器主频性能

腾讯云16核服务器配置大全&#xff0c;CVM云服务器可选择标准型S6、标准型SA3、计算型C6或标准型S5等&#xff0c;目前标准型S5云服务器有优惠活动&#xff0c;性价比高&#xff0c;计算型C6云服务器16核性能更高&#xff0c;轻量16核32G28M带宽优惠价3468元15个月&#xff0c;…

acwing算法基础之搜索与图论--BFS

目录 1 基础知识2 模板3 工程化 1 基础知识 BFS可以用来求取最短路&#xff0c;前提条件是所有边的权重一样。 2 模板 题目1&#xff1a;走迷宫&#xff0c;从左上角走到右下角&#xff0c;求最短路。 #include <iostream> #include <queue> #include <cstr…

《嵌入式虚拟化技术与应用》:深入浅出阐述嵌入式虚拟机原理,实现“小而能”嵌入式虚拟机!

随着物联网设备的爆炸式增长和万物互联应用的快速发展&#xff0c;虚拟化技术在嵌入式系统上受到了业界越来越多的关注、重视和实际应用。嵌入式系统与虚拟化技术这个跨界创新组合应运而生&#xff0c;其典型的应用场景包括软件定义汽车驾驶舱、工业领域的工作负载整合等。 虚…

PHP网站源码 知识付费分站代理自助下单系统 自带多款模板

源码测评&#xff1a;功能很齐全&#xff0c;有可以对接的总站&#xff0c;应该是对接好就可以推广赚钱了&#xff0c;但是这种感觉能赚钱的就那么几个人&#xff0c;见仁见智吧&#xff01; 截图演示&#xff1a; 转载自 https://www.qnziyw.cn/cmsmb/qtcms/3952.html

Go进阶之rpc和grpc

文章目录 Go环境安装1&#xff09;windows2&#xff09;linux go语言编码规范1.1 包名&#xff1a;package1.2 ⽂件名1.3 结构体命名1.4 接⼝命名1.5 变量命名1.6 常量命名2.1 包注释2.2 结构&#xff08;接⼝&#xff09;注释2.3 函数&#xff08;⽅法&#xff09;注释2.4 代码…

Javascript知识点详解:正则表达式

目录 RegExp 对象 概述 实例属性 实例方法 RegExp.prototype.test() RegExp.prototype.exec() 字符串的实例方法 String.prototype.match() String.prototype.search() String.prototype.replace() String.prototype.split() 匹配规则 字面量字符和元字符 转义符…

python使用pysqlcipher3对sqlite数据库进行加密

python对很多项目都需要对sqlite数据库的数据进行加密&#xff0c;最流行的加密方式是使用pysqlcipher3&#xff0c;当前使用的python版本为3.7&#xff0c;本博文是直接使用pysqlcipher3在项目上的应用&#xff0c;使用的是已编译好的pysqlcipher3包&#xff0c;如果你需要pys…