pixhawk的姿态控制算法解读

作为学控制的，多轴的姿态控制简单了解一些，当初看px4代码时，对其姿态控制算法表示不能理解，又很难找到一些资料详细解释，经过仔细的推导，大概对其有了一定的认识，因此记录下思路，大概解释下算法代码，如有错误，欢迎指出。

概述：px4的姿态控制，是根据SO3数学基础，推导旋转矩阵模型，采用串级PID控制，控制角度环和角速度环。实际上将姿态控制问题转换成平滑轨迹跟踪问题。

如图所示，该姿态控制主要关注如何平滑的转动。理论上来讲，该算法机动性强。

算法代码详解：

1.构建姿态设计旋转矩阵R_sp和当前姿态旋转矩阵R；

R_sp.set(_v_att_sp.R_body);

R.set(_v_att.R);

2.取出两个矩阵的Z轴向量，

math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));

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

根据这两个z轴计算出误差向量（参考坐标系），并转换到机体坐标系；

math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);

注：%已经符号重载过了，并不是求余，这里指的是向量叉积运算，后面的*，表示点积。

3.计算两个z轴的误差角度；

由公式a×b=︱a︱︱b︱sinθ，a•b=︱a︱︱b︱cosθ，这里的R_z和R_sp_z又是单位向量，模为1，因此误差向量e_R 的模就是sinθ，点积就是cosθ。

float e_R_z_sin = e_R.length();

float e_R_z_cos = R_z * R_sp_z;

4.计算yaw误差角度的权重；

yaw_w = R_sp(2, 2) * R_sp(2, 2);

5.因为多轴的yaw响应一般比roll/pitch慢一倍，因此将两者解耦，先补偿roll/pitch的变化，计算R_rp；

判定两个z轴是否存在误差if (e_R_z_sin > 0.0f)

将e_R转成单位向量；

e_R = e_R_z_axis * e_R_z_angle;

计算e_R_z_axis 的叉积矩阵，根据欧拉旋转公式，求出R_rp；

R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));

6.现在z轴都已经重合了，只需求yaw的误差角度；

取出两个矩阵的x轴（现在只有x轴存在误差）；

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;

7.以上旋转方法，适用于小角度的误差，当转动的角度偏大时，还需另一种方法；

直接计算参考系到机体系的旋转矩阵，并转换成四元数形式；

q.from_dcm(R.transposed() * R_sp);

math::Vector<3> e_R_d = q.imag();

取出虚部，并归一化，根据

e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));

同样得到一个误差角度向量；

求出互补系数

direct_w = e_R_z_cos * e_R_z_cos * yaw_w;

更新e_R，包含两种旋转方法，互补；

e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;

8.求出角速度的期望值，供内环角速度控制使用；

_rates_sp = _params.att_p.emult(e_R);

9.yaw响应较慢，因此再加入一个前馈控制；

_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;

上述用到的相关理论，读者自己查阅书籍。

参考资料：

pixhawk源码 mc_att_control；

High performance full attitude control of a quadrotor on SO(3)；

捷联惯性导航；

pixhawk的姿态控制算法解读相关推荐

姿态控制算法前期准备（四元数+PID算法理论+欧拉角理论）
姿态控制算法前期准备(四元数+PID算法理论+欧拉角理论) 一.需要的知识储备: 由于输入数据为四元组,因此需要对四元组有一些初步的了解: **什么是四元数?**形如: q = a + b ∗ i + ...
Pixhawk原生固件PX4之位姿控制算法解读
参考文献:Minimum Snap Trajectory Generation and Control for Quadrotors PX4中多旋翼无人机的控制分为姿态控制和位置控制两个部分. 大致解 ...
Pixhawk之姿态解算篇（5）_ECF/EKF/GD介绍
一.开篇很久没更新blog了, 最近研究的东西比较杂乱,也整理了很多东西,没有来的及更新,最近发现很多小伙伴都开始写blog了,在不更新就要"被落后了".兄弟们,等等我啊~~~ ...
Pixhawk之姿态解算篇（4）_补充篇
一.开篇大家期待已久的第四篇来了,但是本篇可能比较水啊~~~见谅~~~ 首先,上一周没有什么收获,虽然看了不少的论文,但是却没有什么质的飞越~~~~ 看的论文都是关于姿态解算的,用的算法大部分也都是 ...
Pixhawk之姿态解算篇（3）_源码姿态解算算法分析
一.开篇终于到ardupilot源代码的姿态解算了,有了前期关于mahony姿态解算算法的基础以后,理解源代码的姿态解算算法就快多了,所有的东西都在脑海中初步有了一个框架:首先要做什么,然后再做什么 ...
Pixhawk之姿态解算篇（2）_mahony算法分析
一.开篇还是没能进入到源码部分研究,对姿态解算过程太过于模糊,所以主要开始研究一下关于姿态解算的过程和实现,本篇博文主要是以mahony的算法为基础理解姿态解算的过程,主要参考的论文就是Willia ...
Pixhawk代码分析-姿态解算篇A
姿态解算篇A 基本知识 1.如何实现控制一个无人机系统的算法主要有两类:姿态检测算法.姿态控制算法.姿态控制.被控对象.姿态检测三个部分构成一个闭环控制系统.被控对象的模型是由其物理系统决定,设计无 ...
Pixhawk学习6.1——姿态解算
19年12月份写完传感器标定之后停了得有两个半月了,主要是因为年底试验事情比较多,个人也比较心烦,所以就停滞了.过完年疫情严重,在家里隔离办公,整好有心思整理和学习了一些pix的内容,今天一并发出来. ...
基于串级PID的直立车控制算法研究
徐厚华何哲嘉向颖锋 2020-10-31 Saturday 00摘要直立平衡车因其动力学系统同时具有多变量,非线性,不稳定,强耦合等特性,传统的单环PID控制平衡已经略显吃力.本文将采用内环 ...

pixhawk的姿态控制算法解读

pixhawk的姿态控制算法解读相关推荐

最新文章

热门文章