【摘要】 本发明公开了一种飞行载体姿态确定方法，属姿态测量领域。主要步骤包括：建立飞行载体的坐标系及姿态矩阵；采集传感器信号；利用陀螺测量值进行捷联姿态解算，得到飞行载体的姿态四元数及姿态角；利用磁强计测量值解算飞行载体航向角；利用速度信息对加速度测量值补偿，求解俯仰角和横滚角；求解由磁强计及加速度计信息确定的姿态四元数；设计卡尔曼滤波器，估计状态向量；校正姿态四元数及提取姿态角。本方法将陀螺、加速度计、磁强计、速度传感器组合，满足飞行载体实时性姿态控制的要求；将陀螺漂移引入到系统状态向量中，对其实时估计校正，同时采用磁强计测量值直接计算航向角，避免地磁场矢量在地理坐标系下的计算，提高了姿态确定的精度。 【专利类型】发明申请 【申请人】西北工业大学 【申请人类型】学校 【申请人地址】710072 陕西省西安市友谊西路127号 【申请人地区】中国 【申请人城市】西安市 【申请人区县】碑林区 【申请号】CN200810076417.0 【申请日】2008-08-18 【申请年份】2008 【公开公告号】CN106342284B 【公开公告日】2011-11-23 【公开公告年份】2011 【授权公告号】CN106342284B 【授权公告日】2011-11-23 【授权公告年份】2011.0 【IPC分类号】G05D1/08 【发明人】姜澄宇; 薛亮; 苑伟政; 常洪龙; 秦伟; 袁广民 【主权项内容】1.一种飞行载体姿态确定方法，其特征在于包括下述步骤： 步骤一：建立飞行载体的坐标系及姿态矩阵：机体坐标系b系中Xb，Yb，Zb三个正交轴分别沿飞行载体的横轴向右、纵轴向前、竖轴向上；地理坐标系n系，Xn，Yn，Zn三个正交轴分别指向东、北、天；姿态角定义为：航向角ψ为(-180°，180°)，俯仰角θ为(-90°，90°)，横滚角γ为(-180°，180°)；按照航向-俯仰-横滚的旋转顺序，得到飞行载体的姿态角形式的姿态矩阵 为： 其中s和c分别为函数sin和cos的简写，定义飞行载体姿态四元数Q＝[q0 q1 q2 q3]T，得到四元数形式的姿态矩阵 为： 步骤二：采集传感器信号：利用陀螺、加速度计、单轴速度传感器分别测量飞行载体的角速度、加速度及速度，经过A/D转换得到角速度ωb＝[ωbx ωby ωbz]T和加速度fb＝[fbx fby fbz]T及速度vy，利用磁强计得到地磁场矢量mb＝[mbx mby mbz]T； 步骤三：利用角速度ωb，采用捷联姿态求解算法，求取利用角速度ωb解算的飞行载体姿态四元数QI和俯仰角θI及横滚角γI； 步骤四：利用地磁场矢量mb求解飞行载体的航向角ψS：利用步骤三获得的俯仰角θI和横滚角γI建立机体坐标系b系和水平坐标系h系之间的转换矩阵，将地磁场矢量mb转换到水平坐标系h系中，解算出飞行载体的磁航向角，经过磁偏角修正得到航向角ψS； 步骤五：利用速度vy对加速度fb补偿，得到重力矢量在机体坐标系b系中的投影值，利用重力矢量在地理坐标系n系和机体坐标系b系之间的转换关系，解算出飞行载体的俯仰角θS和横滚角γS； 对加速度fb利用速度vy补偿如下： 其中f′bx和f′by分别为经过由飞行载体的变速运动及转动引起的线加速度和向心加速度补偿后的加速度值， 为沿Zb轴的陀螺经过陀螺漂移补偿后的角速度，vy为沿飞行载体机体坐标系Yb轴放置的单轴速度传感器测量值，则求取横滚角γS和俯仰角θS为： 步骤六：对于航向角ψS、俯仰角θS及横滚角γS，利用姿态四元数与姿态角之间的关系，求取由地磁场矢量mb和加速度fb解算的飞行载体姿态四元数QS； 步骤七：采用卡尔曼滤波技术，设计卡尔曼滤波器，对利用角速度ωb解算的飞行载体姿态四元数QI、利用地磁场矢量mb和加速度fb解算的飞行载体姿态四元数QS、陀螺测量值ωb，进行卡尔曼滤波的数据处理，估计出利用角速度ωb解算的飞行载体姿态四元数QI对应的姿态误差四元数 和陀螺漂移误差 包括如下子步骤： ①建立系统卡尔曼滤波状态方程 陀螺测量模型建立为：ωb＝ω+b+wg，其中ωb为陀螺输出值，ω为真实角速率，wg为陀螺漂移白噪声，b为陀螺漂移，满足方程 为陀螺漂移随机游走白噪声； 定义利用角速度ωb解算的飞行载体姿态四元数QI对应的姿态误差四元数为 其中 真实的飞行载体姿态四元数为Q，则Q可以表示为： 对(5)式两边求微分，得： 则： 其中 为陀螺漂移估计误差，b为陀螺真实漂移， 为陀螺漂移估计值， 为 的斜对称矩阵， 为角速度ωb经过陀螺漂移补偿后的值，建立系统卡尔曼滤波状态方程为： 其中X(t)为t时刻的系统状态向量，F(t)和G(t)为t时刻系统状态转移矩阵及噪声矩阵，W(t)为系统噪声向量； 系统状态向量：X(t)＝[qe1 qe2 qe3 Δbx Δby Δbz]T 系统噪声向量：W(t)＝[wgx wgy wgz wbx wby wbz]T 其中qe1、qe2、qe3为姿态误差四元数QIe的三个分量，Δbx、Δby、Δbz为陀螺漂移估计误差，wgx、wgy、wgz为陀螺漂移白噪声，wbx、wby、wbz为陀螺漂移随机游走白噪声，系统状态转移矩阵F(t)和噪声矩阵G(t)为： ②建立系统卡尔曼滤波量测方程 定义利用地磁场矢量mb和加速度fb解算的飞行载体姿态四元数QS对应的姿态误差四元数为QSe＝[1 qse1 qse2 qse3]T，则： 定义向量 由式(10)得： 忽略二阶项，得到系统卡尔曼滤波量测方程为： 其中量测矩阵H(t)＝[-I3×3 03×3]，V(t)为量测噪声，近似为白噪声； ③系统卡尔曼滤波方程离散化及状态估计 对①和②建立的系统卡尔曼滤波方程进行离散化，得到离散化卡尔曼滤波状态方程和量测方程为： 其中一步转移阵 系统噪声驱动阵 T为离散化周期，系统噪声方差阵Qk和量测噪声方差阵Rk为： 其中σgx，σgy，σgz为陀螺漂移白噪声均方根，σbx，σby，σbz为陀螺漂移随机游走白噪声均方根，δqe1，δqe2，δqe3为飞行载体姿态四元数估计误差，对利用角速度ωb解算的飞行载体姿态四元数QI和地磁场矢量mb及加速度fb解算的飞行载体姿态四元数QS，通过(11)式计算卡尔曼滤波量测值Zk，采用离散型卡尔曼滤波基本方程进行滤波估计，得到利用角速度ωb解算的飞行载体姿态四元数QI对应的姿态误差四元数QIe的估计值 及陀螺漂移估计误差 步骤八：校正利用角速度ωb解算的飞行载体姿态四元数QI及提取飞行载体姿态角，即航向角ψ、俯仰角θ及横滚角γ；利用步骤七中估计出的姿态误差四元数 校正利用角速度ωb解算的飞行载体姿态四元数QI，得到当前时刻飞行载体姿态四元数最优估计值 利用获得的 得到对应的姿态矩阵，利用姿态矩阵提取飞行载体姿态角，即航向角ψ、俯仰角θ及横滚角γ；对经过卡尔曼滤波得到的陀螺漂移估计误差 获得陀螺漂移估计值 采用 校正陀螺测量值，从而提高陀螺测量精度；将 作为下一时刻姿态解算四元数的初始值，返回步骤二采集传感器信号，进行下一时刻的姿态解算，循环计算。 【当前权利人】西北工业大学 【当前专利权人地址】陕西省西安市长安区西北工业大学长安校区 【统一社会信用代码】12100000435231705W 【引证次数】2.0 【被引证次数】39 【他引次数】2.0 【被他引次数】39.0 【家族引证次数】2.0 【家族被引证次数】39

未经允许不得转载：http://www.zhongzhencnc.com/1782459674.html