可编程飞行器姿态控制ip核的制作方法
【专利摘要】本发明公开了一种可编程飞行器姿态控制IP核，所述IP核包括：第一片上外设；第一DMA通道；用于对三轴加速度、三轴角速度和三轴磁场数据分别进行滤波处理的滤波运算器；连接滤波运算器，用于根据滤波后的三轴加速度、三轴角速度和三轴磁场数据进行姿态融合计算输出飞行器的欧拉角的姿态融合计算簇；连接姿态融合计算簇的PID运算簇；第二DMA通道；第二片上外设；所述第二片上外设将第二DMA通道传输过来的欧拉角传输至飞行器；本发明采用了硬件方式对飞行器的姿态进行了相应的控制，专用电路的使用使得控制系统的反应延迟相比单片机或CPU控制大幅降低，将原来处于姿态控制的单片机或CPU从算法中解放出来。
【专利说明】可编程飞行器姿态控制IP核
【技术领域】
[0001] 本发明涉及飞行器姿态控制领域，具体为一种可编程飞行器姿态控制IP核。
【背景技术】
[0002] 航空器按照航迹飞行以及航天器在轨道运行时，为了完成所承担的任务，必须具 有一定的姿态，如四旋翼悬停飞行时，需要与基准面保持姿态平行的状态。对于飞行器而 言，当其处于飞行状态时，姿态控制系统需要获得飞行器的各个轴向的偏转角，而后通过滤 波算法将各个传感器数据滤波处理，得到各个传感器的稳定数据，然后还要使用姿态融合 算法对各个传感器进行零漂修正和融合，积分出飞行器的四元数和欧拉角，最后通过PID 算法对飞行器推进系统加以修正；前述各传感器一般包括加速度传感器、角速度传感器和 磁场传感器。
[0003] 现有技术中的姿态控制系统，一般采用通用CPU+通用A/D采样+通用D/A输出的 架构，这种架构设计简单，且通用CPU和现有编程模型相同。采用这种方式意味着通用CPU 需要时刻进行A/D采样的获取、传感器数据的滤波、姿态的融合、PID控制参数的计算、以及 D/A的输出，对于一般嵌入式处理器而言，这个运算量是十分庞大的，处理器往往无法再分 配于其它应用之中，使得一些设计和架构不得不采用两个以上的通用CPU，其中一个用于姿 态控制，另一个用于实现机载应用；同时，对于姿态控制过程，其传感数据具有明显的生产 者-消费者的数据特性，现有的通用CPU针对控制密集型应用，无法做到并行处理这些数据 流，因而整个系统的处理效率大大降低；现有技术中的姿态控制系统，还有一部分采用通用 CPU+硬件PID的设计，这种设计仅能够缓解PID部分的运算压力，对于改善整个姿态控制系 统的作用甚微。
【发明内容】
[0004] 本发明针对以上问题的提出，而研制一种可编程飞行器姿态控制IP核。
[0005] 本发明的技术手段如下：
[0006] -种可编程飞行器姿态控制IP核，所述IP核包括：
[0007] 第一片上外设；所述第一片上外设用于接收飞行器的三轴加速度、三轴角速度和 三轴磁场数据；
[0008] 连接第一片上外设和滤波运算器，用于将三轴加速度、三轴角速度和三轴磁场数 据传输至滤波运算器的第一 DM通道；
[0009] 用于对三轴加速度、三轴角速度和三轴磁场数据分别进行滤波处理的滤波运算 器；所述滤波运算器包括9个滤波运算单元；每一滤波运算单元分别对应三轴加速度、三轴 角速度和三轴磁场数据中的一种数据；各滤波运算单元均包括FIR滤波器、IIR数字滤波器 和滑动窗口滤波器中的至少一种；
[0010] 连接滤波运算器，用于根据滤波后的三轴加速度、三轴角速度和三轴磁场数据进 行姿态融合计算输出飞行器的欧拉角的姿态融合计算簇；
[0011] 连接姿态融合计算簇，用于对姿态融合计算簇输出的欧拉角进行PID修正的PID 运算簇；
[0012] 连接PID运算簇，用于将PID修正后的欧拉角通过第二片上外设传输至飞行器的 第二DM通道；
[0013] 第二片上外设；所述第二片上外设将第二DMA通道传输过来的欧拉角传输至飞行 器；
[0014] 所述姿态融合计算簇包括：
[0015] 连接滤波运算器，用于输入滤波后的三轴加速度、三轴角速度和三轴磁场数据，并 对其中的三轴加速度和三轴磁场数据进行单位化的输入数据处理簇；
[0016] 连接四元数积分簇，用于针对四元数积分簇输出的四元数q〇, ql，q2, q3,得到四元 数中间流 q〇*q〇、q〇*ql、q〇*q2、q0*q3、ql*ql、ql*q2、ql*q3、q2*q2、q2*q3 和 q3*q3 的中间 流计算簇；
[0017] 连接输入数据处理簇和中间流计算簇，用于对输入数据处理簇输出的三轴磁场数 据由机体坐标系转换为地面坐标系，以及根据输入数据处理簇输出的三轴加速度、三轴角 速度和三轴磁场数据，结合中间流计算簇输出的四元数中间流得出四元数误差量的坐标转
【权利要求】
1. 一种可编程飞行器姿态控制IP核，其特征在于，所述IP核包括： 第一片上外设；所述第一片上外设用于接收飞行器的三轴加速度、三轴角速度和三轴 磁场数据； 连接第一片上外设和滤波运算器，用于将三轴加速度、三轴角速度和三轴磁场数据传 输至滤波运算器的第一DMA通道； 用于对三轴加速度、三轴角速度和三轴磁场数据分别进行滤波处理的滤波运算器；所 述滤波运算器包括9个滤波运算单元；每一滤波运算单元分别对应三轴加速度、三轴角速 度和三轴磁场数据中的一种数据；各滤波运算单元均包括FIR滤波器、IIR数字滤波器和滑 动窗口滤波器中的至少一种； 连接滤波运算器，用于根据滤波后的三轴加速度、三轴角速度和三轴磁场数据进行姿 态融合计算输出飞行器的欧拉角的姿态融合计算簇； 连接姿态融合计算簇，用于对姿态融合计算簇输出的欧拉角进行PID修正的PID运算 簇； 连接PID运算簇，用于将PID修正后的欧拉角通过第二片上外设传输至飞行器的第二DMA通道； 第二片上外设；所述第二片上外设将第二DMA通道传输过来的欧拉角传输至飞行器； 所述姿态融合计算簇包括： 连接滤波运算器，用于输入滤波后的三轴加速度、三轴角速度和三轴磁场数据，并对其 中的三轴加速度和三轴磁场数据进行单位化的输入数据处理簇； 连接四元数积分簇，用于针对四元数积分簇输出的四元数q〇,ql，q2,q3,得到四元数中 间流q〇*q〇、q〇*ql、q〇*q2、q0*q3、ql*ql、ql*q2、ql*q3、q2*q2、q2*q3、q3*q3 的中间流计算 簇； 连接输入数据处理簇和中间流计算簇，用于对输入数据处理簇输出的三轴磁场数据由 机体坐标系转换为地面坐标系，以及根据输入数据处理簇输出的三轴加速度、三轴角速度 和三轴磁场数据，结合中间流计算簇输出的四元数中间流得出四元数误差量的坐标转换计 算簇；所述坐标转换计算簇利用公式
计算出由机体坐标系转换为地面坐标系的三轴磁场数据 (bx,by,bz)，其中
、（mx，my，mz)为输入数据处理簇输出的三轴加速度数据；所述坐标转换计算簇利用公式
计算出四元数误差量（ex，ey，ez)，其中 (ax,ay,az)为输入数据处理簇输出的三轴加速度数据、
连接坐标转换计算簇，用于利用四元数误差量对三轴角速度数据执行比例积分修正的 PI修正簇；所述PI修正簇利用公式
得到PI修正后的三轴角速 度数据（11!￡，1!7，1〇，其中（仏，&，仏）为输入数据处理簇输出的三轴角速度数据、1( 1)为设定的 比例系数、（ex,ey,ez)为四元数误差量、
、（exInt,eyInt,ezInt)为 对上一积分周期中的（ex，ey，ez)的各自积分、Kp为设定的积分系数； 连接PI修正簇，用于根据修正后的三轴角速度数据和上一积分周期的四元数 生成当前积分周期四元数的四元数积分计算簇；所述四元数积分计算簇利用公式
得出当前积分周期的四元数 q(T，qP，q2 ^ ，q3 \ 其中qOb,qlb,q2b,q3b 为上一积分周期的四元数、（ux,uy,uz) 为PI修正后的三轴角速度数据、halfT为设定参数；在生成当前积分周期四元数 qO'，ql'，q2'，q3'后所述四元数积分计算簇还用于对qO'，ql'，q2'，q3'进行单 位化并得到q〇,ql,q2,q3输出给中间流计算簇和快速转换簇； 连接四元数积分簇，用于根据四元数积分簇输出的q〇,ql，q2,q3得到欧拉角的快速转 换簇；所述快速转换簇利用公式
计算出滚 转角Roll的正切三角函数值tan(Roll)、俯仰角Pitch的正弦三角函数值sin(Pitch)、偏 航角Yaw的正切三角函数值tan(Yaw);所述快速转换簇还包括预先存储各角度与对应各 角度的正切三角函数值和正弦三角函数值的查找表；所述快速转换簇根据正切三角函数值 tan(Roll)、正弦三角函数值sin(Pitch)和正切三角函数值tan(Yaw)，结合查找表确定滚 转角Roll、俯仰角Pitch和偏航角Yaw。
2. 根据权利要求1所述的一种可编程飞行器姿态控制IP核，其特征在于所述IP核还 包括快速互联总线；所述快速互联总线用于连接第一DMA通道和滤波运算器，以及连接姿 态融合计算簇和PID运算簇。
3. 根据权利要求1所述的一种可编程飞行器姿态控制IP核，其特征在于所述滑动窗口 滤波器包括具有8个存储单元的第一FIFO存储器、加法器单元、加法选择器、以及桶形移位 寄存器；所述加法器单元用于计算出第一FIFO存储器的前2个存储单元存放的数据之和、 前4个存储单元存放的数据之和、以及8个存储单元存放的数据之和；所述加法选择器用于 对上述三个数据之和进行选择并输出至桶形移位寄存器。
4. 根据权利要求3所述的一种可编程飞行器姿态控制IP核，其特征在于所述第一DMA 通道和第二DMA通道均具有9个输入通道、输入端连接9个输入通道的输入选择器、连接输 入选择器输出端的第二FIFO存储器、连接第二FIFO存储器的输出选择器、以及连接输出选 择器的输出通道。
5. 根据权利要求4所述的一种可编程飞行器姿态控制IP核，其特征在于所述快速互联 总线包括多个输入总线和多个输出总线；通过控制各输入总线和输出总线的通断实现数据 的选择输入或输出。
6. 根据权利要求5所述的一种可编程飞行器姿态控制IP核，其特征在于IP核还包括 连接第一DMA通道、快速互联总线、姿态融合计算簇、PID运算簇的控制寄存器组；所述控制 寄存器组用于控制所述输入选择器的选择操作，控制各输入总线和输出总线的通断，设定 PI修正簇的比例系数和积分系数，以及设定PID运算簇的比例系数、积分系数和微分系数。
7. 根据权利要求5所述的一种可编程飞行器姿态控制IP核，其特征在于所述IP核还 包括通用快速计算簇，该通用快速计算簇包括连接各输出总线的总线监视器和连接总线监 视器的微处理器； 所述总线监视器包括连接输出总线的一级数据缓冲寄存器、连接一级数据缓冲寄存器 的二级数据缓冲寄存器、连接一级数据缓冲寄存器和接收参考限值的第一比较单元、连接 一级数据缓冲寄存器和二级数据缓冲寄存器的第二比较单元、以及连接第一比较单元和第 二比较单元的第一结果选择器；所述第一比较单元用于将一级数据缓冲寄存器中存放的当 前数据与参考限值进行比较，并在当前数据超过参考限值时输出异常信号；所述第二比较 单元用于将二级数据缓冲寄存器中存放的当前数据的上一数据与一级数据缓冲寄存器中 存放的当前数据进行比较，在当前数据与其上一数据的差值超出一定范围时第二比较单元 输出异常信号；所述第一结果选择器对上述两个异常信号进行选择并发送给中断事件生成 器；所述总线监视器还包括存放备用数据的监视寄存器、连接输出总线和监视寄存器的第 二结果选择器；所述微处理器连接中断事件生成器和所述第二结果选择器，用于当接收到 中断事件生成器发出的中断信号时，控制第二结果选择器输出监视寄存器存放的备用数据 至结果总线上。
8.根据权利要求7所述的一种可编程飞行器姿态控制IP核，其特征在于所述微处理器 采用超长指令集架构，并深度集成于IP核内部。
【文档编号】G06F19/00GK104407617SQ201410804667
【公开日】2015年3月11日 申请日期:2014年12月22日 优先权日:2014年12月22日
【发明者】夏广庆, 赵楠, 吕睿, 李辉, 王晓彤, 吴志刚 申请人:大连理工大学