本帖最后由 空中飞鹅 于 2018-2-2 19:04 编辑
2018年1月30日修改。 把最开始放出来的代码和伪码去掉了。 直接给出一个GPS飞控的工作逻辑框架。 为方便网站审核，尽量使用中文而避免使用非中文。因为非中文一多，帖子发了一天也未必审核通过。全是中文的话，几秒钟就审核完了。 —————————————————————————————————————————————————————————— 一、通讯逻辑
为简单起见，暂时不考虑地面站（PC 端或移动端），而只采用遥控器控制。地面站功能跟遥控器工功能有很多重叠的地方，而不重叠的地方对飞控主体程序影响不大。为了简单起见，很多地方的细节都暂时忽略，而主要针对躯干部分进行描述。
遥控器有8个通道，其中摇杆占四个通道，多段开关占两个通道，另外有两个按键通道。
摇杆通道定义为美国手模式，即左手油门。注：按照“左西右东”的原则，日本为东，美国为西，很容易记得美国手和日本手的区别。
通道1-通道4分别为：横滚、俯仰、油门和航向。左手摇杆左右方向为航向通道，打到最左端对应1000us，落在中间为1500us，打到最右端对应2000us。左手摇杆上下方向为油门通道，打到最下段为1000us，中间为1500us，最上端为2000us。右手摇杆左右方向为横滚通道，打到最左端为1000us，中间为1500us，最右端为2000us。右手摇杆上下方向为俯仰通道，打到最上端为1000us，中间为1500us，最下段为2000us。注：俯仰通道和油门通道不一样，位时映射关系正好相反。
有头状态下，并且在摇杆控制有效的情况下：航向通道打到左边，机头逆时针转动，航向通道打到右边，机头顺时针转动；油门通道打到上边，飞机往上走，油门通道打到下边，飞机往下走；俯仰通道打到上边，飞机往前走，俯仰通道打到下边，飞机往后走；横滚通道打到左边，飞机往左走，横滚通道打到右边，飞机往右走。
遥控器左上角三段开关定义为通道5，右上角三段开关定义为通道6。
假定飞控支持手动、增稳、定高、自动起飞/自动降落，无头/有头、悬停、返航、绕点和跟随。通道5和通道6搭配起来实现：手动、增稳、定高、悬停、绕点、跟随和返航。通道7实现自动起飞/自动降落，通道8实现无头/有头。
通道5在上段，通道6在上段，为手动模式；通道5在上段，通道6在中段，为增稳模式，通道5在上段，通道6在下段，为定高模式。通道5在中段，通道6在上段，为悬停模式，通道5在中段，通道6在中段，为绕点模式，通道5在下段，通道6在下段，为跟随模式。通道5在下段，为返航模式。
两个按键开关定义为通道7-8。
飞机尚未起飞的时候，长按遥控器中间偏左的通道7按键，飞机为自动起飞状态，飞机已经在空中并非是自动起飞状态，这时候按下通道7按键，飞机为自动降落状态。飞机上电后，飞机默认为有头状态，上电后长按遥控器中间偏右的通道8按键，飞机为无头状态，再长按返回有头状态。
二、控制逻辑
一共有12个控制环。其中位置3个，速度3个，角度3个，角速度3个。
GPS定位之后，位置环和速度环参与运作。GPS丢失或未定位，位置环和速度环失效。
假定飞机的姿态和位置都已经算好了，控制部分代码可以分为四个大函数：水平控制函数、高度控制函数、航向控制函数和 电机控制函数。
1 、水平控制函数又分速度计算、角度计算、舵机输出计算三大块。
速度计算部分，分为两段：有GSP和无GPS。
1)无GPS时，速度环失效，速度环PID参数清零。
2)有GPS时，又分速度打杆、速度刹车和位置锁定三种情况。
设横滚俯仰死区都是50。为了严格区分三种情况，需要设定一个位置锁定标志。
情况一（速度打杆）：横滚杆量（或俯仰杆量）大于1450，或小于1550。位置锁定标志为0。
此时杆量直接对应机体坐标系的目标速度。当杆量小于1450时，目标速度为10米每秒乘以（杆量-1450）再除以（1450-1000）。当杆量大于1550时，目标速度为10米每秒乘以（杆量-1550）再除以（2000-1550）。
机体坐标系目标速度经矢量分解，变到NED坐标系，得到东向目标速度和北向目标速度。
情况二（速度刹车）：横滚杆量（或俯仰杆量）在1450与1550之间。位置锁定标志为零。
此时直接令目标速度为0。飞机进入刹车状态。
当飞机实际速度足够小（在正负0.1米之间），把位置锁定标志设为1，把当前位置设为目标位置。
情况三（位置锁定）：横滚杆量（或俯仰杆量）在1450与1550之间。位置锁定标志为1。
A)当飞机处于悬停模式，由目标位置和当前位置做PID计算，得到NED坐标系的目标速度。
B)当飞机处于返航模式。切入返航瞬间，把目标位置设为当前位置。先进入返航三状态中的原地爬高状态。
①先判断飞机当前位置与回家点之间的距离。如果距离超过20米，把目标高度设为20米，等飞机高度到达20米后才进入原高返航状态。如果距小于20米，直接进入原高返航状态。
在原高爬高状态，由目标位置和当前位置做PID计算，得到北向目标速度和东向目标速度。由目标高度得到垂直目标速度。
②在原高返航状态，目标位置改为回家点。同样由位置环得到NED目标速度。
当飞机与回家点之间的距离小于1米时，进入自动降落状态。
③在自动降落状态，目标位置还是回家点，目标高度为零。由位置环得到NED目标速度。由高度环得到垂直目标速度。
C) 当飞机处于绕点模式时，刚切入时目标位置就是当前位置。当右边摇杆打杆时，临时执行打杆控制操作，松杆刹车后飞机停住。这个位置到之前的目标位置之间的距离，如果小于3米，就认为绕点无效，飞机原地悬停。如果距离大于3米，则飞机之后的目标位置，就是以刚切入时的目标位置为圆心，以距离为半径的圆。利用圆的方程，以固定的角速度改变从圆心出发到飞机的方位角，就能让飞机沿着圆形轨迹飞行。飞行过程中，目标航向角就是从飞机出发到圆心的方位角。依然由目标位置和当前位置得到NED目标速度。
D) 当飞机处于跟随模式时，刚切入时计算一个飞机与人之间的距离（人的位置由遥控器上的GPS给出），然后，当人的位置发生改变时，把人的位置改变量加到飞机的位置坐标上去，目标位置就出来了。目标位置有了，当前位置有了，NED目标速度也就有了。航向角控制上，让目标航向角等于从飞机到人的方位角。
角度计算部分。
还是分为有GPS和无GPS两种情况。
有GPS的时候，目标速度与当前速度做PID计算，可以得到北向的目标倾角和东向的目标倾角。经过矢量分解，得到机体坐标系的目标倾角，即目标横滚角和目标俯仰角。
无GPS情况下。
如果是手动模式，目标角速度由杆量给出。映射关系与杆量到速度的非常类似。
如果是手动模式，目标角度由杆量给出。映射关系与杆量到速度的非常类似。
舵机输出计算部分。
经过角度角速度串级PID（或单角速度环），可以得到横滚俯仰两个通道的舵机输出。
2、高度控制
摇杆设死区。死区操作如前。
如果飞机没有起飞，自动起飞/自动降落按键有值，就给一个目标高度，经过高度环和垂直速度环的串级PID计算，得到油门通道的舵机输出。
如果飞机已经在空中，自动起飞/自动降落按键有值，同样操作。
如果飞机已经在空中，并且工作在手动模式、增稳模式，对高度环、垂直速度环PID参数清零，其他情况，杆量对应于高度增量。经高度环和垂直速度环，可以得到舵机输出。
3、航向控制
摇杆设死区。死区操作如前。
杆量对应角度增量。经角度环和角速度环，可得航向舵机输出。
4、电机控制函数
把电机状态分为关闭和打开两种状态。
当飞机没有起飞，并且自动起飞按钮未按下，飞机掰杆解锁（外八或内八）。
飞机解锁后，由四个通道的舵机输出（通道控制量）进行动力分配，得到四个电机对应的控制量。
这里给出动力分配矩阵的数学推导过程。
设机身右上角的电机为电机1（逆时针转），右下角的电机为电机2（顺时针转），左下角的电机为电机3（逆时针转），左上角的电机为电机4（顺时针转）。
当俯仰角增加时，电机1和电机4转速加大，电机2和电机3转速减小。
当横滚角增加时，电机1和电机2转速减小，电机3和电机4转速增大。
当高度增加时，四个电机同时增加。
当航向角增加时，电机1和电机3转速增大，电机2和电机4转速减小。
得：俯仰控制量=电机1控制量-电机2控制量-电机3控制量+电机4控制量
横滚控制量=-电机1控制量-电机2控制量+电机3控制量+电机4控制量
油门控制量=电机1控制量+电机2控制量+电机3控制量+电机4控制量
航向控制量=电机1控制量-电机2控制量+电机3控制量-电机4控制量
写成矩阵形式，求逆矩阵，可得：
电机1控制量=油门控制量-横滚控制量+俯仰控制量+航向控制量
电机2控制量=油门控制量-横滚控制量-俯仰控制量-航向控制量
电机3控制量=油门控制量+横滚控制量-俯仰控制量+航向控制量
电机4控制量=油门控制量+横滚控制量+俯仰控制量-航向控制量
测量部分后面展开。此处从略。
至此，一个GPS飞控的简单框架完全确立。
| 
[复制链接]
/1 
发表于 2018-1-25 15:52
收藏
楼主
发表于 2018-7-30 12:01
