基于ARM11+CPLD的小型无人机飞行控制器设计 (5) 无人机, 控制器, 坐标系

yuyang911220

载体坐标系中的矢量在参考坐标系的表示可通过式(1)计算出:
  (4)


是由其在载体坐标系的对应各轴的角速率分量构成,由姿态传感器ADIS16365测得。本文采用四阶龙格库塔法解(3)式。的求解,构建了姿态矩阵 ,再根据(8),(9),(10)求解俯仰角 、横滚角、偏航角 ,即获得机体的姿态。

基于姿态数据及各轴角速率,PID控制器的输出由(11)式计算[6][7]:

K为采样序号、为第k次控制输出值、为第k次采样输入偏差、为前次采样输入偏差、积分系数、微分系数,为比例系数。