[发明专利]用于传感器融合定位的协方差确定方法、定位方法和装置

说明书

本公开公开了用于传感器融合定位的协方差确定方法、定位方法和装置。其中，协方差确定方法包括：获取定位过程中当前迭代得到的变化步长大于设定步长阈值的传感器参数为第一参数；根据第一参数和当前迭代得到的残差因子，更新当前的雅克比矩阵，所述残差因子是根据传感器的测量数据和传感器参数确定的；根据更新后的雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵，返回执行获取第一参数的步骤，直至满足迭代结束条件，输出最终的海森矩阵；将海森矩阵分解得到上三角矩阵；根据上三角矩阵通过递归的方式确定表征第一参数的准确度和相关性的协方差矩阵。能够在减少计算量的基础上，保证确定的协方差的准确度。

技术领域

本公开涉及定位技术领域，特别涉及用于传感器融合定位的协方差确定方法、定位方法和装置。

背景技术

多传感器融合定位通常是指包括视觉传感器(如相机)、惯性测量单元(InertialMeasurement Unit，IMU，简称“惯导”)和全球导航卫星系统(Global NavigationSatellite System，GNSS)定位等的定位系统，该系统兼具定位精度高和成本低的优势，成为了需要高精度(厘米级)定位结果的应用场景主要选择的定位方案。通过传感器融合定位获得高精度定位结果的过程中，需要确定各传感器的位置和姿态等参数，以及确定这些参数的协方差，以便对融合定位的定位精度半径进行估计和对相关性进行评估等。

现有技术一般采用海森矩阵求逆的方法进行协方差估计，但多传感器融合定位的参数的维数较多，直接矩阵求逆的算力消耗巨大，导致系统成本升高，而为了降低成本，进行近似处理又会影响估计出的协方差的准确性。因此，需要提供新的协方差确定方案，以满足成本和准确性的要求。

发明内容

鉴于上述问题，提出了本公开以便提供一种克服上述问题或者至少部分地解决上述问题的用于传感器融合定位的协方差确定方法、定位方法和装置。

第一方面，本公开实施例提供一种用于传感器融合定位的协方差确定方法，包括：

参数编排步骤，包括：获取定位过程中当前迭代得到的变化步长大于设定步长阈值的传感器参数为第一参数；

雅克比矩阵解算步骤，包括：根据当前迭代得到的残差因子和参数编排步骤获得的第一参数，更新当前的雅克比矩阵，所述残差因子是根据传感器的测量数据和传感器参数确定的；

海森矩阵求解步骤，包括：根据更新后的雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵，返回所述参数编排步骤，直至满足迭代结束条件，输出最终的海森矩阵；

分解步骤，包括：将海森矩阵分解得到上三角矩阵；

协方差解算步骤，包括：根据所述上三角矩阵通过递归的方式确定表征所述第一参数的准确度和相关性的协方差矩阵。

第二方面，本公开实施例提供一种传感器融合定位方法，包括：

根据按照上述方法确定的协方差矩阵中的对角线元素，确定对应的传感器参数的准确度；

将准确度大于设定阈值的参数作为下一步传感器融合定位中对应参数的初值。

第三方面，本公开实施例提供一种用于传感器融合定位的协方差确定装置，包括：

参数编排模块，用于获取定位过程中当前迭代得到的变化步长大于设定步长阈值的传感器参数为第一参数；

雅克比矩阵解算模块，用于根据当前迭代得到的残差因子和所述参数编排模块获得的第一参数，更新当前的雅克比矩阵，所述残差因子是根据多传感器的测量数据和传感器参数确定的；

海森矩阵求解模块，用于根据所述雅克比矩阵解算模块得到的更新后的雅克比矩阵中拆分的稠密子矩阵更新当前的海森矩阵；所述参数编排模块、雅克比矩阵解算模块和海森矩阵求解模块循环工作，直至迭代结束，所述海森矩阵求解模块，还用于输出最终的海森矩阵；