一种里程计结合双天线差分GNSS的组合导航方法与流程

本发明涉及导航定位技术领域，具体涉及一种里程计结合双天线差分gnss(globalnavigationsatellitesystem)的组合导航方法，适用于车辆的导航定位，主要是对于无人驾驶车辆的导航定位。

背景技术：

无人驾驶车辆，是一个集环境感知、动态决策与规划、行为控制与执行多种功能于一体的综合系统，是室外轮式移动机器人在交通领域中的重要应用。无人驾驶车辆使用车载激光雷达、毫米波雷达、摄像头、gnss、里程计和惯性器件传感器感知周围环境，获取自身位置和姿态，从而使车辆能够安全可靠地在道路上行驶，完成预定任务。

其中，gnss作为全球导航卫星系统(globalnavigationsatellitesystem)，泛指所有的卫星导航系统，包括全球的、区域的和增强的，还涵盖在建和以后要建设的其他卫星导航系统。国际gnss系统是个多系统、多层面、多模式的复杂组合系统，具有全天候、实时定位的特点，但是当gnss信号受到遮挡或者在gnss信号弱的区域，无人驾驶车辆不能接受到有效的gnss信号，另外，一般gnss接收机输出车辆位置的频率较低，无法满足无人驾驶车辆的精确定位需求。

在定位方面，目前无人驾驶车辆定位多采用高精度惯性测量单元imu(inertialmeasurementunit)，例如光纤惯导、激光惯导等，实现精确定位，但是高精度惯性测量单元imu成本很高，一般要40～50万人民币，显著增加了无人驾驶车辆的成本。

技术实现要素：

有鉴于此，本发明提供了一种里程计结合双天线差分gnss的组合导航方法，能够利用车辆前轮转向角度以及里程计输出的速度，结合双天线差分gnss的位置与航向进行组合导航，适用于无人驾驶车辆的低成本组合定位，可以满足无人驾驶车辆在城市环境无人驾驶时对车道线定位精度的要求。

本发明是通过以下技术方案来实现的：

实时判断gnss输出的位置是否满足精度要求，若满足，则使用gnss输出的位置更新车辆当前位置；否则，利用车辆当前位置预测值作为车辆当前位置；

其中，车辆当前位置预测值由车辆上一时刻位置以及上一时刻与当前时刻的位置变化量求和获得；其中上一时刻与当前时刻的位置变化量由车辆上一时刻航向、上一时刻与当前时刻的时间间隔以及里程计输出的车辆上一时刻速度决定；

其中车辆航向为：若gnss输出的航向满足精度要求，则gnss输出的航向作为车辆航向，否则以车辆上一时刻航向与上一时刻与当前时刻的航向变化量之和作为车辆航向；其中上一时刻与当前时刻的航向变化量由上一时刻车辆偏航角速度以及上一时刻与当前时刻的时间间隔决定；

其中，车辆偏航角速度由车辆前后轮轴距、车轮偏转角以及里程计输出的车辆速度决定；

基于车辆当前位置、车辆当前航向、里程计的当前速度以及当前时刻与下一时刻的时间间隔，预测下一时刻车辆位置；并将车辆导航到预测的车辆下一时刻位置，重复上述过程，实现车辆实时导航。

其中，判断gnss位置是否满足精度要求的方式为：

当gnss接收机输出的水平方向位置误差不大于水平方向位置误差设定值，且主天线解算的定位状态有效时，gnss位置满足精度要求，否则gnss位置不满足精度要求；

判断gnss航向是否满足精度要求的方式为：

当gnss速度与速度方差之比小于设定比值a，且gnss输出的航向偏差小于航向偏差设定值，双天线解算的定位状态有效时，认为双天线gnss航向满足精度要求，并使用双天线航向更新当前航向；当gnss速度与速度方差之比大于或等于a，并且主天线解算的定位状态有效时，认为主天线gnss航向满足精度要求，并使用主天线航向更新当前航向；当双天线gnss航向和主天线gnss输出航向均不满足上述要求时，则认为gnss航向不满足精度要求。

较佳地，当gnss位置满足精度要求时，使用gnss位置更新车辆位置，对于更新后的车辆位置利用车辆宽度对其进行补偿：

其中为更新后的车辆位置在wgs-84大地坐标系下的gauss-kruger投影，x，y为实际车辆位置在wgs-84大地坐标系下的高斯克吕格投影，w为车辆宽度，为车辆实时航向。

进一步地，gnss位置满足精度要求时，使用当前gnss位置更新车辆位置，车辆位置更新公式为：

其中为更新后的车辆位置，为车辆位置的历史值，为当前gnss接收机输出的车辆位置，σ为gnss接收机输出的水平位置方差，α为滤波常数。

较佳地，gnss航向满足精度要求时，使用当前gnss航向更新车辆航向，航向更新公式为：

其中为更新后的车辆航向，为航向的历史值，为当前gnss接收机输出的航向，β为滤波常数，当主天线解算时，ω取接收机输出的位置方差σ，当双天线解算时，ω取航向方差。

其中，当绝对值大于180°时，将加或者减360°使得转换到与相近的区间内，然后再带入航向更新公式更新航向；其中

较佳地，当gnss接收机输出的双天线航向满足精度要求时，记录主天线指向副天线的方向与车辆前进方向之间的夹角δ，若当前里程计给出的车速为正，则将当前夹角δ与前一时刻的夹角值取平均值，否则将当前夹角δ加180°，加180°后若超过360°则减360°，然后与前一时刻的夹角值取平均值；以平均值作为修正后的双天线航向与车辆前进方向的夹角δ。

较佳地，水平方向位置误差设定值为3m，a取值范围为4.0～10.0，航向偏差取值不大于1°。

有益效果：

(1)与传统的采用高精度imu与gnss组合的导航定位方式相比，本发明通过里程计和双天线差分gnss融合，结合车辆前轮转角即可实现高精度的定位，无须使用昂贵的惯性器件，使无人驾驶车辆的高精度定位成本降低很多，适用无人驾驶技术在城市环境中的商业化推广和应用。

(2)与传统的采用卡尔曼滤波融合imu与gnss数据不同，本发明采用位置方差与航向方差估计当前位置与航向的可信度，并预测下一时刻的位置与航向，不需要建立车辆自身的动力学模型，相比卡尔曼滤波需要精确的数学建模及反复调试测量方差矩阵和系统方差矩阵，本发明所述方法更为简单、效率更高。

附图说明

图1为本发明的里程计结合双天线差分gnss的组合导航方法的流程图。

图2为本发明中双天线差分gnss与车辆的位置关系。

图3为本发明中里程计与gnss接收机输出结果时序示意图。

图4为本发明的实验结果图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种里程计结合双天线差分gnss的组合导航方法，该方法不使用imu传感器，仅通过里程计和双天线差分gnss融合，结合车辆前轮转角即可实现高精度的定位。本发明利用车辆前轮转角与里程计的速度进行航位推算，使用双天线差分gnss对航位推算的位置和航向进行补偿，可以显著提高航位推算的精度。

本发明提供的组合导航方法基于双天线差分gnss、里程计、无人驾驶车辆和角度传感器来实现。双天线差分gnss接收机输出的数据包括水平位置误差、标志位、gnss速度、速度方差、航向与航向方差、车辆位置和水平位置方差。

本发明中的航向即车辆偏航角，为车辆车头朝向与北向的夹角，实际测量时测的是车运动方向与北向的夹角。

本发明提供的方法适用于操作简单的无人驾驶车辆定位导航，使用的硬件包括双天线差分gnss多模接收机(包含gps、glonass与北斗)、里程计、经过改装后的红旗h7轿车和车载计算机。本发明的流程图如图1所示，包括以下步骤：

利用车辆偏航角速度和里程计输出的车辆速度实时获得车辆位置和航向：

采用前轮偏转角度估计车辆偏航角速度，具体公式为：

其中为车辆偏航角速度，v为里程计输出的车辆速度，θ为车辆前轮偏转角，l为车辆前后轮轴距。

计算车辆偏航角：经积分得到的车辆偏航角变化量，当前时刻车辆偏航角与车辆偏航角变化量求和，得到下一时刻的车辆偏航角；

根据车辆当前位置，结合车辆偏航角与里程计输出的车辆速度v，获得车辆下一时刻的位置，从而实现无人驾驶车辆导航。

但是由于只利用车辆偏航角速度和里程计输出的车辆速度计算车辆航向与位置的方法随时间存在累积误差，因此本发明在此基础上结合双天线差分gnss，实时判断gnss位置和gnss航向是否满足精度要求。

当gnss位置满足精度要求时，使用当前gnss位置更新车辆位置，否则利用车辆当前位置预测值作为车辆当前位置；当gnss航向满足精度要求时，使用当前gnss航向更新车辆航向，否则利用对应当前时刻的车辆计算航向作为车辆当前航向；

车辆当前位置预测值由车辆上一时刻位置以及上一时刻与当前时刻的位置变化量求和获得；其中上一时刻与当前时刻的位置变化量由车辆上一时刻航向、上一时刻与当前时刻的时间间隔以及里程计输出的车辆上一时刻速度决定；

其中车辆航向为：若gnss输出的航向满足精度要求，则gnss输出的航向作为车辆航向，否则以车辆上一时刻航向与上一时刻与当前时刻的航向变化量之和作为车辆航向；其中上一时刻与当前时刻的航向变化量由上一时刻车辆偏航角速度以及上一时刻与当前时刻的时间间隔决定；

采用前轮偏转角度估计车辆偏航角速度具体公式为：

基于车辆当前位置、里程计的当前速度、车辆当前航向以及当前时刻与下一时刻的时间间隔，预测下一时刻车辆位置；并将车辆导航到预测的车辆下一时刻位置，重复上述过程，实现车辆实时导航。

判断gnss位置是否满足精度要求的方式为：当gnss接收机输出的水平方向位置误差不大于3.0m(根据车辆定位精度的实际需求设定)，且主天线解算的定位状态有效即solutionstatus标志位为0时，gnss位置满足精度要求，否则gnss位置不满足精度要求。

gnss位置满足精度要求时，使用当前gnss位置更新车辆位置，车辆位置更新公式为：

其中为更新后的车辆位置，为车辆位置的历史值，为当前gnss接收机输出的车辆位置，其中定位结果用wgs-84大地坐标下gauss-kruger(高斯克-吕格)投影表示，σ为gnss接收机输出的水平位置方差，α为滤波常数，本发明取6.0(该取值范围设为5.0～10.0，取值越大，位置更新越平稳，同时滞后效果越明显)。

由于主天线不在车辆中心而是位于车辆后备箱右侧，所以得到的定位位置并不是车辆实际位置，对于更新得到的车辆位置需要利用车辆宽度对其进行补偿：

其中为更新后的车辆位置在wgs-84大地坐标系下的高斯克吕格投影，x，y为实际车辆位置在wgs-84大地坐标系下的高斯克吕格投影，w为车辆宽度，为车辆偏航角。

判断gnss航向是否满足精度要求：当gnss速度与速度方差之比小于6.0(范围为4.0～10.0，根据车辆定位精度的实际需求设定)，且gnss输出的航向偏差小于0.5°(一般为了满足无人驾驶车辆高精度定位要求，该取值不大于1°)，双天线解算的定位状态有效即solutionstatus等于0时，此时双天线gnss航向满足精度要求，使用双天线航向更新当前航向；当gnss速度与速度方差之比大于或等于6.0，并且主天线解算的定位状态有效即solutionstatus等于0时，此时主天线gnss航向满足精度要求，使用主天线航向更新当前航向；双天线gnss航向和主天线gnss输出航向均不满足上述要求时，则gnss航向不满足精度要求。

考虑到gnss接收机输出的位置频率为5hz，车辆底层输出的偏航角速度频率为20hz，车辆行驶速度不超过20m/s，数据可根据实际需求做调整，所以更新航向的方法与更新位置的方法相似，均采用增大历史值权重的方法(α>1,β>1)。航向更新公式表示为：

其中为更新后的车辆航向，为航向的历史值，为当前gnss接收机输出的航向，β为滤波常数，本发明取20.0(该取值范围设为10.0～30.0，取值越大，航向更新越平稳，同时滞后效果越明显)，由于使用主天线定位解算时无法得到航向方差，所以当主天线解算时，ω取接收机输出的位置方差σ，当双天线解算时，ω取航向方差。

考虑到车辆航向在正北附近时，gnss接收机输出的航向在大于0°、小于360°附近跳动，利用航向更新公式对航向进行更新时，可能一个位于大于0°的小区间范围，另一个位于小于360°的小区间范围内，此时将带入航向更新公式更新航向，显然得到的与真实值产生很大偏差。所以本发明将当前gnss接收机输出的航向与原来航向历史值作差，结果记为当绝对值小于180°时，将带入航向更新公式更新航向，当绝对值大于180°时，将加或者减360°使得转换到与相近的区间内，然后带入航向更新公式更新航向。

考虑到双天线的安装位置在汽车后备箱左右两侧，如图2所示，从而主天线指向副天线的方向，其与车辆前进方向存在夹角δ，其中δ≈90°，由于双天线安装位置会有误差，δ不一定为90度。为了消除双天线安装位置带来的误差，本发明采用了存储夹角δ并动态估计δ以得到准确的主天线指向副天线的方向与车辆前进方向存在夹角，进而得到准确的双天线航向：当gnss接收机输出的双天线航向满足精度要求时，记录该夹角δ，若当前里程计给出的车速为正，则将当前夹角与上次存储的值取平均值，否则将当前夹角加180°，超过360°则减360°然后与上次存储的值取平均值；以所述平均值作为修正后的双天线航向与车辆前进方向的夹角δ。

本发明的算法是在ros(robotoperationsystem)操作系统上运行的，其中gnss接收机与车辆底层输出分别作为ros中的节点(node)，gnss接收机输出频率为5hz，无法满足无人驾驶车辆实际行驶的需要，而车辆偏航角速度和车辆速度输出频率为20hz，故本发明仅在gnss接收机输出有效时，利用其位置与航向更新无人驾驶车辆底层位置与航向，从而满足无人驾驶车辆行驶时对位置与航向的精度与更新率的要求。

在得到车辆偏航角后，结合里程计输出的速度就可以得到下一时刻的位置，循环执行该操作，直到下一次gnss接收机输出有效，利用gnss位置或航向更新车辆位置或航向。考虑到更新航向或位置之后，利用车辆偏航角速度和里程计输出的车辆速度积分得到下一时刻的位置时，gnss接收机输出为gnss时间，里程计与偏航角速度积分环节使用的为车载计算机的时间，所以在gnss更新位置或航向后进入车辆偏航角速度和里程计输出的车辆速度积分环节时，需要统一时间。统一时间的具体做法为当收到gnss接收机输出的二进制数据帧时，记录当前ros系统时间，在解算数据帧得到位置、速度或航向后，在ros中发布消息，消息不仅包含gnss解算结果，而且含有该结果对应的系统时间。在之后的更新位置或航向操作中均使用ros系统时间，不含gnss时间。如图3所示，使用gnss航向更新车辆航向后，首次积分使用里程计输出时的ros系统时间减gnss解算时记录的ros系统时间，其后积分使用相邻里程计输出之间的时间差。

最后本发明通过ros操作系统的节点发出位置、速度、航向和时间等信息。本发明经过实际上车测试，测试结果如图4所示，*表示整个行车过程中双天线差分gnss接收机输出的定位结果，连线为里程计航位推算的结果，无人驾驶车辆使用连线表示的定位结果实现无人驾驶。从图4可以看出当通过立交桥或天桥时，gnss接收机由于受到遮挡没有定位输出，此时使用本发明的组合算法可以得到较为理想的定位结果。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。