一种基于地空交叉视角视觉里程计的定位方法

本发明属于同时定位和建图定位，具体涉及一种基于地空交叉视角视觉里程计的定位方法。

背景技术：

1、道路提取旨在从空中视角图像中提取道路网络。一些方法通过道路特征检测道路，例如线条、边缘、纹理。然而，由于光照、阴影和遮挡，道路并非在所有情况下都具有一般特征。人工描述的特征很难描述道路。随着神经网络技术的快速发展，越来越多的研究使用神经网络来提取特征。

2、地空交叉视角图像度量定位是将确定地面视角图像在空中视角图像中的位置这一任务视为图像回归问题。一般来说，车辆的大致位置可以由其他设备给出，例如imu、里程计等，车辆只需要通过一张地面视角图像在小范围地理位置内获得准确的位置。因此，可以将确定地面视角图像在空中视角图像中的位置这一任务视为图像回归问题：大致位置的空中视角图像可以由其他不准确的设备确定，而全景图像可以由车辆上的全景相机给出，接着使用神经网络从空中视角图像中预测地景图像的位置。

3、地面无人平台视觉里程计通过地面无人平台自身携带的相机，以及imu、轮速计等其他传感器，对周围环境进行观测，从而测得地面无人平台自身的位置。通常视觉里程计在小范围内较为准确，随着地面无人平台的运动，其测量误差会累积，使得其无法长时间使用。另外，视觉里程计在高动态的场景中容易失效，从而无法继续估计位姿。

技术实现思路

1、有鉴于此，本发明的目的是提供一种基于地空交叉视角视觉里程计的定位方法，可以提高定位精度。

2、一种基于地空交叉视角视觉里程计的定位方法，包括：

3、步骤s1、获取空中视角图像、地面视角图像及其相应的道路网络真值，建立图像训练集；

4、步骤s2、提供地面视角图像以及在地面视角图像位置的空中视角图像，以及空中视角图像对应的路网图像，一并输入地空交叉视角定位网络中，并建立损失函数，反向传播优化网络参数以实现训练；

5、其中，地空交叉视角定位网络包括孪生网络、交叉激励模块、特征图融合模块、概率图估计模块以及道路提取辅助训练模块；

6、孪生网络是指两个结构相同的网络，分为空中视角图像孪生网络分支和地面视角图像孪生网络分支；孪生网络的2个分支使用的都是resnet34网络；

7、空中视角图像孪生网络分支输入图像为空中视角图像；resnet34有4层：第1层有3个残差块，输出特征图a11；第2层有3个残差块，输出特征图a21；第3层有6个残差块，输出特征图a31；第4层有3个残差块，输出空中特征图a41；空中视角图像孪生网络分支的第2层输出的特征图a21送入给道路提取辅助训练模块；

8、地面视角图像孪生网络分支采用同样的结构，输入为地面全景图像；每一层的输出特征图为a12、a22、a32、a42；

9、在所述交叉激励模块中，经孪生网络主干输出的空中特征图a41和地面特征图a42分别经过特征激励层，得到空中激励向量b1和地面激励向量b2；再将空中激励向量b1与地面特征图a42按通道相乘，将地面激励向量b2与空中特征图a41按通道相乘，分别对应得到激励后空中特征图c1和激励后地面特征图c2；

10、在特征图融合模块中，将激励后空中特征图c1依次经过3×3卷积、归一化层、激活函数，得到卷积后空中特征图d1；将激励后地面特征图c2依次经过3×3卷积和激活函数，得到卷积后地面特征图d2；将卷积后空中的特征图d1和卷积后地面的特征图d2按的每个通道展开成256×1的矩阵；将每个向量按对应通道进行矩阵乘法，得到融合后的特征图e；

11、对融合后的特征图e，先经过大小3×3为，步长为2的卷积块得到特征图f；

12、在概率图估计模块中，特征图f经过反卷积网络，得到概率图g0，具体为：反卷积网络由3个反卷积块组成；反卷积块由1×1卷积块、反卷积、1×1卷积块连接而成；特征图f与空中视角图像孪生网络分支的第2层残差块输出的特征图a21拼接，经过第1个反卷积模块，得到特征图g2；特征图g2与空中视角图像孪生网络分支的第1层残差块输出的特征图a11拼接，经过第2个反卷积模块，得到特征图g1；特征图g1经过第3个反卷积模块得到概率图g0；

13、道路提取辅助训练模块是由反卷积模块组成，使用特征图f与空中图像孪生网络的支干网络输出a21进行按通道拼接，得到特征图h3；使用3个反卷积块将特征图h3上采样到大小为32×512×512的特征图h1；最后，使用3×3卷积层和sigmoid激活函数，得到空中视角图像视角的道路图像掩码图像h0；

14、步骤s3、在初始位姿已知的定位任务中，无人平台通过内部里程计进行定位；在运行的过程中，通过鱼眼相机或环视相机获得地面平台估计的当前位置的全景图像，并得到地面视角图像；根据本里程计估计出的前一时刻位姿，获得空中视角图像：

15、步骤s4、将空中视角图像同地面视角图像输入训练好的地空交叉视角定位网络中，得到估计的概率图g0；

16、步骤s5、计算概率图g0的数学特征，根据其数学特征判断是否使用地空交叉视角修正内部里程计，如果是，执行步骤6；

17、步骤s6、将地空交叉视角定位与内部里程计的位姿图中进行联合优化，获取定位后的结果。

18、较佳的，所述损失函数为：

19、loss＝lheadmap+bce；

20、其中：

21、

22、其中，代表网络输出的概率图g0每个像素的值，x代表概率图g0对应的真值图中每个像素值，n代表概率图像素的数量，α,β为参数；

23、

24、其中，y分别表示道路图像掩码图像h0中的每个像素的真值，分别表示整个图像中每个像素预测值，n代表概率图像素的数量。

25、较佳的，所述步骤s5中，设概率图g0的矩阵表示为h，其高为h，宽为w，对其中任一点(,y)∈h，有0≤h(x,y)≤1；设概率图g0像素值的和为

26、概率图g0在x和y两个方向的均值为：

27、

28、

29、概率图g0的协方差矩阵s为：

30、

31、设p(x,y)为均值方差为s的正态分布，q(x,y)为h(x,y)归一化后的分布，

32、q(x,y)＝h(,y)/m，采用kl散度计算q(x,y)与正态分布的相似性：

33、

34、记det|s|为s的行列式，当kl<k1且det|s|<k2且时，认为估计位置准确，该地空交叉定位结果可信；定义是否进行地空交叉视角定位系数为：

35、

36、其中，c(k)＝1时，使用地空交叉视角修正内部里程计；c(k)＝2时，不使用地空交叉视角修正内部里程计；k1,k2,k3为设定的阈值。

37、较佳的，取k1＝60000,k2＝50000,k3＝0.5。

38、较佳的，所述步骤s6中，若地空交叉定位结果可信，将概率图g0最大值像素坐标作为地面视角在空中图像中的位置；将其转换为交叉定位后的全景相机在全局坐标系下的位置为假设全景相机的高度和内部里程计的高度相同，均为则在第k帧全景相机的位姿估计表示为：

39、

40、对于全景相机的地空交叉视角定位结果，代表当前帧进行地空交叉匹配得到的全景相机在世界坐标系中的位姿，对于下一次地空交叉定位，与本次地空交叉定位相隔m帧，待优化的值为对于内部里程计的测量结果，代表第k+i+1帧相对于第k+i帧的位姿；由于全景相机和内部视觉里程计刚性连接，则优化后全景相机的位置变化和内部里程计位姿变化应该相等，则优化目标为：

41、

42、在每一个经过筛选的地空交叉视角匹配定位后，使用列文伯格马夸尔特算法等非线性优化方法，使得e最小，求得上式的最优解，得到优化后的每一帧的位姿即得到经过地空交叉视角匹配后优化的定位结果；

43、以[tq]表示位姿，其中t为平移，q为旋转对应的四元数。对于平移变量，内部里程计获得的在世界坐标系下的位置为下一帧在世界坐标系下的位置为和为待优化的变量，则在世界坐标系下，下一帧相对于当前帧的平移为：

44、

45、设为当前帧相对于世界坐标系的旋转四元数，在当前帧ck坐标系下，下一帧ck+1的平移为：

46、

47、设内部里程计得到的下一帧相对于当前帧的位置变化为则位置的残差为：

48、

49、这里tvar代表内部里程计平移的方差，取0.1；

50、对于旋转变量，当前帧在世界坐标系下的旋转四元数为下一帧在世界坐标系下的旋转四元数为和为待优化变量，在当前帧ck坐标系下，下一帧ck+1的旋转为：

51、

52、设内部里程计得到的下一帧相对于当前帧的旋转变化为则旋转的残差为为：

53、

54、这里qvar代表地图优化模块旋转的方差；

55、对于地空交叉视角定位，用代表当前帧进行地空交叉匹配得到的全景相机在世界坐标系中的位置，代表待优化的位置变量。对于下一次地空交叉定位，与本次地空交叉定位相隔m帧，其观测值为待优化的值为由于全景相机和内部里程计刚性连接，则优化后全景相机的位置变化和内部里程计位置变化应该相等，则有：

56、

57、第k帧地空交叉定位误差为：

58、

59、这里ovaqr代表地空交叉视角定位的方差；

60、选择距当前帧k的w帧以内的帧进行优化；最后待优化的误差为所有误差之和，其中[·；·；·]代表向量按列拼接：

61、

62、在每一个经过筛选的地空交叉视角匹配定位后，使用列文伯格马夸尔特算法等非线性优化方法，使得e最小，求得上式的最优解，得到优化后的每一帧的平移量和旋转量即得到对内部里程计经过地空交叉视角匹配后优化的定位结果。

63、本发明具有如下有益效果：

64、本发明通过判断地空视角定位网络输出的概率图的数学特征判断定位精度，作为其是否与内部里程计融合的条件，可以将不准确的地空教程定位结果剔除，将精度较高的定位结果和内部里程计融合，以获得更高的精度。本发明使用地面全景图像和离线获取的的空中视角图像进行匹配定位，可以在gnss信号受干扰的条件下提供全局定位信息；

65、本发明使用地空交叉视角定位和里程计融合，消除内部里程计长期运行带来的累积误差，提高定位精度。