一种基于eskf的紧耦合定位方法、设备及存储介质
技术领域
1.本发明属于自动驾驶车辆定位技术领域，具体涉及一种基于eskf的紧耦合定位方法、设备及存储介质。
背景技术：2.在定位算法方面，基于先验地图的定位方法是比较常用的定位方法之一，该方法将在线观测的环境数据与先验地图匹配，得到差异最小的最佳匹配位置。先验地图需要通过即时定位与地图构建(simultaneous localization and mapping，slam)进行构建。
3.基于点云地图的匹配定位方法是目前定位精准度最高的方法。点云地图匹配精度虽然高，但是资源占用率和计算复杂度也很高，如何对地图进行维护更新以应对环境的动态变化也是该方法需要解决的难点问题之一。将imu数据与轮速数据和视觉定位数据进行融合能有效提高定位算法的可靠性和运行效率。为此martinelli研究了视觉-惯性结构在运动问题中的应用，mourikis提出使用多状态约束的卡尔曼滤波(multi-state constraint kalman filter，msckf)方法来融合视觉-惯性导航，在提高运行效率方面，biber等使用里程计信息来加速定位迭代的收敛速度。
4.现有技术中：如cn107221007a——《一种基于图像特征降维的无人车单目视觉定位方法》，使用orb-slam框架进行无人车定位，但是该方法中最终融合结果的鲁棒性较差，有待进一步改进。
技术实现要素：5.针对现有技术的上述不足，本发明要解决的技术问题是提供一种基于eskf的紧耦合定位方法、设备及存储介质，避免单一传感器的定位方案带来的结果不够准确、鲁棒性差的问题，取得保持较高定位精度的情况下能有效降低定位算法的计算资源需求的效果。
6.为解决上述技术问题，本发明采用如下技术方案：
7.一种基于eskf的紧耦合定位方法，具体包括以下步骤：
8.获取惯性测量单元测量值和轮速计读数，并进行融合，同时进行状态传递；
9.通过车辆后端的鱼眼摄像头获取视觉数据并进行视觉建图定位得到当前时刻相机的定位结果；
10.基于eskf算法将惯性测量单元测量值和轮速计读数的融合结果作为预测，鱼眼摄像头的定位结果作为观测来完成eskf融合，获得最终的定位结果。
11.进一步完善上述技术方案，基于惯性测量单元测量值和轮速计读数的融合结果构建系统状态，并推断错误状态，完成状态传递。
12.进一步地，构建系统状态时，状态量x的表达式如下：
13.14.其中，p为当前时刻车辆位姿；r为旋转；bg为惯性测量单元中陀螺仪的偏差；a为轮速因子；
15.通过下列各式得到错误状态δx
t
和系统状态协方差矩阵p
t
：
16.δx
t
＝(i+f
t
δt)δx
t-1
17.p
t
＝(i+f
t
)p
t-1
(i+f
t
)
t
+(g
t
)q(g
t
)
t
18.其中，δt＝t
k-t
k-1
；tk和t
k-1
均为惯性测量单元的时间戳；p
t
为系统状态协方差矩阵；f
t
为误差状态转移矩阵；g
t
为时间矩阵；q为惯性测量单元提供的角速度ω的协方差矩阵。
19.进一步地，使用orb特征法建立局部地图，并保存地图到本地文件夹，随后运行车载地图，通过鱼眼摄像头获取视觉数据后进行去畸变处理得到去畸变的图像，使用orb特征法获取初始十帧图像的特征点与所述局部地图中的关键帧进行特征点匹配，得到当前时刻相机的定位结果。
20.进一步地，在使用orb特征法建立局部地图之前还需对相机位姿进行三角化恢复尺度，具体包括：
21.通过轮速预积分计算得到相机两帧之间的欧式距离，同时计算欧式距离的误差值，若误差值≤0.01则使用轮速预积分得到的值替代相机两帧之间的欧式距离，反之则不用轮速预积分得到的值替代两帧之间的欧氏距离，完成三角化恢复尺度。
22.进一步地，计算所述欧氏距离的误差值包括：
23.根据轮速预积分得到相机两帧之间的欧氏距离，再利用pnp算法得到两帧之间的估计欧式距离，将轮速预积分得到的欧氏距离减去两帧之间的估计欧式距离即可得到所述欧式距离的误差值：
24.若误差值≤0.01则使用轮速预积分得到的值替代相机两帧之间的欧式距离，完成三角化恢复尺度。
25.进一步地，使用orb特征法建立局部地图具体包括：
26.基于鱼眼摄像机获取的视觉图像，使用orb特征法进行特征提取和特征匹配，得到当前时刻的相机位姿，根据相机位姿中所包含的关键帧建立局部共视地图，然后在下一时刻获取新的关键帧，将新的关键帧插入局部共视地图中并更新局部共视地图，同时剔除局部共视地图中多余的关键帧得到局部地图。
27.进一步地，进行eskf融合具体包括：
28.设置eskf中的误差状态矩阵es、误差状态协方差矩阵ec和观测协方差矩阵mc，根据当前时刻相机的定位结果求得所述观测协方差矩阵mc；
29.根据惯性测量单元测量值和轮速计读数的融合结果和当前时刻相机的定位结果得到残差，所述残差包括车辆的位姿和旋转；
30.通过下式计算卡尔曼增益k：
31.k＝ph
t
(hph
t
+mc)-1
32.其中，p为所述系统状态协方差矩阵p
t
；h为观测矩阵；
33.然后进行状态更新：
34.误差状态矩阵更新es＝k
×
残差；协方差矩阵更新g＝(i-kh)；误差状态协方差矩阵更新ec＝gecg
t
+k
×
mc
×kt
；
35.其中，为观测矩阵；
36.最后将最后更新的误差状态矩阵es与观测协方差矩阵mc相加得到最终的定位结果。
37.本发明还涉及一种设备，所述设备包括存储器和处理器，所述存储器存储计算机处理指令，所述处理器通过调用所述计算机处理指令来执行上述的一种基于eskf的紧耦合定位方法。
38.本发明还涉及一种存储介质，所述存储介质存储有一个或者多个程序，所述一个或者多个程序被处理器运行时，执行如上所述的一种基于eskf的紧耦合定位方法的步骤。
39.相比现有技术，本发明具有如下有益效果：
40.本发明的一种基于eskf的紧耦合定位方法，使用imu(惯性测量单元)测量值融合轮速计读数预测运动汽车的位姿，通过鱼眼摄像头获取的视觉数据提取结构化环境中的平面特征并进行跟踪；然后使用误差状态卡尔曼滤波器(eskf)方法对鱼眼摄像头和imu与轮速计融合后的数据进行融合。本发明的多传感器组合的定位方案相较于单一传感器的定位方案，理论上结果会更准确、鲁棒性更强；同时使用eskf算法，在保持较高定位精度的情况下能有效降低定位算法的计算资源需求。
附图说明
41.图1为实施例的一种基于eskf的紧耦合定位方法的流程示意图。
具体实施方式
42.下面结合附图对本发明的具体实施方式作进一步的详细说明。
43.请参见图1，具体实施例的一种基于eskf的紧耦合定位方法，具体包括以下步骤：
44.获取惯性测量单元测量值和轮速计读数，并进行融合，同时进行状态传递；
45.通过车辆后端的鱼眼摄像头获取视觉数据并进行视觉建图定位得到当前时刻相机的定位结果；
46.基于eskf算法将惯性测量单元测量值和轮速计读数的融合结果作为预测，鱼眼摄像头的定位结果作为观测来完成eskf融合，获得最终的定位结果。
47.实施例的一种基于eskf的紧耦合定位方法，使用imu(惯性测量单元)测量值融合轮速计读数预测运动汽车的位姿，通过鱼眼摄像头获取的视觉数据提取结构化环境中的平面特征并进行跟踪；然后使用误差状态卡尔曼滤波器(eskf)方法对鱼眼摄像头和imu与轮速计融合后的数据进行融合。本发明的多传感器组合的定位方案相较于单一传感器的定位方案，理论上结果会更准确、鲁棒性更强；同时使用eskf算法，在保持较高定位精度的情况下能有效降低定位算法的计算资源需求。
48.实施时，本发明的imu以100hz的频率输出加速度和角速度数据、轮速计以50hz的频率输出轮脉冲信号、后视鱼眼摄像头以20hz的频率输出视觉数据，相较于单一传感器，多种传感器的输入可以使得模型在不同场景下都能有一个输入为可靠有效的，保证定位结果的准确性。
49.其中，基于惯性测量单元测量值和轮速计读数的融合结果构建系统状态，并推断
错误状态，完成状态传递。
50.构建系统状态时，状态量x的表达式如下：
[0051][0052]
其中，p为当前时刻车辆位姿；r为旋转；bg为惯性测量单元中陀螺仪的偏差；a为轮速因子，imu测量值和轮速计读数融合后得到的加速度值；
[0053]
通过下列各式得到错误状态δx
t
和系统状态协方差矩阵p
t
：
[0054]
δx
t
＝(i+f
t
δt)δx
t-1
[0055]
p
t
＝(i+f
t
)p
t-1
(i+f
t
)
t
+(g
t
)q(g
t
)
t
[0056]
其中，δt＝t
k-t
k-1
；tk和t
k-1
均为惯性测量单元的时间戳；p
t
为系统状态协方差矩阵；f
t
为误差状态转移矩阵；g
t
为时间矩阵；q为惯性测量单元提供的角速度ω的协方差矩阵。
[0057]
通过下式构建误差状态转移矩阵：
[0058][0059]
通过下式计算得到时间矩阵：
[0060][0061]
其中，v为当前时刻的轮速。
[0062]
这样，将imu的测量值和轮速计读数进行融合，是因为两种传感器各有各的优势。从imu得到的数据包括偏航角yaw，使用yaw来替代由编码器计算得到的角度，因为车辆出现打滑的时候，yaw的值并不会受到影响，得到的里程也能正确反映出来；imu包含了三个单轴的加速度计和三个单轴的陀螺仪，通过加速度二次积分得到位移信息，通过角速度积分得到三个角度。但imu的频率较高，适合短时间、快速的运动定位，它的主要缺点是存在零偏、低精度imu积分位姿发散，故不使用imu单独实现定位。所以我们结合odom里程计(即轮速计)和imu的优点，来优化odom里程计的误差。
[0063]
其中，使用orb特征法建立局部地图，并保存地图到本地文件夹，随后运行车载地图，通过鱼眼摄像头获取视觉数据后进行去畸变处理得到去畸变的图像，使用orb特征法获取初始10帧图像的特征点与所述局部地图中的关键帧进行特征点匹配，得到当前时刻相机的定位结果。
[0064]
orb(oriented fast and rotated brief)是一种快速特征点提取和描述的算法,提取均匀分布的特征点检测角点(oriented fast)，得到旋转多尺度角点orb特征描述子(rotated brief)，并计算角点方向，再得到orb特征描述子，然后进行单目稀疏特征匹配，
得到当前时刻的相机位姿。这里去畸变的处理方法为现有技术，故不多加赘述。
[0065]
其中，在使用orb特征法建立局部地图之前还需对相机位姿进行三角化恢复尺度，具体包括：
[0066]
通过轮速预积分计算得到相机两帧之间的欧式距离，同时计算欧式距离的误差值，若误差值≤0.01则使用轮速预积分得到的值替代相机两帧之间的欧式距离，反之则不用轮速预积分得到的值替代两帧之间的欧氏距离，完成三角化恢复尺度。
[0067]
计算所述欧氏距离的误差值包括：
[0068]
根据轮速预积分得到相机两帧之间的欧氏距离，再利用pnp算法(perspective-n-point是求解3d到2d点对运动的方法)得到两帧之间的估计欧式距离，将轮速预积分得到的欧氏距离减去两帧之间的估计欧式距离即可得到所述欧式距离的误差值：
[0069]
若误差值≤0.01则使用轮速预积分得到的值替代相机两帧之间的欧式距离，完成三角化恢复尺度。
[0070]
实施时，由于单目摄像头无法精确恢复尺度信息，本发明将轮速信息(由轮速计获取)加入优化矩阵中进一步优化对相机位姿的估计。首先由轮速预积分计算得出相机两帧ci和cj之间的欧式距离，然后利用pnp算法得出两帧之间的估计欧式距离||tc
icj
||，由于估计值无法精确等于测量值，故可以计算误差值，将计算得到的欧氏距离和估计欧氏距离相减即可得到误差值，其中误差值为标量，维数为1，当误差值≤0.01则使用轮速预积分得到的值替代两帧之间的欧式距离，即相机的尺度，也就是两帧之间的平移量(t)。
[0071]
其中，使用orb特征法建立局部地图具体包括：
[0072]
基于鱼眼摄像机获取的视觉图像，使用orb特征法进行特征提取和特征匹配，得到当前时刻的相机位姿，根据相机位姿中所包含的关键帧建立局部共视地图，然后在下一时刻获取新的关键帧，将新的关键帧插入局部共视地图中并更新局部共视地图，同时剔除局部共视地图中多余的关键帧得到局部地图。
[0073]
其中，进行eskf融合具体包括：
[0074]
设置eskf中的误差状态矩阵es、误差状态协方差矩阵ec和观测协方差矩阵mc，根据当前时刻相机的定位结果求得所述观测协方差矩阵mc；
[0075]
根据惯性测量单元测量值和轮速计读数的融合结果和当前时刻相机的定位结果得到残差，因为本实施例进行eskf融合时，将惯性测量单元测量值和轮速计读数的融合结果作为预测，当前时刻相机的定位结果作为观测，而预测和观测之间的差即是残差；所述残差包括车辆的位姿和旋转；
[0076]
通过下式计算卡尔曼增益k：
[0077]
k＝ph
t
(hph
t
+mc)-1
[0078]
其中，p为所述系统状态协方差矩阵p
t
；h为观测矩阵；
[0079]
然后进行状态更新：
[0080]
误差状态矩阵更新es＝k
×
残差；协方差矩阵更新g＝(i-kh)；误差状态协方差矩阵更新ec＝gecg
t
+k
×
mc
×kt
；
[0081]
其中，为观测矩阵；
[0082]
最后将最后更新的误差状态矩阵es与观测协方差矩阵mc相加得到最终的定位结
果。
[0083]
实施时，残差即为预测和观测之间的差，残差包括位姿和旋转；预测是惯性测量单元测量值和轮速计读数的融合结果，该模型中的信息是二维的；而观测则是由鱼眼摄像头的定位结果得出的观测协方差矩阵mc，该模型中的信息是三维的；故需要将三维的信息压缩至二维，这里，我们将z设置为0就可以了。
[0084]
本发明还提供一种设备，＝所述设备包括存储器和处理器，所述存储器存储计算机处理指令，所述处理器通过调用所述计算机处理指令来执行前述的一种基于eskf的紧耦合定位方法。
[0085]
本发明还提供一种存储介质，所述存储介质存储有一个或者多个程序，所述一个或者多个程序被处理器运行时，执行如前述的一种基于eskf的紧耦合定位方法的步骤。
[0086]
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-only memory)、随机存取存储器(ram，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质。
[0087]
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。
技术特征：1.一种基于eskf的紧耦合定位方法，其特征在于：具体包括以下步骤：获取惯性测量单元测量值和轮速计读数，并进行融合，同时进行状态传递；通过车辆后端的鱼眼摄像头获取视觉数据并进行视觉建图定位得到当前时刻相机的定位结果；基于eskf算法将惯性测量单元测量值和轮速计读数的融合结果作为预测，鱼眼摄像头的定位结果作为观测来完成eskf融合，获得最终的定位结果。2.根据权利要求1所述一种基于eskf的紧耦合定位方法，其特征在于：基于惯性测量单元测量值和轮速计读数的融合结果构建系统状态，并推断错误状态，完成状态传递。3.根据权利要求2所述一种基于eskf的紧耦合定位方法，其特征在于：构建系统状态时，状态量x的表达式如下：其中，p为当前时刻车辆位姿；r为旋转；b
g
为惯性测量单元中陀螺仪的偏差；a为轮速因子；通过下列各式得到错误状态δx
t
和系统状态协方差矩阵p
t
：δx
t
＝(i+f
t
δt)δx
t-1
p
t
＝(i+f
t
)p
t-1
(i+f
t
)
t
+(g
t
)q(g
t
)
t
其中，δt＝t
k-t
k-1
；t
k
和t
k-1
均为惯性测量单元的时间戳；p
t
为系统状态协方差矩阵；f
t
为误差状态转移矩阵；g
t
为时间矩阵；q为惯性测量单元提供的角速度ω的协方差矩阵。4.根据权利要求1所述一种基于eskf的紧耦合定位方法，其特征在于：使用orb特征法建立局部地图，并保存地图到本地文件夹，随后运行车载地图，通过鱼眼摄像头获取视觉数据后进行去畸变处理得到去畸变的图像，使用orb特征法获取初始十帧图像的特征点与所述局部地图中的关键帧进行特征点匹配，得到当前时刻相机的定位结果。5.根据权利要求4所述一种基于eskf的紧耦合定位方法，其特征在于：在使用orb特征法建立局部地图之前还需对相机位姿进行三角化恢复尺度，具体包括：通过轮速预积分计算得到相机两帧之间的欧式距离，同时计算欧式距离的误差值，若误差值≤0.01则使用轮速预积分得到的值替代相机两帧之间的欧式距离，反之则不用轮速预积分得到的值替代两帧之间的欧氏距离，完成三角化恢复尺度。6.根据权利要求5所述一种基于eskf的紧耦合定位方法，其特征在于：计算所述欧氏距离的误差值包括：根据轮速预积分得到相机两帧之间的欧氏距离，再利用pnp算法得到两帧之间的估计欧式距离，将轮速预积分得到的欧氏距离减去两帧之间的估计欧式距离即可得到所述欧式距离的误差值：若误差值≤0.01则使用轮速预积分得到的值替代相机两帧之间的欧式距离，完成三角化恢复尺度。7.根据权利要求4所述一种基于eskf的紧耦合定位方法，其特征在于：使用orb特征法建立局部地图具体包括：基于鱼眼摄像机获取的视觉图像，使用orb特征法进行特征提取和特征匹配，得到当前
时刻的相机位姿，根据相机位姿中所包含的关键帧建立局部共视地图，然后在下一时刻获取新的关键帧，将新的关键帧插入局部共视地图中并更新局部共视地图，同时剔除局部共视地图中多余的关键帧得到局部地图。8.根据权利要求1所述一种基于eskf的紧耦合定位方法，其特征在于：进行eskf融合具体包括：设置eskf中的误差状态矩阵es、误差状态协方差矩阵ec和观测协方差矩阵mc，根据当前时刻相机的定位结果求得所述观测协方差矩阵mc；根据惯性测量单元测量值和轮速计读数的融合结果和当前时刻相机的定位结果得到残差，所述残差包括车辆的位姿和旋转；通过下式计算卡尔曼增益k：k＝ph
t
(hph
t
+mc)-1
其中，p为所述系统状态协方差矩阵p
t
；h为观测矩阵；然后进行状态更新：误差状态矩阵更新es＝k
×
残差；协方差矩阵更新g＝(i-kh)；误差状态协方差矩阵更新ec＝gecg
t
+k
×
mc
×
k
t
；其中，为观测矩阵；最后将最后更新的误差状态矩阵es与观测协方差矩阵mc相加得到最终的定位结果。9.一种设备，其特征在于：所述设备包括存储器和处理器，所述存储器存储计算机处理指令，所述处理器通过调用所述计算机处理指令来执行上述权利要求1-8中任一项所述的一种基于eskf的紧耦合定位方法。10.一种存储介质，其特征在于：所述存储介质存储有一个或者多个程序，所述一个或者多个程序被处理器运行时，执行如权利要求1-8中任一项所述的一种基于eskf的紧耦合定位方法的步骤。
技术总结本发明涉及一种基于ESKF的紧耦合定位方法、设备及存储介质，方法具体包括以下步骤：获取惯性测量单元测量值和轮速计读数，并进行融合，同时进行状态传递；通过车辆后端的鱼眼摄像头获取视觉数据并进行视觉建图定位得到当前时刻相机的定位结果；基于ESKF算法将惯性测量单元测量值和轮速计读数的融合结果作为预测，鱼眼摄像头的定位结果作为观测来完成ESKF融合，获得最终的定位结果。本发明的多传感器组合的定位方案相较于单一传感器的定位方案，理论上结果会更准确、鲁棒性更强；同时使用ESKF算法，在保持较高定位精度的情况下能有效降低定位算法的计算资源需求。降低定位算法的计算资源需求。降低定位算法的计算资源需求。
技术研发人员：闫耀威 彭祥军 罗毅 王宽 康轶非 万凯林
受保护的技术使用者：重庆长安汽车股份有限公司
技术研发日：2022.04.29
技术公布日：2022/7/4
