基于速度输入的单目视觉惯性里程计虚拟初始化方法

1.本发明属于定位技术领域。


背景技术：

2.单目视觉惯性系统以其结构简单、占用空间小、获取信息丰富等特点，现已成为自主式导航研究领域的热门。单目视觉惯性里程计作为一种较为成熟的导航手段，被广泛应用于诸如无人车与无人机驾驶、物流运输、搜索救援、基础设施检查领域等等。但是通常视觉里程计算法在退化运动状态下难以完成初始化过程的问题。


技术实现要素：

3.发明目的：为了解决上述现有技术存在的问题，本发明提供了一种基于速度输入的单目视觉惯性里程计虚拟初始化方法。
4.技术方案：本发明提供了一种基于速度输入的单目视觉惯性里程计虚拟初始化方法，具体包括如下步骤：
5.步骤1：设置模拟载体，自适应的设置单目视觉惯性里程计虚拟初始化过程轨迹和路标点位置，设置虚拟初始化过程的配置参数文件和惯性传感器的噪声参数；
6.步骤2：在仿真软件中生成步骤1中的虚拟初始化仿真数据；
7.步骤3：在设定的虚拟初始化时间段内运行步骤2中生成的虚拟初始化仿真数据，获得模拟载体的状态量初值，完成在虚拟初始化结束时刻模拟载体的状态量与实际载体的状态量的对接，从而完成单目视觉惯性里程计虚拟初始化。
8.进一步的，所述步骤1中生成的虚拟初始化过程轨迹满足如下两条规则：
9.规则1：过程轨迹中模拟载体的初始速度和姿态角值均为连续的值；
10.规则2：过程轨迹的模拟载体的初始速度对时间的一阶导数值和模拟载体姿态角对时间的一阶导数值均为连续的值。
11.进一步的，所述步骤1中自适应的设置单目视觉惯性里程计虚拟初始化过程轨迹和路标点位置具体为：
12.步骤1.1：使用三轴上的正余弦运动进行初始化激励，形成正余弦轨迹，所述三轴为指向正北方的北向轴y，指向正东方的东向轴x和天向轴z，正余弦运动时模拟载体的位置p1(t)、速度v1(t)、加速度量a1(t)表达式如下：
[0013][0014]
[0015][0016]
其中，t表示时间变量，a
x
、ay、az分别代表三轴向正余弦运动轨迹的振幅，k为天向轴运动角速率倍数，t1表示正弦运动的预设耗时，π(
·
)为正比例映射关系，a
x
＝a
x0
·
π(v0),ay＝a
y0
·
π(v0),az＝a
z0
·
π(v0)，a
x0
，a
y0
和a
z0
均表示无量纲值；其中v0表示载体实际运动过程中初始时刻的速度的模值，也即完成虚拟初始化解算后实际载体当前时刻的运动瞬时速度；
[0017]
正余弦轨迹中的路标点设置在轨迹行径过程的视野范围之内；
[0018]
步骤1.2：完成正余弦运动后模拟载体进行类平抛运动，使得类平抛运动结束时模拟载体的速度与实际载体实际运动过程中初始时刻的速度相同，类平抛运动过程载体的位置p2(t)、速度v2(t)、加速度量a2(t)表达式如下所述：
[0019][0020][0021][0022]
其中t2表示类平抛运动的预设耗时，vy、vz分别代表y轴和z轴方向上的速度分量，且分量vy、vz满足如下关系：
[0023][0024]
类平抛运动过程中路标点设置在平抛轨迹行径过程的视野范围之内。
[0025]
进一步的，所述步骤3中运行仿真数据后得到模拟载体的状态量初值，此过程进行两次状态对接，第一次为正余弦运动结束时刻与类平抛运动初始时刻时模拟载体的状态对接第二次为类平抛运动结束时刻模拟载体的状态和虚拟初始化结束时实际载体在实际运动中的状态对接；为每一次对接过程设置滑动窗口，一个滑动窗口中共有k帧关键帧，在进行某次对接时，滑动窗口中第i帧关键帧的位置、姿态、速度分别记为psi、rsi、vsi，其中i＝1,2,
…
,k；对第i帧关键帧中的位置、姿态、速度进行角度补偿，得到补偿后的位置ps
i'
、姿态rs
i'
、速度vs
i'
为：
[0026][0027]
rs
comp
表示姿态角补偿，表达式如下所示：
[0028][0029]
其中，δk表示滑动窗口中第k帧的速度方向的yaw角度值。
[0030]
有益效果：本发明对于在难以完成初始化求解的退化运动状态下运行单目视觉惯性里程计算法提供了重要方法，仅需输入初始运动状态的速度参数，即可自适应生成虚拟初始化过程的视觉、惯性数据并完成初始化求解，具备简单易用，高精度，高可靠性特点。本发明实现了将单目视觉惯性里程计算法拓展至任意状态启动的条件中使用。
附图说明
[0031]
图1为本发明整体算法流程图；
[0032]
图2为本发明设计的虚拟初始化轨迹示意图；
[0033]
图3为本发明设计的正余弦运动过程中路标点设置示意图；
[0034]
图4为本发明设计的类平抛运动过程中路标点设置示意图；
[0035]
图5为本发明设计的城市环境仿真场景以及环境路标点设置示意图；
[0036]
图6为无人机运动仿真视野示意图，其中图(a)为无人机在第0s～第7s内的视野图，图(b)为无人机在第7s～第14s内的视野图，图(c)为无人机在第14s～第20s内的视野图；
[0037]
图7为车载运动仿真视野示意图，其中图(a)为车载在第0s～第5s内的视野图，图(b)为车载在第5s～第7s内的视野图；图(c)为车载在第7s～第10s内的视野图。
具体实施方式
[0038]
构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
[0039]
如图1所示，一种基于速度输入的单目视觉惯性里程计虚拟初始化方法。该方法使用单目视觉惯性里程计算法进行求解，具体包括如下步骤：
[0040]
步骤1：在计算机中，输入实际载体实际运动过程中初始时刻的速度参数，自适应设置虚拟初始化过程中模拟载体的运动轨迹方程、路标点位置。设置虚拟初始化过程的配置参数文件，惯性器件噪声参数设置为适当的较小值，其余参数保持与实际使用配置文件相同。
[0041]
步骤2：使用仿真工具imusim按照步骤1设置并生成虚拟初始化仿真数据，之后，按照时间顺序，将虚拟初始化中视觉关键帧与实际运动过程中的视觉关键帧对应，将虚拟初始化中惯性传感器数据与实际运动过程中的惯性传感器数据对应。
[0042]
步骤3：启动单目视觉惯性里程计算法。在设定的虚拟初始化时间段内首先运行步骤2中生成的虚拟初始化仿真数据，获得模拟载体的状态量初值，完成在虚拟初始化结束时
刻模拟载体的状态量与实际载体的状态量的对接；而后使用非线性优化过程求解实际运动轨迹参数。该步骤中需要先后执行两次状态对接操作。
[0043]
对于步骤1所述的基于速度输入的单目视觉惯性里程计虚拟初始化方法，输入到计算的实际载体实际运动过程中初始时刻的速度参数，是指：在单目视觉惯性里程计算法完成虚拟初始化解算后实际载体当前时刻的运动瞬时速度，理想情况下，若虚拟初始化可以在较短的时间内完成，则可不需要进行速度预测，直接输入当前时刻下的载体的实际运动速度。
[0044]
步骤1、2、3中对于初始化仿真、计算的过程均是一个虚拟化过程，需在使用单目视觉惯性里程计算法使用非线性优化环节求解实际运动轨迹前完成。
[0045]
所述步骤1中自适应生成的虚拟初始化过程轨迹需满足如下两条规则：
[0046]
规则1：轨迹过程中模拟载体的初始速度和姿态角值必须连续；
[0047]
规则2：轨迹过程的模拟载体的初始速度对时间的一阶导数值和姿态角对时间的一阶导数值必须连续。
[0048]
所述步骤1中自适应设置的虚拟初始化轨迹和路标点，需满足初始化过程中有充足的三轴向变化加速度激励、视觉视差激励，具体步骤为：
[0049]
步骤1.1：为满足三轴向变化加速度激励条件，首先使用三轴上的正余弦运动进行初始化激励，所述三轴为指向正北方的北向轴y，指向正东方的东向轴x和天向轴z，模拟载体的位置p1(t)、速度v1(t)、加速度量a1(t)表达式如下，如图2所示的轨迹前半部分：
[0050][0051][0052][0053]
公式中p1(t)、v1(t)、a1(t)中的t表示以时间量作为函数自变量。公式中a
x
、ay、az分别代表三轴向正余弦函数运动轨迹振幅，k为天向轴运动角速率倍数，t1表示初始化步骤1.1阶段的预设耗时，π代表圆周率常数。自适应函数π(
·
)为正比例映射关系：设步骤1中输入载体速度的模值为v0，该速度在xyz三轴振幅值之比设为a
x0
:a
y0
:a
z0
，则三轴振幅的设置与输入速度的模值成正比，可表示为如下形式：
[0054]ax
＝a
x0
·
π(v0),ay＝a
y0
·
π(v0),az＝a
z0
·
π(v0)
[0055]ax0
,a
y0az0
的均为无量纲值。
[0056]
公式中k、t1设置成恰当的正整数即可，与自适应函数π(
·
)无关。与步骤1.1所述的轨迹对应的路标点则设置在轨迹行径过程的视野范围之内，需依据轨迹形态进行设置。如图3所示，为步骤1.1的路标点设置示意图。
[0057]
步骤1.2：在完成初始化步骤1.1后执行类平抛运动，目的是使步骤1.2结束时刻模拟载体的速度与实际初始运动速度相同(步骤1中输入载体速度)。类平抛运动过程相应的位置p2(t)、速度v2(t)、加速度量a2(t)表达式如下，为图2中的轨迹后半部分：
[0058][0059][0060][0061]
公式中t2表示步骤1.2阶段的预设耗时，vy、vz分别代表y轴、z轴方向上的速度分量，也分别表示经过步骤1.1设置轨迹的虚拟初始化过程完成后，为顺利过渡到下一阶段，通过第一次状态对接获得的速度分量(第一次对接是指虚拟点从正余弦运动转换为类平抛运动)。完成对接后的输入速度将固定于航向角为0的平面，且分量vy、vz满足如下关系：
[0062][0063]
公式中t2设置成恰当的正整数即可，与自适应函数π(
·
)无关。与该轨迹对应的路标点则设置在轨迹行径过程的视野范围之内，需依据轨迹形态进行设置。图4为类平抛运动中路标点设置示意图。
[0064]
步骤1.3：设置步骤1中所使用的虚拟初始化过程配置参数文件。虚拟初始化所使用的惯性数据为无噪声数据，因此只需设置较小惯性噪声参数完成初始化解算即可。
[0065]
所述步骤3中运行仿真数据后得到模拟载体的状态量初值，此过程进行两次状态对接，第一次为正余弦运动结束时刻与类平抛运动初始时刻时模拟载体的状态对接，第二次状态对接为类平抛运动结束时刻虚拟载体的状态和虚拟初始化结束时实际载体的状态对接；
[0066]
步骤3.1：第一次状态对接过程。在步骤1.1中自适应设置的虚拟初始化过程的结束时刻，由于初始化阶段完成节点具有不确定性，且工具imusim中与单目视觉惯性里程计代码中对于航向角为0的平面定义相差90度(两者坐标系中两个x轴相差90度，两个y轴相差90度)，因此需在状态对接过程中对角度进行补偿，以此能够顺利将步骤1中的输入速度固定在航向角为0的平面内进行二维分解，采用这种方法，将速度固定在航向角为0的平面，对速度在该平面进行二维分解，由于该方案中，默认完成虚拟初始化后的载体速度矢量位于航向角为0的平面内，所以此速度在航向角为0的平面中有且仅有两个分量。姿态角补偿公式如下：
[0067][0068]
设滑动窗口关键帧序号i从0至k计数，公式中rs
comp
表示姿态角补偿矩阵，δk表示状
态对接时刻里程计速度方向的yaw角度值(δk表示滑动窗口中第k帧的速度方向的yaw角度值)。步骤1.1设置轨迹结束时滑动窗口中各关键帧的位置、姿态、速度分别记为psi、rsi、vsi，其中i＝1,2,
…
,k；运动状态量的对接完成后各帧状态为ps
i'
、rs
i'
、vs
i'
，则状态对接过程公式如下：
[0069][0070]
步骤3.2：在依据步骤1.2中设置的类平抛运动速度对接阶段结束时刻，执行第二次状态对接过程，该过程方法与步骤3.1中的方法相同，这里的姿态角补偿矩阵和各帧状态与也和步骤3.1中相同，滑动窗口中k的值可以不同。经过步骤3.2后，可确保执行实际轨迹解算的开始时刻，载体初始运动速度处于航向角为0的平面内。以此，保留虚拟初始化获得的系统状态量估计值，使得系统继续执行里程计非线性解算过程。
[0071]
本发明的方法可在单目视觉惯性里程计算法本身无法完成初始化求解的退化运动状态中恢复系统初值。此处提及的退化运动是使得系统初始化过程失效的运动状态，如：初始运动状态为匀速直线运动、定加速直线运动等。
[0072]
为了验证本发明的有效性，此处采用基于无人机、无人车的城市仿真实验场景对实际运动情况进行算法验证，如图5所示。首先，依据实际运动中使用惯性器件标定的噪声参数修改配置文件，需要修改图像分辨率、相机内参矩阵g、联合标定外参数矩阵加速度计白噪声方差σ
1n
、加速度计偏置方差σ
1w
、角速度计白噪声方差σ
2n
、角速度计偏置方差σ
2w
，本发明提供参考参数设置如表1所示。
[0073]
表1
[0074][0075]
为功率谱密度，用来描述惯性器件大小的单位。
[0076]
图5中表示运动过程的起始阶段：以两个建筑为一组记作单个区块，区块总长度为
d1米，以前行d2米作为完整运动过程。无人机从d3米高空开始作单轴匀加速俯冲运动，汽车从首个区块前d4米起步首先作匀速运动，后作匀减速运动至停车，这两种运动情况的初始运动状态均为退化运动。其中，本发明提供参考设置，d1设为100，d2设为300，d3设为150，d4设为50。
[0077]
无人机运动过程路标点视野如图6所示，为模拟真实的视觉场景，设置视野中的路标点存在重合部分。无人机初始运动条件vy为v
fy m/s、vz为v
fz m/s，前向作匀速直线运动，垂向加速度为总运动时间为tf秒。其中，本发明提供参考设置，v
fy
设为15，v
fz
设为-10，设为+0.5，tf设为20。
[0078]
车载运动过程的前三部分路标点视野如图7所示，类比无人机实际飞行，同样设置视野中存在共视区域。运动初始运动条件vy为v
cy m/s、vz为v
cz m/s，首先作匀速直线运动t
c1
秒，之后作匀减速直线运动t
c2
秒，加速度为a
c m/s2，总运动时间为tc秒。其中，本发明提供参考设置，v
cy
设为20，v
cz
设为0，ac设为-2，tc设为20，t
c1
设为10，t
c2
设为10。
[0079]
实验在相同加噪条件下重复十次，将东、北、天方向结果求平均值再与真值比较进行评估。求概率误差sep值计算公式如下所示，该公式中pe、pn、pu表示里程计求解的东、北、天向位置值，p
egt
、p
ngt
、p
ugt
表示当前时刻的东、北、天向位置值真值。公式中n表示样本个数，图像数据频率为f hz，本发明中的图像数据频率f设为30，因此20秒内的n最大取值为600。
[0080][0081]
采用本发明的参考数值，无人机运动实验得到的sep值统计结果如表2所示：
[0082]
表2
[0083][0084]
车载运动实验得到的sep值统计结果如表3所示：
[0085]
表3
[0086][0087][0088]
依据表中的统计数据可得到如下结论：使用本发明可以在输入速度参数后正确地完成里程计的初始化过程，并延续里程计的实际运动轨迹求解，精度良好。
[0089]
上面结合附图对本发明的实施方式作了详细说明，但是本发明并不局限于上述实
施方式。在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。