PX4 姿态环控制算法

最新推荐文章于 2025-03-29 21:27:12 发布

原创

最新推荐文章于 2025-03-29 21:27:12 发布 · 1.5k 阅读

27 ·

CC 4.0 BY-SA版权

文章标签：

#无人机 #matlab

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元/天解锁文章