L1 control——ArduPilot——更新航点update_waypoint(一)

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

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

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

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

L1 control——ArduPilot——更新航点update_waypoint(一)_第1张图片

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

 

你可能感兴趣的:(ArduPilot,ArduPilot)