PX4姿态控制算法分析

2023-05-16

PX4姿态控制流程图

在这里插入图片描述
图片来源
Px4的姿态控制分为角度环(外环)角速度环(内环),角度环使用P控制,角速度环使用PID控制,由于偏航通道响应较慢(多旋翼飞行器的俯仰和滚转运动由旋翼的升力力矩产生,偏航运动由旋翼的反扭矩来产生,而升力力矩要比反扭矩大得多(可从旋翼的升力系数和反扭矩系数中看出),这造成了偏航运动能力相比滚转和俯仰运动能力要弱),故在偏航通道内环加入前馈环节;

控制方法

PX4的姿态控制部分使用的是roll-pitch和yaw分开控制的(是为了解耦控制行为),即tilt和torsion两个环节。如下图:
在这里插入图片描述
根据经验所得,控制toll-pitch比控制yaw更容易实现。比如同样是实现10°的变化,roll-pitch需要60ms左右;但是yaw控制器却需要接近150ms。**使用分步控制的优点在于解耦控制行为,即分别执行响应较快的动作和响应较慢的动作;同时,相比传统的RPY三轴解耦姿态控制,三个姿态通道的姿态动作范围更小,能耗更小。**下图则比较形象地表示了这一点:
在这里插入图片描述

控制算法分析

 math::Matrix<3, 3> R_sp;
 ...
 /* rotation matrix for current state */
 math::Matrix<3, 3> R;
 R.set(_v_att.R);

此处,R_sp表示期望姿态角所对应的期望机体系到地理坐标系的旋转矩阵;R:根据当前的姿态角得到当前机体系到地理坐标系的旋转矩阵;

/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));

得到当前机体系下的z轴在地理系下的坐标表示(旋转矩阵R左乘[0,0,1])

math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));

得到期望机体系下的z轴在地理系下的坐标表示(旋转矩阵R_sp左乘[0,0,1])

/* axis and sin(angle) of desired rotation */
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);

R_z和 R_sp_z叉乘求出由R_z到R_sp_z的旋转轴,由右手定则判断该旋转轴的方向;注意:此处叉乘得到的旋转轴为地理系下的坐标表示。R为当前机体系到地理系的旋转矩阵,其转置为地理系到当前坐标系的旋转矩阵,因为旋转矩阵为单位正交矩阵,逆等于其转置;最终这一步得到旋转轴在当前机体系下的表示。

/* calculate angle error */
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;

a×b=︱a︱︱b︱sinθ,a•b=︱a︱︱b︱cosθ,叉乘得到旋转角正弦,点乘得到旋转角余弦。

/* calculate weight for yaw control */
float yaw_w = R_sp(2, 2) * R_sp(2, 2);

yaw的权值(不是很懂),可能为期望机体系z轴与惯性系z轴之间的重合程度?

/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
if (e_R_z_sin > 0.0f) {
	/* get axis-angle representation */
	float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);//求出旋转角
        //单位化
	math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
	e_R = e_R_z_axis * e_R_z_angle;//注意:此处乘旋转角得到旋转角在机体各个轴上的投影,用于后续的姿态角控制
	/* cross product matrix for e_R_axis */
	//构造叉乘算子
	math::Matrix<3, 3> e_R_cp;
		e_R_cp.zero();
		e_R_cp(0, 1) = -e_R_z_axis(2);
                e_R_cp(0, 2) =  e_R_z_axis(1);
                e_R_cp(1, 0) =  e_R_z_axis(2);
		e_R_cp(1, 2) = -e_R_z_axis(0);
		e_R_cp(2, 0) = -e_R_z_axis(1);
                e_R_cp(2, 1) =  e_R_z_axis(0);
	/* rotation matrix for roll/pitch only rotation */
	R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));//罗德里格斯公式
} else {
	/* zero roll/pitch rotation */
	R_rp = R;
}

得到经过俯仰滚转运动后的坐标系,该坐标系与期望坐标系只有偏航上的偏差,z轴已经对齐;R_rp为该坐标系到地理坐标系的旋转矩阵。

罗德里格斯公式(得到以轴角形式表示的旋转矩阵)
在这里插入图片描述

/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;

求出z轴对齐后两坐标系的x轴偏差角度,用作计算yaw的期望角速度。

if (e_R_z_cos < 0.0f) {//大机动时
	/* for large thrust vector rotations use another rotation method:
	 * calculate angle and axis for R -> R_sp rotation directly */
	math::Quaternion q;
	q.from_dcm(R.transposed() * R_sp);//直接求从当前机体系到目标机体系的四元数表示
	math::Vector<3> e_R_d = q.imag();//根据四元数的定义可知,虚轴即为旋转轴
	e_R_d.normalize();//旋转轴单位化
	e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));//求出旋转角
	
	/* use fusion of Z axis based rotation and direct rotation */
	float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;//直接一步旋转的权重
	e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;//最终得到的形式为直接一步旋转与分步旋转的加权和
}

上述代码为大角度变化时的控制策略(大于90°时);当角度过大时,直接一步旋转的权重较大,使系统快速回到90°的范围以内,再进行分步旋转控制。

/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);//外环P控制

/* limit yaw rate */
_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);//限幅

/* feed forward yaw setpoint rate */
_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;//前馈

相关知识补充

向量叉乘

在这里插入图片描述
a×b=︱a︱︱b︱sinθ

向量点乘

a•b=︱a︱︱b︱cosθ

叉乘算子

将向量叉乘形式表示为叉乘矩阵与向量的乘积

在这里插入图片描述

写的非常好的资料

Pixhawk之姿态控制篇
Pixhawk原生固件PX4之位姿控制算法解读

本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)

PX4姿态控制算法分析 的相关文章

  • 【Android安全】IDA 处理伪代码JUMPOUT指令(Undefine + Create Function)

    IDA 处理伪代码JUMPOUT指令 函数被IDA错误合并 IDA分析so时 xff0c 可能会遇到反编译结果不准确的情况 xff0c 如下 xff1a 这里的两个JUMPOUT其实解析有问题 xff0c 如下 xff1a 例如loc 18
  • 【Android安全】小米8刷机、救砖、root教程

    线刷 xff1a 通过计算机上的刷机软件把ROM 通过数据线传输 并安装到手机内存中 ROM包以tgz为后缀 卡刷 xff1a 把所需要的ROM下载或者复制到内存 SD卡根目录中 小米8救砖教程 xff08 线刷 xff09 按照https
  • 【Android抓包】Ubuntu mitmProxy配置

    Ubuntu 安装 mitmProxy 直接使用编译好的二进制包 参考 xff1a https cuiqingcai com 31053 html Linux E4 B8 8B E7 9A 84 E5 AE 89 E8 A3 85 直接下载
  • 【CSDN】查看自己的CSDN积分

    查看自己的CSDN积分 如何查看自己的CSDN博客积分 CSDN藏的比较深 xff0c 链接如下 xff1a https mp csdn net mp blog analysis article all CSDN博客积分与博客等级 参考 x
  • 【符号输入】打出撇号′

    打出撇号 撇号 xff08 apostrophe xff09 xff1a 搜狗输入法调成中文 xff0c 输入fen xff0c 第5个就是撇号
  • 【Android安全】xiaomi手机关闭adb安装应用时的确认提示

    xiaomi手机关闭adb安装应用时的确认提示 为了自动化测试 xff0c 需要关闭adb安装应用时的确认提示 需要分两步来关闭 xff1a 首先 xff0c 开发者选项 gt 启动MIUI优化 gt 关闭 xff08 第一步过后授权管理
  • 【python】报错UnicodeDecodeError: ‘gbk‘ codec can‘t decode byte in position : illegal multibyte

    python读文件时报错 xff1a Traceback span class token punctuation span most recent call last span class token punctuation span F
  • STM32介绍

    目录 STM32 分类 STM8 和 STM32 分类 STM32 命名方法 STM32F103RCT6 寻找 IO 的功能 存储器映射 存储器 Block0 内部区域功能划分 存储器 Block1 内部区域功能划分 存储器 Block2
  • Putty串口打开无反应

    第一次使用putty的串口 xff0c 可能理所当然认为在Serial那里设置好参数 xff0c 然后点击Open就行了 但是显然不是 xff0c Putty的UI设计有问题 xff0c 不管你点击哪一个项 xff0c Open按钮始终都存
  • 【Android安全】r0capture使用

    r0capture使用 下载地址 xff1a https github com r0ysue r0capture 手机端启动frida server PC端安装frida client 命令 xff1a python r0capture s
  • MobaXterm或Xshell连接不上虚拟机ubuntu

    MobaXterm使用教程 xff1a MobaXterm官网下载 MobaXterm使用教程1 MobaXterm使用教程2 Xshell 使用教程 xff1a 恒源云远程登录Linux实例 包含下载地址和使用教程 Xshell使用教程
  • 591 标签验证器(模拟、栈匹配括号)

    1 问题描述 xff1a 给定一个表示代码片段的字符串 xff0c 你需要实现一个验证器来解析这段代码 xff0c 并返回它是否合法 合法的代码片段需要遵守以下的所有规则 xff1a 代码必须被合法的闭合标签包围 否则 xff0c 代码是无
  • 算法:最长公共子序列

    10 8算法实验报告 最长公共子序列 题目 输出两个字符串的最长公共子序列 要求1 不使用辅助数组 span class token comment 要求1 xff1a 不使用辅助数组 span span class token keywo
  • 呆呆和你谈谈入职CVTE一个月的感受

    呆呆和你谈谈入职CVTE一个月的感受 你盼世界 xff0c 我盼望你无bug Hello 大家好 xff01 我是霖呆呆 xff01 啊啊啊啊啊 至6 18日入职新公司CVTE已经一个多月了 xff0c 在 你盼世界 xff0c 我盼望你无
  • 编程就是调用API?如何成为造轮子的程序员

    是 xff0c 编程就是调用各种API 什么是API xff0c 就是别人把较复杂的代码封装成一个个函数 xff0c 你不用管函数怎么实现的 xff0c 直接用就好 从这个角度讲 xff0c 使用所有库 xff0c 框架 xff0c 模板
  • 【电赛】2019电子设计竞赛 纸张计数显示装置(F题)

    点击 Github项目地址 设计下载 内含 xff1a 电赛论文 程序设计 机械结构设计 硬件电路设计 综合测评相关设计 交互显示设计 设计详细说明 2019年全国大学生电子设计竞赛 纸张计数显示装置 xff08 F题 xff09 本科组
  • 【ARM裸板】LCD硬件原理、时序及初始化

    文章目录 1 LCD与OLED的区别2 LCD原理2 1 颜色如何确定 xff1f 2 2 LCD如何 行扫描 xff1f 2 3 如何跳到下一行进行 行扫描 xff1f 2 4 如何进行下一个 场扫描 xff1f 3 LCD时序4 LCD
  • 【电赛】2019电赛纸张计数显示装置Github仓库说明

    Github项目地址 设计下载 内含 xff1a 电赛论文 程序设计 机械结构设计 硬件电路设计 综合测评相关设计 交互显示设计 设计详细说明 纸张计数显示装置Github仓库说明 x1f604 个人主页 x1f57a 电赛论文 x1f4d
  • 【Linux】mjpg-streamer 源码分析

    文章目录 1 总体流程2 主进程的源码分析2 1 参数接收与解析2 2 获取参数2 3 调用输入函数2 3 1 程序手动中断信号2 3 2 strchr 函数2 3 3 strndup 函数2 3 4 分离参数 3 输入通道源码分析3 1
  • STM32之TIM 舵机控制PWM

    目录 大概步骤 定时器介绍 输入通道 输入滤波器和边沿检测器 捕获通道 定时器初始化结构体详解 1 TIM TimeBaseInitTypeDef 定时器基本初始化结构体 TIM OCInitTypeDef 定时器比较输出初始化结构体 3

随机推荐

  • 【树莓派】树莓派采用MJPG-Streamer双摄推流至上位机,实测延时低至200ms[CSI摄像头+USB摄像头]

    树莓派采用MJPG Streamer双摄推流至上位机 实测延时低至200ms CSI摄像头 43 USB摄像头 总体流程1 硬件连接与软件及驱动配置1 xff09 检测是否存在USB摄像头设备2 xff09 安装 MJPG Streamer
  • 【DIY】基于OpenMV的STM32追球小车

    目录 xff1a 总体设计1 基础硬件DIY设计1 xff09 整体原理图2 xff09 PCB电路 2 OpenMV简单识别程序设计 与 STM32控制程序设计1 xff09 OpenMV简单识别程序设计 microPython 2 xf
  • 【电赛】2017年电赛A题——三相逆变电源EG8030测试

    目录 xff1a 一 相关简介二 专用逆变芯片E8030控制板三 驱动板四 实物测试 xff1a Github项目地址 设计下载 注 xff1a 本文仅用于学习交流分享 xff0c 若有不妥之处 xff0c 请指正 xff0c 感谢 关键词
  • 【STM32】STM32 OLED打点划线画圆 OLED电子罗盘 程序

    目录 xff1a 一 画点函数二 动态划线效果演示 xff1a 三 画圆函数效果演示 四 实心圆函数 注 xff1a 本文仅用于学习分享 用到的工具 xff1a STM32 MCU Keil 5 用到的库函数为 正点原子 STM32F4 库
  • 【STM32】OV2640摄像头学习笔记

    目录 xff1a 一 OV2640 Camera二 读取OV2640模块图像数据过程 xff1a 三 DCMI xff08 Digital camera interface xff09 接口四 SCCB协议1 起始信号2 停止信号 五 OV
  • 【笔记】MS5837-30BA压力传感器调试笔记

    文章目录 一 MS5837 30BA相关介绍1 技术参数2 典型应用电路3 PROM中的标定参数 二 MS5837 30BA数据解算1 解算流程图2 初始化读取标定参数并进行CRC校验 MS5837复位 MS5837 CRC4 bit 校验
  • 【通信协议】1-Wire 单总线

    文章目录 一 1 Wire相关介绍1 典型命令序列 xff1a 2 典型电路图 xff1a 二 1 Wire通信过程1 初始化2 写操作3 读操作 三 1 Wire程序 xff08 以DS18B20为例 xff09 DS18B20功能命令
  • linux 安裝mitmproxy

    1 安装mitmproxy sudo apt install python3 pip amp amp sudo pip3 install U pip amp amp sudo pip3 install mitmproxy 接下来需要安装证书
  • C++ 多态性的一些个人总结

    关于继承 xff1a public继承 xff0c 和其它两种继承方式 xff0c 子类对象可以访问基类的Public成员 xff0c 保护成员和私有成员只能在子类中访问 xff0c 而不能由子类对象进行访问 关于虚函数 xff08 每个虚
  • ubuntu用Dockerfile配置ros+cuda+torch镜像及rviz可视化

    dockerfile配置ros 43 cuda 43 torch镜像及rviz可视化 Dockerfile创建容器 Dockerfile 因工作环境 xff0c 需要有深度学习的那一套环境 xff0c 还要用到一些可视化的东西 xff0c
  • 简单理解TCP/IP协议栈

    协议定义的是一系列的通信标准 xff0c 通信双方需要共同按照这一标准进行正常的数据收发 xff1b 信的双方需要共同按照这一个标准进行正常的数据收发 xff1b xff08 两人 xff0c 说共同的语言 xff0c 不然不能交流 xff
  • ubuntu查看系统版本和linux内核版本

    lsb release a No LSB modules are available Distributor ID Ubuntu Description Ubuntu span class token number 18 04 span 5
  • 电路设计——教你如何阅读数据手册

    我们为什么要看数据手册 xff0c 数据手册又有什么作用呢 xff1f 我们能够从中得到哪些东西呢 xff1f 哪些是我们所需要的呢 xff1f 下面我们以AD847芯片为例来说一说我们在工作中以及设计中需要注意哪些方面 下面是芯片的数据手
  • ORB-SLAM3笔记(编译、踩坑、论文、看代码)

    目前基于orb slam想做的方向 提升动态建图精度 xff08 东西Map就是上不去 KITTI有几个groundtruth官网下架了找不到而且 红外相机退化环境下的点线融合 数据集https sites google com view
  • 【树莓派】Ubuntu-mate安装及ROS安装

    树莓派使用之Ubuntu mate 烧录镜像至SD卡下载镜像烧录SD卡 将SD插入树莓派实物GIF安装流程 树莓派开机sudo reboot换源下载SSH首先得下载net tools下载openssh 电脑远程操作下载 Xshell设置远程
  • 【SLAM】ORB_SLAM3 初步调试运行详细记录

    前言 相关解析及参考 xff1a 超详细解读ORB SLAM3单目初始化 xff08 下篇 xff09 ORB SLAM3和之前版本有什么不同 xff1f 小白学视觉的技术博客 51CTO博客 orbslam3 官方源码地址 xff1a h
  • 如何实现一个简单的Ubuntu远程虚拟桌面

    文章目录 前言一 什么是noVNC xff1f 二 如何部署1 安装VNC服务端1 1 安装tigervnc standalone server1 2 安装tigervnc standalone server1 3 安装xserver xo
  • 软件开发经验总结 读源代码的艺术

    读取源代码是每一个开发人员成长的必经之路 xff0c 一份优秀的源代码 xff0c 是作者多年开发技术的心血结晶 xff0c 研究一份优秀的源代码 xff0c 总是能够让你的技术得到一定程度的提升 然后 xff0c 读别人的源代码并不是拿着
  • vsCode用户设置vue.js、保存格式化代码

    34 window zoomLevel 34 0 34 workbench iconTheme 34 34 vscode icons 34 34 editor wordWrap 34 34 on 34 vscode默认启用了根据文件类型自动
  • PX4姿态控制算法分析

    PX4姿态控制流程图 图片来源 Px4的姿态控制分为角度环 外环 和角速度环 内环 xff0c 角度环使用P控制 xff0c 角速度环使用PID控制 xff0c 由于偏航通道响应较慢 多旋翼飞行器的俯仰和滚转运动由旋翼的升力力矩产生 xff