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 |