px4代码解析（一）：航点位置到点判据

前言

今天要给大家介绍的是px4在执行航线任务时是如何进行到点判断的，大部分无人机最终都是按照地面站规划的航线进行自动飞行，地面站中的航线也都是由一个一个航点组成，那无人机在起飞、跟踪航线中的航段或者盘旋飞行时应该如何进行到点判断就成为了我们非常重要的关注点。

PX4中关于航点到点判断的内容主要集中在navigator模块的mission.cpp和mission_block.cpp两个文件中。而对于固定翼无人机、多旋翼无人机来说在到点判据上基本是一致的，但是也有一些差异。本文主要针对固定翼飞行器、多旋翼飞行器在起飞、盘旋、航线跟踪时的到点判据进行具体分析。到点判据又分成水平到点和高度到点两个部分。

在开始分析这些判断逻辑之前，先明确几个参数以及它的含义：

NAV_LOITER_RAD：固定翼飞机盘旋的半径，默认50m；

NAV_ACC_RAD：水平位置到点判据半径，默认10m；

NAV_FW_ALT_RAD：固定翼飞机高度到点判据半径，默认10m；

NAV_FW_ALTL_RAD：固定翼飞机着陆前一点高度到点判据半径，默认5m；

NAV_MC_ALT_RAD：多旋翼飞机高度到点判据半径，默认0.8m；

固定翼飞机高度跟踪异常处理方案

首先对于固定翼飞机来说，到点判据设计了一个比较合理的逻辑，那就是在水平位置即将到点之际，而高度离目标值尚远的情况下，飞机会进入顺时针盘旋跟踪高度期望，直到满足条件后退出。

大概的逻辑就是：

如果D_xy<2* NAV_LOITER_RAD且D_z>2* NAV_FW_ALT_RAD，那么飞行器开始顺时针盘旋跟踪高度。

如果D_xy<1.2* NAV_LOITER_RAD且D_z< NAV_LOITER_RAD，那么飞行器退出盘旋回到航线跟踪。

这里我觉得设计得颇为不合理，根据默认参数来看，2* NAV_FW_ALT_RAD为20m，而NAV_LOITER_RAD为50m，也就是说高度的判断条件反而放宽了，当然实际情况下这种方式肯定会改善高度尚未达到指定值的情况，因为当你切出航线去顺时针盘旋的时候是给了更多的时间去跟踪高度期望的。这个不合理的地方大家可以自己去解决，相信不难。还有就是在规划航线的时候应该根据飞机的实际性能来规划，不要规划过度不协调的航线，超过了飞机的爬升或者下降极限。

具体的代码在mission_block.cpp文件中，这里贴出来给大家看下：

多旋翼飞行器起飞到点判据

多旋翼或者垂直起降固定翼飞机在起飞时的到点判据比较简单，只要高度误差小于设定的参数NAV_MC_ALT_RAD即可，当然在代码里还考虑了起飞高度过小的情况，在实际情况中只要你设置起飞高度时不要小于多旋翼高度阈值参数NAV_MC_ALT_RAD即可。

固定翼飞行器起飞到点判据

固定翼飞机起飞判据跟多旋翼飞机有点不同，除了高度判据以外还有一个水平位置的判据，高度误差小于NAV_FW_ALT_RAD。水平位置误差小于L1控制器中计算的值。

盘旋任务到点判据

关于固定翼飞行器在执行盘旋任务时这里分了三种情况，有限时间盘旋、无限时间盘旋，改变高度盘旋。这三种情况的位置到点判据其实基本都一致：

D_xy<1.2*NAV_LOITER_RAD 且D_z< NAV_FW_ALT_RAD

只不过执行逻辑上有点差别：

有限时间、无限时间盘旋：单纯的判断水平和高度是否在阈值范围内；

改变高度盘旋：如果设定的高度和之前任务点高度不一致，先等飞行器满足D_xy<1.2* NAV_LOITER_RAD 且D_z< NAV_LOITER_RAD条件后再将高度设置为指定高度。

普通航点到点判据

对于一般情况下到点判据会出现三种情况：多旋翼、固定翼、垂直起降固定翼。在这里要注意的是你在地面站上设定的这个盘旋半径具有比较高的优先级，所以没事别乱在航线规划时设置这个航点参数，不过地面站上的也没有显性出现这个参数设置，一般大家也不会去设置它。

在这里，水平位置阈值d_r分成三种情况来计算：

多旋翼：设定的参数NAV_ACC_RAD；

固定翼：L1控制器中计算所得；

垂直起降固定翼：这里指的是固定翼切换旋翼时有个到点切换距离（v*v/(2*a) v*t）,v为当前地速，a为设置的参数VT_B_DEC_MSS，t为参数VT_B_REV_DEL。

而对于高度判据来说，只是固定翼参数NAV_FW_ALT_RAD和多旋翼NAV_MC_ALT_RAD的差别。

水平阈值和高度阈值都满足条件则认为位置已经到点。

总结

本文中贴的代码应该看起来不困难，重点地方已经用红框标注，不过如果你没有接触过这些代码的话可能还是云里雾里，但配合我的解说应该勉强可以看懂。要想进一步理解透彻，可以结合QGC进行仿真测试。读者可以在QGC中设计不同的航线、不同的任务、参数进行调试，观察飞行效果。

航点是否到点除了本篇内容的位置判据外还有航向角判据，实际航向角和期望航向角是否在一定误差范围内。关于航线任务中期望航向角的计算以及航向角误差判据，我们在以后的文章中会继续介绍。

本篇内容主要介绍了px4软件中关于各种机型对于飞行中航点位置是否到点的判断逻辑，这些逻辑不一定就是最好的，但是总体上保证了飞行器在执行自动任务时能够顺利切换航点，完成任务。如有疑问或者更好的方案，欢迎探讨。

-------The End-------