CN104596515A - 一种惯导/激光测速仪组合导航的半实物仿真方法 - Google Patents

Abstract

一种惯导/激光测速仪组合导航的半实物仿真方法，有七大步骤：一、将惯导/激光测速仪安装载体上并上电启动；二、装订初始参数至导航计算机；三、载体静止，采集保存1小时陀螺和加速度计输出数据；求其平均值并分别与平均值作差，得到陀螺和加速度计实际噪声；四、载体静止，惯导进行5分钟静态初始对准；五、载体运动，惯导计算，采集并保存运动后50秒内惯导输出速度值、姿态矩阵等数据；六、把速度投影到激光测速仪速度方向上得参考速度；将其速度与参考速度作差，得其输出速度的噪声值；七、设定仿真轨迹，得姿态矩阵、速度分量及速度标准值；将各相关数据与步骤3、6中噪声数据融合，得到器件的模拟数据进行惯导/激光测速仪组合导航仿真。

Description

一种惯导/激光测速仪组合导航的半实物仿真方法

技术领域：

本发明涉及一种惯导/激光测速仪组合导航的半实物仿真方法，属于惯性导航技术领域。

背景技术：

激光测速仪根据多普勒频移可以实时测量运动载体相对地面移动的速度，且其测量误差不随时间积累，与惯导***组合能优势互补，能实现全自主、高精度导航定位。

在进行惯导/激光测速仪组合导航研究时，一般是设定仿真轨迹(包含载体的运动速度、位置、姿态)，利用轨迹发生器生成加速度计、陀螺的标准值，由设定仿真轨迹中载体的运动速度等信息计算得到激光测速仪的速度标准值，并给定激光测速仪的标度误差、安装角及安装角误差以及噪声，给定加速度计和陀螺的标度误差、零偏、失准角误差以及噪声，给定惯导的安装角及安装角误差，将以上数据融合得到用于组合导航仿真的器件模拟数据，然后采用这些模拟数据进行组合导航的仿真计算与分析。其中，给定的仿真噪声不能全面反映激光测速仪、加速度计、陀螺的噪声特性，导致仿真结果不能较全面地反映实际情况。为了更好地进行惯导/激光测速仪组合导航研究，本文提出了一种惯导/激光测速仪组合导航的半实物仿真方法。

发明内容

1、发明目的：本发明的目的是提供了一种惯导/激光测速仪组合导航的半实物仿真方法，它能够更好地模拟惯导/激光测速仪组合导航过程。

2、技术方案： 

本发明一种惯导/激光测速仪组合导航的半实物仿真方法，该方法具体步骤如下：

步骤1、将惯导/激光测速仪组合***安装到载体上，并上电启动。

步骤2、装订初始参数(包括初始的经度、纬度、高度、激光测速仪标度值、激光测速仪安装角、惯导安装角)至导航计算机。

步骤3、保持载体静止，采集保存1小时陀螺和加速度计的输出数据。求取陀螺和加速度计的输出数据的平均值，将采集的陀螺和加速度计数据分别与其平均值作差，得到陀螺和加速度计的实际噪声。

步骤4、载体保持静止，惯导进行5分钟静态初始对准。

步骤5、完成对准后载体开始运动，运动过程中惯导进行惯性导航计算，采集并保存开始运动后50秒内惯导输出的速度值、惯导输出的姿态矩阵、激光测速仪输出的速度。

步骤6、惯导输出的速度为东北天坐标系下的速度，由惯导输出姿态矩阵将东北天坐标系下的速度转换到惯组坐标系下，再根据惯导与载体、激光测速仪与载体之间的位置关系，把惯组坐标系下的速度投影到激光测速仪的速度方向上得到参考速度。将激光测速仪的速度与参考速度作差，得到激光测速仪输出速度的噪声值。

步骤7、设定仿真轨迹(包含载体的运动速度、位置、姿态)。由设定仿真轨迹中载体的姿态得到仿真载体的姿态矩阵，根据得到的姿态矩阵和仿真轨迹中载体的运动速度得到载体在载体系下的速度分量，再由载体的速度分量和给定的激光测速仪的安装角、惯导安装角计算得到激光测速仪的速度标准值；利用轨迹发生器生成加速度计的标准值、陀螺的标准值；并给定加速度计和陀螺的标度误差、零偏、失准角误差；给定激光测速仪的标度误差、安装角误差；给定惯导的安装角误差。将这些数据与步骤3、步骤6中得到的噪声数据融合，得到用于惯导/激光测速仪组合导航半实物仿真的器件模拟数据，然后采用这些模拟数据进行惯导/激光测速仪组合导航仿真。

其中，步骤3中所述的“得到陀螺、加速度计的实际噪声”，其实现过程说明如下：

定义w_g为陀螺噪声，w_a为加速度计噪声，采集陀螺x、y、z三个轴输出的数据分别为ω_x(1)、ω_x(2)…ω_x(n)，ω_y(1)、ω_y(2)…ω_y(n)，ω_z(1)、ω_z(2)…ω_z(n)，平均值分别为采集加速度计x、y、z三个轴输出的数据分别为a_x(1)、a_x(2)…a_x(n)，a_y(1)、a_y(2)…a_y(n)，a_z(1)、a_z(2)…a_z(n)，平均值分别为陀螺噪声计算公式如下：

$w_g\left(i\right)=\left[\begin{array}{c}{w}_{\mathrm{gx}}\left(i\right)\\ w_{\mathrm{gy}}\left(i\right)\\ w_{\mathrm{gz}}\left(i\right)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x\left(i\right)\\ {\mathrm{\ω}}_y\left(i\right)\\ {\mathrm{\ω}}_z\left(i\right)\end{array}\right]-\left[\begin{array}{c}{\stackrel{\‾}{\mathrm{\ω}}}_x\\ {\stackrel{\‾}{\mathrm{\ω}}}_y\\ {\stackrel{\‾}{\mathrm{\ω}}}_z\end{array}\right],(i=1...n)$

加速度计噪声计算公式如下：

$w_a\left(i\right)=\left[\begin{array}{c}{w}_{\mathrm{ax}}\left(i\right)\\ w_{\mathrm{ay}}\left(i\right)\\ w_{\mathrm{az}}\left(i\right)\end{array}\right]=\left[\begin{array}{c}{a}_x\left(i\right)\\ a_y\left(i\right)\\ a_z\left(i\right)\end{array}\right]-\left[\begin{array}{c}{\stackrel{\‾}{a}}_x\\ {\stackrel{\‾}{a}}_y\\ {\stackrel{\‾}{a}}_z\end{array}\right],(i=1...n)$

其中，步骤6中所述的“得到激光测速仪输出速度的噪声值”，其实现过程说明如下：

载体开始运动，采集得到了50秒内激光测速仪的速度输出惯导输出东北天坐标系下速度 $V_E^n\left(1\right),V_E^n\left(2\right)...V_E^n\left(n\right),V_N^n\left(1\right),V_N^n\left(2\right)...V_N^n\left(n\right),V_U^n\left(1\right),V_U^n\left(2\right)...V_U^n\left(n\right),$ 惯导输出的姿态矩阵导航计算机装订的激光测速仪的安装角为θ，惯导的安装角为α、β、γ；惯导在惯组坐标系下的速度输出分别为V_x(1)、V_x(2)…V_x(n)，V_y(1)、V_y(2)…V_y(n)，V_z(1)、V_z(2)…V_z(n)。惯导在惯组坐标系下的速度投影到激光测速仪的速度方向上的参考速度为惯导在东北天坐标系下输出的速度转到惯组坐标系下，计算公式如下：

$\left[\begin{array}{c}{V}_x\left(i\right)\\ V_y\left(i\right)\\ V_z\left(i\right)\end{array}\right]=C_n^b\left(i\right)\left[\begin{array}{c}{V}_E^n\left(i\right)\\ V_N^n\left(i\right)\\ V_U^n\left(i\right)\end{array}\right](i=1...n)$

惯导在惯组坐标系下的速度投影到激光测速仪的速度方向上的参考速度为V^m(i)，计算公式如下：

V^m(i)＝[V_x(i)cosα+V_y(i)cosβ+V_z(i)cosγ]cosθ     (i＝1...n)

激光测速仪输出的速度噪声值为w_L(i)，计算公式如下：

$w_L\left(i\right)=V_L^m\left(i\right)-V^m\left(i\right),(i=1...n)$

其中，步骤7中所述的“得到用于惯导/激光测速仪组合导航半实物仿真的器件模拟数据，然后采用这些模拟数据进行惯导/里程计组合导航仿真”，其实现过程说明如下：

设定仿真轨迹(包含载体的运动速度、位置、姿态)。由设定仿真轨迹中载体的姿态得到仿真载体的姿态矩阵，根据得到的姿态矩阵和仿真轨迹中载体的运动速度得到载体在载体系下的速度分量，再由载体的速度分量和给定的激光测速仪的安装角、惯导安装角计算得到激光测速仪的速度标准值；利用轨迹发生器生成加速度计的标准值、陀螺的标准值；并给定加速度计和陀螺的标度误差、零偏、失准角误差；给定激光测速仪的标度误差、安装角误差；给定惯导的安装角误差。将这些数据与步骤3、步骤6中得到的噪声数据融合，得到用于惯导/激光测速仪组合导航半实物仿真的器件模拟数据，然后采用这些模拟数据进行惯导/里程计组合导航仿真。

定义：仿真轨迹中导航坐标系下载体运动速度向量为Vⁿ，在导航坐标系下三个方向的分量分别为仿真轨迹中载体的运动速度在载体系下x、y、z三个轴的分量 分别为由仿真轨迹中载体的姿态得到的载体姿态矩阵为给定激光测速仪的安装角θ_f，激光测速仪安装角误差为δθ_f，惯导的安装角分别为α_b、β_b、γ_b以及惯导的安装角误差分别为δα_b、δβ_b、δγ_b，激光测速仪的标度误差值δK_L；加速度计x、y、z三个轴的标准值分别为a_tx(1)、a_tx(2)…a_tx(n)，a_ty(1)、a_ty(2)…a_ty(n)，a_tz(1)、a_tz(2)…a_tz(n)；加速度计x、y、z三个轴的标度误差分别为δK_ax、δK_ay、δK_az；加速度计的失准角误差为δM_axy、δM_axz、δM_ayx、δM_ayz、δM_azx、δM_azy；加速度计x、y、z三个轴的零偏分别为δB_ax、δB_ay、δB_az；陀螺x、y、z三个轴的标准值分别为ω_tx(1)、ω_tx(2)…ω_tx(n)，ω_ty(1)、ω_ty(2)…ω_ty(n)，ω_tz(1)、ω_tz(2)…ω_tz(n)；陀螺x、y、z三个轴的标度误差分别为δK_gx、δK_gy、δK_gz；陀螺的失准角误差为δM_gxy、δM_gxz、δM_gyx、δM_gyz、δM_gzx、δM_gzy；陀螺x、y、z三个轴的零偏分别为δB_gx、δB_gy、δB_gz。陀螺x、y、z三个轴的噪声分别为w_gx(1)、w_gx(2)…w_gx(n)，w_gy(1)、w_gy(2)…w_gy(n)，w_gz(1)、w_gz(2)…w_gz(n)；加速度计x、y、z三个轴的噪声分别为w_ax(1)、w_ax(2)…w_ax(n)，w_ay(1)、w_ay(2)…w_ay(n)，w_az(1)、w_az(2)…w_az(n)；激光测速仪的速度噪声为w_L(1)、w_L(2)…w_L(n)。

仿真轨迹中载体在导航系下的速度转换到载体系下x、y、z三个轴的速度计算如下：

$\left[\begin{array}{c}{v}_x^b\left(i\right)\\ v_y^b\left(i\right)\\ v_z^b\left(i\right)\end{array}\right]=T_n^b\left(i\right)\left[\begin{array}{c}{v}_x^n\left(i\right)\\ v_y^n\left(i\right)\\ v_z^n\left(i\right)\end{array}\right](i=1...n)$

计算得到激光测速仪的速度标准值为计算公式如下：

$V_{\mathrm{Lt}}^m\left(i\right)=[v_x^b\left(i\right)\cos {\mathrm{\α}}_f+v_y^b\left(i\right)\cos {\mathrm{\β}}_f+v_z^b\left(i\right)\cos {\mathrm{\γ}}_f]\cos {\mathrm{\θ}}_f(i=1...n)$

由计算得到激光测速仪的速度标准值、给定的激光测速仪的标度误差值、安装角误差、惯导安装角误差以及提取的激光测速仪的速度噪声，综合得到用于组合导航半实物仿真的激光测速仪的速度计算公式如下：

$\begin{array}{c}{V}_{\mathrm{Lf}}^m\left(i\right)=(1+\mathrm{\δ}{K}_L)[v_x^b\left(i\right)\cos ({\mathrm{\α}}_f+\mathrm{\δ}{\mathrm{\α}}_f)+v_y^b\left(i\right)\cos ({\mathrm{\β}}_f+\mathrm{\δ}{\mathrm{\β}}_f)+v_z^b\left(i\right)\cos ({\mathrm{\γ}}_f+\mathrm{\δ}{\mathrm{\γ}}_f)]\cos ({\mathrm{\θ}}_f+\mathrm{\δ}{\mathrm{\θ}}_f)+w_L\left(i\right)\\ (i=1...n)\end{array}$

由计算得到的陀螺、加速度计标准值，给定的陀螺、加速度计零偏、标度误差、失准角误差以及提取的陀螺、加速度计的噪声，综合得到用于组合导航半实物仿真的陀螺、加速度计数据计算公式如下：

${\mathrm{\ω}}_f\left(i\right)=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{fx}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{fy}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{fz}}\left(i\right)\end{array}\right]=\left[\begin{array}{ccc}1+\mathrm{\δ}{K}_{\mathrm{gx}}& \mathrm{\δ}{M}_{\mathrm{gxy}}& \mathrm{\δ}{M}_{\mathrm{gxz}}\\ {\mathrm{\δM}}_{\mathrm{gyx}}& 1+\mathrm{\δ}{K}_{\mathrm{gy}}& \mathrm{\δ}{M}_{\mathrm{gyz}}\\ \mathrm{\δ}{M}_{\mathrm{gzx}}& \mathrm{\δ}{M}_{\mathrm{gzy}}& 1+\mathrm{\δ}{K}_{\mathrm{gz}}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{tx}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{ty}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{tz}\left(i\right)}\end{array}\right]+\left[\begin{array}{c}\mathrm{\δ}{B}_{\mathrm{gx}}\\ {\mathrm{\δB}}_{\mathrm{gy}}\\ {\mathrm{\δB}}_{\mathrm{gz}}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{gx}}\left(i\right)\\ w_{\mathrm{gy}}\left(i\right)\\ w_{\mathrm{gz}}\left(i\right)\end{array}\right](i=1...n)$

$a_f\left(i\right)=\left[\begin{array}{c}{a}_{\mathrm{fx}}\left(i\right)\\ a_{\mathrm{fy}}\left(i\right)\\ a_{\mathrm{fz}}\left(i\right)\end{array}\right]=\left[\begin{array}{ccc}1+\mathrm{\δ}{K}_{\mathrm{ax}}& \mathrm{\δ}{M}_{\mathrm{axy}}& \mathrm{\δ}{M}_{\mathrm{axz}}\\ {\mathrm{\δM}}_{\mathrm{ayx}}& 1+\mathrm{\δ}{K}_{\mathrm{ay}}& \mathrm{\δ}{M}_{\mathrm{ayz}}\\ \mathrm{\δ}{M}_{\mathrm{azx}}& \mathrm{\δ}{M}_{\mathrm{azy}}& 1+\mathrm{\δ}{K}_{\mathrm{az}}\end{array}\right]\left[\begin{array}{c}{a}_{\mathrm{tx}}\left(i\right)\\ a_{\mathrm{ty}}\left(i\right)\\ a_{\mathrm{tz}\left(i\right)}\end{array}\right]+\left[\begin{array}{c}\mathrm{\δ}{B}_{\mathrm{ax}}\\ {\mathrm{\δB}}_{\mathrm{ay}}\\ {\mathrm{\δB}}_{\mathrm{az}}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{ax}}\left(i\right)\\ w_{\mathrm{ay}}\left(i\right)\\ w_{\mathrm{az}}\left(i\right)\end{array}\right](i=1...n)$

采用上述得到的激光测速仪模拟数据加速度计模拟数据a_f、陀螺模拟数据ω_f，结合由仿真轨迹中给定的载体初始位置、初始姿态、初始速度，进行组合导航半实物仿真。

3、优点及功效：该方法的优点是用激光测速仪、陀螺、加速度计的实际噪声替代仿真噪声，然后进行惯导/激光测速仪组合导航的半实物仿真，为研究惯导/激光测速仪组合导航提供了更好的支持。

附图说明

图1为本发明噪声提取框图

图2为本发明惯导/激光测速仪组合导航的半实物仿真方法框图；

图3为本发明惯导/激光测速仪组合导航的半实物仿真方法流程图；

图1中符号说明如下：

惯导输出的东、北、天方向的速度

V_x、V_y、V_z：惯导在惯组坐标系下x、y、z三个方向的速度分量

：激光测速仪输出的速度

V^m：激光测速仪测速方向上的参考速度

a：加速度计输出值

：加速度计输出值的平均值

ω：陀螺输出值

：陀螺输出值的平均值

w_L：激光测速仪的噪声

w_a：加速度计的噪声

w_g：陀螺的噪声 

图2中符号说明如下：

：计算得到的激光测速仪的速度标准值

δK_L：给定的激光测速仪的标度误差

ω_t：轨迹发生器生成的陀螺数据标准值

a_t：轨迹发生器生成的加速度计数据标准值

δB_g：给定的陀螺零偏

δB_a：给定的加速度计零偏

ω_c：包含标度误差、安装角误差的陀螺数据

a_c：包含标度误差、安装角误差的加速度计数据

f：用于组合导航半实物仿真的激光测速仪的速度值

ω_f：用于组合导航半实物仿真的陀螺数据

a_f：用于组合导航半实物仿真的加速度计数据

P：仿真轨迹中设定的载***置

A：仿真轨迹中设定的载体姿态

V：仿真轨迹中设定的载体速度

P₀：仿真轨迹中设定的载体初始位置

A₀：仿真轨迹中设定的载体初始姿态

V₀：仿真轨迹中设定的载体初始速度

w_L：激光测速仪的速度噪声

w_a：加速度计噪声

w_g：陀螺噪声 

具体实施方式

见图1—图3，本发明一种惯导/激光测速仪组合导航的半实物仿真方法，该方法具体步骤如下：

步骤1、将惯导/激光测速仪组合***安装到载体上，并上电启动。

步骤2、装订初始参数(包括初始的经度、纬度、高度、激光测速仪标度值、激光测速仪安装角、惯导安装角)至导航计算机。

步骤3、保持载体静止，采集保存1小时陀螺和加速度计的输出数据。求取陀螺和加速度计的输出数据的平均值，将采集的陀螺和加速度计数据分别与其平均值作差，得到陀螺和加速度计的实际噪声。

步骤4、载体保持静止，惯导进行5分钟静态初始对准。

步骤5、完成对准后载体开始运动，运动过程中惯导进行惯性导航计算，采集并保存开始运动后50秒内惯导输出的速度值、惯导输出的姿态矩阵、激光测速仪输出的速度。

步骤6、惯导输出的速度为东北天坐标系下的速度，由惯导输出姿态矩阵将东北天坐标系下的速度转换到惯组坐标系下，再根据惯导与载体、激光测速仪与载体之间的位置关系，把惯组坐标系下的速度投影到激光测速仪的速度方向上得到参考速度。将激光测速仪的速度与参考速度作差，得到激光测速仪输出速度的噪声值。

步骤7、设定仿真轨迹(包含载体的运动速度、位置、姿态)。由设定仿真轨迹中载体的姿态得到仿真载体的姿态矩阵，根据得到的姿态矩阵和仿真轨迹中载体的运动速度得到载体在载体系下的速度分量，再由载体的速度分量和给定的激光测速仪的安装角、惯导安装角计算得到激光测速仪的速度标准值；利用轨迹发生器生成加速度计的标准值、陀螺的标准值；并给定加速度计和陀螺的标度误差、零偏、失准角误差；给定激光测速仪的标度误差、安装角误差；给定惯导的安装角误差。将这些数据与步骤3、步骤6中得到的噪声数据融合，得到用于惯导/激光测速仪组合导航半实物仿真的器件模拟数据，然后采用这些模拟数据进行惯导/激光测速仪组合导航仿真。

其中，步骤3中所述的“得到陀螺、加速度计的实际噪声”，其实现过程说明如下：

定义w_g为陀螺噪声，w_a为加速度计噪声，采集陀螺x、y、z三个轴输出的数据分别为ω_x(1)、ω_x(2)…ω_x(n)，ω_y(1)、ω_y(2)…ω_y(n)，ω_z(1)、ω_z(2)…ω_z(n)，平均值分别为采集加速度计x、y、z三个轴输出的数据分别为a_x(1)、a_x(2)…a_x(n)，a_y(1)、a_y(2)…a_y(n)，a_z(1)、a_z(2)…a_z(n)，平均值分别为

陀螺噪声计算公式如下：

$w_g\left(i\right)=\left[\begin{array}{c}{w}_{\mathrm{gx}}\left(i\right)\\ w_{\mathrm{gy}}\left(i\right)\\ w_{\mathrm{gz}}\left(i\right)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x\left(i\right)\\ {\mathrm{\ω}}_y\left(i\right)\\ {\mathrm{\ω}}_z\left(i\right)\end{array}\right]-\left[\begin{array}{c}{\stackrel{\‾}{\mathrm{\ω}}}_x\\ {\stackrel{\‾}{\mathrm{\ω}}}_y\\ {\stackrel{\‾}{\mathrm{\ω}}}_z\end{array}\right],(i=1...n)$

加速度计噪声计算公式如下：

$w_a\left(i\right)=\left[\begin{array}{c}{w}_{\mathrm{ax}}\left(i\right)\\ w_{\mathrm{ay}}\left(i\right)\\ w_{\mathrm{az}}\left(i\right)\end{array}\right]=\left[\begin{array}{c}{a}_x\left(i\right)\\ a_y\left(i\right)\\ a_z\left(i\right)\end{array}\right]-\left[\begin{array}{c}{\stackrel{\‾}{a}}_x\\ {\stackrel{\‾}{a}}_y\\ {\stackrel{\‾}{a}}_z\end{array}\right],(i=1...n)$

其中，步骤6中所述的“得到激光测速仪输出速度的噪声值”，其实现过程说明如下：

载体开始运动，采集得到了50秒内激光测速仪的速度输出惯导输出东北天坐标系下速度 $V_E^n\left(1\right),V_E^n\left(2\right)...V_E^n\left(n\right),V_N^n\left(1\right),V_N^n\left(2\right)...V_N^n\left(n\right),V_U^n\left(n\right),V_U^n\left(1\right),V_U^n\left(2\right)...V_U^n\left(n\right),$ 惯导输出的姿态矩阵导航计算机装订的激光测速仪的安装角为θ，惯导的安装角为α、β、γ；惯导在惯组坐标系下的速度输出分别为V_x(1)、V_x(2)…V_x(n)，V_y(1)、V_y(2)…V_y(n)，V_z(1)、V_z(2)…V_z(n)。惯导在惯组坐标系下的速度投影到激光测速仪的速度方向上的参考速度为惯导在东北天坐标系下输出的速度转到惯组坐标系下，计算公式如下：

$\left[\begin{array}{c}{V}_i\left(i\right)\\ V_y\left(i\right)\\ V_z\left(i\right)\end{array}\right]=C_n^b\left(i\right)\left[\begin{array}{c}{V}_E^n\left(i\right)\\ V_N^n\left(i\right)\\ V_U^n\left(i\right)\end{array}\right](i=1...n)$

惯导在惯组坐标系下的速度投影到激光测速仪的速度方向上的参考速度为V^m(i)，计算公式如下：

V^m(i)＝[V_x(i)cosα+V_y(i)cosβ+V_z(i)cosγ]cosθ    (i＝1...n)

激光测速仪输出的速度噪声值为w_L(i)，计算公式如下：

$w_L\left(i\right)=V_L^m\left(i\right)-V^m\left(i\right),(i=1...n)$

其中，步骤7中所述的“得到用于惯导/激光测速仪组合导航半实物仿真的器件模拟数据，然后采用这些模拟数据进行惯导/里程计组合导航仿真”，其实现过程说明如下：

设定仿真轨迹(包含载体的运动速度、位置、姿态)。由设定仿真轨迹中载体的姿态得到仿真载体的姿态矩阵，根据得到的姿态矩阵和仿真轨迹中载体的运动速度得到载体在载体系下的速度分量，再由载体的速度分量和给定的激光测速仪的安装角、惯导安装角计算得到激光测速仪的速度标准值；利用轨迹发生器生成加速度计的标准值、陀螺的标准值；并给定加速度计和陀螺的标度误差、零偏、失准角误差；给定激光测速仪的标度误差、安装角误差；给定惯导的安装角误差。将这些数据与步骤3、步骤6中得到的噪声数据融合，得到用于惯导/激光测速仪组合导航半实物仿真的器件模拟数据，然后采用这些模拟数据进行惯导/里程计组合导航仿真。

定义：仿真轨迹中导航坐标系下载体运动速度向量为Vⁿ，在导航坐标系下三个方向的分量分别为仿真轨迹中载体的运动速度在载体系下x、y、z三个轴的分量 分别为由仿真轨迹中载体的姿态得到的载体姿态矩阵为给定激光测速仪的安装角θ_f，激光测速仪安装角误差为δθ_f，惯导的安装角分别为α_b、β_b、γ_b以及惯导的安装角误差分别为δα_b、δβ_b、δ_γb，激光测速仪的标度误差值δK_L；加速度计x、y、z三个轴的标准值分别为a_tx(1)、a_tx(2)…a_tx(n)，a_ty(1)、a_ty(2)…a_ty(n)，a_tz(1)、a_tz(2)…a_tz(n)；加速度计x、y、z三个轴的标度误差分别为δK_ax、δK_ay、δK_az；加速度计的失准角误差为δM_axy、δM_axz、δM_ayx、δM_ayz、δM_azx、δM_azy；加速度计x、y、z三个轴的零偏分别为δB_ax、δB_ay、δB_az；陀螺x、y、z三个轴的标准值分别为ω_tx(1)、ω_tx(2)…ω_tx(n)，ω_ty(1)、ω_ty(2)…ω_ty(n)，ω_tz(1)、ω_tz(2)…ω_tz(n)；陀螺x、y、z三个轴的标度误差分别为δK_gx、δK_gy、δK_gz；陀螺的失准角误差为δM_gxy、δM_gxz、δM_gyx、δM_gyz、δM_gzx、δM_gzy；陀螺x、y、z三个轴的零偏分别为δB_gx、δB_gy、δB_gz。陀螺x、y、z三个轴的噪声分别为w_gx(1)、w_gx(2)…w_gx(n)，w_gy(1)、w_gy(2)…w_gy(n)，w_gz(1)、w_gz(2)…w_gz(n)；加速度计x、y、z三个轴的噪声分别为w_ax(1)、w_ax(2)…w_ax(n)，w_ay(1)、w_ay(2)…w_ay(n)，w_az(1)、w_az(2)…w_az(n)；激光测速仪的速度噪声为w_L(1)、w_L(2)…w_L(n)。

仿真轨迹中载体在导航系下的速度转换到载体系下x、y、z三个轴的速度计算如下：

$\left[\begin{array}{c}{v}_x^b\left(i\right)\\ v_y^b\left(i\right)\\ v_z^b\left(i\right)\end{array}\right]=T_n^b\left(i\right)\left[\begin{array}{c}{v}_x^n\left(i\right)\\ v_y^n\left(i\right)\\ v_z^n\left(i\right)\end{array}\right](i=1...n)$

计算得到激光测速仪的速度标准值为计算公式如下：

$V_{\mathrm{Lt}}^m\left(i\right)=[v_x^b\left(i\right)\cos {\mathrm{\α}}_f+v_y^b\left(i\right)\cos {\mathrm{\β}}_f+v_z^b\left(i\right)\cos {\mathrm{\γ}}_f]\cos {\mathrm{\θ}}_f(i=1...n)$

由计算得到激光测速仪的速度标准值、给定的激光测速仪的标度误差值、安装角误差、惯导安装角误差以及提取的激光测速仪的速度噪声，综合得到用于组合导航半实物仿真的激光测速仪的速度计算公式如下：

$\begin{array}{c}{V}_{\mathrm{Lf}}^m\left(i\right)=(1+\mathrm{\δ}{K}_L)[v_x^b\left(i\right)\cos ({\mathrm{\α}}_f+\mathrm{\δ}{\mathrm{\α}}_f)+v_y^b\left(i\right)\cos ({\mathrm{\β}}_f+\mathrm{\δ}{\mathrm{\β}}_f)+v_z^b\left(i\right)\cos ({\mathrm{\γ}}_f+\mathrm{\δ}{\mathrm{\γ}}_f)]\cos ({\mathrm{\θ}}_f+\mathrm{\δ}{\mathrm{\θ}}_f)+w_L\left(i\right)\\ (i=1...n)\end{array}$

由计算得到的陀螺、加速度计标准值，给定的陀螺、加速度计零偏、标度误差、失准角误差以及提取的陀螺、加速度计的噪声，综合得到用于组合导航半实物仿真的陀螺、加速度计数据计算公式如下：

${\mathrm{\ω}}_f\left(i\right)=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{fx}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{fy}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{fz}}\left(i\right)\end{array}\right]=\left[\begin{array}{ccc}1+\mathrm{\δ}{K}_{\mathrm{gx}}& \mathrm{\δ}{M}_{\mathrm{gxy}}& \mathrm{\δ}{M}_{\mathrm{gxz}}\\ {\mathrm{\δM}}_{\mathrm{gyx}}& 1+\mathrm{\δ}{K}_{\mathrm{gy}}& \mathrm{\δ}{M}_{\mathrm{gyz}}\\ \mathrm{\δ}{M}_{\mathrm{gzx}}& \mathrm{\δ}{M}_{\mathrm{gzy}}& 1+\mathrm{\δ}{K}_{\mathrm{gz}}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{tx}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{ty}}\left(i\right)\\ {\mathrm{\ω}}_{\mathrm{tz}\left(i\right)}\end{array}\right]+\left[\begin{array}{c}\mathrm{\δ}{B}_{\mathrm{gx}}\\ {\mathrm{\δB}}_{\mathrm{gy}}\\ {\mathrm{\δB}}_{\mathrm{gz}}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{gx}}\left(i\right)\\ w_{\mathrm{gy}}\left(i\right)\\ w_{\mathrm{gz}}\left(i\right)\end{array}\right](i=1...n)$

$a_f\left(i\right)=\left[\begin{array}{c}{a}_{\mathrm{fx}}\left(i\right)\\ a_{\mathrm{fy}}\left(i\right)\\ a_{\mathrm{fz}}\left(i\right)\end{array}\right]=\left[\begin{array}{ccc}1+\mathrm{\δ}{K}_{\mathrm{ax}}& \mathrm{\δ}{M}_{\mathrm{axy}}& \mathrm{\δ}{M}_{\mathrm{axz}}\\ {\mathrm{\δM}}_{\mathrm{ayx}}& 1+\mathrm{\δ}{K}_{\mathrm{ay}}& \mathrm{\δ}{M}_{\mathrm{ayz}}\\ \mathrm{\δ}{M}_{\mathrm{azx}}& \mathrm{\δ}{M}_{\mathrm{azy}}& 1+\mathrm{\δ}{K}_{\mathrm{az}}\end{array}\right]\left[\begin{array}{c}{a}_{\mathrm{tx}}\left(i\right)\\ a_{\mathrm{ty}}\left(i\right)\\ a_{\mathrm{tz}\left(i\right)}\end{array}\right]+\left[\begin{array}{c}\mathrm{\δ}{B}_{\mathrm{ax}}\\ {\mathrm{\δB}}_{\mathrm{ay}}\\ {\mathrm{\δB}}_{\mathrm{az}}\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{ax}}\left(i\right)\\ w_{\mathrm{ay}}\left(i\right)\\ w_{\mathrm{az}}\left(i\right)\end{array}\right](i=1...n)$

采用上述得到的激光测速仪模拟数据加速度计模拟数据a_f、陀螺模拟数据ω_f，结合由仿真轨迹中给定的载体初始位置、初始姿态、初始速度，进行组合导航半实物仿真。

Claims (4)

1.一种惯导/激光测速仪组合导航的半实物仿真方法，其特征在于：该方法具体步骤如下：

步骤1、将惯导/激光测速仪组合***安装到载体上，并上电启动；

步骤2、装订初始参数，包括初始的经度、纬度、高度、激光测速仪标度值、激光测速仪安装角、惯导安装角至导航计算机；

步骤3、保持载体静止，采集保存1小时陀螺和加速度计的输出数据；求取陀螺和加速度计的输出数据的平均值，将采集的陀螺和加速度计数据分别与其平均值作差，得到陀螺和加速度计的实际噪声；

步骤4、载体保持静止，惯导进行5分钟静态初始对准；

步骤5、完成对准后载体开始运动，运动过程中惯导进行惯性导航计算，采集并保存开始运动后50秒内惯导输出的速度值、惯导输出的姿态矩阵、激光测速仪输出的速度；

步骤6、惯导输出的速度为东北天坐标系下的速度，由惯导输出姿态矩阵将东北天坐标系下的速度转换到惯组坐标系下，再根据惯导与载体、激光测速仪与载体之间的位置关系，把惯组坐标系下的速度投影到激光测速仪的速度方向上得到参考速度；将激光测速仪的速度与参考速度作差，得到激光测速仪输出速度的噪声值；

步骤7、设定仿真轨迹，包含载体的运动速度、位置、姿态，由设定仿真轨迹中载体的姿态得到仿真载体的姿态矩阵，根据得到的姿态矩阵和仿真轨迹中载体的运动速度得到载体在载体系下的速度分量，再由载体的速度分量和给定的激光测速仪的安装角、惯导安装角计算得到激光测速仪的速度标准值；利用轨迹发生器生成加速度计的标准值、陀螺的标准值；并给定加速度计和陀螺的标度误差、零偏、失准角误差；给定激光测速仪的标度误差、安装角误差；给定惯导的安装角误差；将这些数据与步骤3、步骤6中得到的噪声数据融合，得到用于惯导/激光测速仪组合导航半实物仿真的器件模拟数据，然后采用这些模拟数据进行惯导/激光测速仪组合导航仿真。

2.根据权利要求1所述的一种惯导/激光测速仪组合导航的半实物仿真方法，其特征在于：

步骤3中所述的“得到陀螺、加速度计的实际噪声”，其实现过程说明如下：

定义w_g为陀螺噪声，w_a为加速度计噪声，采集陀螺x、y、z三个轴输出的数据分别为ω_x(1)、 ω_x(2)…ω_x(n)，ω_y(1)、ω_y(2)…ω_y(n)，ω_z(1)、ω_z(2)…ω_z(n)，平均值分别为采集加速度计x、y、z三个轴输出的数据分别为a_x(1)、a_x(2)…a_x(n)，a_y(1)、a_y(2)…a_y(n)，a_z(1)、a_z(2)…a_z(n)，平均值分别为

陀螺噪声计算公式如下：

加速度计噪声计算公式如下：

3.根据权利要求1所述的一种惯导/激光测速仪组合导航的半实物仿真方法，其特征在于：步骤6中所述的“得到激光测速仪输出速度的噪声值”，其实现过程说明如下：

载体开始运动，采集得到了50秒内激光测速仪的速度输出惯导输出东北天坐标系下速度惯导输出的姿态矩阵导航计算机装订的激光测速仪的安装角为θ，惯导的安装角为α、β、γ；惯导在惯组坐标系下的速度输出分别为V_x(1)、V_x(2)…V_x(n)，V_y(1)、V_y(2)…V_y(n)，V_z(1)、V_z(2)…V_z(n)；惯导在惯组坐标系下的速度投影到激光测速仪的速度方向上的参考速度为惯导在东北天坐标系下输出的速度转到惯组坐标系下，计算公式如下：

惯导在惯组坐标系下的速度投影到激光测速仪的速度方向上的参考速度为V^m(i)，计算公式如下：

V^m(i)＝[V_x(i)cosα+V_y(i)cosβ+V_z(i)cosγ]cosθ   (i＝1...n)；

激光测速仪输出的速度噪声值为w_L(i)，计算公式如下：

4.根据权利要求1所述的一种惯导/激光测速仪组合导航的半实物仿真方法，其特征在于：步骤7中所述的“得到用于惯导/激光测速仪组合导航半实物仿真的器件模拟数据，然后采用这些模拟数据进行惯导/里程计组合导航仿真”，其实现过程说明如下：

设定仿真轨迹，包含载体的运动速度、位置、姿态，由设定仿真轨迹中载体的姿态得到仿真载体的姿态矩阵，根据得到的姿态矩阵和仿真轨迹中载体的运动速度得到载体在载体系下的速度分量，再由载体的速度分量和给定的激光测速仪的安装角、惯导安装角计算得到激光测速仪的速度标准值；利用轨迹发生器生成加速度计的标准值、陀螺的标准值；并给定加速度计和陀螺的标度误差、零偏、失准角误差；给定激光测速仪的标度误差、安装角误差；给定惯导的安装角误差；将这些数据与步骤3、步骤6中得到的噪声数据融合，得到用于惯导/激光测速仪组合导航半实物仿真的器件模拟数据，然后采用这些模拟数据进行惯导/里程计组合导航仿真；

定义：仿真轨迹中导航坐标系下载体运动速度向量为Vⁿ，在导航坐标系下三个方向的分量分别为仿真轨迹中载体的运动速度在载体系下x、y、z三个轴的分量分别为由仿真轨迹中载体的姿态得到的载体姿态矩阵为给定激光测速仪的安装角θ_f，激光测速仪安装角误差为δθ_f，惯导的安装角分别为α_b、β_b、γ_b以及惯导的安装角误差分别为δα_b、δβ_b、δγ_b，激光测速仪的标度误差值δK_L；加速度计x、y、z三个轴的标准值分别为a_tx(1)、a_tx(2)…a_tx(n)，a_ty(1)、a_ty(2)…a_ty(n)，a_tz(1)、a_tz(2)…a_tz(n)；加速度计x、y、z三个轴的标度误差分别为δK_ax、δK_ay、δK_az；加速度计的失准角误差为δM_axy、δM_axz、δM_ayx、δM_ayz、δM_azx、δM_azy；加速度计x、y、z三个轴的零偏分别为δB_ax、δB_ay、δB_az；陀螺x、y、z三个轴的标准值分别为ω_tx(1)、ω_tx(2)…ω_tx(n)，ω_ty(1)、ω_ty(2)…ω_ty(n)，ω_tz(1)、ω_tz(2)…ω_tz(n)；陀螺x、y、z三个轴的标度误差分别为δK_gx、δK_gy、δK_gz；陀螺的失准角误差为δM_gxy、δM_gxz、δM_gyx、δM_gyz、δM_gzx、δM_gzy；陀螺x、y、z三个轴的零偏分别为δB_gx、δB_gy、δB_gz；陀螺x、y、z三个轴的噪声分别为w_gx(1)、w_gx(2)…w_gx(n)，w_gy(1)、w_gy(2)…w_gy(n)，w_gz(1)、w_gz(2)…w_gz(n)；加速度计x、y、z三个轴的噪声分别为w_ax(1)、w_ax(2)…w_ax(n)，w_ay(1)、w_ay(2)…w_ay(n)，w_az(1)、w_az(2)…w_az(n)；激光测速仪的速度噪声为w_L(1)、w_L(2)…w_L(n)；

仿真轨迹中载体在导航系下的速度转换到载体系下x、y、z三个轴的速度计算如下：

计算得到激光测速仪的速度标准值为计算公式如下：

由计算得到激光测速仪的速度标准值、给定的激光测速仪的标度误差值、安装角误差、惯导安装角误差以及提取的激光测速仪的速度噪声，综合得到用于组合导航半实物仿真的激光测速仪的速度计算公式如下：

由计算得到的陀螺、加速度计标准值，给定的陀螺、加速度计零偏、标度误差、失准角误差以及提取的陀螺、加速度计的噪声，综合得到用于组合导航半实物仿真的陀螺、加速度计数据计算公式如下：

采用上述得到的激光测速仪模拟数据加速度计模拟数据a_f、陀螺模拟数据ω_f，结合由仿真轨迹中给定的载体初始位置、初始姿态、初始速度，进行组合导航半实物仿真。