当前位置：首页 > news >正文

四输入六输出的欠驱动系统建模与仿真

news 2025/10/16 17:05:31

欠驱动系统核心特征

欠驱动系统是指控制输入维度小于系统状态自由度的系统：

输入数 (4) < 输出/状态自由度 (6)
存在内部动力学，不能直接控制所有状态
控制设计更具挑战性，需要巧妙的耦合控制策略

典型示例：四旋翼吊挂负载系统

具体的四旋翼吊挂负载系统为例进行详细说明：

系统配置

4个输入：四个旋翼的升力 (F₁, F₂, F₃, F₄)
6个输出：无人机位置 (x, y, z) + 吊负载摆动角度 (α, β, γ)

classdef QuadrotorLoadSystem < handle% 四旋翼吊挂负载系统 - 四输入六输出欠驱动系统properties% 无人机参数m_q = 1.2;      % 无人机质量 (kg)m_l = 0.3;      % 负载质量 (kg)L = 1.0;        % 吊绳长度 (m)g = 9.81;       % 重力加速度I = diag([0.03, 0.03, 0.04]); % 转动惯量% 状态变量 [x,y,z, φ,θ,ψ, dx,dy,dz, dφ,dθ,dψ, α,β, dα,dβ]state = zeros(16,1);endmethodsfunction obj = QuadrotorLoadSystem(initial_state)if nargin > 0obj.state = initial_state;endend

系统动力学建模

function dstate = dynamics(obj, t, state, U)% 系统动力学方程% U = [F1, F2, F3, F4] - 四个旋翼的升力% 状态提取x = state(1); y = state(2); z = state(3);phi = state(4); theta = state(5); psi = state(6);dx = state(7); dy = state(8); dz = state(9);dphi = state(10); dtheta = state(11); dpsi = state(12);alpha = state(13); beta = state(14);dalpha = state(15); dbeta = state(16);% 总升力和力矩计算F_total = sum(U);tau_phi = obj.L * (U(2) - U(4));    % 滚转力矩tau_theta = obj.L * (U(3) - U(1));   % 俯仰力矩  tau_psi = 0.1 * (U(1) - U(2) + U(3) - U(4)); % 偏航力矩% 旋转矩阵 (机体坐标系到世界坐标系)R = [cos(theta)*cos(psi), sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi), cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi);cos(theta)*sin(psi), sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi), cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi);-sin(theta),         sin(phi)*cos(theta),                              cos(phi)*cos(theta)];% 无人机平动动力学F_gravity = [0; 0; -obj.m_q * obj.g];F_thrust = R * [0; 0; F_total];ddx = (F_thrust(1) + F_gravity(1)) / obj.m_q;ddy = (F_thrust(2) + F_gravity(2)) / obj.m_q;ddz = (F_thrust(3) + F_gravity(3)) / obj.m_q;% 无人机转动动力学I = obj.I;omega = [dphi; dtheta; dpsi];tau = [tau_phi; tau_theta; tau_psi] - cross(omega, I*omega);ddphi = (tau(1) - (I(3,3)-I(2,2))*dtheta*dpsi) / I(1,1);ddtheta = (tau(2) - (I(1,1)-I(3,3))*dphi*dpsi) / I(2,2);ddpsi = (tau(3) - (I(2,2)-I(1,1))*dphi*dtheta) / I(3,3);% 负载摆动动力学 (简化球摆模型)% 这是一个典型的欠驱动子系统ddx_l = ddx + obj.L * (ddalpha*cos(alpha)*cos(beta) - dalpha^2*sin(alpha)*cos(beta) ...- 2*dalpha*dbeta*cos(alpha)*sin(beta) - dbeta^2*cos(alpha)*cos(beta));ddy_l = ddy + obj.L * (ddbeta*cos(beta) - dbeta^2*sin(beta));% 负载运动引起的耦合力F_coupling_x = obj.m_l * ddx_l;F_coupling_y = obj.m_l * ddy_l;% 完整的耦合动力学方程ddalpha = (F_coupling_x*cos(alpha)*cos(beta) - obj.m_l*obj.g*sin(alpha)) / (obj.m_l*obj.L);ddbeta = (F_coupling_y*cos(beta) - obj.m_l*obj.g*sin(beta)*cos(alpha)) / (obj.m_l*obj.L);dstate = [dx; dy; dz; dphi; dtheta; dpsi; ddx; ddy; ddz; ddphi; ddtheta; ddpsi; dalpha; dbeta; ddalpha; ddbeta];
end

分层控制器设计

对于欠驱动系统，通常采用分层控制策略：

function U = hierarchical_controller(obj, state, desired_output)% 分层控制器设计% desired_output = [x_d, y_d, z_d, alpha_d, beta_d, gamma_d]% 第一层：位置控制器 (生成期望姿态)[theta_d, phi_d, F_total] = position_controller(state, desired_output(1:3));% 第二层：姿态控制器 (生成力矩指令)  [tau_phi, tau_theta, tau_psi] = attitude_controller(state, [phi_d, theta_d, 0]);% 第三层：摆动抑制控制器[tau_swing_phi, tau_swing_theta] = swing_damping_controller(state, desired_output(4:6));% 控制分配 (将虚拟控制量映射到4个电机)U = control_allocation(F_total, tau_phi + tau_swing_phi, tau_theta + tau_swing_theta, tau_psi);
endfunction [theta_d, phi_d, F_total] = position_controller(state, pos_desired)% 位置控制：通过无人机姿态控制位置Kp_pos = [2.5, 2.5, 8.0];Kd_pos = [3.0, 3.0, 6.0];pos_error = pos_desired - state(1:3);vel_error = -state(7:9);% PID控制acc_desired = Kp_pos .* pos_error + Kd_pos .* vel_error;% 总升力计算 (高度控制)F_total = obj.m_q * (obj.g + acc_desired(3));% 期望姿态角计算 (水平位置控制)theta_d = asin(acc_desired(1) / (obj.g + acc_desired(3)));phi_d = -asin(acc_desired(2) / (obj.g + acc_desired(3)));
endfunction U = control_allocation(F_total, tau_phi, tau_theta, tau_psi)% 控制分配：将虚拟控制量分配到4个电机% 控制效率矩阵A = [1, 1, 1, 1;0, -obj.L, 0, obj.L;-obj.L, 0, obj.L, 0;0.1, -0.1, 0.1, -0.1];virtual_controls = [F_total; tau_phi; tau_theta; tau_psi];% 求解电机指令U = A \ virtual_controls;% 饱和限制U = max(0, min(U, 15)); % 电机最大升力限制
end

完整仿真例程

%% 四输入六输出欠驱动系统仿真主程序
clear; close all; clc;% 系统初始化
initial_state = [0; 0; 2; 0; 0; 0;  % 位置和姿态0; 0; 0; 0; 0; 0;   % 速度0.2; -0.1; 0; 0];   % 负载摆动状态system = QuadrotorLoadSystem(initial_state);% 仿真参数
dt = 0.01;
T = 10;
t = 0:dt:T;
N = length(t);% 期望轨迹
desired_trajectory = zeros(6, N);
desired_trajectory(1,:) = 2 * sin(0.5*t);      % x轨迹
desired_trajectory(2,:) = 1 * (1 - cos(0.5*t));% y轨迹  
desired_trajectory(3,:) = 2 + 0.1*sin(0.2*t);  % z轨迹
desired_trajectory(4:6,:) = 0;                 % 期望摆动角为0% 存储仿真结果
state_history = zeros(16, N);
control_history = zeros(4, N);
output_history = zeros(6, N);% 主仿真循环
current_state = initial_state;
for k = 1:N% 获取当前期望输出desired_output = desired_trajectory(:,k);% 控制器计算U = system.hierarchical_controller(current_state, desired_output);% 动力学积分 (RK4方法)k1 = system.dynamics(t(k), current_state, U);k2 = system.dynamics(t(k)+dt/2, current_state + dt*k1/2, U);k3 = system.dynamics(t(k)+dt/2, current_state + dt*k2/2, U);k4 = system.dynamics(t(k)+dt, current_state + dt*k3, U);current_state = current_state + dt*(k1 + 2*k2 + 2*k3 + k4)/6;% 存储结果state_history(:,k) = current_state;control_history(:,k) = U;output_history(:,k) = [current_state(1:3); current_state(13:14); 0]; % 6个输出
end% 结果可视化
plot_results(t, state_history, control_history, desired_trajectory);

参考代码四输入六输出欠驱动系统建模仿真 www.youwenfan.com/contentcnj/59554.html

技术与挑战

1. 欠驱动系统的可控性分析

% 线性化系统的可控性分析
[A_lin, B_lin] = linearize_system(nominal_state);
ctrb_matrix = ctrb(A_lin, B_lin);
rank_ctrb = rank(ctrb_matrix);if rank_ctrb < size(A_lin,1)fprintf('系统是欠驱动的，可控性秩: %d/%d\n', rank_ctrb, size(A_lin,1));
end

2. 能量整形控制

对于欠驱动系统，能量整形是一种有效的控制方法：

function U = energy_shaping_controller(state, desired_state)% 基于能量整形的摆动抑制total_energy = compute_total_energy(state);desired_energy = compute_total_energy(desired_state);% 能量差控制energy_error = desired_energy - total_energy;damping_term = -0.5 * state(15:16); % 摆动角速度阻尼U_energy = energy_error * damping_term;
end