(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 20221047486 0.3 (22)申请日 2022.04.29 (71)申请人 重庆长安汽车股份有限公司 地址 400020 重庆市江北区建新 东路260号 (72)发明人 闫耀威　彭祥军　罗毅　王宽　 康轶非　万凯林　 (74)专利代理 机构 重庆博凯知识产权代理有限 公司 50212 专利代理师 李晓兵 (51)Int.Cl. G01C 21/34(2006.01) G01C 21/00(2006.01) G01C 21/16(2006.01) G06T 7/73(2017.01) (54)发明名称 一种基于ESKF的紧耦合定位方法、 设备及存 储介质 (57)摘要 本发明涉及一种基于ESKF的紧耦合定位方 法、 设备及存储介质， 方法具体包括以下步骤： 获 取惯性测量单元测量值和轮速计读数， 并进行融 合， 同时进行状态传递； 通过车辆后端的鱼眼摄 像头获取视觉数据并进行视觉建图定位得到当 前时刻相机的定位结果； 基于ESKF算法将惯性测 量单元测量值和轮速计读数的融合结果作为预 测， 鱼眼摄像头的定位结果作为观测来完成ESKF 融合， 获得最终的定位结果。 本发明的多传感器 组合的定位方案相较于单一传感器的定位方案， 理论上结果会更准确、 鲁棒性更强； 同时使用 ESKF算法， 在保持较高定位精度的情况下能有效 降低定位 算法的计算资源需求。 权利要求书2页 说明书6页 附图1页 CN 114705210 A 2022.07.05 CN 114705210 A 1.一种基于 ESKF的紧耦合定位方法， 其特 征在于： 具体包括以下步骤： 获取惯性测量单 元测量值和轮速计读数， 并进行融合， 同时进行状态传递； 通过车辆后端的鱼眼摄像头获取视觉数据并进行视觉建图定位得到当前时刻相机的 定位结果； 基于ESKF算法将惯性测量单元测量值和轮速计读数的融合结果作为预测， 鱼眼摄像头 的定位结果作为观测来完成ESK F融合， 获得最终的定位结果。 2.根据权利要求1所述一种基于ESKF的紧耦合定位方法， 其特征在于： 基于惯性测量单 元测量值和轮速计读数的融合结果构建系统状态， 并推断错 误状态， 完成状态传递。 3.根据权利要求2所述一种基于ESKF的紧耦合定位方法， 其特征在于： 构建系统状态 时， 状态量x的表达式如下： 其中， p为当前时刻车辆位姿； R为旋转； bg为惯性测量单元中陀螺仪的偏差； a为轮速因 子； 通过下列各式得到错 误状态 δxt和系统状态 协方差矩阵pt： δxt＝(I+FtΔt)δxt‑1 pt＝(I+Ft)pt‑1(I+Ft)T+(Gt)Q(Gt)T 其中， Δt＝tk‑tk‑1； tk和tk‑1均为惯性测量单元的时间戳； pt为系统状态协方差矩阵； Ft 为误差状态转移 矩阵； Gt为时间矩阵； Q 为惯性测量单 元提供的角速度ω的协方差矩阵。 4.根据权利要求1所述一种基于ESKF的紧耦合定位方法， 其特征在于： 使用ORB特征法 建立局部地图， 并保存地图到本地文件夹， 随后运行车载地图， 通过鱼眼摄像头获取视觉数 据后进行去畸变处理得到去畸变的图像， 使用ORB特征法获取初始十帧图像的特征点与所 述局部地图中的关键帧进行 特征点匹配， 得到当前时刻相机的定位结果。 5.根据权利要求4所述一种基于ESKF的紧耦合定位方法， 其特征在于： 在使用ORB特征 法建立局部地图之前还需对相机位姿 进行三角化恢复尺度， 具体包括： 通过轮速预积分计算得到相机两帧之间的欧式距离， 同时计算欧式距离的误差值， 若 误差值≤0.01则使用轮速预积分得到的值替代相机两帧之 间的欧式距离， 反之则不用轮速 预积分得到的值 替代两帧之间的欧氏距离， 完成三角化恢复尺度。 6.根据权利要求5所述一种基于ESKF的紧耦合定位方法， 其特征在于： 计算所述欧氏距 离的误差值包括： 根据轮速预积分得到相机两帧之间的欧氏距离， 再利用PnP算法得到两帧之间的估计 欧式距离， 将轮速预积分得到的欧氏距离减去两帧之 间的估计欧式距离即可得到所述欧式 距离的误差值： 若误差值≤0.01则使用轮速预积分得到的值替代相机两帧之间的欧式距离， 完成三角 化恢复尺度。 7.根据权利要求4所述一种基于ESKF的紧耦合定位方法， 其特征在于： 使用ORB特征法 建立局部地图具体包括： 基于鱼眼摄像机获取的视觉图像， 使用ORB特征法进行特征提取和特征匹配， 得到当前权　利　要　求　书 1/2 页 2 CN 114705210 A 2时刻的相 机位姿， 根据相 机位姿中所包含的关键帧建立局部共视地图， 然后在下一时刻获 取新的关键 帧， 将新的关键帧插入局部共视地图中并更新局部共视地图， 同时剔除局部共 视地图中多余的关键帧得到局部地图。 8.根据权利 要求1所述一种基于ESKF的紧耦合定位方法， 其特征在于： 进行ESKF融合具 体包括： 设置ESKF中的误差状态矩阵ES、 误差状态协方差矩阵EC和观测协方差矩阵MC， 根据当 前时刻相机的定位结果 求得所述观测协方差矩阵M C； 根据惯性测量单元测量值和轮速计读数的融合结果和当前时刻相机的定位结果得到 残差， 所述残差包括车辆的位姿和旋转； 通过下式计算 卡尔曼增益K： K＝PHT(HPHT+MC)‑1 其中， P为所述系统状态 协方差矩阵pt； H为观测矩阵； 然后进行状态更新： 误差状态矩阵更新ES＝K ×残差； 协方差矩阵更新G＝(I ‑KH)； 误差状态协方差矩阵更 新EC＝GECGT+K×MC×KT； 其中， 为观测矩阵； 最后将最后更新的误差状态 矩阵ES与观测协方差矩阵M C相加得到最终的定位结果。 9.一种设备， 其特征在于： 所述设备包括存储器和处理器， 所述存储器存储计算机处理 指令， 所述处理器通过调用所述计算机处理指令来执行上述权利要求1 ‑8中任一项所述的 一种基于 ESKF的紧耦合定位方法。 10.一种存储介质， 其特征在于： 所述存储介质存储有一个或者多个程序， 所述一个或 者多个程序被处理器运行时， 执行如权利要求1 ‑8中任一项所述的一种基于ESKF的紧耦合 定位方法的步骤。权　利　要　求　书 2/2 页 3 CN 114705210 A 3