一种基于车联网的多车联合绝对定位的方法与流程

本发明涉及多源融合导航领域，尤其是涉及一种车联网环境下融合多车的全球卫星导航系统(globalnavigationsatellitesystem,gnss)、惯性导航系统(inertialnavigationsystem,ins)、激光雷达(lightdetectionandranging,lidar)数据实现车辆的高精度绝对定位的方法。

背景技术：

随着自动驾驶和物联网的发展，智能汽车和互联汽车越来越受到关注，这两者都需要在运行中精确定位汽车，目前汽车的高精度导航定位主要依赖于全球导航卫星系统(gnss)，惯性导航系统(ins)构成的组合导航系统等。gnss是在组合导航系统获得高精度位置的关键传感器，在开阔环境下，gnss-rtk(realtimekinematic)可以提供实时厘米级的绝对位置，但是由于在车辆的实际运行环境中会出现高架、城市峡谷等遮挡环境，可见卫星数快速下降，卫星几何结构差，rtk很难获取模糊度固定解，很难实现厘米级的高精度定位。随着车联网的发展，多个车辆之间的数据可以实现实时通信，利用多个车辆的gnss数据可有效的改善单车的gnss观测的卫星几何结构，所以如何利用车联网的特点，将多车的gnss数据融合处理实现车辆的高精度绝对定位是十分有必要且具有重要意义。

技术实现要素：

针对上述问题，本发明提出了一种基于车联网的多车联合绝对定位的方法，充分的利用了智能车辆配备的激光雷达环境感知传感器，利用激光雷达同步定位与制图技术(simultaneouspositionandmapping,slam)技术和特征匹配的技术实现车辆间的高精度相对定位。基于激光雷达获取的高精度相对位置信息，辅助车辆间的相对模糊度估计，提高车辆间相对模糊度解算的性能。最后利用多车的gnss数据构建多车联合rtk的模型，在模型中加入车辆间的高精度相对位置信息和相对模糊度信息约束，从而实现多车联合rtk的模糊度固定，相比单车rtk模型极大提高了模糊度固定的性能，可以实现在遮挡环境下的车辆高精度绝对定位。

本发明的技术方案为一种基于车联网的多车联合绝对定位的方法，包括以下步骤：

步骤1，各辆车根据当前gnss的定位结果与ins进行ekf松组合获取当前历元k的导航信息包括位置、姿态速度其中n表示导航系，b表示ins坐标系，姿态表示b系相对于n系的方向余弦矩，k表示卡尔曼滤波器中第k个滤波历元；

步骤2.将车辆上激光雷达采集的静态参照物的点云数据进行坐标转换，使当前历元点云观测按当前帧点云观测时相对于前一历元占有似然图的激光雷达三维位置和姿态角更新前一历元占有似然图中栅格占有概率，形成概率拟合像，即当前历元的占有似然图，从而将b系的点云数据转换到n系下占有似然图中；

所述占有似然图由q层虚拟栅格平面组成，每层虚拟栅格平面是由q×q个大小相同的虚拟栅格组成正方形平面，所述栅格能根据栅格占有概率更新规则形成点云数据的概率拟合像；

步骤3.根据步骤2中得到的车辆及周围联网车辆的点云观测相对各自车辆的占有似然图的激光雷达三维位置和姿态角，将车辆当前的点云帧分别与周围联网车辆存储的占有似然图依据进行匹配，从而获取车辆点云观测相对周围联网车辆的占有似然图的激光雷达三维位置和姿态角，从而获取车辆与周围联网车辆的在n系下的相对位置；

所述车辆与周围联网车辆通过短程通信进行通信连接，短程通信距离不大于所述激光雷达每次扫描的点云观测的画面横向距离，步骤3中的车辆及周围联网车辆的点云观测是各车辆的激光雷达按相同的频率从初始扫描方向角开始沿相同旋转方向旋转扫描的同一历元的点云观测，所述旋转方向即激光雷达沿顺时针或逆时针对车辆四周360o扫描，车辆和周围联网车辆的激光雷达的初始扫描方向角相同；

步骤4.根据两两组队的车辆的gnss观测数据和步骤3获得的车辆与周围联网车辆的在n系下的相对位置，利用gnssrtk的方法解算两两组队的车辆间的相对模糊度

步骤5.将当前历元所有参与解算的联网车辆的gnss观测，与附近的基准站ref数据构成联合rtk双差观测方程，将步骤3计算的车辆间相对位置加入到联合rtk观测方程中，再将步骤4计算车辆间的相对模糊度作为约束条件，利用附加约束条件的最小二乘的方法对模糊度浮点解进行求解，然后利用lambda搜索整数模糊度，获取模糊度固定解及相应的位置固定解，从而解算所有参与解算的联网车辆当前历元的绝对位置；

步骤6.将步骤5中得到的车辆的绝对位置重复步骤1与ins进行ekf松组合，获取第k个历元下的车辆的组合导航信息，包括位置姿态速度

进一步地，步骤2包括以下步骤：

步骤2.1.根据标定好的lidar相对于ins的安装角将lidar点云数据转换到ins坐标系b系下；

步骤2.2.生成占有似然图，所述占有似然图的范围为400×400×400m，并具有以下四级网格：第一级栅格的分辨率为5×5×5cm，第二级栅格的分辨率为10×10×10cm，第三级栅格的分辨率为20×20×20cm，第四级栅格的分辨率为40×40×40cm；

设置占有似然图中栅格占有概率更新规则为：

初始化各个占有似然图的初始占有概率为0并存储，设此时的占有似然图为m0；

激光脚点所在的栅格为中心栅格，中心栅格、非中心栅格的占有概率value按下式计算：

其中abs(dx)、abs(dy)占有似然图xoy平面上为非中心栅格在占有似然图xoy平面的x和y方向距离中心栅格的栅格个数，abs(dz)为占有似然图zoy平面上非中心栅格在占有似然图zoy平面的z方向距离中心栅格的栅格个数，γ表示立面位置系数；

比较计算得到的各栅格占有概率于已存的各栅格占有概率的大小，按下式更新各栅格占有概率：

value0为该栅格已存在的栅格占有概率值；

步骤2.3.第一帧点云scan1观测时，设激光雷达中心点的三维位置px,py,pz设为(0，0，0)，设其相对于占有似然图的旋转余弦矩阵为为根据步骤1求得的第一帧点云观测时的余弦矩阵，基于三维位置和余弦矩阵将观测的点云进行坐标转换，坐标转换公式如下：

其中[laserxlaserylaserz]^t为转换前的脚点坐标，为转换后的脚点坐标，根据转换后激光脚点的坐标找到对应的栅格，并根据步骤2.2中的更新规则对步骤2.2生成的占有似然图中各栅格占有概率进行更新，得到m1；

步骤2.4.利用高斯牛顿迭代法将下一帧点云scan2与m1进行scan-to-map匹配获得点云观测时的激光雷达三维位置与相对于占有似然图旋转的姿态角；

高斯牛顿法实现scan-to-map匹配的准则如下：

为scan2观测时，激光雷达的中心点的三维位置和余弦矩阵对应的姿态角向量的初值，其三维位置px,py,pz的初值设为上一帧点云scan1观测时的位置，横滚角θ、俯仰角γ和航向角的初值通过imu的机械编排从上一帧点云scan1观测时的姿态角来推算获取，δt为相对于初始位置和初始姿态角的改正数，其中i为scan2的第i个激光脚点，为点云中包含的点数，t为点云帧观测的历元，s(t)为激光脚点基于t进行坐标转换，m(s)为转换后坐标所占栅格的栅格占有概率，根据高斯牛顿法求解有：

其中为在所占栅格处的梯度，为在t处的导数，则匹配得到的三维位置和姿态角为t^*＝t+δt。

scan-to-map匹配的过程中按照网格级数从后向前逐级匹配，后一级栅格匹配结果作为前一级栅格的初始值，直到第一级栅格，即从低分辨率网格匹配开始逐步到与高分辨率网格，低分辨率网格匹配的结果作为高分辨率网格匹配的初始值直到最高分辨率网格；

步骤2.5.根据步骤2.4的匹配获得的三维位置和姿态角，将姿态角转换为余弦矩阵，转换公式为：再根据步骤2.3的坐标转换公式将scan2进行坐标转换，进而根据转换后的脚点坐标所占的栅格位置，对m1中栅格的占有概率进行更新，得到m2；

步骤2.6.重复步骤2.4～步骤2.5迭代，利用后续的点云帧数据对占有似然图进行更新。

进一步地，步骤3包括以下步骤：

车俩c1观测到的当前点云帧为其存储的占有似然图为其对应的坐标系为n1系,从步骤2中获取的车辆c1当前历元的点云帧的激光雷达三维位置和姿态角信息为t^*c1为n1系下的位姿，同样的，周围联网车辆c2观测到的当前点云帧为其存储的olm占有似然图为其对应的坐标系为n2系,从步骤2中获取的周围联网车辆c2当前历元的点云帧的激光雷达三维位置和姿态角信息为t^*c2，根据和t^*c1，基于步骤2.4中的高斯牛顿的scan-to-map匹配方法获取到周围联网车辆c2当前历元下在n1系下的激光雷达三维位置和姿态角信息从)而获取当前历元下车辆c1和周围联网车辆c2在n系下的相对位置：

根据和t^*c2，基于步骤2.4中的高斯牛顿的scan-to-map匹配方法获取到车辆c1当前历元下在n2系下的激光雷达三维位置和姿态角信息t^*c2-c1，从而获取车辆c1当前历元下和周围联网车辆c2在n系下的相对位置对两次求得的车辆c1和周围联网车辆c2在n系下的相对位置取平均值更车辆c1和周围联网车辆c2的在n系下的相对位置为：

进一步地，步骤4包括以下步骤：步骤4.1.根据两两组队的车辆的gnss观测数据建立双差观测方程并线性化：

其中δ▽为双差算子，为双差伪距观测值，为双差载波相位观测值，b表示流动站，a表示参考站，w和j表示卫星号，λ为对应频率载波的波长，为双差卫地距，为对应频率载波的双差模糊度，为伪距双差后的残余误差，为载波相位双差后的残余误差；

以上双差观测方程以两辆车构成的一条基线为例，双差后忽略轨道误差和大气延迟误差；

双差观测方程线性化后的方程为：

其中(l,m,`n)为gnss天线到卫星视线方向的单位向量，w为参考星，为线性化时近似双差卫地距，(b0,l0)为线性化时近似坐标的纬度和经度；

4.2.根据线性化的双差观测方程建立模糊度方程组：

其中为观测向量，为位置向量，为n系下的相对位置向量，为模糊度向量，和分别为位置向量和模糊度向量的系数矩阵。

4.3.在该模糊度方程组中，根据步骤3获取的车辆间在n系下的相对位置基于阻尼lambda方法对模糊度向量进行解算：

根据阻尼最小二乘计算的浮点解y％，其法方程为：

其中p为观测值的权阵，px为阻尼因子；

4.4.基于lambda方法根据的浮点解y％估计y的固定解固定解即为车辆间的相对模糊度。

进一步地，步骤5中：车辆c1和周围联网车辆c2的联合rtk双差观测方程为：

j′＝1,2,3,5

j″＝5,6,7,8

车辆c1观测到卫星j’：1、2、3、5和w4，周围联网车辆c2观测到卫星为w4、和j”：5、6、7、8，以w4号星为参考星，

方程中的参数与步骤4中相同，将步骤3获取的车辆c1和周围联网车辆c2在n下的相对位置加入到联合rtk的观测模型中，线性化则有：

j′＝1,2,3,5

j″＝5,6,7,8

de＝e2,0+dec2-(e1,0+dec1)

du＝u2,0+duc2-(u1,0+duc1)

其中，和分别为车辆c1和周围联网车辆c2在n系下的初始位置；

将步骤4中得到的车辆c1和周围联网车辆c2的相对模糊度约束加入到联合rtk的观测模型中有：

j′＝1,2,3,5

j″＝5,6,7,8

de＝e2,0+dec2-(e1,0+dec1)

du＝u2,0+duc2-(u1,0+duc1)

即为步骤4获取的辆车1和周围联网车辆2之间公共的w4-j5号卫星对的相对模糊度；

最后利用附加约束条件的最小二乘方法求取每辆车的模糊度浮点解，式为约束条件，可以改写为则附加约束条件的最小二乘的法方程为：

其中k′为拉格朗日乘数，c为根据约束条件求取的一个系数矩阵，解该方程得到模糊度浮点解后即可利用lambda方法进一步估计每辆车模糊度固定解，进而获取每辆车的位置固定解。

本发明处理方法清晰，可操作性强，充分考虑了智能汽车和网联汽车的发展趋势，能够利用激光雷达数据在多车gnss数据间建立起高精度的约束关系，从而能够将多车的gnss数据结合起来提高单车的卫星观测几何结构，利用多车联合rtk方法可以极大的提高rtk的模糊度解算性能，尤其是在遮挡环境下，仍然可以有较高的模糊度固定率实现车辆的高精度定位。

附图说明

图1为本发明实施例的流程图；

图2为本发明实施例的olm更新示意图；

图3为本发明实施例中的车辆观测点云示意图；

图4为本发明实施例的基于激光雷达的车辆间相对位置误差图；

图5为本发明实施例的基于相对位置约束的车辆间lambda相对模糊度固定的ratio和成功率随历元变化图；

图6为本发明实施例的单车gnss卫星星空图和多车联合后的gnss卫星星空图；

图7为本范明实施例的联合rtk的lambda模糊度固定的ratio和成功率随历元变化图；

图8为本范明实施例的单车rtk和联合rtk的定位误差随历元变化图；

具体实施方式

本发明所提供的一种基于车联网的多车联合绝对定位的方法，利用智能车辆配备的gnss、ins、激光雷达等传感器，车联网情况下多车数据可以联合处理的特性，将多车的gnss、ins和激光雷达数据联合处理。该方法是一个逐历元处理的过程，每辆车利用自身携带的gnss的位置结果和ins进行ekf松组合估计当前车辆的组合导航结果包括位置、速度和姿态。其次，通过多车的激光雷达数据实现车辆间的高精度相对定位，其主要利用了基于ins辅助的激光雷达的同步定位与制图方法(slam)，帧与地图匹配的特征匹配方法等；然后在车辆之间构建gnss双差观测模型，利用激光雷达获取的车辆间的高精度相对位置作为约束，辅助车辆间的双差相对模糊度的解算，提高相对模糊度的固定率；进而，构建联合rtk模型，将多车的gnss数据分别与基准站构建双差观测模型，将多车的位置和模糊度联合解算，同时在模型中顾及车辆间高精度相对位置约束和相对模糊度约束，同时估计各个车辆的绝对位置；最后将各个车辆的绝对位置结果。该方法可以加入到已有的车辆gnss/ins组合导航系统，实现实际应用，下面结合附图和具体实施例对本发明作进一步的详细说明。

如图1所示，实施例的流程具体包括以下步骤：

步骤1，各辆车根据当前gnss的定位结果与ins进行ekf松组合获取当前历元k的导航信息包括位置姿态速度其中n表示导航系，b表示ins坐标系，姿态表示b系相对于n系的方向余弦矩，k表示卡尔曼滤波器中第k个滤波历元，包括以下步骤：

步骤1.1.根据ins误差方程构建ekf状态转移方程：δxk＝φkδxk-1+wk，其中δxk和δxk-1分别表示第k和第k-1个滤波历元的状态误差向量，φk为基于ins误差方程离散后的状态转移矩阵，wk为驱动白噪声；

第k个滤波历元的状态误差向量δxk是根据gnss的定位位置结果与imu机械编排预测的位置结果之差，及观测矩阵hk为常量、观测量的驱动白噪声vk，vk是符合高斯分布的噪声，构成ekf测量方程后求得：

ekf测量方程：zk＝hkδxk+vk；

i3×3表示3×3单位矩阵；

φk和wk在现有技术中采用如下过程求得：

误差扰动分析是一种经典的ins误差分析方法，该方法将实际导航参数在其真值附近进行泰勒展开，保留至一阶误差项，得到ins误差微分方程，基于误差扰动分析得到的姿态微分方程、速度微分方程、位置微分方程为：

姿态微分方程反应了计算导航系(n′系)相对于理想导航系(n系)的失准角变化规律，其表达式如下：

其中，表示n系相对于i系的旋转，它包含两部分：地球自转引起的导航系旋转，以及惯导系统在地球表面附近移动因地球表面弯曲而引起的n系旋转，即有其中

ωie为地球自转角速率；l和h分别为地理纬度和高度；为导航系计算误差，为陀螺测量误差，vn,ve分别为北向速度和东向速度，rm,rn分别为子午圈主曲率半径和卯酉圈(东西圈或主垂线)主曲率半径。

速度微分方程表达了惯导系统的计算速度与理想速度之间的偏差的变化规律，其表达式如下：

其中δgⁿ分别为地球自转角速度计算误差、导航系旋转计算误差和重力误差。

位置微分方程的表达式为：

其中，δl，δλ和δh分别表示纬度误差、经度误差和高度误差，并且记惯导速度分量vⁿ＝[vevnvu]^t和速度误差分量δvⁿ＝[δveδvnδvu]^t。

陀螺和加速度计的测量误差可以简写为：

δf＝ba+wa

ba和bg可以建模成一阶高斯马尔可夫过程，其微分方程可以表示为：

其中，和分别为加速度计零偏和陀螺零偏的一阶高斯马尔可夫驱动白噪声；和分别为相应的相关时间。

连续型卡尔曼滤波的状态方程形式为：状态向量中考虑姿态误差、速度误差、位置误差和惯导零偏误差等15维状态即：

其中将上述姿态误差微分方程、速度微分方程、位置微分方程、陀螺和加速度计的测量误差微分方程表达成状态向量x的微分方程有：

其中，

上述式子是连续型卡尔曼滤波的状态方程，离散时间卡尔曼滤波需要将上述方程离散化，得到离散时间误差状态方程：

xk+1＝φk+1/kxk+ωk

其中，

在时间间隔很短的情况下，在积分区间内可以认为保持不变，则状态转移矩阵保留一次项近似为：

φk+1/k≈exp(f(tk)δt)＝i+f(tk)δt

驱动白噪声ω(t)等效离散化处理之后，ωk为白噪声序列。

步骤1.2.利用ekf滤波器实时估计出状态误差向量δxk后对imu机械编排推算的位置速度和姿态进行修正反馈：

其中为第k个滤波历元下的位置误差，为第k个滤波历元下的速度误差，为ins坐标系b系相对于导航系n系的姿态角误差，表示加速度计零偏，表示陀螺零偏，i指单位矩阵，×表示向量的反对称矩阵；

其中，imu机械编排公式和ekf的完整公式可以参见文献：shineh.2005.estimationtechniquesforlow-costinertialnavigation.ucgereport,2005,20219.

步骤2，基于惯性测量单元imu辅助的lidar-slam技术的olm生成，基于imu辅助的lidar-slam技术的olm生成主要包括imu机械编排、基于lidar的scan-to-map匹配和olm的生成与更新等方法。

步骤2.1.根据标定好的lidar相对于ins的安装角将lidar点云数据转换到ins坐标系b系下：

lidar和ins的安装角标定通过预设平面的方法进行获取：首先通过全站仪测量平面在东北天坐标系下的平面参数，然后使用gnss/ins组合导航系统初始化，从lidar观测的点云中提取出点云平面上的点云进而计算点云平面在lidar坐标系下的平面参数，观测不少于3个点云平面即可通过最小二乘法获取lidar相对于东北天坐标系的位置和姿态；

同时根据此时已知的ins相对于东北天坐标系的位置和姿态，计算lidar相对ins的安装角，此安装角是事先在专门的标定场进行标定，在后文中假设lidar坐标系已转到ins坐标系，即每个历元的lidar的点云的脚点坐标均为ins坐标系下的坐标。

lidar安装在车辆车顶，lidar的安装角使车辆上的激光雷达对准车辆四周水平按相同角度扫描，激光雷达lidar是在水平方向360o旋转扫描获取点云观测的装置，激光雷达安装在所有车辆的初始扫描方向角相同，初始扫描方向角即激光雷达初始观测方向与该车辆车头朝向的方向向量。

步骤2.2.生成占有似然图(olm)，所述占有似然图(olm)的范围为400×400×400m，并具有以下四级网格：第一级栅格的分辨率为5×5×5cm，第二级栅格的分辨率为10×10×10cm，第三级栅格的分辨率为20×20×20cm，第四级栅格的分辨率为40×40×40cm；

设置占有似然图(olm)中栅格占有概率更新规则：

初始化各个占有似然图的初始占有概率为0并存储，设此时的olm为m0；

如图2所示，激光脚点所在的栅格为中心栅格，中心栅格、非中心栅格的占有概率value按下式计算：

其中abs(dx)、abs(dy)占有似然图xoy平面上为非中心栅格在占有似然图xoy平面的x和y方向距离中心栅格的栅格个数，abs(dz)为占有似然图zoy平面上非中心栅格在占有似然图zoy平面的z方向距离中心栅格的栅格个数，γ表示立面位置系数；

比较计算得到的各栅格占有概率于已存的各栅格占有概率的大小，按下式更新各栅格占有概率：

value0为该栅格已存在的栅格占有概率值。

步骤2.3.第一帧点云scan1的点云坐标进行转换，根据转换后的点云坐标对步骤2.2生成的olm占有似然图更新各栅格占有概率得到m1：

将第一帧点云scan1观测时，设激光雷达中心点的三维位置px,py,pz设为(0，0，0)，设其相对于olm的旋转余弦矩阵为为根据步骤1求得的第一帧点云观测时的余弦矩阵，基于三维位置和余弦矩阵将观测的点云进行坐标转换，坐标转换的公式如下：

其中[laserxlaserylaserz]^t为转换前的脚点坐标，为转换后的脚点坐标，根据转换后激光脚点的坐标找到对应的栅格，并根据步骤2.2中的更新规则对步骤2.2生成的olm占有似然图中各栅格占有概率进行更新，得到m1；

步骤2.4.将下一帧点云scan2与步骤2.3得到的m1进行匹配获得点云观测时的激光雷达三维位置与相对于olm旋转的姿态角：

下一帧点云scan2观测时激光雷达的中心点三维位置和相对于olm旋转的姿态角通过scan2与m1进行匹配获得：

利用高斯牛顿迭代法将点云scan2与步骤2.3得到的m1进行匹配获得点云观测时的激光雷达三维位置与相对于olm旋转的姿态角，实现scan-to-map匹配，高斯牛顿法实现scan-to-map匹配的准则如下：

为scan2观测时，激光雷达的中心点的三维位置和余弦矩阵对应的姿态角向量的初值，其三维位置px,py,pz的初值设为上一帧点云观测时的位置，横滚角θ、俯仰角γ和航向角的初值通过imu的机械编排从上一帧点云观测时的姿态角来推算获取，δt为相对于初始位置和初始姿态角的改正数，其中i为scan2的第i个激光脚点，为点云中包含的点数，t为点云帧观测的历元，s(t)为激光脚点基于t进行坐标转换，m(s)为转换后坐标所占栅格的栅格占有概率，根据高斯牛顿法求解有：

其中为在所占栅格处的梯度，为在t处的导数，则匹配得到的三维位置和姿态角为t^*＝t+δt。

scan-to-map匹配的过程中按照网格级数从后向前逐级匹配，后一级栅格匹配结果作为前一级栅格的初始值，直到第一级栅格，即从低分辨率网格匹配开始逐步到与高分辨率网格，低分辨率网格匹配的结果作为高分辨率网格匹配的初始值直到最高分辨率网格；

步骤2.5.生成下一帧点云历元的m2:

根据步骤2.4得到t^*中的位置和姿态角信息，即可将scan2中的脚点坐标进行坐标转换，转换时，需要先将t^*中的姿态角信息转换为余弦矩阵，其转换公式为：

再根据激光雷达中心点的三维位置和余弦矩阵将scan2进行转换，根据转换后的脚点坐标所占的栅格位置，对m1中栅格的占有概率进行更新，得到m2。

步骤2.6.重复步骤2.4～步骤2.5迭代，利用后续的点云帧数据对olm进行更新：

直到处理一定时间窗口内的的lidar点云数据得到该窗口时段的olm即mt，该窗口olm利用了该时间窗口内的t帧lidar点云观测逐步对olm进行更新。

其中，imu机械编排的完整公式可以参见文献：shineh.2005.estimationtechniquesforlow-costinertialnavigation.ucgereport,2005,20219.

步骤3.车联网的通信主要基于专用短程通信(dsrc)，所以当车辆之间距离较近时，车辆的激光雷达均同时按相同频率丛初始扫描方向角开始沿顺时针或逆时针对车辆四周360o进行扫描，车辆存储的olm占有似然图和当前观测到点云帧可以在车辆间共享，当车辆间的点云观测中存在相同的静态特征时，如图3所示，灰色为车c1观测的点云，黑色为车c2观测的点云，车c1和车c2采用vlp-16线激光雷达(水平视场角360°)，可以在同一历元共同观测到高架桥墩，此时可以利用scan-to-map实现车辆间在n系下的相对定位。将车辆当前的点云帧分别与周围联网车辆存储的olm占有似然图进行重复2.4的scan-to-map匹配，从而获取车辆之间在n系下的相对位置。

以两辆车为例，设车c1观测到的当前点云帧为其存储的olm占有似然图为其对应的坐标系为n1系,从步骤2中获取的当前历元的点云帧的激光雷达三维位置和姿态角信息为t^*c1为n1系下的位姿，同样的，车c2观测到的当前点云帧为其存储的olm占有似然图为其对应的坐标系为n2系,从步骤2中获取的当前历元的点云帧的激光雷达三维位置和姿态角信息为t^*c2，这里认为n1系和n2系的原点不同，但是均与n系的轴向相同，根据和t^*c1，基于步骤2.4中的高斯牛顿的scan-to-map方法获取到当前历元下车辆c2在n1系下的激光雷达三维位置和姿态角信息从而)获取当前历元下车辆c1和车辆c2在n系下的相对位置：

同样地，根据和t^*c2，基于步骤2.4中的高斯牛顿的scan-to-map方法获取到当前历元下车辆c1在n2系下的激光雷达三维位置和姿态角信息t^*c2-c1，从而获取当前历元下车c1和车c2在n系下的相对位置对两次求得的车c1和车c2在n系下的相对位置取平均值更新车c1和车c2的在n系下的相对位置为：

图4即为获取的每个历元下车辆间的相对位置在东方向、北方向和天向方向下的相对定位误差，其相对定位误差在三个方向上均在厘米级，说明相对定位精度可以达到厘米级。

步骤4.根据两两组队的车辆的gnss观测数据利用gnssrtk的方法解算两两组队的车辆间的相对模糊度

gnssrtk的方法包括以下步骤：

步骤4.1.根据两两组队的车辆的gnss观测数据建立双差观测方程并线性化：

其中δ▽为双差算子，为双差伪距观测值，为双差载波相位观测值，b表示流动站，a表示参考站，w和j表示卫星号，λ为对应频率载波的波长，为双差卫地距，为对应频率载波的双差模糊度，为伪距双差后的残余误差，为载波相位双差后的残余误差；

以上双差观测方程以两辆车构成的一条基线为例，双差后忽略轨道误差和大气延迟误差；

双差观测方程(李征航.gps测量与数据处理[m].武汉大学出版社,2013.)其线性化后的方程为：

其中(l,m,`n)为gnss天线到卫星视线方向的单位向量，w为参考星，为线性化时近似双差卫地距，(b0,l0)为线性化时近似坐标的纬度和经度；

4.2.根据线性化的双差观测方程建立模糊度方程组：

其中为观测向量，为位置向量，为n系下的相对位置向量，为模糊度向量，和分别为位置向量和模糊度向量的系数矩阵。

4.3.在该模糊度方程组中，根据步骤3获取的车辆间在n系下的相对位置基于阻尼lambda方法对模糊度向量进行解算：

根据阻尼最小二乘计算的浮点解y％，其法方程为：

其中p为观测值的权阵，px为阻尼因子，其计算可以参见文献：刘根友,朱耀仲,&韩保民.(2004).gps单历元定位的阻尼lambda算法.武汉大学学报(信息科学版),29(3),195-197.

4.4.基于lambda方法根据的浮点解y％估计y的固定解

lambda方法可以参见文献：teunissen,p.j.g.,dejonge,p.,&tiberius,c.c.j.m.(1996).thelambdamethodforfastgpssurveying.inproceedingsofinternationalsymposium’gpstechnologyapplications’,bucharest,romania,september26(vol.29,pp.203-210).

固定解即为车辆间的相对模糊度。

图5即为开阔环境下、高架桥环境下，车辆相对模糊度在考虑相对位置约束情况和不考虑相对位置约束下lambda模糊度固定的ratio值和成功率随历元的变化，在考虑相对位置约束时，其ratio值和成功率均有显著的提升，即使在高架桥环境下，公共卫星数较少，仍然可以实现相对模糊度固定。

步骤5，将多车的gnss数据分别与基准站ref数据利用rtk的方法构建联合rtk双差观测方程，将多个车辆的双差方程联合解算，获取每辆车的绝对位置。基准站ref是对卫星导航信号进行长期连续观测，并由通信设施将观测数据实时或定时传送至数据中心的地面固定观测站，固定点架设gnss接收机设备。

以两辆车为例，建立联合rtk的观测方程并线性化：

设车c1观测到卫星j’：1、2、3、5和w4，车c2观测到卫星为w4、和j”：5、6、7、8，以w4号星为参考星，则车c1和车c2的联合rtk双差观测方程为：

j′＝1,2,3,5

j″＝5,6,7,8

方程中的参数与步骤4中相同，根据步骤3获取的车c1和车c2在n下的相对位置和联合rtk双差观测方程，线性化则有：

j′＝1,2,3,5

j″＝5,6,7,8

de＝e2,0+dec2-(e1,0+dec1)

du＝u2,0+duc2-(u1,0+duc1)

其中，和分别为车c1和车c2在n系下的初始位置。

将步骤4中得到的车c1和车c2的相对模糊度约束加入到联合rtk的观测模型中，在本例中有：

j′＝1,2,3,5

j″＝5,6,7,8

de＝e2,0+dec2-(e1,0+dec1)

du＝u2,0+duc2-(u1,0+duc1)

即为步骤4获取的车c1和车c2之间公共的w4-j5号卫星对的相对模糊度。

最后利用附加约束条件的最小二乘方法求取每辆车的模糊度浮点解，式为约束条件，可以改写为则附加约束条件的最小二乘的法方程为：

其中k′为拉格朗日乘数，c为根据约束条件求取的一个系数矩阵。

解该方程得到模糊度浮点解后即可利用lambda方法进一步估计其模糊度固定解，进而获取每辆车的位置固定解。附加约束条件的最小二乘方法的详细推导可以参见文献：崔希璋,於宗俦,陶本藻,etal.广义测量平差(新版)[m].2001.

图6为开阔环境下、高架桥环境下车c1、车c2和车c1+车c2的某一个历元的gnss卫星观测的星空图，从图中可以明显的看出，在高架桥这种遮挡环境下，车c1和车c2的卫星观测遮挡较为严重，几何结构较差，车c1+车c2的卫星观测数较多且几何结构较好。

图7为开阔环境下、高架桥环境下，联合rtk在不考虑约束情况下、仅考虑相对位置约束和考虑相对位置+相对模糊度约束下的lambda固定的ratio值、成功率随历元的变化。在开阔环境和高架桥环境下，考虑相对位置+相对模糊度约束下的lambda固定的ratio值、成功率明显优于不考虑约束情况与仅考虑相对位置约束的情况，这说明模糊度解算性能有明显的提升，尤其是在高架桥环境下的模糊度固定率(即模糊度可以正确固定的历元数占总历元数的比例)有显著提升。

图8为开阔环境下、高架桥环境下，联合rtk在考虑相对位置+相对模糊度约束下的情况下与单车rtk的东向、北向和天向定位误差随历元变化图，相比单车rtk，联合rtk可以减下定位误差，提高定位精度，在高架桥环境下的提升更为显著。

步骤6，将步骤5中得到的车辆的绝对位置重复步骤1与ins进行ekf松组合，获取第k个历元下的车辆的组合导航信息，包括位置姿态速度