CN101793523A - 一种组合导航和光电探测一体化系统 - Google Patents

Abstract

一种组合导航和光电探测一体化系统包括惯性测量单元IMU、GPS接收系统、综合计算机、光电稳瞄仪；惯性测量单元内含三个陀螺仪和三个加速度计；综合计算机采集IMU和GPS接收系统的输出信息，实现(1)结合捷联惯导解算算法及卡尔曼滤波算法进行惯性和GPS组合导航解算，得到载体的位置、速度、姿态，同时估计出惯性测量单元所含陀螺仪的漂移；(2)计算视轴稳定指令角速度并控制光电稳瞄仪实现视轴稳定，利用组合导航系统估计得到的陀螺漂移进行视轴稳定误差补偿，提高光电稳瞄仪的视轴稳定精度；(3)利用组合导航解算得到的位置、姿态信息，结合目标的位置信息，计算目标搜索指令角并控制光电稳瞄仪快速搜索并锁定目标。本发明大大降低了系统的体积、重量和成本，同时提高了分系统的性能。

Description

一种组合导航和光电探测一体化系统

技术领域

本发明涉及一种组合导航和光电探测一体化系统，属于惯性导航技术、惯性/GPS组合导航技术和陀螺间接稳定技术领域，所述的组合导航/光电探测一体化系统可应用于无人侦察飞机、战术导弹、地面战车等。

技术背景

导航系统和光电探测系统是制导武器、智能战车、无人侦察飞机等智能运载体不可或缺的分系统。传统的光电探测系统往往采用平台式陀螺稳定方案，不需要借助导航系统的任何信息，其视轴稳定精度较高且技术较为成熟，但体积和重量均较大，不适合对体积和重量要求比较严格的场合。为满足小型战术导弹和微小型无人机对光电探测系统小型化、低成本的要求，间接稳定式光电探测系统越来越成为研究热点。间接稳定技术是指利用载体导航系统的角速率信息，通过坐标变换求得视轴的扰动角速率，并控制视轴转动以补偿载体运动引入的扰动角速率，从而实现视轴的稳定。该技术可大大缩小光电探测系统的体积、重量，并降低成本。自1993年Rutin提出间接稳定技术以来，该技术得到了广泛的研究，我国也开始间接稳定技术的相关研究，但尚未解决视轴稳定精度的问题。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种组合导航和光电探测一体化系统，该系统克服了传统的陀螺稳定光电探测系统体积重量大、成本高、结构复杂等缺点，将导航系统和光电探测系统进行一体化设计，节约了成本，简化了结构，降低了体积、重量，同时将组合导航系统的输出信息应用于光电探测系统，提高了光电探测系统的稳定精度和搜索目标的速度。

本发明的技术解决方案是：一种组合导航和光电探测一体化系统包括：惯性测量单元、GPS接收系统、综合计算机、光电稳瞄仪；综合计算机采集惯性测量单元和GPS接收系统的数据信息，实现三大功能：第一、结合捷联惯导解算算法及卡尔曼滤波算法进行惯性和GPS组合导航解算，得到载体的位置、速度和姿态，同时估计出惯性测量单元所含陀螺仪的漂移；第二、根据组合导航解算得到的载体的位置、速度和姿态，计算视轴稳定指令角速度并控制光电稳瞄仪实现视轴的惯性空间稳定；第三、利用组合导航解算得到的载体的位置和姿态信息，结合目标位置信息，计算目标搜索指令角并控制光电稳瞄仪快速搜索并锁定目标。

所述综合计算机控制光电稳瞄仪实现视轴稳定的具体过程为：

a.综合计算机采集惯性测量单元输出的沿载体坐标系的角速度及比力信息ω_bm、f_bm和GPS接收系统输出纬度

经度λ_gps、东向速度V_gps ^E、北向速度V_gps ^N，并结合捷联惯导解算算法和卡尔曼滤波算法，解算出安装载体的纬度

经度λ、速度V、俯仰角φ_x、横滚角φ_y、航向角φ_z，并且估计出惯性测量单元所含陀螺仪的漂移值ε_b；

b.将惯性测量单元的角速度信息ω_bm进行漂移补偿，得到载体的运动角速度ω_b的表达式为：

${\stackrel{\‾}{\mathrm{\ω}}}_b={\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{bm}}-{\stackrel{\‾}{\mathrm{\ϵ}}}_b$

对载体的运动角速度ω_b进行坐标变换得到光电稳瞄仪视轴扰动角速度ω_los，其表达方式为：

其中C_b ^los为载体坐标系至视轴坐标系的变换矩阵；

c.以ω_los作为光电稳瞄仪的输入指令，控制光电稳瞄仪转动，即实现了视轴的惯性空间稳定。

所述的综合计算机控制光电稳瞄仪快速搜索并锁定目标的具体过程为：

A.目标纬度

和经度λ_T由指挥系统提供，组合导航系统提供载体的纬度

经度λ、俯仰角φ_x、横滚角φ_y、航向角φ_z，利用上述信息计算出目标所在地的地理坐标系相对于安装载体坐标系的转角(φ_cT ^x，φ_cT ^y，φ_cT ^z)^T的表达式为：

B.定义光电稳瞄仪安装基座坐标系相对于载体坐标系的安装偏角为(α_x，α_y，α_z)^T，根据步骤A计算得到的目标地理坐标系相对于载体坐标系的转角(φ_cT ^x，φ_cT ^y，φ_cT ^z)^T，求得目标地理系相对于光电稳瞄仪安装基座坐标系的转角(φ_oT ^x，φ_oT ^y，φ_oT ^z)^T的表达式为：

$\left(\begin{array}{c}{\mathrm{\φ}}_{\mathrm{oT}}^x\\ {\mathrm{\φ}}_{\mathrm{oT}}^y\\ {\mathrm{\φ}}_{\mathrm{oT}}^z\end{array}\right)=\left(\begin{array}{c}{\sin }^{-1}(-T_{23})\\ {\mathrm{tg}}^{-1}(-T_{13}/T_{33})\\ {\mathrm{tg}}^{-1}(-T_{12}/T_{22})\end{array}\right)$

其中， $\left(\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right)=R\left(\stackrel{\‾}{\mathrm{\α}}\right)R\left({\stackrel{\‾}{\mathrm{\φ}}}_{\mathrm{cT}}\right)$

R(φ_cT)为目标地理系至载体坐标系的旋转矩阵，R(α)为载体坐标系至光电稳瞄仪安装基座坐标系的旋转矩阵，可分别表示为：

$R\left({\stackrel{\‾}{\mathrm{\φ}}}_{\mathrm{cT}}\right)=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_{\mathrm{cT}}^y& 0& -\sin {\mathrm{\φ}}_{\mathrm{cT}}^y\\ 0& 1& 0\\ \sin {\mathrm{\φ}}_{\mathrm{cT}}^y& 0& \cos {\mathrm{\φ}}_{\mathrm{cT}}^y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\φ}}_{\mathrm{cT}}^x& \sin {\mathrm{\φ}}_{\mathrm{cT}}^x\\ 0& -\sin {\mathrm{\φ}}_{\mathrm{cT}}^x& \cos {\mathrm{\φ}}_{\mathrm{cT}}^x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_{\mathrm{cT}}^z& \sin {\mathrm{\φ}}_{\mathrm{cT}}^z& 0\\ -{\sin \mathrm{\φ}}_{\mathrm{cT}}^z& \cos {\mathrm{\φ}}_{\mathrm{cT}}^z& 0\\ 0& 0& 1\end{array}\right]$

$R\left(\stackrel{\‾}{\mathrm{\α}}\right)=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y& 0& -\sin {\mathrm{\α}}_y\\ 0& 1& 0\\ \sin {\mathrm{\α}}_y& 0& \cos {\mathrm{\α}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& 0\\ 0& 0& 1\end{array}\right]$

C.根据步骤B的计算结果，求得目标搜索指令角为：方位角

俯仰角

${\mathrm{\ϵ}}_p=-{\mathrm{\φ}}_{\mathrm{oT}}^x.$

D.根据步骤C的计算结果，综合计算机即可控制光电稳瞄仪快速转动方位角η_p和俯仰角ε_p，使光轴迅速指向目标。

本发明与现有技术相比的优点在于：

(1)光电探测系统、惯性导航系统往往是探测、侦察、攻击型自主飞行器两个不可或缺的分系统，两分系统独立设计，需两套惯性测量单元和计算机，成本较高，体积重量较大。本发明设计了一种组合导航和光电探测一体化系统，光电探测系统可借助组合导航系统的信息完成高精度视轴稳定、快速目标搜索等功能，不需要独立的惯性测量单元和控制计算机。相比传统的光电探测系统具有体积小、重量轻、成本低、精度高的特点。

(2)本发明利用组合导航系统估计得到的陀螺漂移进行视轴稳定误差补偿，提高了光电稳瞄仪的视轴稳定精度。

(3)利用组合导航解算得到的载体的位置和姿态信息，结合目标的位置信息，计算目标搜索指令角并控制光电稳瞄仪快速搜索并锁定目标，提高了搜索目标的速度。

附图说明

图1为本发明的结构框架图；

图2为实施本发明的系统工作原理框图；

图3为实施本发明的载体运动过程中视轴的扰动角速率曲线；

图4为实施本发明的组合导航系统的位置误差曲线；

图5为实施本发明的组合导航系统的速度误差曲线；

图6为实施本发明的组合导航系统的姿态误差曲线；

图7为实施本发明的组合导航系统的陀螺漂移估计曲线；

图8为实施本发明的光电稳瞄仪控制系统原理框图；

图9为未补偿陀螺漂移时的视轴稳定结果；

图10为本发明利用组合导航系统估计陀螺漂移并进行补偿后的视轴稳定结果。

具体实施方式

下面以小型无人机机载组合导航和光电探测一体化系统为例来阐述本发明的具体实施过程。

如图1所示，本发明机载组合导航和光电探测一体化系统包括惯性测量单元IMU 1、GPS接收系统2、综合计算机3、光电稳瞄仪6。综合计算机3采集惯性测量单元1和GPS接收系统2的数据信息，实现三大功能：一、结合捷联惯导解算算法4及卡尔曼滤波算法5进行惯性/GPS组合导航解算，得到载体的位置、速度、姿态，同时估计出惯性测量单元所含陀螺仪的漂移值；二、计算视轴稳定指令角速度并控制光电稳瞄仪实现视轴稳定，利用组合导航系统估计得到的陀螺漂移进行视轴稳定误差补偿，提高光电稳瞄仪的视轴稳定精度；三、利用组合导航解算得到的位置、姿态信息，结合目标的位置信息，计算目标搜索指令角并控制光电稳瞄仪6快速搜索并锁定目标。

如附图2所示，本发明的流程如下：

1、惯性测量单元输出载体的扰动角速度及比力信号ω_bm、f_bm：

${\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{bm}}={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{bx}}& {\mathrm{\ω}}_{\mathrm{by}}& {\mathrm{\ω}}_{\mathrm{bz}}\end{array}\right]}^T+{\left[\begin{array}{ccc}{\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T$

${\stackrel{\‾}{f}}_{\mathrm{bm}}={\left[\begin{array}{ccc}{f}_{\mathrm{bx}}& f_{\mathrm{by}}& f_{\mathrm{bz}}\end{array}\right]}^T+{\left[\begin{array}{ccc}{\mathrm{\Δa}}_x& {\mathrm{\Δa}}_y& {\mathrm{\Δa}}_z\end{array}\right]}^T$ 式(1)

其中ω_bx，ω_by，ω_bz分别为载体绕X，Y，Z轴的运动角速率；ε_x，ε_y，ε_z分别为惯性测量单元内含的X，Y，Z陀螺的角速率测量误差，即陀螺漂移；f_bx，f_by，f_bz分别为载体在X，Y，Z方向的比力；Δa_x，Δa_y，Δa_z分别为惯性测量单元内含的X，Y，Z加速度计的测量误差，即加计零偏。

2、光电稳瞄仪6安装基座相对载体的安装偏角α＝[α_x α_y α_z]^T可在光电稳瞄仪安装时进行标定和测量得到，其中α_x，α_y，α_z分别为光电稳瞄仪安装基座相对于载体坐标系X，Y，Z轴的安装偏角；光电稳瞄仪6的视轴可绕方位和俯仰轴运动，其方位角η和俯仰角ε可由光电稳瞄仪6内置的测角装置(光栅或旋转变压器或光电码盘)测量得到；由此可以计算得到载体运动引起的视轴扰动角速率ω_los为：

ω_los＝R(ε)R(η)R(α)ω_bm          式(2)

其中，ω_bm为陀螺测量得到的载体的运动角速率；R(α)为载体坐标系至光电稳瞄仪安装基座坐标系的旋转矩阵；R(η)为绕光电稳瞄仪方位轴的旋转矩阵；R(ε)为绕光电稳瞄仪俯仰轴的旋转矩阵；可分别表示为：

$R\left(\mathrm{\α}\right)=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y& 0& -\sin {\mathrm{\α}}_y\\ 0& 1& 0\\ \sin {\mathrm{\α}}_y& 0& \cos {\mathrm{\α}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& 0\\ 0& 0& 1\end{array}\right]$

$R\left(\mathrm{\η}\right)=\left[\begin{array}{ccc}\cos \mathrm{\η}& \sin \mathrm{\η}& 0\\ -\sin \mathrm{\η}& \cos \mathrm{\η}& 0\\ 0& 0& 1\end{array}\right],$ $R\left(\mathrm{\ϵ}\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\ϵ}& \sin \mathrm{\ϵ}\\ 0& -\sin \mathrm{\ϵ}& \cos \mathrm{\ϵ}\end{array}\right]$

为了使视轴稳定，即

可以求得视轴稳定的指令角速度为：

ω_p＝-ω_los＝-R(ε)R(η)R(α)ω_bm       式(3)

由于陀螺测量值ω_bm包含陀螺漂移ε，所以利用式(3)计算得到的指令角速度控制光电稳瞄仪6时会产生视轴稳定误差。

3、根据捷联惯性导航系统的基本原理，利用惯性测量单元的角速率及比力信息，可解算得到系统的纬度

经度λ_ins、东向速度V_ins ^E、北向速度V_ins ^N和姿态角φ_ins，其中捷联惯导解算算法如下：

(1)姿态计算

使用姿态四元数来更新姿态。四元数微分方程为

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nbx}}^b& -{\mathrm{\ω}}_{\mathrm{nby}}^b& -{\mathrm{\ω}}_{\mathrm{nbz}}^b\\ {\mathrm{\ω}}_{\mathrm{nbx}}^b& 0& {\mathrm{\ω}}_{\mathrm{nbz}}^b& -{\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nby}}^b& -{\mathrm{\ω}}_{\mathrm{nbz}}^b& 0& {\mathrm{\ω}}_{\mathrm{nbx}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b& {\mathrm{\ω}}_{\mathrm{nby}}^b& -{\mathrm{\ω}}_{\mathrm{nbx}}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$ 式(4)

简写为

$\stackrel{\·}{q}=\frac{1}{2}\mathrm{Mq}$

其中，q为所定义的四元数；

ω_ib ^b为载体坐标系下的角速率；T_n ^b为导航坐标系至载体坐标系的转换矩阵；ω_en ⁿ为导航坐标系下的载体位移角速率；ω_ie ^e为地球坐标系下的地球自转角速度；C_e ⁿ为地球坐标系至导航坐标系的转换矩阵；V_nx，V_ny为载体的东向和北向速度；R_N，R_M分别为地球在子午面和垂直子午面方向上的地球曲率半径；

为载体所在地的纬度。

可采用四阶龙格库塔法进行姿态微分方程解算。

(2)速度更新

速度微分方程为：

${\stackrel{\·}{V}}_n=T_b^n{f}^b+({2\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)V_n+g^n$ 式(5)

其中，T_b ⁿ为载体坐标系至导航坐标系的转换矩阵，f^b为载体坐标系下的比力值，ω_ie ⁿ，ω_en ⁿ分别为导航坐标系下的地球自转角速率和载体的位移角速率；V_n为载体在导航坐标系下的运动速度；gⁿ为当地重力加速度。

可采用一阶欧拉法解算速度微分方程。

(3)位置更新

基于位置矩阵更新位置信息的微分方程为

${\stackrel{\·}{C}}_e^n=-{\mathrm{\ω}}_{\mathrm{en}}^n{C}_e^n$ 式(6)

其中，C_e ⁿ为地球坐标系至导航坐标系的变换矩阵，也可称为位置矩阵。可用一阶欧拉法解算位置矩阵微分方程。

4、引入GPS的纬度

经度λ_gps、东向速度V_gps ^E、北向速度V_gps ^N，进行组合导航解算，其解算步骤如下：

(1)选择东北天地理坐标系作为导航坐标系，选择状态观测量X如下：

其中，Δv_x为惯导系统的东向速度误差，Δv_y为北向速度误差，

为纬度误差，Δλ为经度误差，Δφ_x为俯仰角误差，Δφ_y为横滚角误差，Δφ_z为航向角误差，ε_x为东向陀螺漂移，ε_y为北向陀螺漂移，ε_z为天向陀螺漂移，Δα_x为东向加速度计零偏，Δα_y为北向加速度计零偏。

(2)建立惯导系统的误差方程为：

式(7)

${\stackrel{\·}{\mathrm{\ϵ}}}_x=\frac{1}{{\mathrm{\τ}}_g}{\mathrm{\ϵ}}_x,$ ${\stackrel{\·}{\mathrm{\ϵ}}}_y=\frac{1}{{\mathrm{\τ}}_g}{\mathrm{\ϵ}}_y,$ ${\stackrel{\·}{\mathrm{\ϵ}}}_z=\frac{1}{{\mathrm{\τ}}_g}{\mathrm{\ϵ}}_z$

$\mathrm{\Δ}{\stackrel{\·}{a}}_x=-\frac{1}{{\mathrm{\τ}}_a}\mathrm{\Δ}{a}_x,$ $\mathrm{\Δ}{\stackrel{\·}{a}}_y=-\frac{1}{{\mathrm{\τ}}_a}\mathrm{\Δ}{a}_y$

上面各式中，R_N，R_M分别为地球在子午面和垂直子午面方向上的曲率半径，h为载体的高度，

为当地纬度，f_x，f_y，f_z分别为东、北、天向的比力，ω_ie为地球自转角速度，v_x、v_y分别为载体的东向、北向速度，τ_g是陀螺漂移一阶马尔可夫随机过程的相关时间，τ_a是加速度计零偏一阶马尔可夫随机过程的相关时间。

将上面各式写成矩阵形式如下：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$ 式(8)

其中X(t)是系统状态向量，F(t)是系统矩阵，G(t)是系统噪声矩阵，W(t)是系统噪声向量。

(3)确定系统的量测量为：

其中，

λ_ins，V_ins ^E，V_ins ^N分别为惯导系统输出的载体的纬度、经度、东向速度、北向速度，因此可以得到量测方程为：

Y＝BX+V            式(9)

其中B＝[I_4×4 O_4×8]，V为量测噪声，即GPS输出信号的噪声。

(4)根据步骤(2)和(3)给出的状态方程和量测方程，利用卡尔曼滤波算法，可以得到状态变量的误差估计值如下：

并得到组合导航系统的输出信息为：

$V_E\left(t\right)=V_{\mathrm{ins}}^E\left(t\right)-\mathrm{\Δ}{v}_{\mathrm{ex}}\left(t\right)$

$V_N\left(t\right)=V_{\mathrm{ins}}^N\left(t\right)-\mathrm{\Δ}{v}_{\mathrm{ey}}\left(t\right)$

λ(t)＝λ_ins(t)-Δλ_e(t)        式(10)

${\mathrm{\φ}}_x\left(t\right)={\mathrm{\φ}}_{\mathrm{ins}}^x\left(t\right)-\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ex}}\left(t\right)$

${\mathrm{\φ}}_y\left(t\right)={\mathrm{\φ}}_{\mathrm{ins}}^y\left(t\right)-\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ey}}\left(t\right)$

${\mathrm{\φ}}_z\left(t\right)={\mathrm{\φ}}_{\mathrm{ins}}^z\left(t\right)-\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ez}}\left(t\right)$

其中，所用的卡尔曼滤波算法的离散化公式为：

$\left\{\begin{array}{c}{\hat{X}}_{k/k-1}=F_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Y_k-B_k{\hat{X}}_{k/k-1})\\ P_{k/k-1}=F_{k,k-1}{P}_{k-1}{F}_{k,k-1}^T+Q_k\\ K_k=P_{k/k-1}{B}_k^T{(B_k{P}_{k/k-1}{B}_k^T+R_k)}^{-1}\\ P_k=(I-K_k{B}_k)P_{k/k-1}\end{array}\right.$ 式(11)

式11中，

为k-1时刻的状态变量的估计值；

为一步转移向量；F_k，k-1＝I+FΔt为离散化后的系统矩阵，I为单位阵，Δt＝1s；B_k为k时刻的量测矩阵；Y_k为系统的量测量；Q_k为系统噪声向量；R_k为量测噪声向量。

式10中，Δv_ex为东向速度误差的估计值；Δv_ey为北向速度误差的估计值；

纬度误差的估计值；Δλ_e经度误差的估计值；Δφ_ex为俯仰角误差的估计值；Δφ_ey为横滚角误差的估计值；Δφ_ez为航向角误差的估计值；ε_ex为东向陀螺漂移的估计值；ε_ey为北向陀螺漂移的估计值；ε_ez为天向陀螺漂移的估计值；Δα_ex为东向加速度计零偏的估计值；Δα_ey为北向加速度计零偏的估计值；V_ins ^E，V_ins ^N，

λ_ins，φ_ins ^x，φ_ins ^y，φ_ins ^z分别为惯导系统输出的东向速度、北向速度、纬度、经度、俯仰角、横滚角、航向角；V_E，V_N，

λ，φ_x，φ_y，φ_z分别为组合导航系统输出的东向速度、北向速度、纬度、经度、俯仰角、横滚角、航向角。

5、根据组合导航解算结果，可得到机体系的角速度ω_b的表达式为：

${\stackrel{\‾}{\mathrm{\ω}}}_b\left(t\right)={\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{bm}}\left(t\right)-C_t^b{\stackrel{\‾}{\mathrm{\ϵ}}}_e\left(t\right)$ 式(12)

其中ω_bm(t)为陀螺的测量值，ε_e(t)为组合导航系统估计得到的地理系下的陀螺漂移，C_t ^b为地理系至机体系的坐标变换矩阵，其表达式如下：

$C_t^b=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y& 0& -\sin {\mathrm{\φ}}_y\\ 0& 1& 0\\ \sin {\mathrm{\φ}}_y& 0& \cos {\mathrm{\φ}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\φ}}_x& \sin {\mathrm{\φ}}_x\\ 0& -\sin {\mathrm{\φ}}_x& \cos {\mathrm{\φ}}_x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_z& 0\\ -\sin {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_z& 0\\ 0& 0& 1\end{array}\right]$

6、将式(12)得到的ω_b值替代式(3)中的ω_bm则视轴稳定指令角速度可以重写为：

ω_p＝-R(ε)R(η)R(α)ω_b        式(13)

式(13)计算得到的视轴稳定指令角速度已消除了陀螺的漂移误差，可大大提高系统的视轴稳定精度。

7、根据组合导航系统给出载体的位置、速度及姿态角信息，可计算得到目标搜索方位指令角η_p及俯仰指令角ε_p，其计算流程如下：

(1)目标纬度

和经度λ_T由指挥系统提供，组合导航系统提供载体的纬度

经度λ、俯仰角φ_x、横滚角φ_y、航向角φ_z，利用上述信息计算出目标所在地的地理坐标系相对于安装载体坐标系的转角(φ_cT ^x，φ_cT ^y，φ_cT ^z)^T的表达式为：

目标的纬度

和经度λ_T可由指挥系统提供，载体(如飞机、地面车等)的纬度经度λ、俯仰角φ_x、横滚角φ_y、航向角φ_z可由组合导航系统解算得到；利用以上信息可计算出目标所在地的地理坐标系相对于安装载体坐标系的转角(φ_cT ^x，φ_cT ^y，φ_cT ^z)^T的表达式为：

式(14)

(2)定义光电稳瞄仪安装基座坐标系相对于载体坐标系的安装偏角为(α_x，α_y，α_z)^T，该安装偏角可由光电稳瞄仪安装时经标定和测量得到；根据步骤(1)计算得到的目标地理系相对于载体系的转角(φ_cT ^x，φ_cT ^y，φ_cT ^z)^T，可以求得目标地理系相对于光电稳定瞄仪安装基座坐标系的转角(φ_oT ^x，φ_oT ^y，φ_oT ^z)^T的表达式为：

$\left(\begin{array}{c}{\mathrm{\φ}}_{\mathrm{oT}}^x\\ {\mathrm{\φ}}_{\mathrm{oT}}^y\\ {\mathrm{\φ}}_{\mathrm{oT}}^z\end{array}\right)=\left(\begin{array}{c}{\sin }^{-1}(-T_{23})\\ {\mathrm{tg}}^{-1}(-T_{13}/T_{33})\\ {\mathrm{tg}}^{-1}(-T_{12}/T_{22})\end{array}\right)$ 式(15)

其中， $\left(\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right)=R\left(\stackrel{\‾}{\mathrm{\α}}\right)R\left({\stackrel{\‾}{\mathrm{\φ}}}_{\mathrm{cT}}\right)$

R(φ_cT)为目标地理系至载体坐标系的旋转矩阵，R(α)为载体坐标系至光电稳瞄仪基座坐标系的旋转矩阵，可分别表示为：

$R\left({\stackrel{\‾}{\mathrm{\φ}}}_{\mathrm{cT}}\right)=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_{\mathrm{cT}}^y& 0& -\sin {\mathrm{\φ}}_{\mathrm{cT}}^y\\ 0& 1& 0\\ \sin {\mathrm{\φ}}_{\mathrm{cT}}^y& 0& \cos {\mathrm{\φ}}_{\mathrm{cT}}^y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\φ}}_{\mathrm{cT}}^x& \sin {\mathrm{\φ}}_{\mathrm{cT}}^x\\ 0& -\sin {\mathrm{\φ}}_{\mathrm{cT}}^x& \cos {\mathrm{\φ}}_{\mathrm{cT}}^x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_{\mathrm{cT}}^z& \sin {\mathrm{\φ}}_{\mathrm{cT}}^z& 0\\ -{\sin \mathrm{\φ}}_{\mathrm{cT}}^z& \cos {\mathrm{\φ}}_{\mathrm{cT}}^z& 0\\ 0& 0& 1\end{array}\right]$

$R\left(\stackrel{\‾}{\mathrm{\α}}\right)=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y& 0& -\sin {\mathrm{\α}}_y\\ 0& 1& 0\\ \sin {\mathrm{\α}}_y& 0& \cos {\mathrm{\α}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& 0\\ 0& 0& 1\end{array}\right]$

(3)根据步骤(2)的计算结果，可以求得目标搜索指令角为：方位角

俯仰角

8、在计算得到目标搜索方位指令角η_p、俯仰指令角ε_p以及视轴稳定指令角速率ω_p的基础上，控制计算机可控制光电稳瞄仪6运动，完成视轴稳定、目标搜索等功能。

本实施例选用的惯性测量单元内含漂移为50(°)/h的MEMS陀螺和零偏为0.1mg的MEMS加速度计；小飞机在空中以20m/s的速度向东匀速直线飞行，飞行中因气流干扰等因素的影响飞机将绕俯仰、横滚、和方位轴转动，并假设其运动规律为：

光电稳瞄仪与载体的安装偏角α＝[0 0 0]^T，光电稳瞄仪6的方位和俯仰转角均为45°，则可以得到飞行过程中视轴的扰动角速率如附图3所示。

本实施例选用的GPS接收系统的位置误差为2米，速度误差为0.05米/秒。图4、5、6、7为惯性/GPS组合导航算法估计得到的载体的位置误差、速度误差、姿态角误差、陀螺漂移；由图示结果可知，组合导航系统输出的位置精度为2米，速度精度为0.05米/秒，俯仰及横滚角精度为0.2度，方位角精度优于1.0度。在得到载体的位置、姿态等信息后，可计算得到目标搜索指令角η_p及ε_p，其精度优于1.2度。光电稳瞄仪6的视场角一般大于3度，所以光电稳瞄仪6在执行相应的目标搜索指令后可确保被探测目标在视场范围内。

图8为光电稳瞄仪6控制系统原理框图，取C_m＝11.4mNm/A，L＝250uH，R＝19.8Ω，J＝0.48g.cm.s²，K_f＝26.7g.cm/rad/s，T_d＝2g.cm，校正环节K_ω和K_θ均采用PID调节。得到的视轴稳定结果如下图9、10所示，图9为没有补偿陀螺漂移时的视轴稳定结果；图10为利用组合导航系统估计陀螺漂移并进行补偿后的视轴稳定结果。对比图9、10可知，通过组合导航系统估计陀螺漂移并进行补偿后，视轴的稳定精度得到了较大程度的提高。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

最后所应说明的是：以上实施实例仅用以说明而非限制本发明的技术方案，所有的不脱离本发明的精神和范围的修改或局部替换，均应涵盖在本发明的权利要求范围当中。

Claims (3)

1.一种组合导航和光电探测一体化系统，其特征在于包括：惯性测量单元(1)、GPS接收系统(2)、综合计算机(3)、光电稳瞄仪(6)；综合计算机(3)采集惯性测量单元(1)和GPS接收系统(2)的数据信息，实现三大功能：第一、结合捷联惯导解算算法(4)及卡尔曼滤波算法(5)进行惯性和GPS组合导航解算，得到载体的位置、速度和姿态，同时估计出惯性测量单元(1)所含陀螺仪的漂移；第二、根据组合导航解算得到的载体的位置、速度和姿态，计算视轴稳定指令角速度并控制光电稳瞄仪(6)实现视轴的惯性空间稳定；第三、利用组合导航解算得到的位置和姿态信息，结合目标位置信息，计算目标搜索指令角并控制光电稳瞄仪(6)快速搜索并锁定目标。

2.根据权利要求1所述的组合导航和光电探测一体化系统，其特征在于：所述综合计算机(3)控制光电稳瞄仪(6)实现视轴稳定的具体过程为：

a.综合计算机(3)采集惯性测量单元(1)输出的沿载体坐标系的角速度ω_bm及比力f_bm和GPS接收系统(2)输出的纬度经度λ_gps、东向速度V_gps ^E、北向速度V_gps ^N，并结合捷联惯导解算算法(4)和卡尔曼滤波算法(5)，解算出安装载体的纬度

经度λ、速度V、俯仰角φ_x、横滚角φ_y、航向角φ_z，并且估计出惯性测量单元(1)所含陀螺仪的漂移值ε_b；

b.将惯性测量单元(1)输出的角速度信息ω_bm进行漂移补偿，得到载体的运动角速度ω_b的表达式为：

          ω_b＝ω_bm-ε_b

对载体的运动角速度ω_b进行坐标变换得到光电稳瞄仪(6)视轴扰动角速度ω_los，其表达方式为：其中C_b ^los为载体坐标系至视轴坐标系的变换矩阵；

c.以ω_los作为光电稳瞄仪(6)的输入指令，控制光电稳瞄仪(6)转动，即实现了视轴的惯性空间稳定。

3.根据权利要求1所述的组合导航和光电探测一体化系统，其特征在于：所述的综合计算机(3)控制光电稳瞄仪(6)快速搜索并锁定目标的具体过程为：

A.目标纬度和经度λ_T由指挥系统提供，组合导航系统提供载体的纬度

经度λ、俯仰角φ_x、横滚角φ_y、航向角φ_z，利用上述信息计算出目标所在地的地理坐标系相对于安装载体坐标系的转角(φ_cT ^x，φ_cT ^y，φ_cT ^z)^T的表达式为：

B.定义光电稳瞄仪(6)安装基座坐标系相对于载体坐标系的安装偏角为(α_x，α_y，α_z)^T，根据步骤A计算得到的目标地理坐标系相对于载体坐标系的转角(φ_cT ^x，φ_cT ^y，φ_cT ^z)^T，求得目标地理系相对于光电稳瞄仪安装基座坐标系的转角(φ_oT ^x，φ_oT ^y，φ_oT ^z)^T的表达式为：

$\left(\begin{array}{c}{\mathrm{\φ}}_{\mathrm{oT}}^x\\ {\mathrm{\φ}}_{\mathrm{oT}}^y\\ {\mathrm{\φ}}_{\mathrm{oT}}^z\end{array}\right)=\left(\begin{array}{c}{\sin }^{-1}(-T_{23})\\ {\mathrm{tg}}^{-1}(-T_{13}/T_{33})\\ {\mathrm{tg}}^{-1}({-T}_{12}/T_{22})\end{array}\right)$

其中， $\left(\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right)=R\left(\stackrel{\‾}{\mathrm{\α}}\right)R\left({\stackrel{\‾}{\mathrm{\φ}}}_{\mathrm{cT}}\right)$

R(φ_cT)为目标地理系至载体坐标系的旋转矩阵，R(α)为载体坐标系至光电稳瞄仪安装基座坐标系的旋转矩阵，分别表示为：

$R\left({\stackrel{\‾}{\mathrm{\φ}}}_{\mathrm{cT}}\right)=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_{\mathrm{cT}}^y& 0& -\sin {\mathrm{\φ}}_{\mathrm{cT}}^y\\ 0& 1& 0\\ \sin {\mathrm{\φ}}_{\mathrm{cT}}^y& 0& \cos {\mathrm{\φ}}_{\mathrm{cT}}^y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\φ}}_{\mathrm{cT}}^x& \sin {\mathrm{\φ}}_{\mathrm{cT}}^x\\ 0& -\sin {\mathrm{\φ}}_{\mathrm{cT}}^x& \cos {\mathrm{\φ}}_{\mathrm{cT}}^x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_{\mathrm{cT}}^z& \sin {\mathrm{\φ}}_{\mathrm{cT}}^z& 0\\ -{\sin \mathrm{\φ}}_{\mathrm{cT}}^z& \cos {\mathrm{\φ}}_{\mathrm{cT}}^z& 0\\ 0& 0& 1\end{array}\right]$

$R\left(\stackrel{\‾}{\mathrm{\α}}\right)=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y& 0& -\sin {\mathrm{\α}}_y\\ 0& 1& 0\\ \sin {\mathrm{\α}}_y& 0& \cos {\mathrm{\α}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& 0\\ 0& 0& 1\end{array}\right]$

C.根据步骤B的计算结果，求得目标搜索指令角为：方位角

俯仰角 ${\mathrm{\ϵ}}_p={-\mathrm{\φ}}_{\mathrm{oT}}^x;$

D.根据步骤C的计算结果，综合计算机(3)即可控制光电稳瞄仪快速转动方位角η_p和俯仰角ε_p，使光轴迅速指向目标。

Images