PX4 姿态环控制算法

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
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值