本文详细介绍了L1导航算法的原理，包括直线和圆弧路径跟踪，并提供了Matlab仿真代码。L1算法是一种非线性的无人机路径跟随算法，通过选取参考点并计算横向加速度来平滑地接近期望轨迹。文中提到，L1长度和角度的求解是关键，且算法在初始偏差较大时能避免过激调节。提供的Matlab代码展示了如何生成参考轨迹、跟踪常值参考轨迹以及部分源代码实现。
💥💥💥💥💥💥💞💞💞💞💞💞💞💞欢迎来到海神之光博客之家💞💞💞💞💞💞💞💞💥💥💥💥💥💥
✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进；
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式
⛳️座右铭：行百里者，半于九十。
更多Matlab路径规划仿真内容点击👇
①Matlab路径规划（进阶版）
②付费专栏Matlab路径规划（初级版）
⛳️关注优快云海神之光，更多资源等你来！！
⛄一、L1导航算法简介
L1导航算法是非常经典的非线性无人机路径跟随算法，最早由MIT于2004年提出，其导航算法中是先选点，生成一段为L1的路径。
1 直线路径跟踪
L1路径跟随算法的基本思想就是在期望轨迹上选择一个参考点，并且用这个产生一个横向的加速度，加速度表示为，
固定翼无人机在横向加速度作用下飞圆弧接近期望轨迹，公式很容易推导，可以看出，加速度与当前空速、空速与L1期望点夹角，以及无人机与L1期望点之间的距离有关。空速可以观测，故横向加速度的求解主要是要确定L1长度，求解η \etaη角。
对于直线期望路径，如下图所示，所求夹角可以表示为，
2 圆弧路径跟踪
圆弧期望加速度求解主要是几何上的一些推导，具体可以看论文中的推导，主要涉及三个角度空速与该点切线的夹角η2 ，该点到L1连线与L1对应弦的夹角η1，2η3相当于是L1所对应于的圆心角。
3 个人对算法的理解
（1）L1轨迹跟踪是非线性的导航算法，在实际的使用中，主要就是要解决L1的取值问题和sin η \sin \etasinη的求解，因为是基于角度正弦的，所以相比于基于偏距的线性算法，在初始偏差比较大的时候，不会出现过激调节，可以比较平滑的向期望路径过渡，而在进入期望路径后，其控制效果较偏距控制更好，尤其是在圆形轨迹跟踪。
（2）L1轨迹跟踪相当于在跟踪一个L1点，这个点的轨迹就是期望路径，点按一定频率刷新，按一定频率计算角度，刷新侧向加速度输出。
（3）对sin η \sin \etasinη进行小角度线性化，可以得到在期望路径附近的加速度线性化的形式，以跟踪直线为例，实际上就是一个二阶系统，
这个二阶系统固有频率和速度以及L1长度有关，阻尼实际上是与那个增益就是那个2倍有关，在论文中，L1取了固定值150m，而V也只测试了25m/s的速度。对于圆的跟踪和直线差不多，只是固有频率也与盘旋半径有关。
⛄二、部分源代码
clc;
clear all;
%% 参考轨迹生成
N=100;
T=0.05;
% Xout=zeros(2N,3);
% Tout=zeros(2N,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 = Rrw; % 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,NxTsim);
XXX=zeros(Nr,NxTsim);%用于保持每个时刻预测的所有状态值
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.1eye(NuTsim,NuTsim);
for i=1:1:Nr
t_d =Xout(i,3);
a=[1 0 -vd1*sin(t_d)T;
0 1 vd1cos(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;
%title(‘状态量-横向坐标X对比’);
xlabel(‘采样时间T’);
ylabel(‘横向位置X’)
subplot(3,1,2);
plot(Tout(1:Nr),Xout(1:Nr,2),‘k–’);
hold on;
%grid on;
%title(‘状态量-横向坐标Y对比’);
xlabel(‘采样时间T’);
ylabel(‘纵向位置Y’)
subplot(3,1,3);
hold on;
%grid on;
hold on;
%title(‘状态量-\theta对比’);
xlabel(‘采样时间T’);
ylabel(‘\theta’)
figure(3)
subplot(2,1,1);
%grid on;
%title(‘控制量-纵向速度v对比’);
xlabel(‘采样时间T’);
ylabel(‘纵向速度’)
subplot(2,1,2)
%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 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断
3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知
4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化
5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配
6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化
7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化
8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置
9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长
10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合
扫一扫
380
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
