L1导航算法是非常经典的非线性无人机路径跟随算法，最早由MIT于2004年提出，论文为《A New Nonlinear Guidance Logic for Trajectory Tracking》，其导航算法中是先选点，生成一段为L1的路径，最近学习了一下算法和代码，做一下总结，图源论文。

一、直线路径跟踪

L1路径跟随算法的基本思想就是在期望轨迹上选择一个参考点，并且用这个产生一个横向的加速度，加速度表示为，
ascmd=2V2L1sin⁡η{{a}_{{{s}_{cmd}}}}=2\frac{{{V}^{2}}}{{{L}_{1}}}\sin \etaascmd=2L1V2sinη
固定翼无人机在横向加速度作用下飞圆弧接近期望轨迹，公式很容易推导，可以看出，加速度与当前空速、空速与L1期望点夹角，以及无人机与L1期望点之间的距离有关。空速可以观测，故横向加速度的求解主要是要确定L1长度，求解η\etaη角。
对于直线期望路径，如下图所示，所求夹角可以表示为，

sin⁡η=η1+η2η1≈dL1η2≈d˙Vascmd=2V2L1sin⁡η≈2VL1(d˙+VL1d)\sin \eta ={{\eta }_{1}}+{{\eta }_{2}} \\ {{\eta }_{1}}\approx \frac{d}{{{L}_{1}}} \\ {{\eta }_{2}}\approx \frac{{\dot{d}}}{V} \\ {{a}_{{{s}_{cmd}}}}=2\frac{{{V}^{2}}}{{{L}_{1}}}\sin \eta \approx 2\frac{V}{{{L}_{1}}}\left( \dot{d}+\frac{V}{{{L}_{1}}}d \right) sinη=η1+η2η1≈L1dη2≈Vd˙ascmd=2L1V2sinη≈2L1V(d˙+L1Vd)

二、圆弧路径跟踪

sin⁡η3≈L12Rc≡cos⁡η3≈1−(L12R)2η1≈dL1cos⁡η3η2≈d˙V\sin {{\eta }_{3}}\approx \frac{{{L}_{1}}}{2R} \\ c\equiv \cos {{\eta }_{3}}\approx \sqrt{1-{{\left( \frac{{{L}_{1}}}{2R} \right)}^{2}}} \\ {{\eta }_{1}}\approx \frac{d}{{{L}_{1}}}\cos {{\eta }_{3}} \\ {{\eta }_{2}}\approx \frac{{\dot{d}}}{V}sinη3≈2RL1c≡cosη3≈1−(2RL1)2η1≈L1dcosη3η2≈Vd˙
圆弧期望加速度求解主要是几何上的一些推导，具体可以看论文中的推导，主要涉及三个角度空速与该点切线的夹角η2\eta_{2}η2，该点到L1连线与L1对应弦的夹角η1\eta_{1}η1，2η32\eta_{3}2η3相当于是L1所对应于的圆心角。

ascmd=2V2L1sin⁡η=2V2L1sin⁡(η1+η2+η3)=2V2L1sin⁡(η1+η2+η3)≈2V2L1(η1cos⁡η3+η2cos⁡η3+sin⁡η3)≈2V2L12c2d+2VL1cd˙+V2R{{a}_{{{s}_{cmd}}}}=2\frac{{{V}^{2}}}{{{L}_{1}}}\sin \eta =2\frac{{{V}^{2}}}{{{L}_{1}}}\sin \left( {{\eta }_{1}}+{{\eta }_{2}}+{{\eta }_{3}} \right) \\ =2\frac{{{V}^{2}}}{{{L}_{1}}}\sin \left( {{\eta }_{1}}+{{\eta }_{2}}+{{\eta }_{3}} \right) \\ \approx 2\frac{{{V}^{2}}}{{{L}_{1}}}\left( {{\eta }_{1}}\cos {{\eta }_{3}}+{{\eta }_{2}}\cos {{\eta }_{3}}+\sin {{\eta }_{3}} \right) \\ \approx 2\frac{{{V}^{2}}}{{{L}_{1}}^{2}}{{c}^{2}}d+2\frac{V}{{{L}_{1}}}c\dot{d}+\frac{{{V}^{2}}}{R}ascmd=2L1V2sinη=2L1V2sin(η1+η2+η3)=2L1V2sin(η1+η2+η3)≈2L1V2(η1cosη3+η2cosη3+sinη3)≈2L12V2c2d+2L1Vcd˙+RV2

三、个人对算法的理解

（1）L1轨迹跟踪是非线性的导航算法，在实际的使用中，主要就是要解决L1的取值问题和sin⁡η\sin \etasinη的求解，因为是基于角度正弦的，所以相比于基于偏距的线性算法，在初始偏差比较大的时候，不会出现过激调节，可以比较平滑的向期望路径过渡，而在进入期望路径后，其控制效果较偏距控制更好，尤其是在圆形轨迹跟踪。
（2）L1轨迹跟踪相当于在跟踪一个L1点，这个点的轨迹就是期望路径，点按一定频率刷新，按一定频率计算角度，刷新侧向加速度输出。
（3）对sin⁡η\sin \etasinη进行小角度线性化，可以得到在期望路径附近的加速度线性化的形式，以跟踪直线为例，实际上就是一个二阶系统，ascmd≈d¨{{a}_{{{s}_{cmd}}}}\approx \ddot{d}ascmd≈d¨，
d¨+2ζωnd˙+ωn2d=0ζ=0.707ωn=2VL1\ddot{d}+2\zeta {{\omega }_{n}}\dot{d}+{{\omega }_{n}}^{2}d=0 \\ \zeta =0.707 \\ {{\omega }_{n}}=\frac{\sqrt{2}V}{{{L}_{1}}}d¨+2ζωnd˙+ωn2d=0ζ=0.707ωn=L12V
这个二阶系统固有频率和速度以及L1长度有关，阻尼实际上是与那个增益就是那个2倍有关，在论文中，L1取了固定值150m，而V也只测试了25m/s的速度。对于圆的跟踪和直线差不多，只是固有频率也与盘旋半径有关。

四、开源飞控中对算法的改进

L1 Control for Straight and Curved Path Following
（1）改进了L1航点更新，在原论文中L1是定值，改进后L1长度是动态计算的，可以指定一个恒定的跟踪回路周期，并使导航回路增益可以自动调整以适应速度的变化，并增加了两个参数。在ArduPilot中给出的L1更新公式为：L1_dist = 1/π * damping * period * speed ，这样固有频率就变成了1/π/period。
（2）改进了阻尼，在原论文中，近似的二阶系统阻尼是固定的，在ArduPilot中通过修改了加速度的增益，latAccDem = 4 * damping² * speed² * sin(Nu) / L1_dist ，阻尼为1/damping。
关于ArduPilot源码的解读参考了以下两篇博客：
https://blog.csdn.net/u012814946/article/details/79817719
https://blog.csdn.net/u012814946/article/details/81451769

【导航算法】无人机路径跟踪L1导航算法相关推荐

c语言a 寻路算法,JS/HTML5游戏常用算法之路径搜索算法 A*寻路算法完整实例
本文实例讲述了JS/HTML5游戏常用算法之路径搜索算法 A*寻路算法.分享给大家供大家参考,具体如下: 完整实例代码如下: A*寻路算法 #stage { border: 1px solid lig ...
基于人工鱼群算法的路径优化，鱼群算法的原理
目录摘要参数优化的方法鱼群算法的原理基于人工鱼群算法的参数优化基本参数代码效果图结果分析展望摘要详细介绍了人工鱼群算法的原理,并MATLAB编程实现基于人工鱼群算法的参数优化,对 ...
使用HGS算法调整PD控制器增益的无人机动态性能数据——基于启发式的无人机路径跟踪优化（Matlab代码实现）
【计算机科学】【2013.08】无人机路径规划问题的算法研究
本文为美国德克萨斯农工大学(作者:JIANGLEI QIN)的硕士论文,共44页. 在过去的十年里,无人驾驶车辆(UVs)在军事和民用领域得到了广泛的应用.无人车路径规划在有效利用现有资源(如无人车和 ...
合肥市出行地铁路径规划——基于Dijkstra算法
合肥市出行地铁路径规划--基于Dijkstra算法 1. 引言 2. 导入相应的模块 3. 申请高德地图的API 4. 获取合肥地铁数据 5. 计算合肥各地铁站点之间的距离 6.寻找最近的地铁站 7. ...
【轨迹跟踪】无人机轨迹跟踪【含Matlab源码 1152期】
⛄一.L1导航算法简介 L1导航算法是非常经典的非线性无人机路径跟随算法,最早由MIT于2004年提出,其导航算法中是先选点,生成一段为L1的路径. 1 直线路径跟踪 L1路径跟随算法的基本思想就是在 ...
基于Carsim和simulink联合仿真的路径跟踪测试
Hi 今天和大家分享一下利用carsim和simulink联合仿真的路径跟踪测试,算法采用基于预瞄点的PID控制算法. Carsim中Home界面如下: 一,Vehicle 首先是车辆模型,这里采用默 ...
地图导航中的路径规划算法（综述）
综述介绍+本人研究与实践结果--实际是论文相关的路径规划算法,静态路网中,通常使用双向A*算法常见的其他搜索算法,IA*(迭代A*),内存限定A*,分层路网A*(将高速路网构建成一个新的路网), ...
路径跟踪算法之PID路径跟踪与PP跟踪
路径跟踪算法之PID路径跟踪与PP跟踪 1路径跟踪原理 2 常见的路径跟踪算法 2.1 Pure puresuit(pp)纯跟踪 2.2 PID 跟踪 1路径跟踪原理在运用好的路径规划算法,规划好一 ...
dijkstra算法matlab程序_编程习题课 | 用最短路算法为你的小地图导航
简介:路网拓扑的正确导入方式,运筹学算法的完整实战案例,最详细的代码讲解与分享. 引言:在研究路径选择和流量分配等交通问题时,常常会用到最短路算法.用最短路算法解决交通问题存在两个难点:一.算法的选择 ...

【导航算法】无人机路径跟踪L1导航算法

目录

一、直线路径跟踪

二、圆弧路径跟踪

三、个人对算法的理解

四、开源飞控中对算法的改进

【导航算法】无人机路径跟踪L1导航算法相关推荐

最新文章

热门文章