【无人机避障】基于控制障碍函数CBF和卡尔曼滤波实现无人机精准轨迹跟踪 + 静态 动态障碍物实时避障附Matlab代码和Simulink
✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、程序设计科研仿真。
🍎完整代码获取 定制创新 论文复现点击：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：做科研，博学之、审问之、慎思之、明辨之、笃行之，是为：博学慎思，明辨笃行。
🔥 内容介绍
过去十年间，无人飞行器（UAV）取得了显著进展，这得益于电子技术、材料科学及计算能力的突破性发展。这类空中系统已从军事专用技术转变为广泛应用于民用领域，涵盖从业余无人机到先进自主平台等多种类型。在各类无人机配置中，四旋翼无人机凭借其结构简单、用途多样且机动性优异的特点，成为应用最广泛的机型之一。四旋翼无人机配备四个对称分布的旋翼，使其能够悬停、垂直起降并执行灵活机动动作。这些特性使其适用于多种应用场景，包括航空摄影与电影拍摄、精准农业、基础设施检测及环境监测。在工业领域，四旋翼无人机日益广泛应用于危险或难以抵达区域的库存管理与维护作业；同时，其在城市空中交通及最后一公里配送方面的潜力也正得到积极探索。在灾害应对与搜救任务中，四旋翼无人机发挥着关键作用——既能快速获取实时态势信息、定位幸存者，又能运送必需物资。其紧凑的设计及在受限环境中导航的能力，在此类场景中尤为突出。此外，在国防与安全领域，四旋翼无人机被广泛用于侦察、监视及战术行动，通常配备先进的传感器和通信系统。
当前研究重点在于提升四旋翼系统的自主性和鲁棒性。计算机视觉、人工智能及传感器融合技术的进步，推动了自主导航技术的重大突破，尤其在无法使用GPS的环境中表现突出。同步定位与建图（SLAM）等技术使四旋翼无人机能够在复杂动态环境中以最小的人工干预实现自主运行。此外，能效与飞行续航能力仍是关键挑战，相关研究正致力于探索新型电池技术和混合推进系统。四旋翼技术的未来在于通过集群机器人技术实现更高效的协同作业——多架无人机协同工作以完成复杂任务，这在大规模测绘、农业精准喷洒及协同救援等应用场景中尤为重要。随着无人机运营法规的持续完善，四旋翼无人机有望在城市空域自主航空系统集成中发挥核心作用，进一步拓展其功能与影响力。总体而言，四旋翼无人机代表着无人机创新的前沿阵地，集多功能性、适应性强及技术先进性于一身。它们在各个领域的日益普及，凸显了其作为现代航空机器人与智能系统关键推动者的重要性。
2. 无人机在障碍物间进行轨迹跟踪的问题：四旋翼无人机运行面临的核心挑战之一，是在充满固定和移动障碍物的环境中实现精确的轨迹跟踪能力。轨迹跟踪是指无人机能够在保持稳定并避免碰撞的同时，遵循预设或动态更新的飞行路径。这一问题在城市配送、搜救以及动态环境下的自主导航等应用中至关重要。该问题的复杂性源于多个因素：
• 动态且不可预测的障碍物：在实际场景中，车辆、行人或其他无人机等障碍物可能具有不可预测的运动特性，需要实时调整规划轨迹；
• 固定环境约束：建筑物、树木或输电线等静态障碍物会形成狭窄走廊或受限空间，要求具备高精度的路径规划与控制系统；
• 非线性动力学特性：四旋翼无人机具有非线性和耦合的动力学特性，这使得设计能在适应环境突发变化时仍能确保系统稳定性和鲁棒性的控制方案极具挑战性。
传感器的局限性：轨迹跟踪性能通常受限于机载传感器的分辨率、探测距离和响应延迟，这些传感器负责检测障碍物并提供态势感知。为应对这些挑战，最先进的方法将先进的运动规划算法与鲁棒控制技术相结合。诸如快速探索随机树（RRT）和基于优化的方法等运动规划技术常被用于生成无碰撞轨迹；而模型预测控制（MPC）和自适应控制器等控制策略则确保四旋翼无人机即使在动态干扰下也能精确遵循规划轨迹。此外，激光雷达、立体相机和深度传感器等先进传感器，以及同步定位与建图（SLAM）技术，有助于实现实时障碍物检测与跟踪。近期研究还融入了人工智能技术，使无人机能够预测障碍物行为并规划考虑未来运动的轨迹。在动态环境中进行轨迹跟踪仍是活跃的研究领域，当前研究重点在于提升计算效率、减少对外部定位系统的依赖，并增强无人机应对高度动态场景的能力。
⛳️ 运行结果
📣 部分代码
% Parametri
dt = 0.1; % intervallo di tempo (secondi)
Q = diag([0.1, 0.1, 0.1, 0.01, 0.01, 0.01]); % covarianza del processo
R = diag([0.5, 0.5, 0.5]); % covarianza della misura
% Matrice di transizione di stato A
A = [1, 0, 0, dt, 0, 0;
0, 1, 0, 0, dt, 0;
0, 0, 1, 0, 0, dt;
0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 1, 0;
0, 0, 0, 0, 0, 1];
% Matrice di osservazione H
H = [1, 0, 0, 0, 0, 0;
0, 1, 0, 0, 0, 0;
0, 0, 1, 0, 0, 0];
% Inizializzazione
x = [0; 3; 20; 0; 0; 0]; % stato iniziale (posizione e velocità)
P = eye(6); % covarianza iniziale
% Simulazione delle misure e stima
num_steps = 20; % numero di passi temporali
positions = zeros(num_steps, 3); % matrice per salvare le posizioni stimate
velocities = zeros(num_steps, 3); % matrice per salvare le velocità stimate
for k = 1:num_steps
% Predizione
x_pred = A * x; % stima predetta dello stato
P_pred = A * P * A' + Q; % predizione della covarianza
% Simulazione della misurazione (posizione con rumore)
z = [2*k; 3; 20] + randn(3, 1) * 0.05; % posizione simulata con rumore
% Correzione
y = z - H * x_pred; % innovazione (errore di misura)
S = H * P_pred * H' + R; % matrice di innovazione
K = P_pred * H' / S; % guadagno di Kalman
x = x_pred + K * y; % aggiornamento dello stato
P = (eye(6) - K * H) * P_pred; % aggiornamento della covarianza
% Salvataggio delle posizioni e velocità stimate
positions(k, :) = x(1:3)'; % salva la posizione stimata (componente 1, 2, 3)
velocities(k, :) = x(4:6)'; % salva la velocità stimata (componente 4, 5, 6)
end
% Plot della posizione stimata in 3D
figure;
plot3(positions(:, 1), positions(:, 2), positions(:, 3), 'b-', 'LineWidth', 2);
hold on;
scatter3(positions(:, 1), positions(:, 2), positions(:, 3), 20, 'r', 'filled');
xlabel('Posizione X');
ylabel('Posizione Y');
zlabel('Posizione Z');
title('Traiettoria stimata dell''ostacolo');
grid on;
legend('Posizione stimata', 'Posizioni misurate');
hold off;
% Plot della velocità stimata
figure;
subplot(3, 1, 1);
plot(1:num_steps, velocities(:, 1), 'r-', 'LineWidth', 2);
title('Velocità stimata lungo l''asse X');
xlabel('Tempo');
ylabel('Velocità (m/s)');
grid on;
subplot(3, 1, 2);
plot(1:num_steps, velocities(:, 2), 'g-', 'LineWidth', 2);
title('Velocità stimata lungo l''asse Y');
xlabel('Tempo');
ylabel('Velocità (m/s)');
grid on;
subplot(3, 1, 3);
plot(1:num_steps, velocities(:, 3), 'b-', 'LineWidth', 2);
title('Velocità stimata lungo l''asse Z');
xlabel('Tempo');
ylabel('Velocità (m/s)');
grid on;
🔗 参考文献
[1] Andrea Cristofaro, Marco Ferro, and Marilena Vendittelli. Safe trajectory tracking using closed-form controllers based on control barrier functions. In 2022 IEEE 61st Conference on Decision and Control (CDC), pages 3329–3334, 2022.
[2] A. Cristofaro, M. Ferro, F. Galasso, M. Mizzoni, A. Pacciarelli, and M. Vendittelli.Combining 3d planning and control barrier functions for safe motion of quadrotoruavs among obstacles. In 2023 9th International Conference on Control, Decision
and Information Technologies (CoDIT), pages 2445–2450, 2023.
[3] Carlo Bruni and Carlo Ferrone. Metodi di stima e filtraggio e l’identificazione deisistemi. Aracne Editrice, Roma, Italia, 2008.