CN104359492B - 惯性导航和轮速计组成的航迹推算定位***误差估算算法 - Google Patents

Abstract

本发明提供一种惯性导航和轮速计组成的航迹推算定位***误差估算算法，通过将航迹推算坐标和GPS经纬度坐标转换到同一个局部坐标系中，以GPS坐标为标准值，航迹推算坐标为参照值，计算航迹推算坐标和GPS坐标之间的误差值，设定3个误差参数，每个误差参数设定一定数量级的误差范围，在该范围内隔段误差选取一个误差量，使用正交试验法筛选出部分典型的误差组合，找出误差最小的误差参数组合作为误差参数最优值，并代入惯导航向和轮速计的左右轮数据中进行修正。本专利通过简单的算法，估算惯导的***误差，并一次性的给予修正，算法简单，速度快，计算一次便可修正，简化了惯性导航定位***使用前的误差校准过程。

Description

惯性导航和轮速计组成的航迹推算定位***误差估算算法

技术领域

本发明涉及惯性导航***技术领域，尤其涉及一种惯性导航和轮速计组成的航迹推算定位***误差估算算法。

背景技术

惯性导航***依靠机械设备和相应算法工作，能独立完成导航任务。此外惯导***不受外界诸如天气，电磁辐射的环境因素的干扰，并且载体在小范围内的活动拥有非常高的可靠性，因此惯性导航***广泛应用于军用和民用领域。

地面车辆的惯导***在长时间的使用之后，惯导的安装有松动，会导致惯导有安装偏移角，并且惯导受外界影响如磁场时也会导致惯导初始方向角存在偏差，而车轮由于车胎气压的变化，车轮的半径相较于标准车轮半径也会出现偏差，从而导致轮速计在计算车辆移动距离时出现偏移误差。由于这类误差问题的存在，会导致惯导定位***在进行航迹推算定位时，得到的坐标和运行轨迹出现较大的***偏移误差。因此惯导***在开始工作前，通常要进行初始校准以减小惯导初始安装偏差角、惯导初始误差、轮速计偏移误差等误差导致的定位误差。

惯导的初始校准通常分为静基座分析和动态误差分析两种。静基座测试主要测量惯导和其他导航器件的***误差，因需要使用大量精密仪器，虽然精度较高但是需要耗费很长的时间并且成本也非常高。惯导的动态分析主要在惯导***开始工作前进行校准，减小安装偏差角、惯导初始未对准之类的问题导致惯导***工作定位产生***误差。而且动态误差测试在室外场地就能进行，不需要使用精密仪器，用GPS的坐标和航向角作为参考标准就行，不需要太长的时间，使用方便，成本也非常低。但动态误差修正方法主要注重误差的实时修正，算法复杂的同时，不能一次性的估计***误差值并给予修正。

发明内容

本发明的目的是为了简化惯性导航定位***使用前的误差校准过程，提供一种惯性导航和轮速计组成的航迹推算定位***误差估算算法，算法简单，速度快，计算一次便可修正。

为解决上述技术问题，本发明采用如下技术方案：

一种惯性导航和轮速计组成的航迹推算定位***误差估算算法，包括如下步骤：

(1)试验车打开车载的惯导、轮速计和GPS行驶一段路程，分别记录惯导航向、轮速计和GPS经纬度坐标数据；

(2)根据惯导航向数据和轮速计数据进行航迹推算，得出n时刻航迹推算坐标(x_n,y_n)；

(3)以试验车的起点为原点设置局部坐标系，将GPS经纬度坐标和步骤(2)中的航迹推算坐标转换为局部坐标系坐标，并以正北方向作为Y轴正方向，正东方向作为X轴正方向，两个坐标数据整合到同一坐标系中进行对比，以GPS经纬度坐标点为标准参量，航迹推算坐标点为包含误差的对比值，两坐标点之间的距离即为误差值；

(4)设定惯导航向初始误差角、左轮速计初始误差和右轮速计初始误差3个偏差量为导致航迹推算得到的坐标出现误差的误差参数；

(5)确定误差参数值最优值所在的范围：首先对误差参数组进行筛选，当误差参数值水平数为1或2时，通过作图的办法对误差参数组进行筛选，当误差参数值水平数大于等于3时，通过正交试验法对误差参数组进行筛选；然后将筛选出的误差参数组代入局部坐标系中的航迹推算坐标中计算误差大小来确定最优值的取值区间；

(6)将筛选出的误差参数组代入局部坐标系中的航迹推算坐标中计算误差大小，根据计算比较误差最大值和均值能否满足精度要求，判断是否为误差参数组的最优解；

(7)在计算得到误差参数组的最优解后，将最优解代入惯导航向和轮速计的左右轮数据中进行修正，使用修正后的惯导航向和轮速计数据进行航迹推算便可以得到修正后的惯导定位坐标。

步骤(2)中，所述航迹推算的具体方法为：根据试验车的惯导航向和轮速计的车辆行驶距离数据，通过三角函数法计算车辆的位置坐标(x_n,y_n),如下式所示：

x_n＝x_n-1+△Ls*cos(heading_n-1) (i)

y_n＝y_n-1+△Ls*sin(heading_n-1)

其中△Ls为单位采样时间内试验车移动的距离，heading_n-1为第n-1时刻试验车的瞬时惯导航向。

所述单位采样时间内试验车移动的距离△Ls取左轮和右轮的行驶距离的平均值，如下式所示：

其中Ls是轮速计记录的行驶距离平均值，Ls_L为左轮轮速计记录的行驶距离，Ls_R是右轮轮速计记录的行驶距离，单位采样时间内试验车移动的距离△Ls＝Ls_n-Ls_n-1，其中Ls_n为n时刻的左右轮速计记录的行驶距离的平均值。

步骤(3)中，GPS经纬度坐标转换成局部坐标系坐标的计算过程具体为：设n时刻GPS经纬度坐标转化为局部坐标系中的坐标(x’_n,y’_n)，如下式所示：

x’_n＝x’_n-1+R*r_ω，y’_n＝y’_n-1+R*r_φ (iii)

r_ω＝△ω*π/360

r_φ＝△φ*π/360

其中R为地球半径，△ω为n时刻和n-1时刻之间的经度差值，△φ为n时刻和n-1时刻之间的纬度差值，r_ω为经度差值转化为弧度值，r_φ为纬度差值转化为弧度值。

步骤(5)中，误差参数误差范围设定为惯性导航官方标注误差的两倍，且正负方向都设定为误差的分布范围，在误差正负的分布范围内，设定同组等分的水平数，同时0也作为一组水平数，让各个不同组的水平数自由组合，构成全部误差参数组。

步骤(6)中，若对误差参数组有更大的精度需求，需要进一步精确计算最优解时，可以使用响应曲面法来拟合误差曲面，找到误差曲面上误差最小的点，其对应的误差参数即是误差参数的最优解。

由以上技术方案可知，本发明通过将航迹推算坐标和GPS经纬度坐标转换到同一个局部坐标系中，以GPS坐标为标准值，航迹推算坐标为参照值，计算航迹推算坐标和GPS坐标之间的误差值，设定3个误差参数，假定航迹推算坐标和GPS坐标之间的误差主要由这3个误差参数产生，每个误差参数设定一定数量级的误差范围，在该范围内隔段误差选取一个误差量，在所有误差量的组合中，使用正交试验法设计，筛选出部分典型的误差组合，在这部分典型的误差组合中找出误差最小的误差参数组合作为误差参数最优值，并代入惯导航向和轮速计的左右轮数据中进行修正。

本专利通过简单的算法，估算惯导的***误差，并一次性的给予修正，算法简单，速度快，计算一次便可修正，简化了惯性导航定位***使用前的误差校准过程。

附图说明

图1为本发明惯性导航和轮速计组成的航迹推算定位***误差估算算法的流程图；

图2为L₉(3⁴)正交试验表；

图3为本发明实施例中实验车在局部坐标系中航迹推算出的坐标轨迹和GPS坐标轨迹，各坐标点的间距即为误差值。

具体实施方式

下面结合附图对本发明作进一步详细说明。

本发明的目的是简化惯性导航定位***使用前的误差校准过程，在校准时让试验车打开车载的惯导、轮速计和GPS跑较长的一段路程，记录惯导航向和轮速计结合推算的车辆运行轨迹与GPS定位航向数据。设定左轮和右轮速计初始误差和惯导航向初始误差角为3个待修正的误差参数，使用响应曲面法计算最优的误差参数，使得惯导和轮速计推算得到的车辆运行轨迹坐标与GPS记录的车辆运行轨迹坐标之间的误差最小,将此最优误差参数代入航迹推算数据中修正误差。虽然存在修正效果受GPS精度影响的问题，但是只是粗略修正惯导***误差时，这样的精度制约是可以接受的。

如图1所示，惯性导航定位***误差估算算法的具体步骤为：

(1)试验车打开车载的惯导、轮速计和GPS行驶一段路程，分别记录惯导航向、轮速计和GPS经纬度坐标数据。

(2)根据惯导航向数据和轮速计数据进行航迹推算，得出n时刻航迹推算坐标(x_n,y_n)。

所述航迹推算的具体方法为：根据试验车的惯导航向和轮速计的车辆行驶距离数据，通过三角函数法计算车辆的位置坐标(x_n,y_n),如下式所示：

x_n＝x_n-1+△Ls*cos(heading_n-1) (i)

y_n＝y_n-1+△Ls*sin(heading_n-1)

其中△Ls为单位采样时间内试验车移动的距离，heading_n-1为第n-1时刻试验车的瞬时惯导航向。将惯导航向数据标记为heading，试验车行驶时的航向变化率△θ＝heading_n-heading_n-1。

使用惯导输出的方向值和轮速计进行航迹推算定位时，由于车辆转向时左右轮行驶的距离不同，同时转向时的轮胎抓地力和车辆惯性相互作用产生的侧倾力会使得车体侧倾，压缩车辆左右两边的悬挂，作用在车辆左右上的力不同会导致车轮内的气压不同、车轮的半径变化不同。由于轮速计的信号是车辆转过一周之后产生的脉冲信号，需要乘以车轮半径和圆弧值才能得到车轮行驶距离。因此，为了能使车辆行驶的距离能够更加接近真实值，取左轮和右轮的行驶距离的数据，计算其平均值。

所述单位采样时间内试验车移动的距离△Ls取左轮和右轮的行驶距离的平均值，如下式所示：

其中Ls是轮速计记录的行驶距离平均值，Ls_L为左轮轮速计记录的行驶距离，Ls_R是右轮轮速计记录的行驶距离，单位采样时间内试验车移动的距离△Ls＝Ls_n-Ls_n-1，其中Ls_n为n时刻的左右轮速计记录的行驶距离的平均值。

为了方便车辆方向信息和坐标信息同步采集，及航迹推算时避免数据采集时间不同时产生的不必要的误差，优选地，惯导装置和GPS接受装置设定在同一初始时刻以同样的周期采集车辆航向、行驶距离和GPS经纬度坐标信息。

同时，为了同步处理对比GPS获得的经纬度坐标信息和轮速计、惯导航向组成的定位***经过航迹推算得到的坐标信息，并将两者的坐标点之间的误差数值计算出来。可以将车辆坐标的GPS经纬数据和惯导轮速计组合***推算得到的坐标转换到同一个局部坐标系中，以方便两者坐标的对比，具体方法如下步骤(3)。

(3)以试验车的起点为原点设置局部坐标系，将GPS经纬度坐标和步骤(2)中的航迹推算坐标转换为局部坐标系坐标，并以正北方向作为Y轴正方向，正东方向作为X轴正方向，两个坐标数据整合到同一坐标系中进行对比，以GPS经纬度坐标点为标准参量，航迹推算坐标点为包含误差的对比值，两坐标点之间的距离即为误差值。

因为实验是在小范围内进行，为了简化GPS经纬度坐标转换成局部坐标系坐标的计算过程，将地球假设作为球体来计算，地球半径取均值R＝6371393m。GPS获得的数据中，n时刻的纬度为φ_n，n时刻的经度为ω_n，n时刻和n-1时刻之间的纬度差值为△φ，经度差值为△ω，将纬度差值转化为弧度值r_φ＝△φ*π/360，将经度差值转化为弧度值r_ω＝△ω*π/360。设n时刻GPS经纬度坐标转化为局部坐标系中的坐标(x’_n,y’_n)，如下式所示：

x’_n＝x’_n-1+R*r_ω，y’_n＝y’_n-1+R*r_φ (iii)

r_ω＝△ω*π/360

r_φ＝△φ*π/360

(4)设定惯导航向初始误差角、左轮速计初始误差和右轮速计初始误差3个偏差量为导致航迹推算得到的坐标出现误差的误差参数。

以推算得到的坐标数据为待修正值，GPS接收器得到的经纬度坐标为参考标准值，假设左、右轮速与初始设定的标准值存在偏差，惯导的初始对准角与实际值存在偏差，设定这3个偏差量是导致航迹推算得到的坐标出现误差的主要原因，作为误差参数值，通过修正这3个偏差量来达到修正航迹推算误差的途径。

(5)确定误差参数值最优值所在的范围。

确定误差参数值最优值所在的范围，需要对误差参数误差的范围进行设定，一般设定为惯性导航官方标注误差的两倍，且正负方向都设定为误差的分布范围，在误差正负的分布范围内，设定同组等分的水平数，同时0也作为一组水平数，让各个不同组的水平数自由组合，构成全部误差参数组。

通过误差参数组计算误差大小来确定最优值的取值区间，为了简化计算，需要对误差参数值的组数进行筛选。

筛选具有两种方法，当误差参数值水平数为1或2时，可以通过作图的办法对误差参数组进行筛选，当误差参数值水平数大于等于3时，则通过正交试验法对误差参数组进行筛选。

正交试验法使用已经做好了的表格、正交表来安排试验并进行数据分析。在正交试验设计中，因子可以是定量的，也可以是定性的。而定量因子各水平间的距离可以相等，也可以不相等。在三因子三水平的条件试验中，通常有两种试验进行方法。一种是取三因子所有水平之间的组合，三因子三水平就是试验27次，每个点代表一个参数组合，但是这种方法要测试的组合太多了，工作量非常大。第二种是简单对比法，变化一个因素而固定其他因素，知道选出所以最优因素，组成最优解组合，但是第二种方法的代表性很差，很大的范围内都没有选点，因此这种方法不全面，选出的组合也不一定是所以组合中最好的，其次，比较条件时只有单个数据的比较，不能剔除误差的干扰，会造成结论的不稳定。

考虑兼顾这两种试验方法的优点，使用正交表从全面试验的点中选择具有典型性、代表性的点，选出的试验点必须在试验范围内分布很均匀，能反映全面情况才行。三因子三水平试验中，对应参数A有三个平面，对应参数B,C也各有3个平面。在9个平面上，试验点都应当一样多，即对每个因子的每个水平都要同等看待。同时，每个要求平面上的每行、每列上的点都一样多。

当因子数和水平数都不太大时，可通过作图的办法来选择分布均匀的试验点。因子数和水平数较多时，按照正交表来安排试验。正交表如图2所示，各列中的1、2、3都各出现3次，任何两列，例如第3、4列，所构成的有序数从上向下一共有9种，既没有重复也没有遗漏，其他任何两列所构成的有序数也是这9种各出现一次，满足了试验点分布的均匀性。

记录误差数据时，对比参考量为以GPS的坐标和航向为标准参量，惯导航向结合轮速计推算的运行坐标点为包含误差的对比值。误差值为每个采样时刻，将航迹推算得到的局部坐标系中的坐标点，和GPS定位得到的经纬度经过转换，得到在同一局部坐标系中的坐标点，他们之间的距离数值，即作为误差值。为了衡量误差的大小，判定误差是否为最小值，需要同时计算误差的最大值和平均值衡作为误差大小的衡量标准，具体采用的方法如下步骤(6)所述。

(6)将筛选出的误差参数组代入局部坐标系中的航迹推算坐标中计算误差大小，根据计算比较误差最大值和均值能否满足精度要求，判断是否为误差参数组的最优解。

使用正交试验法计算比较误差的最大值和均值，得到误差参数最优解的取值范围后，通过计算误差找出最小值，误差参数精度要求不高时，该误差最小的误差参数组便可以当做是最优解。如果对误差参数组有更大的精度需求，需要进一步精确计算最优解时，可以使用响应曲面法来拟合误差曲面，找到误差曲面上误差最小的点，其对应的误差参数即是误差参数的最优解。曲面响应法都是现有的算法，算法原理简单，所有计算可以通过软件自动完成。

(7)在计算得到误差参数组的最优解后，将最优解代入惯导航向和轮速计的左右轮数据中进行修正，使用修正后的惯导航向和轮速计数据进行航迹推算便可以得到修正后的惯导定位坐标。

对本发明的一种具体实施例说明如下：

试验车设定车轮半径初始的标准误差为0，惯导的初始航向误差为0，左右轮的正负误差范围取-0.03m到0.03m之间，航向角的左右误差范围取-1度到1度。

设定好误差范围后，车轮误差每隔0.01m取值，惯导航向误差每隔0.25度取值。在所有的误差参数组合中，筛选典型的误差参数组合。使用正交试验设计法，选出(-0.03,-0.03,1)，(0,0,0.25)，(0.02,0.02,-1)，(0,-0.03,0.25),(0.03,-0.03,-1)，(0.03,0,0.25)，(-0.03,0.03,-1)这几组误差组合，计算其误差值。

结果是误差参数为(-0.01,0.01,0.25)时，航迹推算与GPS坐标之间的误差最小。航迹推算和GPS坐标的估计如图3所示，这时的坐标距离误差最大值为21.76m，平均值为13.24m。

如果误差平均值和最大值能满足精度要求，正交试验的结果就能作为修正值使用。如果需要更高的精度，则使用能进行曲面响应分析的软件进一步计算最优值来修正车辆的***误差。

以上所述实施方式仅仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案作出的各种变形和改进，均应落入本发明的权利要求书确定的保护范围内。

Claims (6)

1.一种惯性导航和轮速计组成的航迹推算定位***误差估算算法，其特征在于，包括如下步骤：

(1)试验车打开车载的惯导、轮速计和GPS行驶一段路程，分别记录惯导航向、轮速计和GPS经纬度坐标数据；

(2)根据惯导航向数据和轮速计数据进行航迹推算，得出n时刻航迹推算坐标(x_n,y_n)；

(3)以试验车的起点为原点设置局部坐标系，将GPS经纬度坐标和步骤(2)中的航迹推算坐标转换为局部坐标系坐标，并以正北方向作为Y轴正方向，正东方向作为X轴正方向，两个坐标数据整合到同一坐标系中进行对比，以GPS经纬度坐标点为标准参量，航迹推算坐标点为包含误差的对比值，两坐标点之间的距离即为误差值；

(4)设定惯导航向初始误差角、左轮速计初始误差和右轮速计初始误差3个偏差量为导致航迹推算得到的坐标出现误差的误差参数；

(5)确定误差参数值最优值所在的范围：首先对误差参数组进行筛选，当误差参数值水平数为1或2时，通过作图的办法对误差参数组进行筛选，当误差参数值水平数大于等于3时，通过正交试验法对误差参数组进行筛选；然后将筛选出的误差参数组代入局部坐标系中的航迹推算坐标中计算误差大小来确定最优值的取值区间；

(6)将筛选出的误差参数组代入局部坐标系中的航迹推算坐标中计算误差大小，根据计算比较误差最大值和均值能否满足精度要求，判断是否为误差参数组的最优解；

(7)在计算得到误差参数组的最优解后，将最优解代入惯导航向和轮速计的左右轮数据中进行修正，使用修正后的惯导航向和轮速计数据进行航迹推算便可以得到修正后的惯导定位坐标。

2.根据权利要求1所述的误差估算算法，其特征在于，步骤(2)中，航迹推算具体为：根据试验车的惯导航向和轮速计的车辆行驶距离数据，通过三角函数法计算车辆的位置坐标(x_n,y_n),如下式所示：

x_n＝x_n-1+△Ls*cos(heading_n-1) (i)

y_n＝y_n-1+△Ls*sin(heading_n-1)

其中△Ls为单位采样时间内试验车移动的距离，heading_n-1为第n-1时刻试验车的瞬时惯导航向。

3.根据权利要求2所述的误差估算算法，其特征在于，所述单位采样时间内试验车移动的距离△Ls取左轮和右轮的行驶距离的平均值，如下式所示：

$Ls=\frac{({\mathrm{Ls}}_L+{\mathrm{Ls}}_R)}{2}---\left(ii\right)$

其中Ls是轮速计记录的行驶距离平均值，Ls_L为左轮轮速计记录的行驶距离，Ls_R是右轮轮速计记录的行驶距离，单位采样时间内试验车移动的距离△Ls＝Ls_n-Ls_n-1，其中Ls_n为n时刻的左右轮速计记录的行驶距离的平均值。

4.根据权利要求1所述的误差估算算法，其特征在于，步骤(3)中，GPS经纬度坐标转换成局部坐标系坐标的计算过程具体为：设n时刻GPS经纬度坐标转化为局部坐标系中的坐标(x’_n,y’_n)，如下式所示：

x’_n＝x’_n-1+R*r_ω，y’_n＝y’_n-1+R*r_φ (iii)

r_ω＝△ω*π/360

r_φ＝△φ*π/360

其中R为地球半径，△ω为n时刻和n-1时刻之间的经度差值，△φ为n时刻和n-1时刻之间的纬度差值，r_ω为经度差值转化为弧度值，r_φ为纬度差值转化为弧度值。

5.根据权利要求1所述的误差估算算法，其特征在于，步骤(5)中，误差参数误差范围设定为惯性导航官方标注误差的两倍，且正负方向都设定为误差的分布范围，在误差正负的分布范围内，设定同组等分的水平数，同时0也作为一组水平数，让各个不同组的水平数自由组合，构成全部误差参数组。

6.根据权利要求1所述的误差估算算法，其特征在于，步骤(6)中，若对误差参数组有更大的精度需求，需要进一步精确计算最优解时，可以使用响应曲面法来拟合误差曲面，找到误差曲面上误差最小的点，其对应的误差参数即是误差参数的最优解。