PX4 姿态环控制算法

最新推荐文章于 2025-06-26 16:40:12 发布
最新推荐文章于 2025-06-26 16:40:12 发布
阅读量1.3k 收藏 27
点赞数 13
CC 4.0 BY-SA版权
文章标签： 无人机 matlab
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/hkzuibang/article/details/136947829

PX4中四旋翼姿态环控制算法，是根据四元数计算得来，不是传统的欧拉角计算。

主要思想：

        1、倾转分离，优先控制俯仰、其次滚转，最后偏航；

        2、四元数的虚分量作为角速度环的期望。

参考资料：

        开源飞控PX4姿态控制算法详解 - 知乎 (zhihu.com)

        论文：Nonlinear Quadrocopter Attitude Control

        

DJI F450 对应的姿态环控制参数：

滚转

俯仰

偏航

角度环P

7

7

2.8

角速度环

P

0.15

0.15

0.3

I

0.05

0.05

0.1

D

0.003

0.003

0

Matlab复现代码：

clear;
clc;

[num] = readtable("qqdeqAttCtrl.xlsx","Sheet","sheet2");
data = table2array(num);
data = data(:,2:5);
% 将数据前面的空格去掉
dataDeal = zeros(size(data));
for i = 1:size(data,1)
    for j= 1:size(data,2)
        if isempty(data{i,j})
             dataDeal(i,j) = 0;
        else
          %  tempStr = strtrim(data{i,j})
           dataDeal(i,j) = str2double(strip(data{i,j},"left",char(160))); 
        end
    end
end
%data = cell2mat(data);
%
%Cur_angle=[0.001 0.001 0.001];    
%for kkkk = 1:51
eqData = zeros(166,4);
eqPX4 = zeros(166,4);
for k = 1
    q = dataDeal(k,:);
    qd= dataDeal(k+1,:);

    eqPX4((k+2)/3,:) =  dataDeal(k+2,:);
    %弧度
    %cur_angle_radian=deg2rad(Cur_angle);
    %des_angle_radian=deg2rad(Des_angle);
    
    %q = angle2quat(cur_angle_radian(3),cur_angle_radian(2),cur_angle_radian(1),'ZYX');  
    %qd = angle2quat(des_angle_radian(3),des_angle_radian(2),des_angle_radian(1),'ZYX'); % sp
    
    
    e_z =q2dcmz(q);
    e_z_d = q2dcmz(qd);
    
    qd_red = quatFrom2Vetor(e_z,e_z_d);
    
    if abs(qd_red(2))>1-1e-5 || abs(qd_red(3))>1-1e-5
        qd_red = qd; 
    else
        qd_red = quatProduct(qd_red, q);
    end
    
    qd_red_inverse = [qd_red(1),-qd_red(2),-qd_red(3),-qd_red(4)]/norm(qd_red);
    
    q_mix = quatProduct(qd_red_inverse,qd);
    % q_mix = q_mix/norm(q_mix);
    
    for i = 1:length(q_mix)
        if  abs(q_mix(i)) > 1e-5
            q_mix = q_mix * sign(q_mix(i));
            break;
        end
    end
    
    q_mix(1) = min(max(q_mix(1),-1),1);
    q_mix(4) = min(max(q_mix(4),-1),1);
     
    yaw_w = 0.4;
    q_tmp = [cos(yaw_w * acos(q_mix(1))) 0 0 sin(yaw_w * asin(q_mix(4)))];
    qd = quatProduct(qd_red,q_tmp);
    %
    [angle_tmp(1), angle_tmp(2),angle_tmp(3)]= quat2angle(qd);
    % disp(rad2deg(angle_tmp'))
    
    q_inverse =  [q(1),-q(2),-q(3),-q(4)]/norm(q);
    qe = quatProduct(q_inverse ,qd);
    % 标准化
    for i = 1:length(qe)
        if  abs(qe(i)) > 1e-5
            qe = qe * sign(qe(i));
            break;
        end
        disp(1)
    end
    proportional_gain = [1 1 1];
    eq = 2*[qe(2), qe(3), qe(4)]'.*proportional_gain';
    eqData((k+2)/3,:) = [eq',0];
end

%plot(err,k);

% rate_limit = deg2rad([220 220 200]);
figure;
subplot(2,3,1)
plot(eqPX4(:,1));
hold on
plot(eqData(:,1));
subplot(2,3,2)
plot(eqPX4(:,2));
hold on
plot(eqData(:,2));
subplot(2,3,3)
plot(eqPX4(:,3));
hold on
plot(eqData(:,3));

subplot(2,3,4)
plot(eqPX4(:,1)-eqData(:,1));

subplot(2,3,5)
plot(eqPX4(:,2)-eqData(:,2));

subplot(2,3,6)
plot(eqPX4(:,3)-eqData(:,3));

function q = quatFrom2Vetor(src,dst)
    % 两种计算方式都一样，怀疑是计算简化的，难以看懂。
    q = [1 0 0 0];
    eps = 1e-5;
    cr = cross(src,dst);
    dt = src'*dst;
    if norm(cr) < eps && dt<0
        cr = abs(src);
        if cr(1) < cr(2)
            if cr(1)<cr(3)
                cr = [1 0 0]';
            else
                cr = [0 0 1]';
            end
        else
            if cr(2) < cr(3)
                cr = [0 1 0]';
            else
                cr = [0 0 1];
            end
        end
        q(1) = 0;
        cr = cross(src,cr);
    else
        q(1) = dt + sqrt((src'*src)*(dst'*dst));
    end
    q(2) = cr(1);
    q(3) = cr(2);
    q(4) = cr(3);
    q = q/norm(q);
end

function outq = quatProduct(q,p)
    outq = [0 0 0 0];
    outq(1) = q(1) * p(1) - q(2) * p(2) - q(3) * p(3) - q(4) * p(4) ;
    outq(2) = q(2) * p(1) + q(1) * p(2) - q(4) * p(3) + q(3) * p(4);
    outq(3) = q(3) * p(1) + q(4) * p(2) + q(1) * p(3) - q(2) * p(4);
    outq(4) = q(4) * p(1) - q(3) * p(2) + q(2) * p(3) + q(1) * p(4);
end

function dcmZ = q2dcmz(q)
    dcmZ = [0 0 0]';
    a = q(1);b=q(2);c=q(3);d=q(4);
    dcmZ(1) = 2 * (a * c + b * d);
    dcmZ(2) = 2 * (c * d - a * b);
    dcmZ(3) = a * a - b * b - c * c + d * d;
end

function q = quatFrom2Vetor2(src,dst)
    q = [1 0 0 0];
    eps = 1e-5;
    cr = cross(src,dst);
    dt = src'*dst;
    if norm(cr) < eps && dt<0
        cr = abs(src);
        if cr(1) < cr(2)
            if cr(1)<cr(3)
                cr = [1 0 0]';
            else
                cr = [0 0 1]';
            end
        else
            if cr(2) < cr(3)
                cr = [0 1 0]';
            else
                cr = [0 0 1];
            end
        end
        q(1) = 0;
        theta = pi;
        cr = cross(src,cr);
    else
        %size(dst)
        theta = acos(dt / sqrt((src'*src)*(dst'*dst)));
       %theta = abs(theta);
        q(1) = cos(theta/2);%%%
        cr = cr/norm(cr);
    end
    q(2) = cr(1)*sin(theta/2);
    q(3) = cr(2)*sin(theta/2);
    q(4) = cr(3)*sin(theta/2);
    q = q/norm(q);%% kyiquxiao
end

测试数据：

name data1 data2 data3  data4
INFO  [AttitudeControl] q0 0.4123 -0.011 -0.0072 0.9109
INFO  [AttitudeControl] qd0 0.4124 0 0 0.9109
INFO  [AttitudeControl] eq 0.0223 -0.014 0
INFO  [AttitudeControl] q0 0.4124 -0.011 -0.0072 0.9109
INFO  [AttitudeControl] qd0 0.4124 0 0 0.9109
INFO  [AttitudeControl] eq 0.0223 -0.0141 0
INFO  [AttitudeControl] q0 0.4124 -0.011 -0.0072 0.9108
INFO  [AttitudeControl] qd0 0.4125 0 0 0.9109
INFO  [AttitudeControl] eq 0.0222 -0.0141 0
INFO  [AttitudeControl] q0 0.4125 -0.011 -0.0072 0.9108
INFO  [AttitudeControl] qd0 0.4125 0 0 0.9109
INFO  [AttitudeControl] eq 0.0222 -0.0141 0
INFO  [AttitudeControl] q0 0.4125 -0.011 -0.0072 0.9108
INFO  [AttitudeControl] qd0 0.4126 0 0 0.9109
INFO  [AttitudeControl] eq 0.0222 -0.0141 0
INFO  [AttitudeControl] q0 0.4126 -0.011 -0.0072 0.9108
INFO  [AttitudeControl] qd0 0.4126 0 0 0.9108
INFO  [AttitudeControl] eq 0.0222 -0.0141 0
INFO  [AttitudeControl] q0 0.4126 -0.011 -0.0072 0.9107
INFO  [AttitudeControl] qd0 0.4127 0 0 0.9108
INFO  [AttitudeControl] eq 0.0222 -0.0141 0
INFO  [AttitudeControl] q0 0.4127 -0.011 -0.0072 0.9107
INFO  [AttitudeControl] qd0 0.4127 0 0 0.9108
INFO  [AttitudeControl] eq 0.0222 -0.0141 0
INFO  [AttitudeControl] q0 0.4127 -0.011 -0.0072 0.9107
INFO  [AttitudeControl] qd0 0.4128 0 0 0.9108
INFO  [AttitudeControl] eq 0.0223 -0.0141 0
INFO  [AttitudeControl] q0 0.4127 -0.011 -0.0072 0.9107
INFO  [AttitudeControl] qd0 0.4128 0 0 0.9108
INFO  [AttitudeControl] eq 0.0223 -0.0141 0
INFO  [AttitudeControl] q0 0.4128 -0.011 -0.0072 0.9107
INFO  [AttitudeControl] qd0 0.4128 0 0 0.9107
INFO  [AttitudeControl] eq 0.0223 -0.0141 0
INFO  [AttitudeControl] q0 0.4128 -0.011 -0.0072 0.9106
INFO  [AttitudeControl] qd0 0.4129 0 0 0.9107
INFO  [AttitudeControl] eq 0.0223 -0.0141 0
INFO  [AttitudeControl] q0 0.4129 -0.011 -0.0072 0.9106
INFO  [AttitudeControl] qd0 0.4129 0 0 0.9107
INFO  [AttitudeControl] eq 0.0223 -0.0141 0
INFO  [AttitudeControl] q0 0.4129 -0.011 -0.0072 0.9106
INFO  [AttitudeControl] qd0 0.413 0 0 0.9107
INFO  [AttitudeControl] eq 0.0223 -0.0141 0
INFO  [AttitudeControl] q0 0.413 -0.011 -0.0072 0.9106
INFO  [AttitudeControl] qd0 0.413 0 0 0.9106
INFO  [AttitudeControl] eq 0.0223 -0.0141 0
INFO  [AttitudeControl] q0 0.413 -0.011 -0.0072 0.9106
INFO  [AttitudeControl] qd0 0.4131 0 0 0.9106
INFO  [AttitudeControl] eq 0.0222 -0.0141 0
INFO  [AttitudeControl] q0 0.4131 -0.011 -0.0072 0.9105
INFO  [AttitudeControl] qd0 0.4131 0 0 0.9106
INFO  [AttitudeControl] eq 0.0222 -0.014 0
INFO  [AttitudeControl] q0 0.4131 -0.011 -0.0072 0.9105
INFO  [AttitudeControl] qd0 0.4132 0 0 0.9106
INFO  [AttitudeControl] eq 0.0222 -0.014 0
INFO  [AttitudeControl] q0 0.4132 -0.011 -0.0072 0.9105
INFO  [AttitudeControl] qd0 0.4132 0 0 0.9106
INFO  [AttitudeControl] eq 0.0223 -0.0141 0
INFO  [AttitudeControl] q0 0.4132 -0.011 -0.0072 0.9105
INFO  [AttitudeControl] qd0 0.4133 0 0 0.9105
INFO  [AttitudeControl] eq 0.0223 -0.0141 0
INFO  [AttitudeControl] q0 0.4133 -0.011 -0.0072 0.9104
INFO  [AttitudeControl] qd0 0.4133 0 0 0.9105
INFO  [AttitudeControl] eq 0.0223 -0.0141 0
INFO  [AttitudeControl] q0 0.4133 -0.011 -0.0072 0.9104
INFO  [AttitudeControl] qd0 0.4134 0 0 0.9105
INFO  [AttitudeControl] eq 0.0222 -0.0141 0
INFO  [AttitudeControl] q0 0.4134 -0.011 -0.0072 0.9104
INFO  [AttitudeControl] qd0 0.4134 0 0 0.9105
INFO  [AttitudeControl] eq 0.0222 -0.0141 0
INFO  [AttitudeControl] q0 0.4134 -0.011 -0.0072 0.9104
INFO  [AttitudeControl] qd0 0.4135 0 0 0.9104
INFO  [AttitudeControl] eq 0.0222 -0.0141 0
INFO  [AttitudeControl] q0 0.4135 -0.011 -0.0072 0.9104
INFO  [AttitudeControl] qd0 0.4135 0 0 0.9104
INFO  [AttitudeControl] eq 0.0222 -0.014 0
INFO  [AttitudeControl] q0 0.4136 -0.0108 -0.0071 0.9103
INFO  [AttitudeControl] qd0 0.4136 0 0 0.9104
INFO  [AttitudeControl] eq 0.022 -0.0138 0.0001
INFO  [AttitudeControl] q0 0.4142 -0.0098 -0.0068 0.9101
INFO  [AttitudeControl] qd0 0.4137 0 0 0.9103
INFO  [AttitudeControl] eq 0.0205 -0.0122 0.0004
INFO  [AttitudeControl] q0 0.4145 -0.0094 -0.0065 0.9099
INFO  [AttitudeControl] qd0 0.4142 0 0 0.9101
INFO  [AttitudeControl] eq 0.0198 -0.0118 0.0002
INFO  [AttitudeControl] q0 0.4146 -0.0094 -0.0065 0.9099
INFO  [AttitudeControl] qd0
最低0.47元/天 解锁文章

200万优质内容无限畅学
中空为用
关注
ardupilot 及PX4姿态误差计算算法对比分析
陌城烟雨
02-29 830
本节主要记录ardupilot 及PX4姿态误差计算算法差异对比过程，欢迎批评指正。备注：1.创作不易，有问题急时反馈2.需要理解四元物理含义、叉乘及点乘含义、方向余弦矩阵含义、四元数乘法物理含义、四元数“差”的物理含义、四元数连乘法则及物理意义、三个坐标轴对应的四元数的含义、四元数和旋转矩阵的关系、四元数轴角方程、四元数和旋转矢量转换关系等3.注意叉乘的顺序，物理含义的理解4.PX4算法的本质难点关键是理解四元数乘法的本质含义：按照旋转矩阵去理解。
PX4中vtol_att_control 源码解析
m2301023的博客
09-20 1448
src/modules/vtol_att_control/文件夹中包含vtol_att_control_main、vtol_type、standard/tailsitter/tiltrotor等文件。下面是主要控制逻辑：本文主要针对tiltrotor构型飞行器进行代码分析。
PX4姿态控制算法详解
weixin_44616080的博客
09-07 3512
本篇文章首发于公众号：无人机系统技术。更多无人机技术相关文章请关注此公众号，有问题也可在公众号回复“加群”进入技术交流群进行交流。 倾转分离 今天的内容我们来解析开源飞控软件PX4中关于多旋翼飞行器的姿态控制算法。 首先，PX4位置控制器的输出会转换成四元数的期望传递给姿态控制器。姿态控制器在得到期望四元数之后并不是马上进行角速率期望的计算，而是先进行倾转分离的操作。 那什么是倾转分离？倾转其实是...
PX4代码阅读笔记（三）——飞行控制框架
最新发布
星光
06-26 1523
PX4的程序通过模块来实现，即每一个功能程序以模块的形式运行在Nuttx系统上，每一个模块相当于一个进程，同时通过orb_message进行消息的输入和输出，与其它模块进行通信。飞行控制主要通过commander，navigator，pos_control，att_control等模块来实现。 commander - 指令/事件处理模块，处理指令、遥控器输入和各种事件，设定飞行器状态和控制模式 navigator - 导航模块，根据指定的任务输出导航轨迹 pos_control - 位置控制，根据指定的轨迹
px4姿态控制
qq_36417330的博客
02-11 2699
一、开篇         姿态控制篇终于来了、来了、来了~~~         心情爽不爽？愉悦不愉悦？开心不开心？         喜...
开源飞控PX4姿态控制算法详解(转载，个人觉得写得最详细的一篇)
诗筱涵的博客
03-29 1326
这里一定要注意我们所得的四元数qe_tilt并不能参与四元数的链式乘法运算，因为他虽然代表了B系到M系的旋转，但是旋转轴却是由N系下的两个向量叉乘计算所得，也就是在N系下，而不在B系或者M系下，解决此问题有两种方法，一种是先将旋转轴通过与旋转矩阵的运算转换到B系后再用来表示四元数。而倾斜和旋转两种运动的原理是不同的，倾斜运动是由不同电机差速运动产生的升力差形成的力矩驱使的，而旋转运动是由不同电机差速运动产生的反扭矩驱使的，前者在数值上往往要比后者大很多。
PX4固定翼控制器详解(一)——姿态控制器
2302_80148937的博客
05-07 7000
固定翼飞控姿态控制器
PX4姿态控制算法分析
Aromash的博客
03-27 6041
PX4姿态控制流程图 Px4的姿态控制分为角度环(外环)和角速度环(内环)，角度环使用P控制，角速度环使用PID控制，由于偏航通道响应较慢，故在偏航通道内环加入前馈环节； 偏航通道响应较慢的原因，多旋翼飞行器的俯仰和滚转运动由旋翼的升力力矩产生，偏航运动由旋翼的反扭矩来产生，而升力力矩要比反扭矩大得多(可从旋翼的升力系数和反扭矩系数中看出)，这造成了偏航运动能力相比滚转和俯仰运动能力要弱，且响应速...
px4 姿态控制流程相关代码分析
05-10
### PX4姿态控制流程相关代码分析 #### 一、姿态控制代码流程 在PX4飞行控制系统中，姿态控制是确保无人机稳定飞行的关键组件之一。姿态控制模块负责接收来自遥控器或者内部控制器的目标姿态指令，并根据当前...
attitude_estimator_q_PX4姿态互补融合滤波_
09-29
在 PX4 飞行控制系统中，`attitude_estimator_q` 是一个核心模块，负责处理传感器数据并提供高精度的姿态解算。本篇文章将深入探讨“attitude_estimator_q”以及它在 PX4 中使用的互补融合滤波技术。 一、互补融合...
PX4姿态控制基础
wegadsewg的博客
11-07 1064
无人机姿态控制代码实现的四个步骤 订阅数据 （飞机现姿态，飞机期望姿态） PID 外环计算（作用于姿态误差，角速度） PID 内环计算 （角加速度，减少噪声） 发布数据 （混控器矩阵，控制电机转速） 解耦合控制 受制于传统多旋翼无人机结构限制，电机对pitch和roll的控制力矩较大，yaw的控制力矩较小，所以需要解耦控制。 （对其Z轴旋转后再进行yaw旋转） ...
px4-L1自适应控制算法.pdf
09-01
本文首先理清了l1 自适应算法的思路，然后，根据算法的实现步骤，对apm 自适应算法的实现做了细致的分析，读者可以加强对apm代码的了解
PX4控制算法中L1控制流程图.vsdx
07-11
在px4代码的研究学习中，总结了一些代码流程图，可以大大提高代码的阅读效率，谨以此文档，献给开源PX4代码的爱好者，欢迎交流学习
PX4姿态控制流程图mc_att_control_main.cpp.pdf
04-26
PX4姿态控制流程图mc_att_control_main.cpp
PX4中的四元数姿态估计算法推导
06-12
1. int AttitudeEstimatorQ::start( ) 程序启动函数 2. void AttitudeEstimatorQ::task_main( ) 进程入口 3. float gyro[3] 获取传感器数据， DataValidatorGroup，并检验数据可靠度 4. 通过 uORB 模型获取 vision 和 mocap 的数据 5. 位置加速度获取（_pos_acc）
PX4 四元数旋翼姿态控制修正
zhao23333的博客
12-09 2507
PX4 四元数旋翼姿态控制修正1. 四元数介绍2. 旋翼四元数角度控制逻辑3. 最终控制实现逻辑4. 仿真验证 版本：V1.10.2； 源码位置：~\src\modules\ma_att_control\ma_att_control_main.cpp； 1. 旋翼姿态控制分为两部分，第一部分是外环角度控制，通过偏差角度作为输入得到期望角速率输入到内环角速率控制，这里先来解释外环控制。 2. 之前有一篇博客（https://blog.youkuaiyun.com/zhao23333/article/details/
全网最详细PX4控制算法教程
3D视觉工坊
06-09 3381
目前，四旋翼无人机在农业植保、物流配送、竞技飞行及科研实验等领域得到广泛应用。许多公司、科研机构与无人机爱好者都会开发自己的飞控平台，在众多开源飞控平台中，PX4以其完善的功能、优异的性能以及友好的生态脱颖而出。不仅是科研机构大量使用PX4，许多公司也会基于PX4进行改进完善，实现工业级的需求。因此，能够基于PX4进行二次开发对于学习和工作都有着重要的价值和意义。飞控平台是一个复杂的软硬件系统，软...
px4无人机姿态控制原理理解（获取传感器数据--四元数计算--控制输出）
ddddddddddddaa的博客
02-26 1278
其中error为当前角度值和目标角度值的差异，也可以为当前姿态四元数和目标姿态四元数之间的差异。只需要获取旋转角度和目标位置向量即可得到目标姿态的四元数，也可以直接使用旋转角度转换为四元数。通过传感器数据，并结合互补滤波或卡尔曼滤波，估算无人机的姿态信息并生成当前姿态的四元数。PID控制器输出值分别通过转化为PWM信号输入到不同的电机，实现无人机的姿态旋转。利用计算得到的四元数和初始姿态四元数相乘即可得到无人机新的姿态四元数。利用当前的姿态信息和目标姿态信息之间的误差生成PID控制器输出。
Pixhawk之姿态控制篇（1）_源码算法分析（超级有料）
热门推荐
.
05-17 6万+
一、开篇         姿态控制篇终于来了、来了、来了~~~         心情爽不爽？愉悦不愉悦？开心不开心？         喜欢的话就请我吃顿饭吧，哈哈。         其实这篇blog一周前就应该写的，可惜被上一篇blog霸占了。但是也不算晚，整理了很多算法基础知识，使得本篇blog更充实。一人之力总是有限的，难免有不足之处，大家见谅，有写的不好的地方劳烦指正。看到标题了吧，
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值