PX4四旋翼位置控制器的simulink实现

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

当然，最后旋转矩阵需要转化为四元数：