[发明专利]可编程飞行器姿态控制IP核

摘要

本发明公开了一种可编程飞行器姿态控制IP核，所述IP核包括：第一片上外设；第一DMA通道；用于对三轴加速度、三轴角速度和三轴磁场数据分别进行滤波处理的滤波运算器；连接滤波运算器，用于根据滤波后的三轴加速度、三轴角速度和三轴磁场数据进行姿态融合计算输出飞行器的欧拉角的姿态融合计算簇；连接姿态融合计算簇的PID运算簇；第二DMA通道；第二片上外设；所述第二片上外设将第二DMA通道传输过来的欧拉角传输至飞行器；本发明采用了硬件方式对飞行器的姿态进行了相应的控制，专用电路的使用使得控制系统的反应延迟相比单片机或CPU控制大幅降低，将原来处于姿态控制的单片机或CPU从算法中解放出来。

主权项

一种可编程飞行器姿态控制IP核，其特征在于，所述IP核包括：第一片上外设；所述第一片上外设用于接收飞行器的三轴加速度、三轴角速度和三轴磁场数据；连接第一片上外设和滤波运算器，用于将三轴加速度、三轴角速度和三轴磁场数据传输至滤波运算器的第一DMA通道；用于对三轴加速度、三轴角速度和三轴磁场数据分别进行滤波处理的滤波运算器；所述滤波运算器包括9个滤波运算单元；每一滤波运算单元分别对应三轴加速度、三轴角速度和三轴磁场数据中的一种数据；各滤波运算单元均包括FIR滤波器、IIR数字滤波器和滑动窗口滤波器中的至少一种；连接滤波运算器，用于根据滤波后的三轴加速度、三轴角速度和三轴磁场数据进行姿态融合计算输出飞行器的欧拉角的姿态融合计算簇；连接姿态融合计算簇，用于对姿态融合计算簇输出的欧拉角进行PID修正的PID运算簇；连接PID运算簇，用于将PID修正后的欧拉角通过第二片上外设传输至飞行器的第二DMA通道；第二片上外设；所述第二片上外设将第二DMA通道传输过来的欧拉角传输至飞行器；所述姿态融合计算簇包括：连接滤波运算器，用于输入滤波后的三轴加速度、三轴角速度和三轴磁场数据，并对其中的三轴加速度和三轴磁场数据进行单位化的输入数据处理簇；连接四元数积分簇，用于针对四元数积分簇输出的四元数q0,q1,q2,q3，得到四元数中间流q0*q0、q0*q1、q0*q2、q0*q3、q1*q1、q1*q2、q1*q3、q2*q2、q2*q3、q3*q3的中间流计算簇；连接输入数据处理簇和中间流计算簇，用于对输入数据处理簇输出的三轴磁场数据由机体坐标系转换为地面坐标系，以及根据输入数据处理簇输出的三轴加速度、三轴角速度和三轴磁场数据，结合中间流计算簇输出的四元数中间流得出四元数误差量的坐标转换计算簇；所述坐标转换计算簇利用公式$\left\{\begin{array}{c}{b}_x=\sqrt{[\left({h_x}^2\right)+\left({h_y}^2\right)]}\\ b_y=0\\ b_z=h_z\end{array}\right.$计算出由机体坐标系转换为地面坐标系的三轴磁场数据(b_x,b_y,b_z)，其中$\left\{\begin{array}{c}{h}_x=2*m_x*(0.5-q2*q2-q3*q3)+2*m_y*(q1*q2-q0*q3)+2*m_z*(q1*q3+q0*q2)\\ h_y=2*m_x*(q1*q2+q0*q3)+2*m_y*(0.5-q1*q1-q3*q3)+2*m_z*(q2*q3-q0*q1)\\ h_z=2*m_x*(q1*q3-q0*q2)+2*m_y*(q2*q3+q0*q1)+2*m_z*(0.5-q1*q1-q2*q2)\end{array}\right.$、(m_x,m_y,m_z)为输入数据处理簇输出的三轴加速度数据；所述坐标转换计算簇利用公式$\left\{\begin{array}{c}{e}_x=(a_y*v_z-a_z*v_y)+(m_y*w_z-m_z*w_y)\\ e_y=(a_z*v_x-a_x*v_z)+(m_z*w_x-m_x*w_z)\\ e_z=(a_x*v_y-a_y*v_x)+(m_x*w_y-m_y*w_x)\end{array}\right.$计算出四元数误差量(e_x,e_y,e_z)，其中(a_x,a_y,a_z)为输入数据处理簇输出的三轴加速度数据、$\left\{\begin{array}{c}{v}_x=2*(q1*q3-q0*q2)\\ v_y=2*(q0*q1+q2*q3)\\ v_z=q0*q0-q1*q1-q2*q2-q3*q3\end{array}\right.,$$\left\{\begin{array}{c}{w}_x=2*b_x*(0.5-q2*q2-q3*q3)+2*b_z*(q1*q3-q0*q2)\\ w_y=2*b_x*(q1*q2-q0*q3)+2*b_z*(q0*q1+q2*q3)\\ w_z=2*b_x*(q0*q2+q1*q3)+2*b_z*(0.5-q1*q1-q2*q2)\end{array}\right.;$连接坐标转换计算簇，用于利用四元数误差量对三轴角速度数据执行比例积分修正的PI修正簇；所述PI修正簇利用公式$\left\{\begin{array}{c}{u}_x=g_x+K_p*e_x+{e_x}^{\′}\mathrm{Int}\\ u_y=g_y+K_p*e_y+{e_y}^{\′}\mathrm{Int}\\ u_z=g_z+K_p*e_z+{e_z}^{\′}\mathrm{Int}\end{array}\right.$得到PI修正后的三轴角速度数据(u_x,u_y,u_z)，其中(g_x,g_y,g_z)为输入数据处理簇输出的三轴角速度数据、K_p为设定的比例系数、(e_x,e_y,e_z)为四元数误差量、$\left\{\begin{array}{c}{e_x}^{\′}\mathrm{Int}=e_x\mathrm{Int}+e_x*K_i\\ {e_y}^{\′}\mathrm{Int}=e_y\mathrm{Int}+e_y*K_i\\ {e_z}^{\′}\mathrm{Int}=e_z\mathrm{Int}+e_z*K_i\end{array}\right.,$(e_xInt,e_yInt,e_zInt)为对上一积分周期中的(e_x,e_y,e_z)的各自积分、K_p为设定的积分系数；连接PI修正簇，用于根据修正后的三轴角速度数据和上一积分周期的四元数生成当前积分周期四元数的四元数积分计算簇；所述四元数积分计算簇利用公式$\left\{\begin{array}{c}{q0}^{\′}={q0}_b+(-{q1}_b*u_x-{q2}_b*u_y-{q3}_b*u_z)*\mathrm{halfT}\\ {q1}^{\′}={q1}_b+({q0}_b*u_x+{q2}_b*u_z-{q3}_b*u_y)*\mathrm{halfT}\\ {q2}^{\′}={q2}_b+({q0}_b*u_y-{q1}_b*u_z+{q3}_b*u_x)*\mathrm{halfT}\\ {q3}^{\′}={q3}_b+({q0}_b*u_z-{q1}_b*u_y-{q2}_b*u_x)*\mathrm{halfT}\end{array}\right.$得出当前积分周期的四元数q0′,q1′,q2′,q3′，其中q0_b,q1_b,q2_b,q3_b为上一积分周期的四元数、(u_x,u_y,u_z)为PI修正后的三轴角速度数据、halfT为设定参数；在生成当前积分周期四元数q0′,q1′,q2′,q3′后所述四元数积分计算簇还用于对q0′,q1′,q2′,q3′进行单位化并得到q0,q1,q2,q3输出给中间流计算簇和快速转换簇；连接四元数积分簇，用于根据四元数积分簇输出的q0,q1,q2,q3得到欧拉角的快速转换簇；所述快速转换簇利用公式$\left\{\begin{array}{c}\tan \left(\mathrm{Poll}\right)=\frac{(q1*q2+q3*q0)}{(q3*q3-q0*q0-q1*q1+q2*q2)}\\ \sin \left(\mathrm{Pitch}\right)=-2*(q0*q2-q3*q1)\\ \tan \left(\mathrm{Yaw}\right)=\frac{2*(q0*q1+q3*q2)}{(q3*q3+q0*q0-q1*q1-q2*q2)}\end{array}\right.$计算出滚转角Roll的正切三角函数值tan(Roll)、俯仰角Pitch的正弦三角函数值sin(Pitch)、偏航角Yaw的正切三角函数值tan(Yaw)；所述快速转换簇还包括预先存储各角度与对应各角度的正切三角函数值和正弦三角函数值的查找表；所述快速转换簇根据正切三角函数值tan(Roll)、正弦三角函数值sin(Pitch)和正切三角函数值tan(Yaw)，结合查找表确定滚转角Roll、俯仰角Pitch和偏航角Yaw。