考虑到近期可能要出去实习了，抓紧多整理点干货给大家，目前正在把V2.0版本的部分代码往1,0里面整合，博客一直来不及更新，今天抽空先写一点，针对实现GPS定点的算法流程部分，也是售后群之前一些人理不太清楚的地方，给大家梳理下。首先这里给出无名飞控GPS定点部分的流程图，这里只抠出了位置+速度+姿态控制这部分。
最新的定点测试视频见文末。
完整流程图与飞控代码见个人Github托管，链接：https://github.com/wustyuyi/NamelessCotrunQuad_V1.0
这里对位置+速度+姿态控制流程图里面每一部分进行了标号，下面来足一解释：
1、根据GPS状态：星数+定位质量、Home点是否刷新来决定是否允许飞控进入GPS定点模式。
2、当允许飞控进入GPS定点模式时，判断遥控杆位（俯仰、横滚杆是否回中），来决定是否加入位置控制。
3、当不满足定点条件时，此时水平姿态期望角直接来源于水平遥感给定，即水平方向无位置+速度控制，与自稳模式一致。
4、当允许飞控进入GPS定点模式后，水平遥感杆位不在中位，此时放弃水平位置控制。
此时遥感动作是希望飞机进行水平运动，此时有两种方式：a、水平打杆的行程量对应期望姿态角；b、水平打杆行程量对应期望运动速度。其中a模式与自稳状态下，打杆给定期望姿态角模式一致，打杆时无位置+速度控制，也是APM在PosHold模式下采取的方式，DJI精灵3之前的也都是采取此模式，b种模式为水平控速模式，比如精灵4水平最大运动速度达到20m/s，也就是这么来的。a，b两种模式直观的差异在于，a模式是直接给期望角度，会一直保持姿态角，即给定飞机水平运动的加速度，飞机速度在没有大到水平阻力与水平推力抵消时，飞机是会一直做加速运动的。b模式给定的是期望水平运动速度，控制上是水平速度+姿态控制器，刚开始加速运动时，飞机会有一定姿态角，随着速度增加，姿态角度会逐渐回中，匀速后会维持有一定小角度，去克服水平阻力。精灵3以上的控a，b模式都有，需要用App设置才能开启a模式。
5、水平打杆时，直接给定期望姿态角，与自稳模式下打杆一致。
6、水平打杆行程量，对应期望水平运动速度，进行水平速度+姿态控制。
7、8、水平打杆后，把水平位置期望给0（7），这里目的是用作8回中时刻，有且只刷新一次位置悬停点。
9、满足水平方向杆从动作到回中条件，刷新一次位置悬停点。
10、进行水平位置+速度+姿态控制。
以上为无名飞控的GPS定点实现部分的位置流程图解析，其中6里面，水平遥杆行程量到期望运动速度的映射可以用一个非线性函数来处理，7、8、9部分给定悬停点这里，无名飞控是遥感一回中即赋值，实际可以结合当前运动速度，当前姿态角来决策刹车距离、悬停点实现缓冲的效果，视觉感柔和，避免翻机，比如Phantom 4最大飞行20m/s后，需要接近15m的缓冲距离才能刹停，刹停之后是悬停在刹停点，而不是刹停之后，又回到初始刹车点，这部分代码可以参考APM、Pixhawk。
以上梳理了定点实现流程，下面来对具体定点原理进行讲解:
首先，GPS定点控制有：水平位置+水平速度+姿态控制，这里水平位置+水平速度得到的是期望运动加速度，在多旋翼里面，螺旋桨输出的总推力到运动加速度，线运动模型如下，单个轴来看，运动加速度与该方向的姿态角度的正弦值呈正相关，小角度情况下，姿态倾角即对应运动加速度。故这里不再设计一个水平加速度控制器，直接把期望运动加速度，通过航向分解到俯仰横滚方向姿态期望即可。
其中水平位置、水平速度控制器反馈来源于惯导融合，根据前几篇博客讲解，对于GPS与惯导融合得到的是导航系下，正东（E）、北（N）方向上的运动速度，相对Home点正东（E）、北（N）方向的运动速度。
假设你的机头严格朝着正北（不考虑磁偏角影响），如上图，这个时候的定点控制变得很简单，不需做要航向分解，直接对正东方向做位置控制，正东方向位置控制器输出给正东方向期望运动速度，再进行正东方向速度控制，得到正东方向期望运动加速度，即得到横滚方向Roll姿态期望角（正北同理）。
对于实现GPS定点控制这里有两种殊途同归的实现办法：
1、a、对于定点（不打杆）控制来讲，位置、速度控制都是在正东、正北方向上计算误差，最后得到正东（E）、正北（N）方向上期望运动加速度，然后E、N方向运动加速度分解到载体航向上。b、对于打杆时候速度控制来讲，遥杆给定的是导航系上，载体机头（Pitch）与横滚（Roll）方向上的期望运动速度（并非E、N），但是惯性导航得到的是正东、正北方向的运动速度，因此同样需要把E、N方向上的运动速度，分解到Pitch、Roll方向上来，见代码注释。
这里需要注意的一点，也是很多人容易搞混的是，此时分解到载体航向上的V_Pitch,V_Roll依然是在导航系上，只是所用的直角坐标基底不再是E、N，而是随着航向旋转的一组游动坐标基底。
介绍完毕方式1，大家可能不觉得有什么问题，这里提出最直接的一点，方式1从a到b时，对速度控制器来讲，前者为导航系E、N方向的速度控制，后者为导航系Pitch、Roll方向的速度控制，使用起来不方便，需要设计两个速度控制器，单独只有一个速度控制器的话，需要单独处理下切换时速度控制器的积分项。
2、第二种GPS定点模式：方式1来讲是直接利用惯性导航E、N方向上的位置偏移、速度
去做控制，这是最直观的方式，只需要把速度控制器的输出项——E、N方向的期望运动加速度通过航向分解给期望姿态角即可，问题是其在打杆与不打杆是速度控制器所处的坐标基底不一致，需要特殊处理。受到启发，我们想能不能把位置、速度控制也放在导航系Pitch、Roll方向上去做控制，这样就直接一路串级控制到底就可，这样就和我们初始假设机头朝着正北时一样，不用再中间做航向分解，只用提前把位置期望、速度期望、位置反馈、速度反馈分解到导航系Pitch、Roll方向上即可，显然这样的处理方式就可以避免设计两个速度控制器、单速度控制器切换时积分处理的问题。
相关测试视频：
无名科创开源飞控群：540707961
技术博客在毕业前（2019年6月），会持续更新，欢迎大家一起学习交流哈！！！
无名小哥：余义
2018.03.01