L1 control——ArduPilot——更新航点update_waypoint（一）

版权声明：本文为博主原创博文，未经允许不得转载，若要转载，请说明出处并给出博文链接 

本文详细讲解了ArduPilot中AP_L1_control库中的update_waypoint函数的实现理念与示意图！！！

L1控制器的update_waypoint方法的最终输出是一个所需的横向加速度（以下以红色显示为“latAccDem”），它应使飞机回到原点origin和目的地destination之间的线路。

使用的公式如下所示。 阻尼damping来自NAVL1_DAMPING参数。 周期period来自NAVL1_PERIOD参数。

alongTrackDist : 飞机当前位置沿平行于线AB到参考目标点的距离

A_air : A点到飞机当前所在位置的距离

L1_dist : 飞机当前位置到参考目标点的距离        

L1_dist = 1/π * damping * period * speed ≈ 0.3183099* damping * period * speed 

Nu1 : vehicle(飞机等)到L1_dist与从A到B的线形成的夹角

Nu2 ：vehicle velocity（飞机等速度）与线AB的夹角

latAccDem ：理想的飞机横向加速度输出

latAccDem = 4 * damping² * speed² * sin(Nu) / L1_dist .     (Nu = Nu1 + Nu2)

d : 航迹距离，即飞机到线AB的距离

 

相关博客可以参看： L1 control——ArduPilot——更新航点update_waypoint（二）

 

本文提到的L1 control参考的是一篇来自AIAA的论文《A New Nonlinear Guidance Logic for Trajectory Tracking》。想进一步学习的链接给出：

原文

https://download.csdn.net/download/u012814946/10334104    

or

中文翻译版本

https://download.csdn.net/download/u012814946/10392973