基于ARM11+CPLD的小型无人机飞行控制器设计 (4) 无人机, 控制器, 传感器, FLASH, 信息

yuyang911220

导航控制中的高度控制主要通过计算无人机的高度误差,进而计算输出给升降舵机的PWM值来控制高度。高度可由MS5540气压传感器测得的高度与GPS模块的高度数据融合计算后获得。
导航控制中的航向控制流程如图6所示。该流程先读入FLASH中的航点数据,再由GPS获取当前位置信息,进而计算航向角、偏航距、偏航角。航向角的计算是在GPS读出的航向字段基础上,加上前后两时刻的位置差进行修正获得。

偏航距和偏航角的计算如图7所示。

设A点为无人机当前位置,B为第n个航点,C为第n+1个航点。图7中,以B为坐标原点,X轴表示经度,Y轴表示纬度构建直角坐标系。在地球表面,任意两点

离即线段AB的值,无人机与终点距离即线段AC的值,航线起点与终点距离即线段BC的值,再根据点到直线距离公式,可求得线段AD即偏航距,根据三角形余弦定理可求出∠ABD, ∠ABD即为偏航角。
无人机飞行过程中,判断是否过航点是读取下一个航点的必要条件。当无人机飞过航线的垂直线EF既为过航点。判断方法是,过C点绘出Y轴平行的线段CG,求线段BC与CG的夹角减线段AC与CG夹角的差值,计算该差值的绝对值,若|a1-a2|>90则判定为过点。
3.4  姿态控制程序设计
姿态控制主要由俯仰姿态、滚转姿态控制构成,以俯仰姿态控制为例,其控制结构如图8所示:

本文设计了PID控制器实现姿态控制,而俯仰角由姿态传感器ADIS16365测量并经姿态计算而得。ADIS16365测量的是机体坐标系下的数值,而要表达机体相对于参考坐标系的姿态,需进行姿态计算。姿态计算以角速度测量值和运载体的初始条件为输入,姿态计算后,输出飞机的姿态信息和比力坐标变换所需的姿态矩阵。在姿态计算过程中,因四元数法在实际求解中需更新的参数少、精度较高的特点,本文采用四元数法求解。