CN112731483B - 一种自动驾驶组合导航系统中rtk异常值的判断方法 - Google Patents

Abstract

本发明属于自动驾驶车载组合导航定位领域，特别涉及一种自动驾驶组合导航系统中RTK异常值的判断方法，包括步骤：利用惯性和里程计组合导航参数建立判断基准，判断RTK定位精度的有效性；当判断RTK定位精度无效时，航位推算预估自动驾驶汽车的当前位置信息；在自动驾驶汽车RTK解算位置长时间失效后，判断RTK解算位置信息的有效性；当判断RTK解算位置信息有效时，对惯性和里程计组合导航参数进行矫正。本发明可以在RTK信号受阻的情况下，及时判断出RTK导航的异常，进而及时切换导航参数获取的途径，避免自动驾驶汽车脱离正确行驶的方向。

Description

一种自动驾驶组合导航系统中RTK异常值的判断方法

技术领域

本发明属于自动驾驶车载组合导航定位领域，特别涉及一种自动驾驶惯性/里程计/RTK自动驾驶组合导航系统中RTK异常值的判断方法。

背景技术

科学技术的快速发展促进了机械化时代的步伐，为自动化、智能化、机械化的自动驾驶领域提供了前所未有的机遇。自动驾驶领域的崛起需要各种高精尖的核心技术支持，其中最关键的核心之一就是车载组合导航的融合定位技术，这一技术将代替人眼、手、脚的操作，来自动操控汽车，实现汽车的路径规划、行为决策等一系列的操作。

随着车载导航技术的不断成熟，虽给自动驾驶领域提供了机遇和平台，但核心技术中仍存在一些难题有待解决和方案优化。以RTK定位技术为例，目前虽然利用GPS载波差分原理实现了厘米级的高精度定位，但由于RTK导航受信号、环境等影响将造成局部区域导航定位精度裂化的现象，而无法满足自动驾驶导航定位的精度要求。在这部分区域内将需要利用惯性/里程计短期精度高的特点提供自动驾驶汽车导航参数。然而，由于没有准确的RTK信号受阻的反馈，导致无法及时判断出RTK导航的异常，进而就无法及时切换导航参数获取的途径，这将可能造成自动驾驶汽车脱离正确行驶的方向。

因此，为避免上述现象的发生，提供一种自动驾驶惯性/里程计/RTK组合导航系统中RTK异常值判断方法是必要的，也是不可或缺的手段，其研究将在一定程度上推动自动驾驶汽车技术的发展，也将为同类型的工程问题的解决提供一种新的解决思路。

发明内容

为了解决上述问题，本发明提出一种自动驾驶组合导航系统中RTK异常值的判断方法。其中，为判断自动驾驶汽车行驶过程中RTK定位的有效性，首先利用惯性/里程计组合导航参数建立判断基准，当RTK解算的自动驾驶汽车的速度与加速度不满足惯性/里程计组合导航解算结果所确定的阈值时，认定RTK信息异常，之后利用惯性/里程计组合导航短期定位精度高的特点，对当前时刻的载体车的位置进行估计，并将解算结果作为当前时刻自动驾驶汽车的位姿。然而，由于惯性/里程计组合导航具有误差随时间累积发散的不足，当RTK长时间无效时，惯性/里程计所建立的基准可信度将降低。为了准确判断RTK信息，当RTK长时间异常时，利用惯性/里程计在相邻两点距离测量精度高、且姿态精度高的特性，与RTK解算的相邻两点的距离及航向做对比，当满足一定阈值时，则认为RTK信号恢复正常，并利用RTK信息对惯性/里程计组合导航结果进行矫正，并将矫正后的结果作为判断RTK信号是否异常的条件，直到整个自动驾驶过程结束。

本发明提供了一种自动驾驶组合导航系统中RTK异常值的判断方法，其特征在于，包括步骤：

S1：利用惯性和里程计组合导航参数建立判断基准，对自动驾驶车载组合导航中RTK定位精度的有效性进行判断，具体过程如下：

S11：建立惯性和里程计组合导航的速度阈值；

S12：建立惯性和里程计组合导航的加速度阈值；

S13：判断RTK解算位置信息的有效性；

S2：当判断自动驾驶车载组合导航中RTK定位精度无效时，航位推算预估自动驾驶汽车的当前位置信息，具体过程如下：

S21：基于惯性和里程计组合导航进行自动驾驶汽车的姿态解算；

S22：基于里程计位置信息差分原理估计当前时刻自动驾驶汽车的位置；

S3：在自动驾驶汽车RTK解算位置长时间失效后，判断RTK定位精度的有效性；

S4：当判断RTK定位精度有效时，对惯性和里程计组合导航参数进行矫正，然后将矫正后的惯性和里程计组合导航参数作为评价RTK定位精度是否有效的新条件。

进一步，步骤S11具体过程为：

定义RTK解算的自动驾驶汽车的速度为v_RTK(t)，加速度为a_RTK(t)；利用惯性和里程计差分原理解算获得的自动驾驶汽车的速度为v_DR(t)，加速度为a_DR(t)，且里程计的分辨率为m，里程计的分辨率造成的速度误差δv_DR为：

δv_DR≤m (1)

定义自动驾驶汽车的真实速度为v(t)，RTK解算速度误差为δv(t)，惯性和里程计的数据采样率为f，RTK数据采样率为q，当信号不稳定造成RTK定位误差最大值为a时，RTK解算速度误差δv(t)在理想状态下满足下式：

为保证自动驾驶汽车定位精度要求，考虑RTK器件误差条件下，自动驾驶汽车允许的RTK最大速度误差满足：

其中，Δs为最大允许定位误差，

利用惯性和里程计差分原理解算获得的自动驾驶汽车的速度v_DR(t)与RTK解算的自动驾驶汽车的速度v_RTK(t)满足下式(4)时，认为RTK解算位置信息准确：

v_DR(t)-v_RTK(t)≤|δv(t)|+|δv_DR|＝w (4)

其中，w为为可接受范围内速度解算误差阈值。

进一步，步骤S12具体过程为:

定义在自动驾驶汽车定位精度内允许的RTK解算加速度误差为δa(t)，在自动驾驶汽车速度不超过最大速度时，RTK解算加速度误差满足：

其中，s表示在时间段[t_i，t_i-1]内的自动驾驶汽车的真实行驶距离；

在采样时间间隔内默认加速度为定值，则对上式(5)整理得到：

当RTK解算加速度误差δa(t)满足式(6)时，认为RTK解算位置信息准确。

进一步，步骤S13中，自动驾驶车载组合导航中RTK解算位置信息的有效性判断条件为：

其中，b为允许自动驾驶汽车行驶过程中的最大加速度。

进一步，步骤S21具体过程为:

定义自动驾驶汽车的坐标系为载体系，记为b系，设自动驾驶汽车的航向角为ψ，俯仰角为θ，横滚角为γ；定义地理坐标系为导航系，记为n系，根据坐标系转换原理计算出导航系与载体系的坐标转换矩阵为：

根据四元素解算出载体系与导航系的坐标转换矩阵为：

其中，q₀、q₁、q₂、q₃为四元素的系数，

令T₁₂＝2(q₁q₂-q₀q₃)，T₁₃＝2(q₁q₃+q₀q₂)，T₂₁＝2(q₁q₂+q₀q₃)，T₂₃＝2(q₂q₃-q₀q₁)，T₃₁＝2(q₁q₃-q₀q₂)，T₃₂＝2(q₂q₃+q₀q₁)，则记

由于n系到b系的旋转过程始终保持直角坐标系，则为正交矩阵：

则计算出自动驾驶汽车的姿态信息：

进一步，步骤S22具体过程为:

定义自动驾驶汽车的左轮的坐标为A(x_l,y_l)，右轮的坐标为B(x_r,y_r)，左轮角速度为w_l，右轮的角速度为w_r，左右轮的线速度分别为v_l、v_r，轴心线中点坐标为M(x,y)，

通过在汽车两侧轮子安装两个光电编码器，根据编码器输出的脉冲数反解出汽车两轮的行驶距离，设左轮光电编码器在单位时间Δt内的行驶路程为Δs_l，右轮光电编码器在单位时间Δt内的行驶路程为Δs_r，则左、右轮的线速度为：

其中，ΔN_l、ΔN_r分别是单位时间Δt内左、右轮光电编码器输出的脉冲数；P是车轮每转一圈输出的脉冲数；D是车轮直径，

则汽车两轮中轴线中心点速度v_M为：

设根据惯性解算的自动驾驶汽车当前时刻t_i的航向角为θ(t_i)，则从上一时刻t_i-1到当前时刻t_i的偏航角Δθ(t_i,t_i-1)为：

Δθ(t_i,t_i-1)＝Δθ(t_i)-Δθ(t_i-1) (15)

若上一时刻两轮中轴线中心点的地理位置为(x_M(t_i-1),y_M(t_i-1))，则当前时刻自动驾驶汽车所在位置为：

为保证实时性，自动驾驶汽车的两个数据采用点的时间间隔很小，时间间隔接近为0，即：

其中，c为定常值，

则式(16)化简为：

其中，x_M(t_i),y_M(t_i)为当前时刻自动驾驶汽车的地理位置坐标。

进一步，步骤S3具体过程为:

设，长时间失效后，RTK当前时刻定位点地理坐标为(x₁(t_i),y₁(t_i))，上一时刻定位点地理坐标为(x₁(t_i-1),y₁(t_i-1))，且里程计获取的当前时刻到上一时刻两点的路程增量为：

则RTK获取的相邻两点的路程增量为：

其中，R_N表示地球经度圈半径，R_M表示地球纬度圈半径，

定义惯性和里程计解算的姿态角为θ_DR(t_i,t_i-1)，RTK解算的姿态角为θ_RTK(t_i,t_i-1)，当两者解算参数满足下式(21)和(22)时，则认为RTK信号恢复正常，

δ(Δs(t_i,t_i-1))＝|Δs_RTK(t_i,t_i-1)-Δs_DR(t_i,t_i-1)|∈(δs_min,δs_max) (21)

δθ(t_i,t_i-1)＝|θ_RTK(t_i,t_i-1)-θ_DR(t_i,t_i-1)|∈(δθ_min,δθ_max) (22)

其中，δ(Δs(t_i,t_i-1))为RTK与惯性和里程计组合导航定位差，δs_min为置信区间最小值，δs_max为置信区间最大值，δθ(t_i,t_i-1)为RTK与惯性和里程计组合导航姿态误差，δθ_min为姿态可信度置信区间最小值，δθ_max为姿态可信度置信区间最大值。

本发明的有益效果：

当自动驾驶汽车在复杂环境条件下驾驶时，本发明所述方法可以在信号条件较弱时，对RTK定位是否有效进行判断，如果有效，按RTK技术提供自动驾驶汽车车载导航信息；若无效，采用惯性/里程计航位推算提供自动驾驶汽车行驶过程所需的导航信息。因此，本发明可以在RTK信号受阻的情况下，及时判断出RTK导航的异常，进而及时切换导航参数获取的途径，避免自动驾驶汽车脱离正确行驶的方向。

附图说明

图1为本发明实施例的自动驾驶惯性/里程计/RTK组合导航系统中RTK异常值的判断方法总体流程图；

图2为本发明实施例的RTK信号是否异常的判断示意图；

图3为本发明实施例的基于里程计差分原理的自动驾驶汽车航迹推算示意图。

具体实施方式

下面结合附图和实施例进一步描述本发明，应该理解，以下所述实施例旨在便于对本发明的理解，而对其不起任何限定作用。

图1示出了本发明实施例提供的自动驾驶惯性/里程计/RTK组合导航系统中RTK异常值的判断方法总体流程图，具体包括如下步骤：

S1：利用惯性/里程计组合导航参数建立判断基准，对自动驾驶车载组合导航中RTK定位精度的有效性进行判断。具体过程如下：

S11：建立惯性/里程计组合导航的速度阈值；

为保证自动驾驶汽车的正常行驶，本实施例要求车载导航定位精度在20cm之内，即可设当前时刻自动驾驶汽车的定位结果的置信区域为半径R＝20cm的圆，允许最大定位误差为Δs。可以认为RTK定位误差要求最大不超过20cm，即，Δs≤20cm，当超过该范围时就认定RTK解算位置信息异常。

设RTK解算的自动驾驶汽车的速度为v_RTK(t)，加速度为a_RTK(t)。利用惯性/里程计差分原理解算获得的自动驾驶汽车的速度为v_DR(t)，加速度为a_DR(t)，且里程计的分辨率为m＝0.01km/h。此时里程计的分辨率造成的速度误差δv_DR为：

δv_DR≤0.01km/h≈0.003m/s (1)

定义惯性/里程计的数据采样率f＝100Hz，RTK数据采样率q＝10Hz，且自动驾驶汽车的速度为0～60km/h。以最大速度为例，当自动驾驶汽车的真实速度v(t)为60km/h时，设RTK解算速度误差为δv(t)，当信号不稳定造成RTK定位误差最大值为20cm时，RTK解算速度误差δv(t)在理想状态下满足下式：

由于RTK器件存在系统误差，现阶段在系统误差影响下RTK产品可达到2-3cm的定位精度，极差为5cm。因此，为保证自动驾驶汽车定位精度要求，考虑RTK器件误差条件下，自动驾驶汽车允许的RTK最大速度误差应满足：

因此，惯性/里程计组合导航所解算的速度v_DR(t)与RTK解算的速度v_RTK(t)满足下式(4)时，可认为RTK解算位置信息准确：

v_DR(t)-v_RTK(t)≤|δv(t)|+|δv_DR|＝1.5003m/s (4)

S12：建立惯性/里程计组合导航的加速度阈值；

根据自动驾驶汽车的限制，设定汽车加速度不超过3m/s²。设在自动驾驶汽车定位精度内允许的RTK解算加速度误差为δa(t)，则在车速不超过60km/h时，RTK解算加速度误差满足的要求为：

其中，s表示在时间段[t_i，t_i-1]内的自动驾驶汽车的真实行驶距离。

由于自动驾驶汽车的加速度属于离散数据，因此在采样时间间隔内默认加速度为定值，则对上式(5)整理可得：

|δa(t)|≤15m/s² (6)

由式(6)可知，当RTK解算的自动驾驶汽车的加速度与惯性/里程计解算的加速度的误差δa(t)在15m/s²之内，则可认为RTK解算位置信息准确。

S13：判断RTK解算位置信息的有效性。

根据步骤S11和步骤S12的分析可知，自动驾驶汽车在行驶过程中，当RTK信号不稳定时，在20cm的定位精度要求下，允许的RTK解算的自动驾驶汽车的速度与惯性/里程计解算的速度的最大误差为1.5003m/s；而允许的RTK解算的自动驾驶汽车的加速度与惯性/里程计解算的加速度的最大误差为15m/s²。而由于无论以加速度还是速度作为RTK解算位置信息是否有效的判断条件，其都是关于时间的函数，且都是以定位精度要求反推的结果，因此，为尽可能保证自动驾驶汽车的定位精度，应同时满足步骤S11和步骤S12，则自动驾驶车载组合导航中RTK定位精度的有效性判断条件为：

|δv(t)|+|δv_DR|≤1.5003m/s and|δa(t)|≤15m/s² and a_DR(t)＜3m/s² (7)

S2：当判断自动驾驶车载组合导航中RTK定位精度无效时，航位推算预估自动驾驶汽车的当前位置信息。具体过程如下：

S21：基于惯性/里程计组合导航进行自动驾驶汽车的姿态解算。

定义自动驾驶汽车的坐标系为载体系，记为b系，设自动驾驶汽车的航向角为ψ，俯仰角为θ，横滚角为γ；定义地理坐标系为导航系，即东北天坐标系，记为n系，根据坐标系转换原理计算出导航系与载体系的坐标转换矩阵为：

根据四元素可以解算出载体系(b系)与导航系(n系)的坐标转换矩阵为：

其中，q₀、q₁、q₂、q₃为四元素的系数。

令T₁₂＝2(q₁q₂-q₀q₃)，T₁₃＝2(q₁q₃+q₀q₂)，T₂₁＝2(q₁q₂+q₀q₃)，T₂₃＝2(q₂q₃-q₀q₁)，T₃₁＝2(q₁q₃-q₀q₂)，T₃₂＝2(q₂q₃+q₀q₁)，则记

由于n系到b系的旋转过程始终保持直角坐标系，则为正交矩阵：

则可以计算出自动驾驶汽车的姿态信息：

通过上述的解算实现对自动驾驶汽车的姿态解算。

S22：基于里程计位置信息差分原理估计当前时刻自动驾驶汽车的位置。

如图3所示，定义自动驾驶汽车的左轮的坐标为A(x_l,y_l)，右轮的坐标为B(x_r,y_r)，左轮角速度为w_l，右轮的角速度为w_r，左右轮的线速度分别为v_l、v_r，轴心线中点坐标为M(x,y)，θ₁表示单位时间内自动驾驶汽车车轮航向角变化量；利用三角形的性质可知θ₂＝θ₁；θ₃＝θ₁。

里程计的工作原理是通过在汽车两侧轮子安装两个光电编码器，根据编码器输出的脉冲数反解出汽车两轮的行驶距离。设左轮光电编码器在单位时间Δt内的行驶路程为Δs_l，右轮光电编码器在单位时间Δt内的行驶路程为Δs_r。则左、右轮的线速度为：

其中，ΔN_l、ΔN_r分别是Δt时间内左、右轮光电编码器输出的脉冲数；P是车轮每转一圈输出的脉冲数；D是车轮直径。

则汽车两轮中轴线中心点速度v_M为：

设根据惯性解算的自动驾驶汽车当前时刻t_i的航向角为θ(t_i)，则从上一时刻t_i-1到当前时刻t_i的偏航角Δθ(t_i,t_i-1)为：

Δθ(t_i,t_i-1)＝Δθ(t_i)-Δθ(t_i-1) (15)

若上一时刻两轮中轴线中心点的地理位置为(x_M(t_i-1),y_M(t_i-1))，则当前时刻自动驾驶汽车所在位置为：

为保证实时性，自动驾驶汽车的数据更新频率一般很大(在1000kHz)，即两个数据采用点的时间间隔很小，时间间隔接近为0。即：

其中，c为定常值。

则式(16)可化简为：

综上所述，根据惯性/里程计航位推算解的当前时刻自动驾驶汽车的地理位置为(x_M(t_i),y_M(t_i))。

S3：在自动驾驶汽车RTK解算位置长时间失效后，判断RTK定位精度的有效性。

当RTK解算位置信息长时间无效时，由于惯性/里程计组合导航的误差已经积累很长时间，此时精度已经无法衡量RTK解算位置信息的有效性，为准确分析RTK解算位置信息的有效性，采用步长、航向约束判断方法。

由于RTK在信号充足时，具有定位精度、航向精度高的特点，而里程计长时间发散的原因是由于里程计标度误差造成的，标度误差会随时间累积，且惯性航向误差的累积也间接造成了里程计的误差累积，但其都有短期精度高的特点。本步骤的整体思路为：假设在上一点惯性/里程计定位准确的基础上，到下一点引入的误差只有一倍的里程计标度误差，和微弱的航向误差角，即相邻两点的距离量测信息精度仍非常高，且航向精度高。同理，RTK在信号恢复后，相邻两点的定位误差很小，且航向精度高。

基于上述分析，当RTK解算位置长时间失效后，若判断出RTK定位的相邻两点的距离与惯性/里程计相邻两点的定位满足一定阈值，且航向姿态满足一定阈值时，则认为RTK信号恢复正常。

设，长时间失效后，RTK当前时刻定位点地理坐标为(x₁(t_i),y₁(t_i))，上一时刻定位点地理坐标为(x₁(t_i-1),y₁(t_i-1))，且里程计获取的当前时刻到上一时刻两点的路程增量为：

则RTK获取的相邻两点的路程增量为：

其中，R_N表示地球经度圈半径，R_M表示地球纬度圈半径。

定义惯性/里程计解算的姿态角为θ_DR(t_i,t_i-1)，RTK解算的姿态角为θ_RTK(t_i,t_i-1)，当两者解算参数满足下式(21)和(22)时，则认为RTK信号恢复正常。

δ(Δs(t_i,t_i-1))＝|Δs_RTK(t_i,t_i-1)-Δs_DR(t_i,t_i-1)|∈(δs_min,δs_max) (21)

δθ(t_i,t_i-1)＝|θ_RTK(t_i,t_i-1)-θ_DR(t_i,t_i-1)|∈(δθ_min,δθ_max) (22)

其中，δ(Δs(t_i,t_i-1))为RTK与惯性/里程计组合导航定位差，δs_min为置信区间最小值，δs_max为置信区间最大值，δθ(t_i,t_i-1)为RTK与惯性/里程计组合导航姿态误差，δθ_min为姿态可信度置信区间最小值，δθ_max为姿态可信度置信区间最大值。

S4：当判断RTK定位精度有效时，对惯性/里程计组合导航参数进行姿态、位置的矫正，然后利用矫正后的惯性/里程计参数作为新的评价RTK定位精度是否有效的条件。

对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以对本发明的实施例作出若干变型和改进，这些都属于本发明的保护范围。

Claims (4)

1.一种自动驾驶组合导航系统中RTK异常值的判断方法，其特征在于，包括步骤：

S1：利用惯性和里程计组合导航参数建立判断基准，对自动驾驶车载组合导航中RTK定位精度的有效性进行判断，具体过程如下：

S11：建立惯性和里程计组合导航的速度阈值；

S12：建立惯性和里程计组合导航的加速度阈值；

S13：判断RTK解算位置信息的有效性；

S2：当判断自动驾驶车载组合导航中RTK定位精度无效时，航位推算预估自动驾驶汽车的当前位置信息，具体过程如下：

S21：基于惯性和里程计组合导航进行自动驾驶汽车的姿态解算；

S22：基于里程计位置信息差分原理估计当前时刻自动驾驶汽车的位置；

S3：在自动驾驶汽车RTK解算位置长时间失效后，判断RTK定位精度的有效性；

S4：当判断RTK定位精度有效时，对惯性和里程计组合导航参数进行矫正，然后将矫正后的惯性和里程计组合导航参数作为评价RTK定位精度是否有效的新条件；

步骤S11具体过程为：

定义RTK解算的自动驾驶汽车的速度为v_RTK(t)，加速度为a_RTK(t)；利用惯性和里程计差分原理解算获得的自动驾驶汽车的速度为v_DR(t)，加速度为a_DR(t)，且里程计的分辨率为m，里程计的分辨率造成的速度误差δv_DR为：

δv_DR≤m (1)

定义自动驾驶汽车的真实速度为v(t)，RTK解算速度误差为δv(t)，惯性和里程计的数据采样率为f，RTK数据采样率为q，当信号不稳定造成RTK定位误差最大值为a时，RTK解算速度误差δv(t)在理想状态下满足下式：

为保证自动驾驶汽车定位精度要求，考虑RTK器件误差条件下，自动驾驶汽车允许的RTK最大速度误差满足：

其中，Δs为最大允许定位误差，

利用惯性和里程计差分原理解算获得的自动驾驶汽车的速度v_DR(t)与RTK解算的自动驾驶汽车的速度v_RTK(t)满足下式(4)时，认为RTK解算位置信息准确：

v_DR(t)-v_RTK(t)≤|δv(t)|+|δv_DRv＝w (4)

其中，w为可接受范围内速度解算误差阈值；

步骤S12具体过程为:

定义在自动驾驶汽车定位精度内允许的RTK解算加速度误差为δa(t)，在自动驾驶汽车速度不超过最大速度时，RTK解算加速度误差满足：

其中，s表示在时间段[t_i，t_i-1]内的自动驾驶汽车的真实行驶距离；

在采样时间间隔内默认加速度为定值，则对上式(5)整理得到：

当RTK解算加速度误差δa(t)满足式(6)时，认为RTK解算位置信息准确；

步骤S13中，自动驾驶车载组合导航中RTK解算位置信息的有效性判断条件为：

其中，b为允许自动驾驶汽车行驶过程中的最大加速度。

2.根据权利要求1所述的方法，其特征在于，步骤S21具体过程为:

定义自动驾驶汽车的坐标系为载体系，记为b系，设自动驾驶汽车的航向角为ψ，俯仰角为θ，横滚角为γ；定义地理坐标系为导航系，记为n系，根据坐标系转换原理计算出导航系与载体系的坐标转换矩阵为：

根据四元素解算出载体系与导航系的坐标转换矩阵为：

其中，q₀、q₁、q₂、q₃为四元素的系数，

令T₁₂＝2(q₁q₂-q₀q₃)，T₁₃＝2(q₁q₃+q₀q₂)，T₂₁＝2(q₁q₂+q₀q₃)，T₂₃＝2(q₂q₃-q₀q₁)，T₃₁＝2(q₁q₃-q₀q₂)，T₃₂＝2(q₂q₃+q₀q₁)，则记

由于n系到b系的旋转过程始终保持直角坐标系，则为正交矩阵：

则计算出自动驾驶汽车的姿态信息：

3.根据权利要求1所述的方法，其特征在于，步骤S22具体过程为:

定义自动驾驶汽车的左轮的坐标为A(x_l,y_l)，右轮的坐标为B(x_r,y_r)，左轮角速度为w_l，右轮的角速度为w_r，左右轮的线速度分别为v_l、v_r，轴心线中点坐标为M(x,y)，

通过在汽车两侧轮子安装两个光电编码器，根据编码器输出的脉冲数反解出汽车两轮的行驶距离，设左轮光电编码器在单位时间Δt内的行驶路程为Δs_l，右轮光电编码器在单位时间Δt内的行驶路程为Δs_r，则左、右轮的线速度为：

其中，ΔN_l、ΔN_r分别是单位时间Δt内左、右轮光电编码器输出的脉冲数；P是车轮每转一圈输出的脉冲数；D是车轮直径，

则汽车两轮中轴线中心点速度v_M为：

设根据惯性解算的自动驾驶汽车当前时刻t_i的航向角为θ(t_i)，则从上一时刻t_i-1到当前时刻t_i的偏航角Δθ(t_i,t_i-1)为：

Δθ(t_i,t_i-1)＝Δθ(t_i)-Δθ(t_i-1) (15)

若上一时刻两轮中轴线中心点的地理位置为(x_M(t_i-1),y_M(t_i-1))，则当前时刻自动驾驶汽车所在位置为：

为保证实时性，自动驾驶汽车的两个数据采用点的时间间隔很小，时间间隔接近为0，即：

其中，c为定常值，

则式(16)化简为：

其中，x_M(t_i),y_M(t_i)为当前时刻自动驾驶汽车的地理位置坐标。

4.根据权利要求3所述的方法，其特征在于，步骤S3具体过程为:

设，长时间失效后，RTK当前时刻定位点地理坐标为(x₁(t_i),y₁(t_i))，上一时刻定位点地理坐标为(x₁(t_i-1),y₁(t_i-1))，且里程计获取的当前时刻到上一时刻两点的路程增量为：

则RTK获取的相邻两点的路程增量为：

其中，R_N表示地球经度圈半径，R_M表示地球纬度圈半径，

定义惯性和里程计解算的姿态角为θ_DR(t_i,t_i-1)，RTK解算的姿态角为θ_RTK(t_i,t_i-1)，当两者解算参数满足下式(21)和(22)时，则认为RTK信号恢复正常，

δ(Δs(t_i,t_i-1))＝|Δs_RTK(t_i,t_i-1)-Δs_DR(t_i,t_i-1)|∈(δs_min,δs_max) (21)

δθ(t_i,t_i-1)＝|θ_RTK(t_i,t_i-1)-θ_DR(t_i,t_i-1)|∈(δθ_min,δθ_max) (22)

其中，δ(Δs(t_i,t_i-1))为RTK与惯性和里程计组合导航定位差，δs_min为置信区间最小值，δs_max为置信区间最大值，δθ(t_i,t_i-1)为RTK与惯性和里程计组合导航姿态误差，δθ_min为姿态可信度置信区间最小值，δθ_max为姿态可信度置信区间最大值。