【飞控】倾转分离 (二)-PX4 倾转分离,效果验证

通过之前的,我们详细了解了,PX4的思路和计算过程,我们基本能想象到,这个算法的效果。

为了方便,我们依然称呼3个轴角为 roll ,pitch yaw 虽然他们不是欧拉角。

1.如果同时3个轴误差都很大那么,结果会对齐z轴 ,即没有roll 和 pitch 误差,但是会有一定的 yaw 的误差。

2.因为使用的是固定衰减系数,只要roll 和 pitch 的误差不等于0,那么 yaw 的误差就会被保留。

通过这样的处理,整个控制器的第一目标,就变成了保证roll 和 pitch 没有误差,

除非他们已经为0 ,否则不在乎yaw 是否到达期望值,

这就和我们理解的「倾转分离」目的一致,为了快速控制桨平面(roll,pitch)稳定

(相比比之前这样roll,pitch的优先级被再次提高,以前是变化的衰减系数,只有保证roll,pitch的误差比较小即可,现在是必须没有误差,才去控制yaw)

同样我们继续使用 MATLAB 验证一下,我们的猜想和实际效果一致吗?

思路:

验证1:设置当前姿态为【10,10,10】,期望姿态【45,45,45】模拟三轴同时具有较大误差。

绿色为当前机体坐标系,红色为期望机体坐标系

黑色虚线为不进行倾转分离时的旋转,那么得到的误差,可以让坐标系完全重合

蓝色虚线为倾转分离后计算的旋转,只能保证z重合,即roll 和 pitch 没有误差。

验证2:设置当前姿态为【1,1,1】,期望姿态【2,2,45】模拟 roll ,pitch 误差很小,但 yaw 误差很大。

可以看到,即使roll , pitch 误差非常小,因为对 yaw 采用的是固定衰减系数,所以yaw的误差依然别保留了一部分,可以认为,即使在这种情况下 , roll pitch 的优先级 依然高于 yaw 。

验证3:设置当前姿态为【0,0,1】,期望姿态【0,0,45】模拟roll , pitch 没有误差

只有在这种情况,控制器才会对 yaw 进行完全控制,使 yaw 没有误差。

所以,分析和实验结果一致,再次验证我们分析程序的正确性。

你看,经过这几次的分析,倾转分离也没啥大不了吗?

有什么玄学讲不清的吗?

看完之后甚至觉得这样太简单了吧。

简单来说「倾转分离」其实就做了一件事把roll和pitch的优先级在控制上提高了

部分代码:

global x_axis_last;
global y_axis_last;
global z_axis_last;
x_axis_last=[1,0,0];
y_axis_last=[0,1,0];
z_axis_last=[0,0,1];
NED=[0 0 0];
Cur_angle=[10 10 10];    
Des_angle=[45 45 45];
%根据 roll pitch yaw 的外环增益 计算 权重
attitude_gain =[6.5 , 6.5 , 2.8];
roll_pitch_gain = (attitude_gain(1) + attitude_gain(2)) / 2.0;
yaw_w = constrain_float(attitude_gain(3) / roll_pitch_gain, 0.0, 1.0)
attitude_gain(3) = roll_pitch_gain;

%弧度
cur_angle_radian=[Cur_angle(1)*pi/180,Cur_angle(2)*pi/180,Cur_angle(3)*pi/180] ;  
des_angle_radian=[Des_angle(1)*pi/180,Des_angle(2)*pi/180,Des_angle(3)*pi/180];

%这个画图画的是在以N系为基础的旋转 即 地理坐标系的旋转
plot_body_cur_axis_in_NED([0,0,0],NED,['k','k','k'],'-',2) 
plot_body_cur_axis_in_NED([0,0,0],Cur_angle,['g','g','g'],'-',1) 
plot_body_cur_axis_in_NED([0,0,0],Des_angle,['r','r','r'],'-',1) 

%当前姿态 Ccur->N
cur_dcm= euler2dcm(cur_angle_radian(1),cur_angle_radian(2),cur_angle_radian(3));
q=euler2qual( cur_angle_radian(1),cur_angle_radian(2),cur_angle_radian(3) );
q=Q_normalize(q);
%当前姿态的旋转矩阵 取出z轴列(z轴单位向量) Zcur(N;)
e_z=cur_dcm*[0;0;1];
%期望姿态 Ctar->N
des_dcm = euler2dcm( des_angle_radian(1),des_angle_radian(2),des_angle_radian(3) );
qd=euler2qual( des_angle_radian(1),des_angle_radian(2),des_angle_radian(3) );
qd=Q_normalize(qd);
%期望姿态的旋转矩阵 取出z轴列(z轴单位向量) Ztar(N)
e_z_d=des_dcm*[0;0;1];

%PX4_Q(当前,期望) 得到的是 当前->期望的旋转 Q cur-> tb1
qd_red=PX4_Q(e_z,e_z_d);%%增量 可以加在任何坐标系下 可以等同于机体坐标系下想旋转  

if (abs(qd_red(1)) > (1 - 1e-5) || abs(qd_red(2)) > (1 - 1e-5))         
    qd_red = qd;
else 
    %把转轴 转到N系
    %q(cb->n)^-1 qd_red q(cb->n)
    %把 误差 转到 N系
    %q(cb->n) *  q(cb->n)^-1 qd_red q(cb->n)  = qd_red q(cb->n) 
    %得到地理系下的误差 Qtb1-N 
    qd_red = Q_multipli(qd_red , q);  %
    % 画出Qtb1在地理系下的坐标
    %plot_body_tar_axis_in_NED_Q([0,0,0],NED ,qd_red ,['c','c','c'],':',5) 
end

%Qtar->tb1 = Qtb1->n^-1 * Qtar->n
q_mix = Q_multipli( Q_INV(qd_red) , qd );
% 画出 目标 位置 减去 旋转误差 在地理系下的坐标
%plot_body_tar_axis_in_NED_Q([0,0,0],Des_angle ,Q_INV(q_mix) ,['r','r','r'],'--',3) 

%优化旋转误差
q_mix = q_mix*signNoZero(q_mix(1));
q_mix(1) = constrain_float(q_mix(1), -1, 1);
q_mix(4) = constrain_float(q_mix(4), -1, 1);

%限制旋转误差  组合成新的旋转 qd= Qtb1->N * Qtar->tb1 = Qtar->N
qd = Q_multipli( qd_red , [cos(yaw_w * acos(q_mix(1))), 0, 0, sin(yaw_w * asin(q_mix(4)))]);

%新的 Qtar->cur
qe = Q_multipli( Q_INV(q) , qd) ;

plot_body_tar_axis_in_NED_Q([0,0,0],Cur_angle ,qe ,['b','b','b'],'--',3) 

ok,今天就讲这么多,我是zing,一个有趣的算法工程师,我们下期见。

关注微信公众【无人机干货铺】回复【倾转分离】可以获得PX4,APM倾转分离对比程序。