CN102706365B - 一种基于导航***的三波束激光测速仪标定方法 - Google Patents

Abstract

一种基于导航***的三波束激光测速仪标定方法，有七大步骤：一、将惯组、差分GPS和激光测速仪安装到载体上，装订初始位置参数至导航计算机；二、捷联惯组预热，采集陀螺和加速度计的输出数据；三、对采集到的输出数据进行预处理；四、启动差分GPS和激光测速仪，***由对准模式切换到SINS/DGPS模式，载体开始运动；五、采集三个不同时刻的载体不同运动状态下测速仪的输出及SINS/DGPS组合导航***输出；六、利用最小二乘法分别辨识三个激光测速仪的安装角及标度因数误差；七、标定精度评价分析。本发明利用捷联惯导与组合导航***输出速度精度高的特点对三波束激光测速仪实现精确标定，它在惯性导航技术领域里具有实用价值。

Description

一种基于导航***的三波束激光测速仪标定方法

技术领域：

本发明涉及一种基于导航***的三波束激光测速仪标定方法，属于惯性导航技术/组合导航技术领域。 

背景技术：

激光测速仪作为一种速度传感器，具有完全自主、精度高、测速范围宽、动态性能好及非接触测量的优点。单独的激光测速仪不具备导航定位功能，但是与惯导***组合能优势互补，能实现全自主、高精度导航定位。 

组合导航***实际使用中惯导***和三波束激光测速仪分别装在载体的不同位置，需要标定测速仪安装角，同时实验室中测得的激光测速仪标度因数在实际使用中会有变化，需要标定标度因数误差。目前公开文献中用于导航定位领域的激光测速仪并没有统一的标定方法，本文提出了一种三波束激光测速仪安装角和标度因数误差的标定方法，解决了工程实际中激光测速仪与惯导组合***导航定位的基础问题。 

发明内容：

1、目的：本发明的目的是提供了一种基于导航***的三波束激光测速仪标定方法，它克服了现有技术的不足，解决了激光测速仪装到载体上时需要标定安装角和标度因数误差的问题。 

2、技术方案：本发明一种基于导航***的三波束激光测速仪标定方法，该方法具体步骤如下： 

步骤1、将捷联惯导***、差分GPS和激光测速仪安装到载体上，装订初始位置参数（包括初始的经度、纬度和高度）至惯导的导航计算机。 

步骤2、捷联惯导预热，然后采集陀螺和加速度计的输出数据。 

步骤3、对采集到的陀螺和加速度计数据进行处理，根据捷联惯导***误差传播特性和古典控制理论，采用二阶调平和方位估算法来完成***的粗对准，初步确定载体姿态角。粗对准时间为1分钟。粗对准后利用卡尔曼滤波技术精对准5分钟。 

步骤4、启动差分GPS和激光测速仪，惯导***由对准模式切换到SINS/DGPS（捷联惯导***/差分GPS）组合导航模式（组合算法框图见图1），切换完成后载体开始运动。 

步骤5、采取载体不同运动状态下测速仪的输出及SINS/DGPS组合导航***输出，至少采三个不同时刻的输出。 

步骤6、利用最小二乘法分别辨识激光测速仪三个波束的安装角及标度因数误差。 

步骤7、标定精度评价分析。 

步骤1—7分为三个阶段，步骤1—3为准备阶段，步骤4—6为激光测速仪标定阶段（标定方法框图见图2），步骤7为标定结果评价阶段。 

其中，步骤6中所述的最小二乘法辨识三波束激光测速仪的安装角和标度因数误差，具体实现过程说明如下： 

以三波束激光测速仪中某波束为例介绍它的安装角和标度因数误差标定方法，另外两个波束的安装角和标度因数误差标定方法与此相同。 

定义V_L为激光测速仪输出， 分别为SINS/DGPS在载体系x、y、z三个轴向上的输出，δK为激光测速仪标度因数误差，α、β、γ分别为激光测速仪与载体系x、y、z三个轴向的夹角。测得n个点（n≥3）单波束测速仪输出V_L(1)、V_L(2)…V_L(n)及n个点（n≥3）SINS/DGPS输出 

由 $V_L=(1+\mathrm{\δK})(V_x^b\cos \mathrm{\α}+V_y^b\cos \mathrm{\β}+V_z^b\cos \mathrm{\γ})$ 可得 

$\left[\begin{array}{c}{V}_L\left(1\right)\\ V_L\left(2\right)\\ \·\\ \·\\ \·\\ V_L\left(n\right)\end{array}\right]=\left[\begin{array}{ccc}{V}_x^b\left(1\right)& V_y^b\left(1\right)& V_z^b\left(1\right)\\ V_x^b\left(2\right)& V_y^b\left(2\right)& V_z^b\left(2\right)\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ V_x^b\left(n\right)& V_y^b\left(n\right)& V_z^b\left(n\right)\end{array}\right]\left[\begin{array}{c}\cos \mathrm{\α}\\ \cos \mathrm{\β}\\ \cos \mathrm{\γ}\end{array}\right][1+\mathrm{\δK}]$

令 $Z=\left[\begin{array}{c}{V}_L\left(1\right)\\ V_L\left(2\right)\\ \·\\ \·\\ \·\\ V_L\left(n\right)\end{array}\right],$ $H=\left[\begin{array}{ccc}{V}_x^b\left(1\right)& V_y^b\left(1\right)& V_z^b\left(1\right)\\ V_x^b\left(2\right)& V_y^b\left(2\right)& V_z^b\left(2\right)\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ V_x^b\left(n\right)& V_y^b\left(n\right)& V_z^b\left(n\right)\end{array}\right],$ $X=\left[\begin{array}{c}\cos \mathrm{\α}\\ \cos \mathrm{\β}\\ \cos \mathrm{\γ}\end{array}\right][1+\mathrm{\δK}]$

由最小二乘估计公式 即可解得X的最小二乘估计 

由cos²α+cos²β+cos²γ=1得 

$\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2}=\sqrt{{(1+\mathrm{\δK})}^2({\cos }^2\mathrm{\α}+{\cos }^2\mathrm{\β}+{\cos }^2\mathrm{\γ})}=1+\mathrm{\δK},$ 因此  $\mathrm{\δK}=\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2}-1$

$\mathrm{\α}=\arccos (\hat{X}\left(1\right)/\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2})$

$\mathrm{\β}=\arccos (\hat{X}\left(2\right)/\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2})$

$\mathrm{\γ}=\arccos (\hat{X}\left(3\right)/\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2})$

其中，步骤7中所述的标定精度评价分析，具体实现过程说明如下： 

将标定结果δK、α、β、γ代入 

${\hat{V}}_L=(1+\mathrm{\δK})(V_x^b\cos \mathrm{\α}+V_y^b\cos \mathrm{\β}+V_z^b\cos \mathrm{\γ}),$ 得 

通过下列标准差计算公式计算 与测速仪实际输出值V_L(1)、V_L(2)…V_L(n)之间的离散程度,由此判断标定结果的精确度。 

$\mathrm{\σ}=\sqrt{({\left[\widehat{V_L\left(1\right)-V_L\left(1\right)}\right]}^2+{\left[\widehat{V_L\left(2\right)-V_L\left(2\right)}\right]}^2+\·\·\·+{\left[\widehat{V_L\left(n\right)-V_L\left(n\right)}\right]}^2)/(n-1)}.$

3、优点及功效：本发明一种基于导航***的三波束激光测速仪标定方法，该方法的优点是利用捷联惯导与差分GPS组合导航***输出速度精度高的特点对三波束激光测速仪实现精确标定，解决了三波束激光测速仪安装角与标度误差难以测试的问题,为惯导***/激光测速仪组合导航精度的提高提供了基础。 

附图说明

图1为SINS/DGPS组合算法框图； 

图2为激光测速仪标定方法框图； 

图3为本发明三波束激光测速仪标定方法的流程图。 

图中符号说明如下： 

DGPS：差分GPS 

V^t：组合导航***输出的地理系下的速度 

V^b：组合导航***输出的载体系下的速度 

V_L：激光测速仪输出的速度 

α、β、γ：激光测速仪波束与载体系x、y、z三个轴向的夹角 

δK：激光测速仪标度因数误差 

SINS/DGPS：捷联惯导***/差分GPS 

具体实施方式：

见图3，本发明一种基于导航***的三波束激光测速仪标定方法，该方法具体步骤如下：

步骤1、将捷联惯导***、差分GPS和激光测速仪安装到载体上，装订初始位置参数（包括初始的经度、纬度和高度）至惯导的导航计算机。 

步骤2、捷联惯导预热，然后采集陀螺和加速度计的输出数据。 

步骤3、对采集到的陀螺和加速度计数据进行处理，根据捷联惯导***误差传播特性和古典控制理论，采用二阶调平和方位估算法来完成***的粗对准，初步确定载体姿态角。粗对准时间为1分钟。粗对准后利用卡尔曼滤波技术精对准5分钟。 

步骤4、启动差分GPS和激光测速仪，惯导***由对准模式切换到SINS/DGPS组合导航模式（组合算法框图见图1），切换完成后载体开始运动。 

步骤5、采取载体不同运动状态下测速仪的输出及SINS/DGPS组合导航***输出，至少采三个不同时刻的输出。 

步骤6、利用最小二乘法分别辨识激光测速仪三个波束的安装角及标度因数误差。 

步骤7、标定精度评价分析。 

可将步骤1—7分为三个阶段，步骤1—3为准备阶段，步骤4—6为激光测速仪标定阶段（标定方法框图见图2），步骤7为标定结果评价阶段。 

其中，步骤6中所述的最小二乘法辨识三波束激光测速仪的安装角和标度因数误差，具体实现过程说明如下： 

以三波束激光测速仪中某波束为例介绍它的安装角和标度因数误差标定方法，另外两个波束的安装角和标度因数误差标定方法与此相同。 

定义V_L为激光测速仪输出， 分别为SINS/DGPS在载体系x、y、z三个轴向上的输出，δK为激光测速仪标度因数误差，α、β、γ分别为激光测速仪与载体系x、y、z三个轴向的夹角。 

测得n个点（n≥3）单波束测速仪输出V_L(1)、V_L(2)…V_L(n)及n个点（n≥3）SINS/DGPS输出 由  $V_L=(1+\mathrm{\δK})(V_x^b\cos \mathrm{\α}+V_y^b\cos \mathrm{\β}+V_z^b\cos \mathrm{\γ})$ 可得 

$\left[\begin{array}{c}{V}_L\left(1\right)\\ V_L\left(2\right)\\ \·\\ \·\\ \·\\ V_L\left(n\right)\end{array}\right]=\left[\begin{array}{ccc}{V}_x^b\left(1\right)& V_y^b\left(1\right)& V_z^b\left(1\right)\\ V_x^b\left(2\right)& V_y^b\left(2\right)& V_z^b\left(2\right)\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ V_x^b\left(n\right)& V_y^b\left(n\right)& V_z^b\left(n\right)\end{array}\right]\left[\begin{array}{c}\cos \mathrm{\α}\\ \cos \mathrm{\β}\\ \cos \mathrm{\γ}\end{array}\right][1+\mathrm{\δK}]$

令 $Z=\left[\begin{array}{c}{V}_L\left(1\right)\\ V_L\left(2\right)\\ \·\\ \·\\ \·\\ V_L\left(n\right)\end{array}\right],$ $H=\left[\begin{array}{ccc}{V}_x^b\left(1\right)& V_y^b\left(1\right)& V_z^b\left(1\right)\\ V_x^b\left(2\right)& V_y^b\left(2\right)& V_z^b\left(2\right)\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ V_x^b\left(n\right)& V_y^b\left(n\right)& V_z^b\left(n\right)\end{array}\right],$ $X=\left[\begin{array}{c}\cos \mathrm{\α}\\ \cos \mathrm{\β}\\ \cos \mathrm{\γ}\end{array}\right][1+\mathrm{\δK}]$

由最小二乘估计公式 即可解得X的最小二乘估计 

由cos²α+cos²β+cos²γ=1得 

$\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2}=\sqrt{{(1+\mathrm{\δK})}^2({\cos }^2\mathrm{\α}+{\cos }^2\mathrm{\β}+{\cos }^2\mathrm{\γ})}=1+\mathrm{\δK},$ 因此  $\mathrm{\δK}=\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2}-1$

$\mathrm{\α}=\arccos (\hat{X}\left(1\right)/\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2})$

$\mathrm{\β}=\arccos (\hat{X}\left(2\right)/\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2})$

$\mathrm{\γ}=\arccos (\hat{X}\left(3\right)/\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2})$

其中，步骤7中所述的标定精度评价分析，具体实现过程说明如下： 

将标定结果δK、α、β、γ代入 

${\hat{V}}_L=(1+\mathrm{\δK})(V_x^b\cos \mathrm{\α}+V_y^b\cos \mathrm{\β}+V_z^b\cos \mathrm{\γ}),$ 得 

通过标准差计算公式（下式）计算测速仪应敏感到的速度值 与测速仪实际输出值V_L(1)、V_L(2)…V_L(n)之间的离散程度,由此判断标定结果的精确度。 

$\mathrm{\σ}=\sqrt{({\left[\widehat{V_L\left(1\right)-V_L\left(1\right)}\right]}^2+{\left[\widehat{V_L\left(2\right)-V_L\left(2\right)}\right]}^2+\·\·\·+{\left[\widehat{V_L\left(n\right)-V_L\left(n\right)}\right]}^2)/(n-1)}.$ 。

Claims (1)

1.一种基于导航***的三波束激光测速仪标定方法，其特征在于：该方法具体步骤如下：

步骤1、将捷联惯导***、差分GPS和激光测速仪安装到载体上，装订初始位置参数即初始的经度、纬度和高度至惯导的导航计算机；

步骤2、捷联惯导预热，然后采集陀螺和加速度计的输出数据；

步骤3、对采集到的陀螺和加速度计数据进行处理，根据捷联惯导***误差传播特性和古典控制理论，采用二阶调平和方位估算法来完成***的粗对准，初步确定载体姿态角；粗对准时间为1分钟，粗对准后利用卡尔曼滤波技术精对准5分钟；

步骤4、启动差分GPS和激光测速仪，惯导***由对准模式切换到SINS/DGPS即捷联惯导***/差分GPS组合导航模式，切换完成后载体开始运动；

步骤5、采集载体不同运动状态下测速仪的输出及SINS/DGPS组合导航***输出，至少采三个不同时刻的输出；

步骤6、利用最小二乘法分别辨识激光测速仪三个波束的安装角及标度因数误差；

步骤7、标定精度评价分析；

其中，步骤6中所述的最小二乘法辨识三波束激光测速仪的安装角和标度因数误差，具体实现过程说明如下：

以三波束激光测速仪中某波束为例介绍它的安装角和标度因数误差标定方法，另外两个波束的安装角和标度因数误差标定方法与此相同；

定义V_L为激光测速仪输出，分别为SINS/DGPS在载体系x、y、z三个轴向上的输出，δK为激光测速仪标度因数误差，α、β、γ分别为激光测速仪与载体系x、y、z三个轴向的夹角；测得n个点单波束测速仪输出V_L(1)、V_L(2)…V_L(n)及n个点SINS/DGPS输出其中，n≥3；

由 $V_L=(1+\mathrm{\δK})(V_x^b\cos \mathrm{\α}+V_y^b\cos \mathrm{\β}+V_z^b\cos \mathrm{\γ})$ 得

$\left[\begin{array}{c}{V}_L\left(1\right)\\ V_L\left(2\right)\\ \·\\ \·\\ \·\\ V_L\left(n\right)\end{array}\right]=\left[\begin{array}{ccc}{V}_x^b\left(1\right)& V_y^b\left(1\right)& V_z^b\left(1\right)\\ V_x^b\left(2\right)& V_y^b\left(2\right)& V_z^b\left(2\right)\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ V_x^b\left(n\right)& V_y^b\left(n\right)& V_z^b\left(n\right)\end{array}\right]=\left[\begin{array}{c}\cos \mathrm{\α}\\ \cos \mathrm{\β}\\ \cos \mathrm{\γ}\end{array}\right][1+\mathrm{\δK}]$

令 $Z=\left[\begin{array}{c}{V}_L\left(1\right)\\ V_L\left(2\right)\\ \·\\ \·\\ \·\\ V_L\left(n\right)\end{array}\right],H=\left[\begin{array}{ccc}{V}_x^b\left(1\right)& V_y^b\left(1\right)& V_z^b\left(1\right)\\ V_x^b\left(2\right)& V_y^b\left(2\right)& V_z^b\left(2\right)\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ V_x^b\left(n\right)& V_y^b\left(n\right)& V_z^b\left(n\right)\end{array}\right],X=\left[\begin{array}{c}\cos \mathrm{\α}\\ \cos \mathrm{\β}\\ \cos \mathrm{\γ}\end{array}\right][1+\mathrm{\δK}]$

由最小二乘估计公式即解得X的最小二乘估计；

由cos²α+cos²β+cos²γ＝1得

$\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2}=\sqrt{{(1+\mathrm{\δK})}^2({\cos }^2\mathrm{\α}+{\cos }^2\mathrm{\β}+{\cos }^2\mathrm{\γ})}=1+\mathrm{\δK}$ ，因此

$\mathrm{\δK}=\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2}-1$

$\mathrm{\α}=\arccos (\hat{X}\left(1\right)/\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2})$

$\mathrm{\β}=\arccos (\hat{X}\left(1\right)/\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2})$

$\mathrm{\γ}=\arccos (\hat{X}\left(1\right)/\sqrt{\hat{X}{\left(1\right)}^2+\hat{X}{\left(2\right)}^2+\hat{X}{\left(3\right)}^2});$

其中，步骤7中所述的标定精度评价分析，具体实现过程说明如下：

将标定结果δK、α、β、γ代入

${\hat{V}}_L=(1+\mathrm{\δK})(V_x^b\cos \mathrm{\α}+V_y^b\cos \mathrm{\β}+V_z^b\cos \mathrm{\γ})$ ，得

通过下列标准差计算公式计算与测速仪实际输出值V_L(1)、V_L(2)…V_L(n)之间的离散程度，由此判断标定结果的精确度；

$\mathrm{\σ}=\sqrt{({[{\hat{V}}_L\left(1\right)-V_L\left(1\right)]}^2+{[{\hat{V}}_L\left(2\right)-V_L\left(2\right)]}^2+\·\·\·+{[{\hat{V}}_L\left(n\right)-V_L\left(N\right)]}^2)/(n-1)}.$