1、整体框架
位置控制器由位置环与速度环构成,控制器输入为期望位置与当前位置,输出期望油门与期望姿态。
2、位置环
位置环采用P控制,需要对速度指令进行限幅。
限幅代码很简单:
function [Vx_cmd,Vy_cmd] = fcn(Vx,Vy)
Vxy = sqrt(Vx^2+Vy^2);
Vxy_max = 15;
if Vxy > Vxy_max
Vx_cmd = Vx*Vxy_max/Vxy;
Vy_cmd = Vy*Vxy_max/Vxy;
else
Vx_cmd = Vx;
Vy_cmd = Vy;
end
end
3、速度环
速度环采用PID+前馈的方法进行控制,同样的也需要进行加速度限幅,并将期望加速度转换为期望的推力矢量。
限幅代码与位置环类似。
期望加速度到期望推力矢量的转换:(其中包括了倾斜角限幅)
function thr_sp = A_2_T(acc_sp)
CONSTANTS_ONE_G = 9.806650162;
max_angle = 30*pi/180;
hover_thrust = 0.55;
world_unit = [0,0,1]; % Define the world unit vector
lim_thr_min = 0.9;
% Assume standard acceleration due to gravity in vertical direction for attitude generation
body_z = [-acc_sp(1),-acc_sp(2),CONSTANTS_ONE_G];
body_z = body_z / norm(body_z); % Normalize the vector
% determine tilt
dot_product_unit = dot(body_z, world_unit);
angle = acos(dot_product_unit);
% limit tilt
angle = min(angle, max_angle);
rejection = body_z - (dot_product_unit * world_unit);
% corner case exactly parallel vectors
if norm(rejection) < eps
rejection = [1,0,0];
end
body_z = cos(angle) * world_unit + sin(angle) * (rejection / norm(rejection));
% Scale thrust assuming hover thrust produces standard gravity
collective_thrust = acc_sp(3) * (hover_thrust / CONSTANTS_ONE_G) - hover_thrust;
% Project thrust to planned body attitude
collective_thrust = collective_thrust / dot([0,0,1], body_z);
collective_thrust = max(collective_thrust, -lim_thr_min);
thr_sp = body_z * collective_thrust;
end
4、期望推力矢量转化为期望姿态
这里需要注意,期望推力矢量与期望机体z轴是相反的。通过向量叉乘的性质依次得到期望机体x、y轴的方向,组成期望旋转矩阵。期望推力大小就是期望推力矢量的模。
function [thrust_body,R_sp] = T_2_q(yaw_sp,thr_sp)
% Convert thrust vector to attitude
body_z = -thr_sp;
if norm(body_z) < eps
body_z(3) = 1;
end
body_z = body_z / norm(body_z);
y_C = [-sin(yaw_sp), cos(yaw_sp), 0];
body_x = cross(y_C, body_z);
if body_z(3) < 0
body_x = -body_x;
end
if abs(body_z(3)) < 0.000001
body_x = [0, 0, 1];
end
body_x = body_x / norm(body_x);
body_y = cross(body_z, body_x);
R_sp = [body_x; body_y; body_z];
thrust_body = norm(thr_sp);
end
当然,最后旋转矩阵需要转化为四元数: