一种基于网络RTK的无人自动化作业特种车高精度定位方法

一种基于网络rtk的无人自动化作业特种车高精度定位方法
技术领域
1.本发明涉及定位技术领域，具体涉及一种基于网络rtk的无人自动化作业特种车高精度定位方法。


背景技术：

2.rtk，即实时动态差分定位技术(real-time kinematic)，是利用gps载波相位观测值对定位结果进行修正，从而进行实时动态相对定位的技术。rtk的基本工作原理：基准站和附近的流动用户之间建立数据通信链路，流动站通过通信链路接收到基准站的载波相位观测值和基准站的位置信息，再利用静态相对测量的处理方法求解基线长度，最后得到待测点的位置信息。随着rtk技术的不断发展，其已经广泛应用到交通运输、工程测绘、城市规划、农业机械和地质灾害监测等领域，并逐渐取代了传统的导航定位方法。
3.现代社会对导航的定位精度要求越来越高，单一的导航技术会因为其自身的缺陷，导致定位精度不高，难以适应复杂的工作环境，从而达不到现代高精度定位的需求。目前，常用的定位方式主要有：卫星定位、惯性导航定位、视觉里程算法定位、超宽带无线定位等。
4.美国的gps系统起步最早，发展也最为完善，与俄罗斯的格洛纳斯导航卫星系统(glonass)，我国的北斗导航卫星系统(bds)以及欧盟的galileo系统，统称为全球导航卫星系统。目前，gnss的位置服务已经渗透到人类社会生活中的方方面面，如自动驾驶、交通运输、工程测绘、农业机械和地质灾害监测等。虽然广泛应用的gnss定位系统具有覆盖区域广、观测时间短，且在可视卫星数目良好的情况下可以提供稳定的和较高精度的定位等优点，但是一旦进入森林峡谷、高楼林立的城市和结构复杂多变的室内环境中，由于可视卫星数目不足，最终会导致gnss定位精度很低，甚至出现定位失效的情况。
5.惯性导航系统(inertial navigation system，ins)由惯性测量单元(inertial measurement unit，imu)和计算机构成，惯性测量单元集中了陀螺仪，加速度计和地磁传感器等基本元件，是现代惯性导航系统的核心。惯性导航是运用牛顿力学原理，测得运载体的加速度，再运用数学中的微分计算的方法得到载体的位置和速度。惯性导航的优点就是，它可以不受外部环境的影响，在传输过程中也不依靠外部的导航电台或者无线电。不过因为惯性元件自身也存在着一定的制造误差，使得惯性导航的定位偏差也会随着时间而逐渐积累。目前由于生产工艺的进展,陀螺仪的准确度也在日益提升,因此平台式的惯性导航系统也正在逐步地被捷联型的惯性导航系统所替代。
6.视觉里程算法定位就是使用机器视觉获取周围环境的图像，将采集到的图像通过算法处理，提取特征值，最后通过不断地迭代的方式，采用累积求和形成视觉里程计，得到运载体的位置信息。由于摄像头安装在车身，当无人车以比较快地速度运动时，会使摄像头拍摄的图像变得模糊，将导致提取特征值的算法难度增加，甚至使其失效。
7.超宽带(ultra wideband，uwb)无线定位是通过发送和接收具有纳秒级，甚至是低于纳秒级的极窄脉冲来传输数据，从而具有ghz量级的带宽，主要是利用到达基站的时间差
进行定位。如果有三个已知坐标点的基站收到信号，发送的脉冲信号距离三个基站的间隔不同，那么这三个基站收到信号的时间点是不一样的。超宽带系统具有较强的穿透力、较好的抗多径效果和很高的安全性等优点，主要应用于室内定位。
8.无人特种作业车主要用于对观察区域周围的图像、声音、空气质量、目标温度等进行判断的无人作业平台，而如果只是单纯依靠一种导航定位，难以满足无人特种作业车对高精度的要求，而且每种导航方式都存在一定缺陷。就应用最为广泛的惯性导航和卫星导航而言，惯性导航不受外界环境的干扰，但是其误差会随着时间不断积累，而卫星导航则受环境影响比较大，一旦进入遮蔽环境中，很容易造成卫星信号失锁，从而造成定位丢失。因此，实际工程应用中，尤其是针对无人特种作业车，需要一种高精度、高可靠性的定位方法。


技术实现要素：

9.本发明的目的在于提供一种基于网络rtk的无人自动化作业特种车高精度定位方法，用以解决现有定位方法无法满足无人自动化作业特种车在实际应用中对定位精度、可靠性的要求的问题。
10.为实现上述目的，本发明的技术方案为：
11.一种基于网络rtk的无人自动化作业特种车高精度定位方法，包括以下步骤：
12.步骤1：建立gnss差分观测方程，消除卫星钟差、大气延迟误差和接收机钟差等误差；
13.步骤2：对整周模糊度进行估计；
14.步骤3：对整周模糊度进行降相关与搜索；
15.步骤4：对模糊度进行检验；
16.步骤5：周跳的探测与修复；
17.步骤6：对无人自动化作业特种车的姿态信息进行感知，同时将运动测量实时信息转换到导航坐标体系下，并对位置和速度实现更新。
18.进一步地，作为优选技术方案，所述步骤1的具体过程为：
19.步骤1-1：统一时空基准，包括时间系统的统一和坐标系统的统一，统一关系如下：
20.gpst＝utc+1s
×
n-19s
ꢀꢀꢀ
(1)
21.bdt＝gpst-14s
ꢀꢀꢀ
(2)
22.式中，gpst为gps时，表示gps主控站的原子钟控制的专用的时间系统；utc表示协调世界时；n表示调整参数，具体数值由国际地球自转服务局发布；bdt表示北斗时；
23.步骤1-2：建立伪距和载波相位观测方程：
[0024][0025][0026]
式中，上标s表示观测卫星，下标r表示接收机；p和分别为伪距和载波相位观测量；ρ为卫星信号发射时刻与接收机的真实距离；c为真空中的光速；dtr和dts分别为接收机钟差和卫星钟差；t为信号在大气中传播过程中的对流层误差；i为信号在大气中传播过程中的电离层误差；λ为载波相位波长；n为以周为单位的整周模糊度；εp和分别为伪距和载波相位的其他误差(包括噪声误差)；
[0027]
步骤1-3：建立单差观测模型，基准站r与移动站q进行站间差分，基准站接收机r和
移动站接收机q对同一颗卫星s进行差分计算，其中，基准站r的伪距和载波相位观测方程为式(3)和(4)，而移动站q的伪距和载波相位观测方程如下：
[0028][0029][0030]
式(3)与式(5)作差，以及式(4)与式(6)作差可以卫星钟差dtq，从而得到单差模型：
[0031][0032][0033]
式(7)和式(8)中，δ表示单差，对于短基线情况下，基准站和移动站的电离层延迟误差和对流层延迟误差相差很小，所以可以近似将式(7)和式(8)中对流层和电离层的差值等于0，可以得到：
[0034][0035][0036]
步骤1-4：建立双差观测模型，设参考卫星y，则基准站r和移动站q的单差方程与式(9)和(10)类似，
[0037][0038][0039]
将式(9)和(11)，(10)和(12)作差，消除了接收机钟差项得到双差观测方程：
[0040][0041][0042]
式(13)、(14)中，表示双差符号，双差整周模糊度保留了整数特性，并且可以有效分离；
[0043]
设有n个可用卫星，则载波相位和伪距的观测方程的个数为2(n-1)个，其中，未知量包括基线双差伪距的3个方向坐标，还有n-1个双差模糊度，所以根据数学上求解方程的原则：方程数大于等于未知数。为了求解模糊度的浮点解，则需要：2(n-1)≥3+(n-1)，解得n＝4，所以同时观察到4颗可视卫星，是单系统实现差分定位的最低要求；
[0044]
当以4颗卫星a，b，c，d进行解算时，加入伪距双差观测量，可以建立一个6维的方程组，以大高度角的卫星a作为主星，则得到：
[0045][0046]
其中，[l m n]为卫星到接收机的单位向量，[δx δy δz]为接收机概略坐标改正
数，
▽
δr为双差伪距观测值，将式(15)写成矩阵形式：
[0047]
y＝hx
ꢀꢀꢀ
(16)
[0048]
式(16)中，h为接收机到卫星的单位矢量构成的设计矩阵，x为待求的基线矢量和模糊度解，利用最小二乘法可以解算出模糊度的浮点解：
[0049][0050]
式(17)中，为模糊度的浮点解，令基线矢量双差整周模糊度矢量最小二乘结果可以表示为：
[0051][0052][0053]
在式(19)中，q表示向量x的协方差矩阵，表示基线矢量的协方差矩阵，表示模糊度矢量的协方差矩阵，和表示基线矢量和模糊度矢量的互协方差矩阵。
[0054]
进一步地，作为优选技术方案，所述步骤2的具体过程为：
[0055]
步骤2-1：将浮点解分解为基线矢量和整周模糊度矢量的形式，得到：
[0056][0057]
式(20)中，y为伪距和载波相位双差观测量，为基线矢量，m为基线矢量的系数矩阵，n为整周模糊度系数矩阵，为双差整周模糊度，ε为未建模的误差量；
[0058]
步骤2-2：将求解模糊度问题转化为最小二乘问题：
[0059][0060]
式(21)中，qy是y的协方差矩阵；
[0061]
步骤2-3：通过式(18)和式(19)得到整周模糊度浮点解和协方差矩阵q，然后利用模糊度矢量浮点解和协方差矩阵估计模糊度的整数解使模糊度残余平方和最小，得到：
[0062][0063]
进一步地，作为优选技术方案，所述步骤3的具体过程为：
[0064]
设变换矩阵z满足：
[0065][0066]
则其对应的协方差矩阵为：
[0067][0068]
式(24)中，z为一个模为1的整数矩阵，该矩阵的所有元素均为整数，且模值为1，|det(z)|＝1，降到各个模糊度的相关性可以通过选择合适的z矩阵，变换后，使目标函数ω最小；
[0069]
[0070]
得到满足式(25)的整数矩阵最优解，设置搜索空间χ2，即为：
[0071][0072]
搜索空间是一个多维椭球体，形状与协方差阵有关，其大小由常量χ决定。
[0073]
进一步地，作为优选技术方案，所述步骤4的具体过程为：
[0074]
设搜索到的残差最小值为ω
min
，则残差次最小值为ω
sec
，则可以得到：
[0075][0076]
式(27)中，k为经验常数，不能随模型强度、测量环境等的变化而变化，通常k＝3作为检验的判断条件，ratio≥3，则认为搜索得到的最优解是正确解；
[0077]
通过模糊度搜索，得到整周模糊度固定解通过逆变换，可以得到原始双差模糊度的固定解
[0078][0079]
通过整周模糊度求解得到的双差整周模糊度矢量利用模糊度的整数特性，进一步提高基线矢量估计精度，由lambda搜索得到的可以得到：
[0080][0081][0082]
上式中，为基线矢量改正数的固定解，为的协方差矩阵。
[0083]
根据上述结果可以得到流动站的精确三维坐标(xq,yq,zq)
t
为：
[0084]
(xq,yq,zq)
t
＝(x
r0
,y
r0
,z
r0
)
t
+(δxr,δyr,δzr)
t
ꢀꢀꢀ
(31)。
[0085]
进一步地，作为优选技术方案，所述步骤6的具体过程为：
[0086]
步骤6-1：建立无人自动化作业特种车的sins坐标系，包括载体坐标系、惯性坐标系以及地理坐标系；
[0087]
步骤6-2：通过采用四元数法计算出无人自动化作业特种车的姿态参数，包括无人特种作业车的航向角、俯仰角、横滚角，并对捷联式惯性导航的姿态进行更新；
[0088]
步骤6-3：对捷联式惯性导航的速度进行更新；
[0089]
步骤6-4：对捷联式惯性导航的位置进行更新。
[0090]
进一步地，作为优选技术方案，所述步骤6-2的具体过程为：
[0091]
四元数为一个四维数组，由四个元素构成，包含三个虚单位和一个实单位，其数学表达式如下所示：
[0092][0093]
式(32)中，q0、q1、q2和q3为任意实数，和是互相垂直的单位向量；四元数的矩阵式为：
[0094][0095]
式(33)中，θ为角度，可以得到四元数表示的旋转矩阵为：
[0096][0097]
由n旋转到b的过程中，由于两个坐标系中的三条坐标轴两两互相垂直，和表示了两个互逆的坐标系旋转过程，二者的关系如下：
[0098][0099]
式(35)中，ψ为无人特种作业车的航向角；θ为无人特种作业车的俯仰角；γ为无人特种作业车的横滚角；联立式(33)和式(34)得：
[0100][0101]
γ＝arcsin(-c
13
)＝arcsin(-2(q1q
3-q0q2))
ꢀꢀꢀ
(37)
[0102][0103]
观察上式可以得到，只要得到四元数q0、q1、q2和q3的数值，就能计算出无人特种作业车的姿态角ψ、俯仰角θ和横滚角γ的值。
[0104]
进一步地，作为优选技术方案，所述步骤6-3的具体过程为
[0105]
根据比力方程得到速度方程为：
[0106][0107]
在式(39)中，fb为加速度计输出；为哥氏加速度；为牵引加速度，通过积分运算可以完成速度更新，具体表达式为：
[0108][0109]
式(40)中，v
m-1
为前一时刻的速度；δv
smf
为比力积分增量；δv
g/corm
为有害加速度引起的速度增量；
[0110][0111]
从式(39)至式(41)即为惯性导航的速度更新。
[0112]
进一步地，作为优选技术方案，所述步骤6-4的具体过程为：
[0113]
通过导航坐标系下的无人自动化作业特种车的速度，经过积分运算得到无人车所在的经纬度和高度，具体公式如下：
[0114][0115][0116][0117]
其中，l代表纬度，lm代表m时刻纬度，l
m-1
代表m-1时刻纬度；λm代表m时刻经度；λ
m-1
代表m-1时刻经度；hm代表无人车m时刻高度，h
m-1
代表无人车m-1时刻高度；tm代表m时刻，t
m-1
代表m-1时刻；代表x方向速度n次方；代表y方向速度n次方；代表z方向速度n次方；rn代表地球半径；h代表卫星高度。
[0118]
进一步地，作为优选技术方案，还包括初始粗对准步骤，具体为：由于陀螺零偏远大于地球自转角速度，所以陀螺很难感知到地球的自转角速度，因此无法估计初始航向角的大小；由于加速度计有比较高的精度，可以通过加速度计来感知重力加速度的分离通过加速度计来估算初始水平姿态角：俯仰角θ和横滚角γ；
[0119][0120]
式(45)左边三个元素为载体坐标系下加速度计的输出；而右边中括号里的三个元素为导航坐标系下重力加速度的投影，在静态的情形下，一般粗对准中重力加速度只在z轴中有投影为-g，其他轴的投影均为0，即为gn＝[0 0
ꢀ‑
g]
t
；已知带入式(2.50)得到：
[0121][0122][0123][0124]
g为重力加速度，航向角ψ为未知量，可以反解出：
[0125][0126][0127]
本发明相对于现有技术，具有如下有益效果：
[0128]
(1)本发明利用gps双天线和mems惯性单元进行组合导航定位，通过创建推导单差
和双差观测方程，消除了主要误差，再结合整周模糊度的搜索与检验，以及周跳的探测与修复，有效提高了定位的精度以及可靠性，能够极大地满足了无人自动化作业特种车在实际应用中的要求。
[0129]
(2)本发明通过采用初始粗对准估算出捷联惯性导航系统一定精度的初始姿态，便于进一步确定初始姿态转换矩形，从而尽可能减小起始点的误差，避免出现导航系统后续的精度越来越差的情况。
附图说明
[0130]
图1为本发明的方法流程示意图；
[0131]
图2为本发明的单差观测模型；
[0132]
图3为本发明的双差观测模型；
[0133]
图4为本发明的整周模糊度的搜索流程示意图；
[0134]
图5为本发明的相位周跳示意图；
[0135]
图6为捷联惯性导航系统结构示意图。
具体实施方式
[0136]
下面结合实施例及附图，对本发明作进一步的详细说明，但本发明的实施方式不限于此。
[0137]
实施例
[0138]
如图1所示，本实施例所示的一种基于网络rtk的无人自动化作业特种车高精度定位方法，包括以下步骤：
[0139]
步骤1：建立gnss差分观测方程，消除卫星钟差、大气延迟误差和接收机钟差等误差；
[0140]
步骤2：对整周模糊度进行估计；
[0141]
步骤3：对整周模糊度进行降相关与搜索；
[0142]
步骤4：对模糊度进行检验；
[0143]
步骤5：周跳的探测与修复；
[0144]
步骤6：对无人自动化作业特种车的姿态信息进行感知，同时将运动测量实时信息转换到导航坐标体系下，并对位置和速度实现更新。
[0145]
本实施例的具体方法如下：
[0146]
首先是时空基准的统一，包括时间系统和坐标系统的统一：
[0147]
(1)时间系统的统一
[0148]
gps建立了由gps主控站的原子钟控制的专用的时间系统，即为gps时(gps time，gpst)，gpst与协调世界时(universal time coordinated，utc)的时刻规定，起始历元为utc的1986年1月6日00时00分00秒，以后时间不断地积累，gpst和utc之间相差为秒的整数倍，gpst与utc之间的关系为：
[0149]
gpst＝utc+1s
×
n-19s
ꢀꢀꢀ
(1)
[0150]
bdt＝gpst-14s
ꢀꢀꢀ
(2)
[0151]
式中，gpst为gps时，表示gps主控站的原子钟控制的专用的时间系统；utc表示协
调世界时；n表示调整参数，具体数值由国际地球自转服务局发布；bdt表示北斗时；北斗时(beidou time，bdt)的起始历元为2006年1月1日00时00分00秒，采用国际单位制(si)秒为基本单位连续累计，不闰秒。bdt溯源到中国科学院国家授时中心(national time service center，ntsc)维持的协调世界时。gpst比bdt快了14整秒，是因为两者的起始历元不同。而对于坐标系统的统一，wgs-84坐标系与cgcs2000坐标系是一致的，在4个基本常数中，只有扁率略有差异，扁率的差异不影响精度的变化，影响大地纬度和大地高度产生0.1mm左右的差异，然而在目前的定位精度要求的情况下，这种差异几乎可以忽略不计，故wgs-84坐标系和cgcs2000坐标系可以认为是一致的。
[0152]
在gnss数据处理中，接收机的原始观测量和影响信号传播的各种因素之间的函数关系式就是gnss观测方程，在gnss定位中卫星不断播发导航信号，接收机对收到的伪距、载波相位和多普勒等信息进行算法计算，实现导航定位。伪距和载波相位观测量是两类原始观测量，其观测方程如下：
[0153][0154][0155]
式中，上标s表示观测卫星，下标r表示接收机；p和分别为伪距和载波相位观测量；ρ为卫星信号发射时刻与接收机的真实距离；c为真空中的光速；dtr和dts分别为接收机钟差和卫星钟差；t为信号在大气中传播过程中的对流层误差；i为信号在大气中传播过程中的电离层误差；λ为载波相位波长；n为以周为单位的整周模糊度；εp和分别为伪距和载波相位的其他误差(包括噪声误差)；
[0156]
建立单差观测模型。基准站r与移动站q进行站间差分，基准站接收机r和移动站接收机q对同一颗卫星s进行差分计算，单差观测模型如图2所示。其中，基准站r的伪距和载波相位观测方程为式(3)和(4)，而移动站q的伪距和载波相位观测方程如下：
[0157][0158][0159]
式(3)与式(5)作差，以及式(4)与式(6)作差可以卫星钟差dtq，从而得到单差模型：
[0160][0161][0162]
式(7)和式(8)中，δ表示单差，对于短基线情况下，基准站和移动站的电离层延迟误差和对流层延迟误差相差很小，所以可以近似将式(7)和式(8)中对流层和电离层的差值等于0，可以得到：
[0163][0164][0165]
建立双差观测模型。如图3所示，双差是再对另一颗参考卫星进行星间差分的方法，大气延迟误差和接收机钟差都可以通过双差观测模型消除，设参考卫星y，则基准站r和移动站q的单差方程与式(9)和(10)类似，
[0166][0167][0168]
将式(9)和(11)，(10)和(12)作差，消除了接收机钟差项得到双差观测方程：
[0169][0170][0171]
式(13)、(14)中，表示双差符号，双差整周模糊度保留了整数特性，并且可以有效分离；
[0172]
设有n个可用卫星，则载波相位和伪距的观测方程的个数为2(n-1)个，其中，未知量包括基线双差伪距的3个方向坐标，还有n-1个双差模糊度，所以根据数学上求解方程的原则：方程数大于等于未知数。为了求解模糊度的浮点解，则需要：2(n-1)≥3+(n-1)，解得n＝4，所以同时观察到4颗可视卫星，是单系统实现差分定位的最低要求；
[0173]
当以4颗卫星a，b，c，d进行解算时，加入伪距双差观测量，可以建立一个6维的方程组，以大高度角的卫星a作为主星，则得到：
[0174][0175]
其中，[l m n]为卫星到接收机的单位向量，[δx δy δz]为接收机概略坐标改正数，为双差伪距观测值，将式(15)写成矩阵形式：
[0176]
y＝hx
ꢀꢀꢀ
(16)
[0177]
式(16)中，h为接收机到卫星的单位矢量构成的设计矩阵，x为待求的基线矢量和模糊度解，利用最小二乘法可以解算出模糊度的浮点解：
[0178][0179]
式(17)中，为模糊度的浮点解，令基线矢量双差整周模糊度矢量最小二乘结果可以表示为：
[0180][0181][0182]
在式(19)中，q表示向量x的协方差矩阵，表示基线矢量的协方差矩阵，表示模糊度矢量的协方差矩阵，和表示基线矢量和模糊度矢量的互协方差矩阵。
[0183]
本实施例的整周模糊度估计的具体过程为：
[0184]
将浮点解分解为基线矢量和整周模糊度矢量的形式，得到：
[0185][0186]
式(20)中，y为伪距和载波相位双差观测量，为基线矢量，m为基线矢量的系数矩阵，n为整周模糊度系数矩阵，为双差整周模糊度，ε为未建模的误差量；
[0187]
将求解模糊度问题转化为最小二乘问题：
[0188][0189]
式(21)中，qy是y的协方差矩阵；
[0190]
若模糊度相互独立，则模糊度的协方差阵为对角阵，其最优解可以通过就近取整的方法获得。但是，一般情况下，因模糊度之间存在较强的相关性，模糊度协方差阵并非对角阵，这就导致通过四舍五入方法的成功率较低。通过式(18)和式(19)得到整周模糊度浮点解和协方差矩阵q，然后利用模糊度矢量浮点解和协方差矩阵估计模糊度的整数解使模糊度残余平方和最小，得到：
[0191][0192]
本实施例的模糊度降相关与搜索的具体过程为：
[0193]
如图4所示，通过z变换来实现将变成对角矩阵，使模糊度搜索变得简单，并且缩短了搜索时间，设变换矩阵z满足：
[0194][0195]
则其对应的协方差矩阵为：
[0196][0197]
式(24)中，z为一个模为1的整数矩阵，该矩阵的所有元素均为整数，且模值为1，|det(z)|＝1，降到各个模糊度的相关性可以通过选择合适的z矩阵，变换后，使目标函数ω最小；
[0198][0199]
得到满足式(25)的整数矩阵最优解，设置搜索空间χ2，即为：
[0200][0201]
搜索空间是一个多维椭球体，形状与协方差阵有关，其大小由常量χ决定，若χ赋值过大，那么搜索空间中的取值就会过多，导致搜索效率降低；反之，若χ的值取的过小，搜索空间中可能无法包含完整的模糊度整数解，所以χ的取值至关重要。
[0202]
本实施例的模糊度检验的具体过程为：
[0203]
设搜索到的残差最小值为ω
min
，则残差次最小值为ω
sec
，则可以得到：
[0204][0205]
式(27)中，k为经验常数，不能随模型强度、测量环境等的变化而变化，通常k＝3作为检验的判断条件，ratio≥3，则认为搜索得到的最优解是正确解；
[0206]
通过模糊度搜索，得到整周模糊度固定解通过逆变换，可以得到原始双差模糊度
的固定解
[0207][0208]
通过整周模糊度求解得到的双差整周模糊度矢量利用模糊度的整数特性，进一步提高基线矢量估计精度，由lambda搜索得到的可以得到：
[0209][0210][0211]
上式中，为基线矢量改正数的固定解，为的协方差矩阵。
[0212]
根据上述结果可以得到流动站的精确三维坐标(xq,yq,zq)
t
为：
[0213]
(xq,yq,zq)
t
＝(x
r0
,y
r0
,z
r0
)
t
+(δxr,δyr,δzr)
t
ꢀꢀꢀ
(31)。
[0214]
本实施例中，在接收机进行连续的载波相位测量的过程中，往往由于接收机所处的环境会导致信号被遮挡、接收机发生急剧的抖动和信号的信噪比过低等原因，会导致整周计数发生错误，出现整周跳变的现象，这就是周跳(cycle slip)，如图5所示，原本相位值是随时间变化的连续曲线函数，当发生周跳时，该曲线会发生显著变化，t1历元对应载波相位观测值和随后的相位观测仍然包括丢失的周跳值。
[0215]
由于周跳对gnss数据质量会产生重要影响，所以在gnss数据处理中，周跳的探测也是一个比较重要的问题。同时，由于周跳发生的任意性和噪声等因素的影响，所以周跳相关的问题也是比较难解决的。目前常用的周跳探测与修复方法有许多种，但通常可以分为以下三类：
[0216]
(1)利用观测值伴随时间变化的方法，如多项式拟合法；
[0217]
(2)利用组合不同的观测值的方法，如电离层残差法；
[0218]
(3)利用观测值估值残差的方法，如卡尔曼滤波法和历元间差分观测值参数估计法等。
[0219]
捷联式惯性导航系统sins由于具有体积小、可靠性高和结构简单等特点，所以发展非常迅速。其主要分为位置更新、速度更新和姿态更新三大部分。姿态更新是sins的核心，其不仅是感知载体姿态信息，还将运动测量实时信息转换到导航坐标系下，实现位置与速度更新。具体解算过程如下：通过初始对准，能够得到载体的初始姿态、位置和速度等信息，并以此作为导航计算的初始信息；再对导航坐标系下的加速度进行积分运算，得到实时速度和位置信息，然后求解更新后的方向余弦矩阵或四元数，最终得出无人车姿态信息，捷联惯导系统原理图如图6所示。
[0220]
在无人自动化作业特种车的导航系统中，由于传感器测量值和导航系统最终输出的姿态观测值所参考的坐标系不同，本sins所使用的坐标系包括：
[0221]
(1)载体坐标系(b)：顾名思义，载体坐标系是惯导模块安装在运动载体上的坐标系，与载体的运动状态保持一致，其定义如下：
[0222]
原点o：无人特种作业车的质心；
[0223]
x轴：平行于地面并指向无人特种作业车的前进方向；
[0224]
z轴：通过无人特种作业车的质心竖直指向上方；
[0225]
y轴：遵循右手定则且垂直于xoz平面。
[0226]
(2)惯性坐标系(i)：属于空间固定坐标系，在惯性坐标系中，空间点的位置由(x,y,z)来表示；其定义如下：
[0227]
原点o：地球地心；
[0228]
x轴：指向春分点；
[0229]
z轴：指向地球北极；
[0230]
y轴：遵循右手定则且垂直于xoz平面，平面xoy位于赤道平面。
[0231]
(3)地理坐标系(g)：选用东北天坐标系；
[0232]
原点o：无人特种作业车的重心；
[0233]
x轴：指向东方向，即为e；
[0234]
z轴：沿地垂线指向天，即为u；
[0235]
y轴：指向北方向，即为n。
[0236]
本实施例中，捷联式惯性导航系统的姿态包括航向角、俯仰角和横滚角。无人车以东北天坐标系为参考系的方向信息，姿态矩阵表示无人车(载体)坐标系到东北天坐标系的旋转关系。目前，常用的姿态解决方法有欧拉角法、方向余弦矩阵和四元数法三种方法，由于四元数法是以4个变量来表征姿态信息，相较于另外两种方法，在姿态信息的实时更新方面十分方便，而且四元数算法与基于泊松方程设计的算法相比，可以将计算量平均降低30％，所以选择四元数法作为惯性传感器获取姿态的主要方法。
[0237]
从数学的角度来看四元数本质上就是一个四维数组，由四个元素构成，包含三个虚单位和一个实单位。其数学表达式如下所示：
[0238][0239]
式(32)中，q0、q1、q2和q3为任意实数，和是互相垂直的单位向量；四元数的矩阵式为：
[0240][0241]
式(33)中，θ为角度，可以得到四元数表示的旋转矩阵为：
[0242][0243]
由n旋转到b的过程中，由于两个坐标系中的三条坐标轴两两互相垂直，和表示了两个互逆的坐标系旋转过程，二者的关系如下：
[0244][0245]
式(35)中，ψ为无人特种作业车的航向角；θ为无人特种作业车的俯仰角；γ为无人特种作业车的横滚角；联立式(33)和式(34)得：
[0246][0247]
γ＝arcsin(-c
13
)＝arcsin(-2(q1q
3-q0q2))
ꢀꢀꢀ
(37)
[0248][0249]
观察上式可以得到，只要得到四元数q0、q1、q2和q3的数值，就能计算出无人特种作业车的姿态角ψ、俯仰角θ和横滚角γ的值。
[0250]
本实施例的捷联式惯性导航系统实现速度更新的具体过程为：
[0251]
根据比力方程得到速度方程为：
[0252][0253]
在式(39)中，fb为加速度计输出；为哥氏加速度；为牵引加速度，通过积分运算可以完成速度更新，具体表达式为：
[0254][0255]
式(40)中，v
m-1
为前一时刻的速度；δv
smf
为比力积分增量；δv
g/corm
为有害加速度引起的速度增量；
[0256][0257]
从式(39)至式(41)即为惯性导航的速度更新。
[0258]
本实施例捷联式惯性导航系统实现位置更新的具体过程为：
[0259]
通过导航坐标系下的无人自动化作业特种车的速度，经过积分运算得到无人车所在的经纬度和高度，具体公式如下：
[0260][0261][0262][0263]
其中，l代表纬度，lm代表m时刻纬度，l
m-1
代表m-1时刻纬度；λm代表m时刻经度；λ
m-1
代表m-1时刻经度；hm代表无人车m时刻高度，h
m-1
代表无人车m-1时刻高度；tm代表m时刻，t
m-1
代表m-1时刻；代表x方向速度n次方；代表y方向速度n次方；代表z方向速度n次方；rn代表地球半径；h代表卫星高度。
[0264]
捷联惯导系统定位精度的好坏，很大一部分原因取决于初始对准的效果，如果没有初始对准，起始点的误差越大，会导致导航系统后续的精度越差，为此，本实施例还包括初始粗对准步骤，具体为：由于陀螺零偏远大于地球自转角速度，所以陀螺很难感知到地球的自转角速度，因此无法估计初始航向角的大小；由于加速度计有比较高的精度，可以通过加速度计来感知重力加速度的分离通过加速度计来估算初始水平姿态角：俯仰角θ和横滚角γ；
[0265][0266]
式(45)左边三个元素为载体坐标系下加速度计的输出；而右边中括号里的三个元素为导航坐标系下重力加速度的投影，在静态的情形下，一般粗对准中重力加速度只在z轴中有投影为-g，其他轴的投影均为0，即为gn＝[0 0
ꢀ‑
g]
t
；已知带入式(2.50)得到：
[0267][0268][0269][0270]
g为重力加速度，航向角ψ为未知量，可以反解出：
[0271][0272][0273]
本发明采用上述方法，通过实现对bds和gps的时间系统和坐标系统的统一，并推导出单差和双差观测方程，再结合整周模糊度的检索与检验，以及周跳的探测与修复，有效提高了定位的精准度以及可靠性，能够极大地满足了无人自动化作业特种车在实际应用中的要求。
[0274]
虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。