惯性导航和轮速计组成的航迹推算定位系统误差估算算法
【专利摘要】本发明提供一种惯性导航和轮速计组成的航迹推算定位系统误差估算算法，通过将航迹推算坐标和GPS经纬度坐标转换到同一个局部坐标系中，以GPS坐标为标准值，航迹推算坐标为参照值，计算航迹推算坐标和GPS坐标之间的误差值，设定3个误差参数，每个误差参数设定一定数量级的误差范围，在该范围内隔段误差选取一个误差量，使用正交试验法筛选出部分典型的误差组合，找出误差最小的误差参数组合作为误差参数最优值，并代入惯导航向和轮速计的左右轮数据中进行修正。本发明通过简单的算法，估算惯导的系统误差，并一次性的给予修正，算法简单，速度快，计算一次便可修正，简化了惯性导航定位系统使用前的误差校准过程。
【专利说明】惯性导航和轮速计组成的航迹推算定位系统误差估算算法
【技术领域】
[0001] 本发明涉及惯性导航系统【技术领域】，尤其涉及一种惯性导航和轮速计组成的航迹 推算定位系统误差估算算法。
【背景技术】
[0002] 惯性导航系统依靠机械设备和相应算法工作，能独立完成导航任务。此外惯导系 统不受外界诸如天气，电磁福射的环境因素的干扰，并且载体在小范围内的活动拥有非常 高的可靠性，因此惯性导航系统广泛应用于军用和民用领域。
[0003] 地面车辆的惯导系统在长时间的使用之后，惯导的安装有松动，会导致惯导有安 装偏移角，并且惯导受外界影响如磁场时也会导致惯导初始方向角存在偏差，而车轮由于 车胎气压的变化，车轮的半径相较于标准车轮半径也会出现偏差，从而导致轮速计在计算 车辆移动距离时出现偏移误差。由于该类误差问题的存在，会导致惯导定位系统在进行航 迹推算定位时，得到的坐标和运行轨迹出现较大的系统偏移误差。因此惯导系统在开始工 作前，通常要进行初始校准W减小惯导初始安装偏差角、惯导初始误差、轮速计偏移误差等 误差导致的定位误差。
[0004] 惯导的初始校准通常分为静基座分析和动态误差分析两种。静基座测试主要测量 惯导和其他导航器件的系统误差，因需要使用大量精密仪器，虽然精度较高但是需要耗费 很长的时间并且成本也非常高。惯导的动态分析主要在惯导系统开始工作前进行校准，减 小安装偏差角、惯导初始未对准之类的问题导致惯导系统工作定位产生系统误差。而且动 态误差测试在室外场地就能进行，不需要使用精密仪器，用GPS的坐标和航向角作为参考 标准就行，不需要太长的时间，使用方便，成本也非常低。但动态误差修正方法主要注重误 差的实时修正，算法复杂的同时，不能一次性的估计系统误差值并给予修正。
【发明内容】
[0005] 本发明的目的是为了简化惯性导航定位系统使用前的误差校准过程，提供一种惯 性导航和轮速计组成的航迹推算定位系统误差估算算法，算法简单，速度快，计算一次便可 修正。
[0006] 为解决上述技术问题，本发明采用如下技术方案：
[0007] -种惯性导航和轮速计组成的航迹推算定位系统误差估算算法，包括如下步骤：
[000引 （1)试验车打开车载的惯导、轮速计和GPS行驶一段路程，分别记录惯导航向、轮 速计和GI^经缔度坐标数据；
[0009] (2)根据惯导航向数据和轮速计数据进行航迹推算，得出n时刻航迹推算坐标 (Xn，Yn)；
[0010] (3)W试验车的起点为原点设置局部坐标系，将GI^经缔度坐标和步骤（2)中的航 迹推算坐标转换为局部坐标系坐标，并W正北方向作为Y轴正方向，正东方向作为X轴正方 向，两个坐标数据整合到同一坐标系中进行对比，WGI^经缔度坐标点为标准参量，航迹推 算坐标点为包含误差的对比值，两坐标点之间的距离即为误差值；
[0011] (4)设定惯导航向初始误差角、左轮速计初始误差和右轮速计初始误差3个偏差 量为导致航迹推算得到的坐标出现误差的误差参数；
[0012] (5)确定误差参数值最优值所在的范围；首先对误差参数组进行筛选，当误差参 数值水平数为1或2时，通过作图的办法对误差参数组进行筛选，当误差参数值水平数大于 等于3时，通过正交试验法对误差参数组进行筛选；然后将筛选出的误差参数组代入局部 坐标系中的航迹推算坐标中计算误差大小来确定最优值的取值区间；
[0013] (6)将筛选出的误差参数组代入局部坐标系中的航迹推算坐标中计算误差大小， 根据计算比较误差最大值和均值能否满足精度要求，判断是否为误差参数组的最优解；
[0014] (7)在计算得到误差参数组的最优解后，将最优解代入惯导航向和轮速计的左右 轮数据中进行修正，使用修正后的惯导航向和轮速计数据进行航迹推算便可W得到修正后 的惯导定位坐标。
[0015] 步骤（2)中，所述航迹推算的具体方法为：根据试验车的惯导航向和轮速计的车 辆行驶距离数据，通过H角函数法计算车辆的位置坐标（x"，y。)，如下式所示：
[0016] X。=Xn-I+ALs*cos(headingn-i) (:〇
[0017] y。二Yn-I+ ALs*sin(headingn-i)
[001引其中ALs为单位采样时间内试验车移动的距离，heading^为第n-1时刻试验车 的瞬时惯导航向。
[0019] 所述单位采样时间内试验车移动的距离ALs取左轮和右轮的行驶距离的平均 值，如下式所示：
【权利要求】
1. 一种惯性导航和轮速计组成的航迹推算定位系统误差估算算法，其特征在于，包括 如下步骤： (1) 试验车打开车载的惯导、轮速计和GPS行驶一段路程，分别记录惯导航向、轮速计 和GPS经纬度坐标数据； (2) 根据惯导航向数据和轮速计数据进行航迹推算，得出n时刻航迹推算坐标 (xn，yn); (3) 以试验车的起点为原点设置局部坐标系，将GPS经纬度坐标和步骤（2)中的航迹推 算坐标转换为局部坐标系坐标，并以正北方向作为Y轴正方向，正东方向作为X轴正方向， 两个坐标数据整合到同一坐标系中进行对比，以GPS经纬度坐标点为标准参量，航迹推算 坐标点为包含误差的对比值，两坐标点之间的距离即为误差值； (4) 设定惯导航向初始误差角、左轮速计初始误差和右轮速计初始误差3个偏差量为 导致航迹推算得到的坐标出现误差的误差参数； (5) 确定误差参数值最优值所在的范围：首先对误差参数组进行筛选，当误差参数值 水平数为1或2时，通过作图的办法对误差参数组进行筛选，当误差参数值水平数大于等于 3时，通过正交试验法对误差参数组进行筛选；然后将筛选出的误差参数组代入局部坐标 系中的航迹推算坐标中计算误差大小来确定最优值的取值区间； (6) 将筛选出的误差参数组代入局部坐标系中的航迹推算坐标中计算误差大小，根据 计算比较误差最大值和均值能否满足精度要求，判断是否为误差参数组的最优解； (7) 在计算得到误差参数组的最优解后，将最优解代入惯导航向和轮速计的左右轮数 据中进行修正，使用修正后的惯导航向和轮速计数据进行航迹推算便可以得到修正后的惯 导定位坐标。
2. 根据权利要求1所述的误差估算算法，其特征在于，步骤（2)中，航迹推算具体为： 根据试验车的惯导航向和轮速计的车辆行驶距离数据，通过三角函数法计算车辆的位置坐 标（xn，yn)，如下式所示： xn = Xlri+ A Ls*cos (headingn-i) (i) yn = yn-i+ A Ls*sin (heading^) 其中A Ls为单位采样时间内试验车移动的距离，heading^为第n-1时刻试验车的瞬 时惯导航向。
3. 根据权利要求2所述的误差估算算法，其特征在于，所述单位采样时间内试验车移 动的距离A Ls取左轮和右轮的行驶距离的平均值，如下式所示：
其中Ls是轮速计记录的行驶距离平均值，Ls^为左轮轮速计记录的行驶距离，Lsk是右 轮轮速计记录的行驶距离，单位采样时间内试验车移动的距离ALs = Lsn-Lslri，其中LsnS n时刻的左右轮速计记录的行驶距离的平均值。
4. 根据权利要求1所述的误差估算算法，其特征在于，步骤（3)中，GPS经纬度坐标转 换成局部坐标系坐标的计算过程具体为：设n时刻GPS经纬度坐标转化为局部坐标系中的 坐标（x'n，y'n)，如下式所示： X，n = X，n-i+R*!^，y，n = y，Qii) Ta =A CO * 31 /360 T4, =A 4) * 3i /360 其中R为地球半径，A ?和A 为n时刻和n-1时刻之间的经度和纬度差值，和 h为经度和纬度差值转化为弧度值。
5. 根据权利要求1所述的误差估算算法，其特征在于，步骤（5)中，误差参数误差范围 设定为惯性导航官方标注误差的两倍，且正负方向都设定为误差的分布范围，在误差正负 的分布范围内，设定同组等分的水平数，同时0也作为一组水平数，让各个不同组的水平数 自由组合，构成全部误差参数组。
6. 根据权利要求1所述的误差估算算法，其特征在于，步骤￠)中，若对误差参数组有 更大的精度需求，需要进一步精确计算最优解时，可以使用响应曲面法来拟合误差曲面，找 到误差曲面上误差最小的点，其对应的误差参数即是误差参数的最优解。
【文档编号】G01C25/00GK104359492SQ201410613529
【公开日】2015年2月18日 申请日期:2014年11月3日 优先权日:2014年11月3日
【发明者】祝辉, 何笔华, 梁华为, 余彪 申请人:中国科学院合肥物质科学研究院