【轨迹跟踪】基于matlab无人机轨迹跟踪【含Matlab源码 1152期】
💥💥💥💥💥💥💞💞💞💞💞💞💞💞欢迎来到海神之光博客之家💞💞💞💞💞💞💞💞💥💥💥💥💥💥
博主简介：专注于科研领域的Matlab仿真开发者，在追求技术卓越的同时也在不断精进个人修养；
个人主页：海神之光
在学习路径中我已经整理了大量Matlab相关资源，在此分享给有需要的朋友；
�️座右铭：跋涉千里路始于九千。
更多关于Matlab路径规划的仿真内容，请点击此处了解更多信息。
① Matlab路径规划（进阶版）：[()
② 付费学习包 Matlab路径规划（初级版）：[()
⛳️关注海神之光，更多资源等你来！！
⛄一、L1导航算法简介
改写内容
改写内容
L1路径跟随算法的核心概念在于，在预设的目标轨迹上系统性地确定了一个基准位置，并通过这一基准位置导致了一个横向加速度的出现。这种加速度通常被表示为：
固定翼无人机在横向加速度的影响下进行圆弧轨迹的接近过程相对复杂，在实际应用中其数学表达式相对容易推导出来。通过分析可知其受以下因素的影响：一是飞行器当前的速度值；二是飞行器相对于目标点L1的角度（即空速与L1期望点夹角）；三是目标点与飞行器之间的距离关系（即L1长度）。由于飞行器的速度可以通过传感器直接获取到，则横向加速度的计算只需确定目标点与飞行器之间的几何关系即可实现。其中所求的角度即为目标方向与当前方向之间的夹角，在此基础上可进一步计算出所需的纵向偏移量η（eta）值。
当无人机沿着直线路径飞行时（如图所示），其中所求的角度即为目标方向与当前方向之间的夹角。这种角度可以通过测量或计算的方式得到。
2 圆弧路径跟踪
圆弧期望加速度求解主要涉及几何上的相关推导；其中详细阐述了其中三个关键角度及其对应的切线夹角η_2、以及该点至L_1连线与其对应的弦之间的夹角θ_1；此外，在计算过程中发现2θ_3等于对应的中心角。
3 个人对算法的理解
（1）L1轨迹跟踪是非线性的导航算法，在实际的使用中，主要就是要解决L1的取值问题和sin η \sin \etasinη的求解，因为是基于角度正弦的，所以相比于基于偏距的线性算法，在初始偏差比较大的时候，不会出现过激调节，可以比较平滑的向期望路径过渡，而在进入期望路径后，其控制效果较偏距控制更好，尤其是在圆形轨迹跟踪。
（2）L1轨迹跟踪相当于在跟踪一个L1点，这个点的轨迹就是期望路径，点按一定频率刷新，按一定频率计算角度，刷新侧向加速度输出。
（3）对sin η \sin \etasinη进行小角度线性化，可以得到在期望路径附近的加速度线性化的形式，以跟踪直线为例，实际上就是一个二阶系统，
该二阶系统的固有频率、速度以及L1长度均与其相关。在论文研究中发现：阻尼系数与增益直接成正比关系；其中L1取值固定为150米；其速度仅测试了25米/秒；值得注意的是，在实际应用中发现圆轨迹的跟踪效果与直线运动基本一致，并且系统的固有频率还与其盘旋半径相关。
⛄二、部分源代码
clc;
clear all;
%% 参考轨迹生成
N=100;
T=0.05;
% Xout=zeros(2 _N,3);
% Tout=zeros(2_N,1);
Xout=zeros(N,3);
Tout=zeros(N,1);
for k=1:1:N
Xout(k,1)=k*T;
Xout(k,2)=2;
Xout(k,3)=0;
Tout(k,1)=(k-1)*T;
end
%% Tracking a constant reference trajectory
Nx=3;
Nu =2;
Tsim =20;
X0 = [0 0 pi/3];
[Nr,Nc] = size(Xout); % Nr is the number of rows of Xout
% Mobile Robot Parameters
c = [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
L = 1;
Rr = 1;
w = 1;
% Mobile Robot variable Model
vd1 = Rr _w; % For circular trajectory
vd2 = 0;
x_real=zeros(Nr,Nc);
x_piao=zeros(Nr,Nc);
u_real=zeros(Nr,2);
u_piao=zeros(Nr,2);
x_real(1,:)=X0;
x_piao(1,:)=x_real(1,:)-Xout(1,:);
X_PIAO=zeros(Nr,Nx_Tsim);
XXX=zeros(Nr,Nx _Tsim);%用于保持每个时刻预测的所有状态值
q=[1 0 0;0 1 0;0 0 0.5];
Q_cell=cell(Tsim,Tsim);
for i=1:1:Tsim
for j=1:1:Tsim
if i==j
Q_cell{i,j}=q;
else
Q_cell{i,j}=zeros(Nx,Nx);
end
end
end
Q=cell2mat(Q_cell);
R=0.1_eye(Nu Tsim,Nu Tsim);
for i=1:1:Nr
t_d =Xout(i,3);
a=[1 0 -vd1*sin(t_d)_T;
0 1 vd1_cos(t_d)*T;
0 0 1;];
b=[cos(t_d)*T 0;
sin(t_d)*T 0;
0 T;];
A_cell=cell(Tsim,1);
B_cell=cell(Tsim,Tsim);
for j=1:1:Tsim
A_cell{j,1}=a^j;
for k=1:1:Tsim
if k<=j
B_cell{j,k}=(a^(j-k))*b;
else
B_cell{j,k}=zeros(Nx,Nu);
end
end
end
A=cell2mat(A_cell);
B=cell2mat(B_cell);
b_cons=[];
lb=[-1;-1];
ub=[1;1];
tic
[X,fval(i,1),exitflag(i,1),output(i,1)]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub);
toc
X_PIAO(i,:)=(A*x_piao(i,:)'+B*X)';
if i+j<Nr
for j=1:1:Tsim
XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(i+j,1);
XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(i+j,2);
XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(i+j,3);
end
else
for j=1:1:Tsim
XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(Nr,1);
XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(Nr,2);
XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(Nr,3);
end
end
u_piao(i,1)=X(1,1);
u_piao(i,2)=X(2,1);
Tvec=[0:0.05:4];
X00=x_real(i,:);
vd11=vd1+u_piao(i,1);
vd22=vd2+u_piao(i,2);
XOUT=dsolve('Dx-vd11*cos(z)=0','Dy-vd11*sin(z)=0','Dz-vd22=0','x(0)=X00(1)','y(0)=X00(2)','z(0)=X00(3)');
t=T;
x_real(i+1,1)=eval(XOUT.x);
x_real(i+1,2)=eval(XOUT.y);
x_real(i+1,3)=eval(XOUT.z);
if(i<Nr)
x_piao(i+1,:)=x_real(i+1,:)-Xout(i+1,:);
end
u_real(i,1)=vd1+u_piao(i,1);
u_real(i,2)=vd2+u_piao(i,2);
figure(1);
hold on;
title('跟踪结果对比');
xlabel('横向位置X');
axis([-1 5 -1 3]);
ylabel('纵向位置Y');
hold on;
for k=1:1:Tsim
X(i,k+1)=XXX(i,1+3*(k-1));
Y(i,k+1)=XXX(i,2+3*(k-1));
end
X(i,1)=x_real(i,1);
Y(i,1)=x_real(i,2);
plot(X(i,:),Y(i,:),'y')
hold on;end
%% 以下为绘图部分
figure(2)
subplot(3,1,1);
hold on;
%grid on;
在绘图窗口中显示网格线；
设置图形标题为状态量-横向坐标X对比；
设置x轴标签为采样时间T；
设置y轴标签为横向位置X；
在图形窗口中创建一个3行1列的子图布局，并选择第二个子图进行显示；
绘制从时间序列数据中提取的曲线数据；
将当前绘图加入到当前图形中而不清除。
设置网格线显示为打开。
本图形窗口标题设定为"对比分析：状态量与横向坐标Y的关系"。
x轴变量标注设定为"采样时间（单位：秒）"。
y轴变量标注设定为"纵向位置（单位：米）"。
设置子图布局为3行1列中的第三块并显示。
hold on;
%grid on;
hold on;
%title(‘状态量-\theta对比’);
xlabel(‘采样时间T’);
ylabel(‘\theta’)
figure(3)
subplot(2,1,1);
%grid on;
%title('控制量与纵向速度v的比较');
xlabel('采样时刻t_s');
ylabel('纵向速度');
位于第二幅子图的底部位置.
%grid on;
%title(‘控制量-角加速度对比’);
xlabel(‘采样时间T’);
ylabel(‘角加速度’)
figure(4)
subplot(3,1,1);
%grid on;
xlabel(‘采样时间T’);
ylabel(‘e(x)’);
subplot(3,1,2);
%grid on;
xlabel(‘采样时间T’);
ylabel(‘e(y)’);
subplot(3,1,3);
%grid on;
xlabel(‘采样时间T’);
ylabel(‘e(\theta)’);
⛄三、运行结果
⛄四、matlab版本及参考文献
1 matlab版本
2014a
2 参考文献
[1] 张萍. 四旋翼飞行器姿态控制建模与仿真研究论文[J]. 电机与自动化领域. 2019,46(12)
[2] 刘岩, 杨牧. 四旋翼飞行器飞行控制系统设计研究论文[J]. 山东工业技术. 2019,(07)
3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除
🍅 仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化
2 机器学习和深度学习方面
3 图像处理方面
基于当前研究现状, 提出了一系列相关的技术和方法: 包括基于深度学习的图象识别技术, 基于区域划分的图象分割方法, 以及基于特征提取的检测算法等; 同时, 在信息隐藏领域也取得了一些突破性进展: 如先进的图象隐藏技术和高效的融合算法; 在模式匹配领域, 提出了多种改进型配准方法和新型拼接技术; 此外, 在增强效果方面也取得了显著成果: 如智能增强技术和智能压缩感知技术等
4 路径规划方面
无人机路径规划方案、多架次无人飞行器编队协调、多智能体协同作业、多目标任务分配策略
6 无线传感器定位及布局方面
传感器部署方案的完善、通信协议效能的提升、路由机制的改进、目标节点定位精度的提高、基于距离辅助的位置估计技术的应用、低功耗高数据传输率协议的应用、无线传感器网络覆盖区域的有效扩展、组播数据传输效率的最大化、基于接收信号强度的位置估计技术的应用
7 信号处理方面
8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置
9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长
10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合