版权声明:本文为博主原创博文,未经允许不得转载,若要转载,请说明出处并给出博文链接
本文详细讲解了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