一种用于多源融合定位的鲁棒性控制方法和系统

1.本发明属于无人驾驶车辆多源融合定位技术领域，特别涉及一种用于多源融合定位的鲁棒性控制方法和系统。


背景技术：

2.多源融合定位技术已经成为无人驾驶车辆位置导航的主流技术，多源融合定位技术有效解决了无人驾驶车辆上单一传感器误差累积、稳定性差等缺点。多源融合定位技术基于信息融合技术将来自不同导航源的同构或者异构的导航信息按照相应的融合算法进行融合，从而得到最佳的融合结果。相对于传统的单一导航源，多源融合导航可以充分利用每一个导航源的优势，从而提供最好的无人驾驶车辆定位与导航服务。因此，对多源融合定位系统的鲁棒性研究是十分必要的，有助于多源融合定位技术的长久发展。
3.现有技术对多源融合定位技术的鲁棒性研究已经公开了很多方案。第一种方案提出了基于惯导、里程计、超宽带的多源传感器信息融合导航系统，使用因子图模型建立融合框架，将各个传感器信息抽象成因子，采用一种基于贝叶斯树形结构的增量平滑优化算法（isam2）对传感器信息进行处理和融合。第二种方案提出了一种采用无迹卡尔曼滤波（ukf）技术融合imu和uwb信息室内定位方法，能够有效抑制imu的误差积累，有效提高了非线性观测方程的求解精度。第三种方案使用扩展卡尔曼滤波融合uwb（高精度定位单元）、imu、视觉传感器，该方法允许在预测步骤中使用低成本的惯性测量单元（imu），将视觉测程法集成到标记物检测中，达到了10cm的精度。
4.综上所述可知，现有技术中多传感器信息融合的常用方法基本上可概括为概率估计算法、识别算法、参数算法和人工智能算法四大类。每一大类算法又由多种不同的典型方法组成，多传感器信息融合的本质思想是加权平均，为不同传感器采集的数据根据准确性赋予不同的权值。很多传统经典的滤波算法思想都起源于贝叶斯滤波，如粒子滤波、卡尔曼滤波、隐含马尔科夫算法、动态贝叶斯网络等模型算法都是滤波算法中的实际体现。贝叶斯算法的核心就是根据已知传感器输入数据计算对应的先验概率分布和信息融合特征值，该特征值描述了先验模型提供的整个环境特征。然而，目前传感器信息采样时间不同步等问题存在于导航系统中。且现有技术中的导航系统一般都集成多种传感器。不同传感器会受到不同工作环境的影响，传感器信息在传输过程中也存在时延等问题。现有技术在信息融合过程中大都采用外推法，内插法，曲线拟合等方法。然而，这些方法无法保证较高的同步精度。不同的工作环境和任务对传感器的选择也有要求，此外，传感器的组合方式与运行环境和任务有关，存在信息融合不充分，未充分发挥传感器特性优势等问题。因此，传统数据融合方法缺乏一定的自适应能力，难以在复杂环境下使无人驾驶车辆取得满意的性能。


技术实现要素：

5.为了解决上述技术问题，本发明提出了一种用于多源融合定位的鲁棒性控制方法和系统。不仅能够提高无人驾驶车辆系统的解算速度，提高无人驾驶车辆系统环境适应性
及精度，而且提高无人驾驶车辆系统的鲁棒性，减少错误的点或者误差较大的点参与解算。
6.为实现上述目的，本发明采用以下技术方案：一种用于多源融合定位的鲁棒性控制方法，包括以下步骤：针对同一场景的同一帧图像数据进行视觉特征点提取获得视觉特征点在图像像素坐标系下的像素位置，以及通过视觉惯导融合得到第一位姿，根据所述第一位姿计算视觉特征点误差和第一重投影误差；针对同一场景的同一帧的地面点云数据进行惯导与激光雷达特征点提取获得点云特征点在世界坐标下三维坐标，以及通过惯导激光雷达融合得到第二位姿；根据所述第二位姿计算点云特征点误差和第二重投影误差；基于所述视觉特征点在图像像素坐标系下的像素位置和点云特征点在世界坐标下三维坐标，采用pnp方法计算激光雷达和相机的第三位姿，根据所述第三位姿计算相机位置特征点误差和第三重投影误差；对比第一重投影误差、第二重投影误差和第三重投影误差，分别对视觉特征点误差、点云特征点误差和相位位置特征点误差进行权重排名，在环境特征匹配之后，进行质量等级划分。
7.进一步的，在特征点提取之前还包括：初始化组合导航系统；所述初始化组合导航系统的过程为：在姿态更新时不考虑由载体速度引起的导航坐标系相对于地球坐标系的角速度，在速度更新时不考虑载体速度引起的导航坐标系的速度前提下，开环初始对准时姿态更新的公式为：（6）开环初始对准时速度更新的公式为：（7）其中：表示载体坐标系在导航坐标系下的姿态矩阵，表示载体坐标系相对于地心惯性坐标系的角速度，表示地球自转引起的导航系的旋转角速度；表示比力在导航坐标系下的投影；表示重力加速度；组合导航系统的误差方程为：（8）（9）其中：表示姿态矩阵的误差，表示导航系相对于惯性系的角速度，表示陀螺仪的零漂误差，表示速度误差，表示比力在导航坐标系下的投影，表示加速度计测量的比力值，加速度计常值偏置。
8.进一步的，所述图像数据特征提取的过程包括：
采用改进式fast角点提取图像数据中有明显差异的像素点，所述有明显差异的像素点为与周围相邻的像素灰度值差别大于设定阈值；采用灰度质心算法提高特征点的旋转不变性，再通过brief描述子判断像素点的特性来确定图像数据中orb特征点的位置。
9.进一步的，所述视觉惯导融合的详细过程包括：定义滑动窗口内所有状态向量：（14）其中，表示第i帧惯导与世界坐标系之间的旋转矩阵；表示平移偏差；表示惯导在世界坐标系中的速度向量；表示加速度计偏置；表示旋转偏差；n、m、o分别表示滑窗中imu状态向量、路标点、路标线的索引；n、m和o分别表示滑窗中关键帧的数量、被滑动窗口中所有关键帧观测到的路标点和路标线数量；表示第k个路标点第一次被观测到时的逆深度，表示第l个特征线的正交表达；通过最小化滑动窗口内所有状态量的残差完成基于滑动窗口的非线性优化，其中目标函数表示为：为：；（15）其中，则是滑动窗口中因时间长或者共视特征少而被剔除的帧的先验信息；表示残差；表示雅克比矩阵；是滑动窗口内所有状态向量；为柯西鲁棒函数用于迭代约束过程中的惩罚项；为第i帧与第i+1帧状态量之间的惯导预积分，代表惯导帧之间的误差；代表惯导帧观测值；代表视觉特征点重投影误差，代表归一化平面在视觉特征点重投影误差计算时的观测值；为视觉特征线的重投影误差；代表归一化平面在视觉特征线重投
影误差计算时的观测值；为避免迭代步长大造成振荡情况，从初始值开始，通过增加微小量迭代更新，直至满足要求：（16）其中，是误差向量关于的雅各比矩阵，分别为迭代前与迭代后的先验误差，分别为迭代前与迭代后的惯导预积分误差，分别为迭代前与迭代后的视觉特征点重投影误差，分别为迭代前与迭代后的视觉特征线重投影误差。
10.进一步的，所述惯导与激光雷达特征点提取的过程包括：将每一帧的地面点云使用体素栅格滤波的方法进行下采样，标记为地面特征点；在剔除动态障碍物后，将剩余的非地面点云进行线特征点和表面特征点提取，定义平滑度；设置平滑度阈值，将平滑度大于平滑度阈值的点为线特征点；将平滑度小于阈值的点视为表面特征点。
11.进一步的，所述惯导激光雷达融合的过程包括：提取所有特征组成的激光雷达帧；（17）其中第i时刻提取的边缘特征；为第i时刻提取的平面特征；选取关键帧以及通过滑动窗口来包含固定数量的子关键帧；子关键帧在世界坐标系下的变换为，最终合并到体素地图中；（18）其中，表示为边缘特征体素地图；表示为平面特征体素地图；表示为平面特征体素地图；是世界坐标系下表示的第i帧边缘特征；是世界坐标系下表示的第i-1帧
边缘特征；是世界坐标系下表示的第i-n帧边缘特征；是世界坐标系下表示的第i帧面特征；是世界坐标系下表示的第i-1帧面特征；是世界坐标系下表示的第i-n帧面特征；对边缘特征和平面特征进行匹配，并利用电线、点面之间的距离关系建立位姿求解方程：（19）（20）其中表示第i+1时刻k点边缘坐标；表示第i时刻u点边缘坐标；表示第i时刻v点边缘坐标；表示第i时刻u点面坐标；表示第i时刻v点面坐标；表示第i+1时刻k点面坐标；表示第i时刻w点面坐标；将扫描匹配中的非线性问题表示成迭代求解的线性问题：（21）其中，为关键帧在世界坐标系下的变换矩阵，a和b由线性化位姿得到，a表示旋转，b表示平移。
12.进一步的，所述提取相机和激光雷达的特征点的过程包括：在三维空间中有n个空间点p，其坐标为，i=1,2,3,4,
…
,n,投影到像素坐标系的坐标为；相机空间点和像素位置关系可得：（22）其中，表示图像像素点的尺度信息，k矩阵表示为相机的内参参数，表示为相机用李群表示的旋转和平移矩阵；通过最小二乘法构建误差项方程估计出相机位姿：
（23）式（23）表示为取最小值时的值，其优化变量为矩阵。
13.进一步的，所述方法还包括构建的最小化重投影方程调整相机的位姿，使得相机位置重投影误差达到最小值；特征点的坐标p通过变换到相机坐标系下为点,其变换坐标为：（24）相对于的相机投影模型为：，展开可得（25）其中，为相机内参x轴焦距，为相机内参y轴焦距，为相机x轴光心，分别为相机y轴光心。
14.进一步的，所述方法还包括在进行质量等级划分之后，控制参与下一帧解算特征点的数量和质量，保证以最少特征点数量参与解算，使系统达到最高精度。
15.本发明还提出了一种用于多源融合定位的鲁棒性控制系统，所述系统包括第一提取融合模块、第二提取融合模块、计算模块和划分模块；所述第一提取融合模块用于针对同一场景的同一帧图像数据进行视觉特征点提取获得视觉特征点在图像像素坐标系下的像素位置，以及通过视觉惯导融合得到第一位姿，根据所述第一位姿计算视觉特征点误差和第一重投影误差；所述第二提取融合模块用于针对同一场景的同一帧的地面点云数据进行惯导与激光雷达特征点提取获得点云特征点在世界坐标下三维坐标，以及通过惯导激光雷达融合得到第二位姿；根据所述第二位姿计算点云特征点误差和第二重投影误差；所述计算模块用于基于所述视觉特征点在图像像素坐标系下的像素位置和点云特征点在世界坐标下三维坐标，采用pnp方法计算激光雷达和相机的第三位姿，根据所述第三位姿计算相机位置特征点误差和第三重投影误差；所述划分模块用于对比第一重投影误差、第二重投影误差和第三重投影误差，分别对视觉特征点误差、点云特征点误差和相位位置特征点误差进行权重排名，在环境特征匹配之后，进行质量等级划分。
16.发明内容中提供的效果仅仅是实施例的效果，而不是发明所有的全部效果，上述技术方案中的一个技术方案具有如下优点或有益效果：
本发明提出了一种用于多源融合定位的鲁棒性控制方法和系统，该方法包括以下步骤：针对同一场景的同一帧图像数据进行视觉特征点提取获得视觉特征点在图像像素坐标系下的像素位置，以及通过视觉惯导融合得到第一位姿，根据第一位姿计算视觉特征点误差和第一重投影误差；针对同一场景的同一帧的地面点云数据进行惯导与激光雷达特征点提取获得点云特征点在世界坐标下三维坐标，以及通过惯导激光雷达融合得到第二位姿；根据第二位姿计算点云特征点误差和第二重投影误差；基于视觉特征点在图像像素坐标系下的像素位置和点云特征点在世界坐标下三维坐标，采用pnp方法计算激光雷达和相机的第三位姿，根据第三位姿计算相机位置特征点误差和第三重投影误差；对比第一重投影误差、第二重投影误差和第三重投影误差，分别对视觉特征点误差、点云特征点误差和相位位置特征点误差进行权重排名，在环境特征匹配之后，进行质量等级划分。基于一种用于多源融合定位的鲁棒性控制方法，还提出了一种用于多源融合定位的鲁棒性控制系统。本发明能够控制下一帧参与解算的特征点的数量和质量，保证以最小的特征点数量参与解算，能够使无人驾驶车辆系统达到能达到的最高精度，而且相对于以前所有特征点都参与解算的方案，本发明不仅能够提高无人驾驶车辆系统的解算速度，提高系统环境适应性及精度，而且提高无人驾驶车辆系统的鲁棒性，减少错误或者误差较大的点参与解算，便于多源传感器数据融合的工程应用。
附图说明
17.如图1为本发明实施例1一种用于多源融合定位的鲁棒性控制方法实现的框架图；如图2为本发明实施例1一种用于多源融合定位的鲁棒性控制方法实现的详细流程图；如图3为本发明实施例1角点提取金字塔示意图；如图4为本发明实施例1筛选点示意图；如图5为本发明实施例2一种用于多源融合定位的鲁棒性控制系统示意图。
具体实施方式
18.为能清楚说明本方案的技术特点，下面通过具体实施方式，并结合其附图，对本发明进行详细阐述。下文的公开提供了许多不同的实施例或例子用来实现本发明的不同结构。为了简化本发明的公开，下文中对特定例子的部件和设置进行描述。此外，本发明可以在不同例子中重复参考数字和/或字母。这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施例和/或设置之间的关系。应当注意，在附图中所图示的部件不一定按比例绘制。本发明省略了对公知组件和处理技术及工艺的描述以避免不必要地限制本发明。
19.实施例1本发明实施例1提出了一种用于多源融合定位的鲁棒性控制方法，基于惯导（imu）、相机、激光雷达的多源传感器数据融合。其中惯导（imu）、相机、激光雷达均位于无人驾驶车系统上。如图1为本发明实施例1一种用于多源融合定位的鲁棒性控制方法实现的框架图；如图2为本发明实施例1一种用于多源融合定位的鲁棒性控制方法详细流程图。
20.在步骤s200中，进行组合系统初始化。
21.初始化组合导航系统的过程为：在姿态更新时不考虑由载体速度引起的导航坐标
系相对于地球坐标系的角速度，在速度更新时不考虑载体速度引起的导航坐标系的速度前提下，开环初始对准时姿态更新的公式为：（6）开环初始对准时速度更新的公式为：（7）其中：表示载体坐标系在导航坐标系下的姿态矩阵，表示载体坐标系相对于地心惯性坐标系的角速度，表示地球自转引起的导航系的旋转角速度；表示比力在导航坐标系下的投影；表示重力加速度；组合导航系统的误差方程为：（8）（9）其中：表示姿态矩阵的误差，表示导航系相对于惯性系的角速度，表示陀螺仪的零漂误差，表示速度误差，表示比力在导航坐标系下的投影，表示加速度计测量的比力值，加速度计常值偏置。
22.在步骤s210中，针对同一场景的同一帧图像数据进行视觉特征点提取获得视觉特征点在图像像素坐标系下的像素位置，以及通过视觉惯导融合得到第一位姿，根据第一位姿计算视觉特征点误差和第一重投影误差。
23.其中，针对同一场景的同一帧图像数据进行视觉特征点提取获得视觉特征点在图像像素坐标系下的像素位置的过程包括：采用改进式fast角点提取图像数据中有明显差异的像素点，有明显差异的像素点为与周围相邻的像素灰度值差别大于设定阈值；采用灰度质心算法提高特征点的旋转不变性，再通过brief描述子判断像素点的特性来确定图像数据中orb特征点的位置。
24.在本步骤中，判断fast角点的条件为图像中的像素灰度值变化明显程度，如果该点的像素与其周围相邻的像素灰度值差别大于设定阈值，可以确定该点为fast角点。fast角点的提取中需要定义像素的距离关系，本方法中的特征提取方法使用城区距离描述像素点和的位置关系如下式所示：；（10）其中，表示像素点p与q的距离。
25.提取像素点p和城区距离为3的周围像素点灰度值。并设定在若干连续n个像素点与p点的灰度值相差25%以上，则确定点p为fast角点，阈值设定可以根据室内光照环境差异调节。
26.本发明中为了确定角点p的方向信息，结合了周围图像的质心夹角信息。引入灰度质心算法保证当图像旋转时，依然可准确识别到相同角点。角点的灰度值信息用i（x,y）表示，可得被采集图像块b的矩定义为：；（11）图像块b的质心通过图像的矩可表示为：；（12）根据图像块的几何中心和质心连线可以得到fast角点的方向为：；（13）将采集后的角点信息加入了brief描述子信息，使用了0和1描述角点附近的像素大小关系，使用概率分布的方法随机选择周围像素点p，q，若p比q像素值大，则取1，否则取0。使用二进制的描述子表示角点周围的环境，选取速度快，存储方便可以应用于实时匹配，同时引入了图像金字塔算法改进了图像缩放变化的特征匹配影响。如图3为本发明实施例1角点提取金字塔示意图；第一张采集数据为原图像，依次通过降采样过程得到原图尺寸的一半，并置于最顶层。经过8次降采样，当同一个角点在每层均被确定为特征点时，则该角点被确认作为最终的特征点提取并进行后续的匹配工作。
27.通过视觉惯导融合得到第一位姿，根据第一位姿计算视觉特征点误差和第一重投影误差的过程包括：视觉惯导融合系统分为数据预处理、估计器初始化、vio等模块，主要解决光照变化、缺少纹理和运动模糊导致的视觉定位失败等问题，使用滑动窗口法进行纯视觉的位资估计，检测最近帧与帧之间特征匹配，在滑动窗口中匹配对应稳定且足够视差的特征跟踪。为了减少计算量，选取部分具有关键信息的共视帧来解决计算量的问题，具有关键信息的帧被称为关键帧，然后通过一个策略控制窗口中包含的关键帧数量。
28.定义滑动窗口内所有状态向量：（14）其中，表示第i帧惯导与世界坐标系之间的旋转矩阵；表示平移偏差；表示惯导在世界坐标系中的速度向量；表示加速度计偏置；表示旋转偏差；n、m、o分别表示滑窗中imu状态向量、路标点、路标线的索引；n、m和o分别表示滑窗中关键帧的数量、被滑动窗口中所有关键帧观测到的路标点和路标线数量；表示第k个路标点第一次被观测到时的逆深度，表示第l个特征线的正交表达；
通过最小化滑动窗口内所有状态量的残差完成基于滑动窗口的非线性优化，其中目标函数表示为：目标函数表示为：；（15）其中，则是滑动窗口中因时间长或者共视特征少而被剔除的帧的先验信息；表示残差；表示雅克比矩阵；是滑动窗口内所有状态向量；为柯西鲁棒函数用于迭代约束过程中的惩罚项；为第i帧与第i+1帧状态量之间的惯导预积分，代表惯导帧之间的误差；代表惯导帧观测值；代表视觉特征点重投影误差，代表归一化平面在视觉特征点重投影误差计算时的观测值；为视觉特征线的重投影误差；代表归一化平面在视觉特征线重投影误差计算时的观测值；为避免迭代步长过大造成振荡情况，从初始值开始，通过增加微小量迭代更新，直至满足要求：；（16）其中，是误差向量关于的雅各比矩阵，分别为迭代前与迭代后的先验误差，分别为迭代前与迭代后的惯导预积分误差，分别为迭代前与迭代后的视觉特征点重投影误差，分别为迭代前与迭代后的视觉特征线重投影误差。
29.在步骤s220中，针对同一场景的同一帧的地面点云数据进行惯导与激光雷达特征点提取获得点云特征点在世界坐标下三维坐标，以及通过惯导激光雷达融合得到第二位姿；根据第二位姿计算点云特征点误差和第二重投影误差；
针对同一场景的同一帧的地面点云数据进行惯导与激光雷达特征点提取获得点云特征点在世界坐标下三维坐标实现的详细过程包括将每一帧的地面点云使用体素栅格滤波的方法进行下采样，标记为地面特征点；在剔除动态障碍物后，将剩余的非地面点云进行线特征点和表面特征点提取，定义平滑度；设置平滑度阈值，将平滑度大于平滑度阈值的点为线特征点；将平滑度小于阈值的点视为表面特征点。
30.对于地面特征点的提取，在深度图像基础上加入几何约束，利用竖直角度和高程差异联合约束地面点。由于激光雷达是水平安置在载体上，该地面提取方法的优势是并不严格要求地面处于完全水平状态。将每一帧的地面点云使用体素栅格滤波的方法进行不同程度的下采样，标记为地面特征点和。
31.在剔除动态障碍物后，将剩余的非地面点云进行线特征点和表面特征点提取，定义平滑度u，步骤为：在同一扫描内，对于任意点a，选取左右各五个点；每个点的x坐标与点a的x坐标求差，将所有的差求和，得到；每个点的y坐标与点a的y坐标求差，将所有的差求和，得到；每个点的z坐标与点a的z坐标求差，将所有的差求和，得到；计算平滑度。
32.每个扫描点的边缘五个点不参与特征点选取，因为不满于前后五个点计算平滑度的条件。为了防止一帧数据中的特征点过于聚集，将每一帧点云中每个扫描从起始扫描到终止扫描均分为六份，设置平滑度阈值。线特征点选择条件：将平滑度大于阈值的点视为线特征点，若该点前后五个点中已存在线特征点，则跳过该点，从平滑度更小的点中选择。表面特征点选择条件：将平滑度小于阈值的点视为表面特征点，若该点前后五个点中已存在表面特征点，则跳过该点，从平滑度更大的点中选择。对于每一帧点云，将u值较大的点标记为线特征点，将u值较小的点标记为表面特征点，从中挑选个u值最大的点构成，并满足，从中挑选个u值最小点构成，并满足。对每一帧激光点云进行三种特征点提取，包括地面特征点、非地面线特征点、非地面表面特征点，并对每一种特征点云分为两份进行存储，即和、和、和。如图4为本发明实施例1筛选点示意图。
33.剔除以下不稳定的噪声点：激光雷达视场边缘的点存在变形，会产生大曲率现象，影响特征提取的质量，所以要去掉视场边缘的点即深度值大于60m的点。
34.去除激光束和平面夹角过小的点，即激光束和平面接近平行的情况。
35.去除部分被遮挡的激光点，由于激光雷达在移动时，被遮挡的点所处的面可能在
下一帧中消失，如下图中点b的情况。定义点a、b的深度值为、，遮挡点的判别标准为：；通过惯导激光雷达融合得到第二位姿；根据第二位姿计算点云特征点误差和第二重投影误差的详细过程包括：提取所有特征组成的激光雷达帧；（17）其中第i时刻提取的边缘特征；为第i时刻提取的平面特征；选取关键帧以及通过滑动窗口来包含固定数量的子关键帧；子关键帧在世界坐标系下的变换为，最终合并到体素地图中；（18）其中，表示为边缘特征体素地图；表示为平面特征体素地图；表示为平面特征体素地图；是世界坐标系下表示的第i帧边缘特征；是世界坐标系下表示的第i-1帧边缘特征；是世界坐标系下表示的第i-n帧边缘特征；是世界坐标系下表示的第i帧面特征；是世界坐标系下表示的第i-1帧面特征；是世界坐标系下表示的第i-n帧面特征；对边缘特征和平面特征进行匹配，并利用电线、点面之间的距离关系建立位姿求解方程：（19）（20）其中表示第i+1时刻k点边缘坐标；表示第i时刻u点边缘坐标；表
示第i时刻v点边缘坐标；表示第i时刻u点面坐标；表示第i时刻v点面坐标；表示第i+1时刻k点面坐标；表示第i时刻w点面坐标；将扫描匹配中的非线性问题表示成迭代求解的线性问题：；（21）其中，为关键帧在世界坐标系下的变换矩阵，a和b由线性化位姿得到，a表示旋转，b表示平移。
36.在步骤s230中，基于视觉特征点在图像像素坐标系下的像素位置和点云特征点在世界坐标下三维坐标，采用pnp方法计算激光雷达和相机的第三位姿，根据第三位姿计算相机位置特征点误差和第三重投影误差；pnp算法为3d点到2d点对点运动的一种计算方法，该方法通过n个3d点及其在图像坐标系下的投影位置来估计相机的位姿。3d-2d点对的计算方法在计算过程中不需要要求相机的对级约束，同时在特征点正确匹配的数量较少的情景下依旧可以实现比较好的相机运动估计。
37.在三维空间中有n个空间点p，其坐标为，i=1,2,3,4,
…
,n,投影到像素坐标系的坐标为；相机空间点和像素位置关系可得：；（22）其中，表示图像像素点的尺度信息，k矩阵表示为相机的内参参数，表示为相机用李群表示的旋转和平移矩阵；通过最小二乘法构建误差项方程估计出相机位姿：；（23）式（23）表示为取最小值时的值，其优化变量为矩阵。
38.方法还包括构建的最小化重投影方程调整相机的位姿，使得相机位置重投影误差达到最小值；特征点的坐标p通过变换到相机坐标系下为点,其变换坐标为：
；（24）相对于的相机投影模型为：，展开可得；（25）其中，为相机内参x轴焦距，为相机内参y轴焦距，为相机x轴光心，分别为相机y轴光心。
39.在步骤s230中，对比第一重投影误差、第二重投影误差和第三重投影误差，分别对视觉特征点误差、点云特征点误差和相位位置特征点误差进行权重排名，在环境特征匹配之后，进行质量等级划分。
40.经过以上步骤，得到了相机与激光雷达提取的特征点，以及基于特征点计算得到的位置姿态结果，将所得到结果计算特征点误差和重投影误差，对比每一种融合方法得到的误差，采用投票机制对各个特征点进行权重排名，根据应用场景设置相应的阈值，将特征点根据投票结果进行分类，对视觉与雷达提供的场景时空关联信息进行信息匹配质量等级划分，将视觉与雷达提供的场景时空关联信息分成三种信息集合：质量一级集合、质量二级集合、质量三级集合。质量二级集合包含质量一级集合，质量三级集合包含质量一、二级集合。采用投票机制策略，分别利用不同质量等级的关联信息集合进行融合定位。动态维护多种不同信息融合模式，保证存在有效融合模式，进行可靠性联合评估与筛选完成有效模式的输出选择。选择排名靠前的特征点主要参与下一时刻的解算位姿，同时根据下一时刻特征点的数量进行决策，是否选择相对较弱的特征点参与下一时刻的解算。
41.本发明实施例1提出的一种用于多源融合定位的鲁棒性控制方法，能够控制下一帧参与解算的特征点的数量和质量，保证以最小的特征点数量参与解算，能够使无人驾驶车辆系统达到能达到的最高精度，而且相对于以前所有特征点都参与解算的方案，本发明不仅能够提高系统的解算速度，提高无人驾驶车辆系统环境适应性及精度，而且提高系统的鲁棒性，减少错误或者误差较大的点参与解算，便于多源传感器数据融合的工程应用。
42.实施例2基于本发明实施例1提出的一种用于多源融合定位的鲁棒性控制方法，本发明实施例2还提出了一种用于多源融合定位的鲁棒性控制系统，如图5为本发明实施例一种用于多源融合定位的鲁棒性控制系统示意图，该系统包括第一提取融合模块、第二提取融合模块、计算模块和划分模块；第一提取融合模块用于针对同一场景的同一帧图像数据进行视觉特征点提取获得视觉特征点在图像像素坐标系下的像素位置，以及通过视觉惯导融合得到第一位姿，根据第一位姿计算视觉特征点误差和第一重投影误差；第二提取融合模块用于针对同一场景的同一帧的地面点云数据进行惯导与激光雷达特征点提取获得点云特征点在世界坐标下三维坐标，以及通过惯导激光雷达融合得到第二位姿；根据第二位姿计算点云特征点误差和第二重投影误差；
计算模块用于基于视觉特征点在图像像素坐标系下的像素位置和点云特征点在世界坐标下三维坐标，采用pnp方法计算激光雷达和相机的第三位姿，根据第三位姿计算相机位置特征点误差和第三重投影误差；划分模块用于对比第一重投影误差、第二重投影误差和第三重投影误差，分别对视觉特征点误差、点云特征点误差和相位位置特征点误差进行权重排名，在环境特征匹配之后，进行质量等级划分。
43.该系统还包括初始化模块，用于进行组合系统初始化，在初始化模块完成之后执行相应的第一提取融合模块、第二提取融合模块、计算模块和划分模块。
44.本发明实施例2中各个模块实现的过程，和实施例1中相应步骤中的过程相同，在此不再详细赘述。
45.本发明实施例2提出的一种用于多源融合定位的鲁棒性控制系统，将实施例1中的方法步骤相应模块化，能够实现控制下一帧参与解算的特征点的数量和质量，保证以最小的特征点数量参与解算，能够使无人驾驶车辆系统达到能达到的最高精度，而且相对于以前所有特征点都参与解算的方案，本发明不仅能够提高无人驾驶车辆系统的解算速度，提高系统环境适应性及精度，而且提高系统的鲁棒性，减少错误或者误差较大的点参与解算，便于多源传感器数据融合的工程应用。
46.需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。另外，本技术实施例提供的上述技术方案中与现有技术中对应技术方案实现原理一致的部分并未详细说明，以免过多赘述。
47.上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制。对于所属领域的技术人员来说，在上述说明的基础上还可以做出其它不同形式的修改或变形。这里无需也无法对所有的实施方式予以穷举。在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。