[发明专利]一种GNSS/惯导/车道线约束/里程计多源融合方法

权利要求书

1.一种GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，该方法中，通过视觉传感器采集图像，离线生成车道线地图数据库；在载体进行实时解算导航定位时，以差分GNSS/INS紧组合为基础，GNSS遮挡时，通过载体上安装的视觉传感器探测载体和车道线的相对关系，在车道线地图的辅助下，动态加入车道线约束观测方程辅助侧向、高程定位；并利用载体上方和道路前进方向的可观测卫星形成差分观测值参与紧组合，约束载体前向误差发散；该方法具体包括以下步骤：

步骤1、对实时采集的影像预处理得到车道线影像的俯视影像，对影像进行颜色空间转换分离出车道线，并基于滑动窗口的灰度直方图统计确定车道线的坐标，最后利用基于匀速假设的卡尔曼滤波对车道线进行跟踪，得到连续平滑的相机中心距离左右影像的距离；

步骤2、利用八叉树数据结构快速搜索到惯导预报位置的最小体素，利用欧氏几何距离法找到最邻近车道线节点，并对节点进行航向/拓扑一致性检验，检验通过后根据曲率返回最佳匹配的P_L1、P_L2、P_R1、P_R2；

步骤3、根据惯导预报位置，经过车高和与相机的外参改正后，位于车道面上的假设，构建车道线高程约束观测方程；

步骤4、将P_L1、P_L2、P_R1、P_R2四个点抬高至相机中心处，根据相机中心预报值对车道线的距离应等于观测距离的假设构建相机中心与车道线的侧向约束观测方程，并利用链式法则，将其转换至惯导中心；

步骤5、根据载体在直线运动时打滑较少且转弯时速度较慢的假设，构建里程计前向约束的观测方程；

步骤6、根据车道线高程、侧向约束的方程以及里程计的前向约束观测方程，实现多源融合观测更新。

2.根据权利要求1所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤1中进行车道线跟踪的方法具体为：

步骤1.1、预处理：通过Matlab标定工具箱对单目相机进行单目标定，获得相机内参数，利用相机内参数对单目相机采集的车道线图像进行畸变校正；根据单目相机安装的位置和视野范围，划定感兴趣区域，排除其他无关区域干扰，并利用单应性变换，将车道线恢复到俯视状态下；

步骤1.2、车道线提取：输入车道线正射影像，通过颜色和边缘信息的阈值分割，将车道线正射影像二值化；对图像底部划分像素窗口，并对窗口灰度值进行从左到右的直方图统计，采用高斯核平滑得到灰度均值曲线图，提取波峰位置作为车道线位置；

步骤1.3、车道线跟踪：采用卡尔曼滤波对车道线进行跟踪，使用速度模型建立前后历元的关系，状态方程为：

其中，q_x和q_v分别是位置x和速度v的过程噪声，k为时刻；

观测方程为：

y_k+1＝x_k+1+ε

其中，y_k+1为当前提取到的车道线位置；ε表示提取车道线位置的观测噪声；

滤波后，得到每个窗口i的位置再将连续窗口的位置进行二次曲线拟合，从而得到全时段连续平滑的车道线提取，实现对车道线虚线、斑马线的平滑过度；成功提取车道线后，将获得底部左右车道线在相机坐标系下的x坐标，即相机和左右车道线的距离。

3.根据权利要求2所述的GNSS/惯导/车道线约束/里程计多源融合方法，其特征在于，步骤2中八叉树辅助的车道线快速搜索的具体方法为：

步骤2.1、最邻近车道线节点搜索：根据惯导预报位置，从车道线的空间八叉树存储结构寻找预报点对应的子节点体素；找到体素后，遍历体素中的所有车道线坐标点，根据欧式几何距离法找到距离最邻近的点；

步骤2.2、一致性判断：根据惯导预报的位置点的姿态中的航向信息得到载体的前进方向，通过前进方向和搜索到的车道线节点的航向进行一致性判断，若两者航向之差大于阈值，则表明搜索到的车道线节点有误；

根据历史时刻成功搜索到的车道线节点和当前搜索到的车道线节点的拓扑关系检验，判断历史节点和当前节点是否具备邻接拓扑关系，若不具备，则表明搜索到的车道线节点有误；

反之，表明搜索到的车道线节点正确。