一种多传感器融合的旋转定位方法及计算机可读存储介质与流程

1.本发明涉及导航技术领域，具体而言，涉及一种多传感器融合的旋转定位方法及计算机可读存储介质。


背景技术：

2.在过去的几年里，机器人配备了越来越多的传感器来实现各种功能。相机、惯性传感器和激光雷达是最常用的传感器。相机可以捕捉丰富的环境信息，包括纹理和颜色，惯性传感器可以直接提供设备的运动学信息，激光雷达可以获得宽角度范围的精确距离测量。
3.由于测量是在不同的时间接收的，对于机器人运动估计存在运动失真的问题。解决这个问题的一种方法是结合同时定位和建图的多传感器融合算法，但这些算法大多依赖于精确的外部校准。现有的外部校准技术需要环境中的人工标记和适当的初始值来使校准收敛。此外，当前的校准方法仅适用于一对传感器，并且需要精确的传感器时间同步。这使得多传感器融合的校准成为一项复杂而专业的工作。因此亟待提供一种多传感器融合的旋转定位方法及计算机可读存储介质，以至少解决上述问题。


技术实现要素：

4.本发明的目的是提供一种多传感器融合的旋转定位方法及计算机可读存储介质，其能够至少部分地克服了现有技术中的不足。
5.根据本发明的一个方面，提供一种多传感器融合的旋转定位方法，包括如下步骤：
6.选取第一时刻和第二时刻；
7.获取摄像机在所述第一时刻和所述第二时刻之间的第一预估旋转矩阵，获取惯性传感器在所述第一时刻和所述第二时刻之间的第二预估旋转矩阵，获取激光雷达在所述第一时刻和所述第二时刻之间的第三预估旋转矩阵；
8.基于所述第一预估旋转矩阵、所述第二预估旋转矩阵和所述第三预估旋转矩阵，通过线性插值获得输出旋转向量，并基于所述输出旋转向量进行旋转定位。
9.优选地，所述获取摄像机在所述第一时刻和所述第二时刻之间的第一预估旋转矩阵包括：
10.获取所述摄像机在所述第一时刻的第一投影点坐标和在所述第二时刻的第二投影点坐标；
11.基于所述第一投影点坐标和所述第二投影点坐标，获得所述第一预估旋转矩阵。
12.优选地，所述基于所述第一投影点坐标和所述第二投影点坐标，获得所述第一预估旋转矩阵包括：
13.基于所述第一投影点坐标和所述第二投影点坐标，建立针孔成像模型；
14.基于所述针孔成像模型，获得所述第一预估旋转矩阵。
15.优选地，获取惯性传感器在所述第一时刻和所述第二时刻之间的第二预估旋转矩阵包括：
16.获取所述惯性传感器在所述第一时刻和所述第二时刻之间的线性加速度、角速度和重力加速度；
17.基于所述线性加速度、所述角速度和所述重力加速度，通过积分获得所述第二预估旋转矩阵。
18.优选地，所述获取激光雷达在所述第一时刻和所述第二时刻之间的第三预估旋转矩阵包括：
19.获取所述激光雷达在所述第一时刻的第一投影点集和在所述第二时刻的第二投影点集；
20.基于所述第一投影点集和所述第二投影点集之间的对应关系，获得所述第三预估旋转矩阵。
21.优选地，提取所述激光雷达在所述第一时刻和所述第二时刻之间的投影点云，将其重新投影至所述第二时刻，获得所述第二投影点集。
22.优选地，所述第一投影点集和所述第二投影点集均至少包括边缘点集和平面点集。
23.优选地，所述基于所述第一预估旋转矩阵、所述第二预估旋转矩阵和所述第三预估旋转矩阵，通过线性插值获得输出旋转向量包括：
24.获取时间伸缩阈值；
25.基于所述时间伸缩阈值对所述第一时刻和所述第二时刻进行伸缩，得到期望时间区间；
26.基于所述第一预估旋转矩阵、所述第二预估旋转矩阵和所述第三预估旋转矩阵，在所述期望时间区间内通过线性插值获得输出旋转向量。
27.优选地，所述基于所述第一预估旋转矩阵、所述第二预估旋转矩阵和所述第三预估旋转矩阵，通过线性插值获得输出旋转向量包括：构建过约束线性系统，基于所述第一预估旋转矩阵、所述第二预估旋转矩阵和所述第三预估旋转矩阵，通过线性插值获得输出旋转向量。
28.根据本发明另一方面，提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上任一项所述的多传感器融合的旋转定位方法。
29.本发明提供一种多传感器融合的旋转定位方法，包括选取第一时刻和第二时刻；获取摄像机、惯性传感器、激光雷达在第一时刻和第二时刻之间的预估旋转矩阵；基于预估旋转矩阵，通过线性插值获得输出旋转向量，并基于输出旋转向量进行旋转定位。通过本发明的旋转定位方法，使得基于多传感器的旋转标定不再需要精确的时间同步，也不需要传感器位置的初始信息以及在场景中放置标记和做配准。这种方法相比传统方法能够提供更精确的旋转定位。
附图说明
30.通过阅读参照以下附图所作的对非限制性实施例的详细描述，本发明的其它特征、目的和优点将会变得更明显：
31.图1是根据本发明的多传感器融合的旋转定位方法的流程图；
32.图2是根据本发明的获得第一预估旋转矩阵的一种实现方式的流程图；
33.图3是根据本发明的获得第一预估旋转矩阵的一种实现方式的流程图；
34.图4是根据本发明的获得第一预估旋转矩阵的一种实现方式的流程图；
35.图5是根据本发明的通过线性插值获得输出旋转向量的流程图。
具体实施方式
36.下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。为了便于描述，附图中仅示出了与发明相关的部分。
37.需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
38.下面结合附图和实施例对本技术作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与发明相关的部分。
39.本技术实施例中的摄像机，可以是单目摄像机、双目摄像机等多种选择，能够提供估计旋转量并提供第一预估旋转矩阵即可，本技术实施例中摄像机提供第一预估旋转矩阵，可以是基于本领域技术人员所熟知的双目视觉算法获得，也可以是基于本技术实施例所提供的基于针孔成像模型的算法获得。
40.本技术实施例中的惯性传感器，可以是具备陀螺仪和加速计的本领域常见的惯性传感器的设置方式，其能够在一定的精度范围内感受旋转角度、加速度和重力加速度，优选的情况下还可以具备重力加速度计。
41.本技术实施例中的激光雷达，可以是固体激光雷达、气体激光雷达或半导体激光雷达等多种本领域技术人员所熟知的设置方式，其能够提供多点集合扫描功能即可，这样可以为本技术实施例中基于点集进行旋转矩阵预估提供硬件基础。
42.如图1所示，本发明提供一种多传感器融合的旋转定位方法，包括如下步骤：
43.s101：选取第一时刻和第二时刻；
44.s102：获取摄像机在第一时刻和第二时刻之间的第一预估旋转矩阵，获取惯性传感器在第一时刻和第二时刻之间的第二预估旋转矩阵，获取激光雷达在第一时刻和第二时刻之间的第三预估旋转矩阵；
45.s103：基于第一预估旋转矩阵、第二预估旋转矩阵和第三预估旋转矩阵，通过线性插值获得输出旋转向量，并基于输出旋转向量进行旋转定位。
46.在处理s101中，第一时刻和第二时刻的选取可以是两个间隔较小，如0.1秒或更少的时间间隔的两端，这样可以在一段时间内反复执行本技术实施例所提供的旋转定位方法，并结合拟合、排除噪声等技术获得一个较为准确的定位，也可以是在一个有一定长度的时间间隔的两端选取上述的第一时刻和第二时刻，从而获取搭载有摄像机、惯性传感器和激光雷达的无人机、车辆等装置在第一时刻到第二时刻之间的旋转程度。
47.在处理s102中，获取摄像机在第一时刻和第二时刻之间的第一预估旋转矩阵，获取惯性传感器在第一时刻和第二时刻之间的第二预估旋转矩阵，获取激光雷达在第一时刻和第二时刻之间的第三预估旋转矩阵主要是指通过摄像机所采集的数据、通过惯性传感器
所采集的数据和通过激光雷达所采集的数据获取上述的预估旋转矩阵，这里的预估旋转矩阵主要是指以旋转角度、旋转方向和旋转位移构成的矩阵，本领域技术人员清楚其内涵，此处作解释说明。
48.由于摄像机、惯性传感器和激光雷达所采集到的数据是基于自身为原点的，因此上述的第一预估旋转矩阵、第二预估旋转矩阵和第三预估旋转矩阵均是相对估计，并不能代表整个无人机或车辆的旋转程度，需要进行处理后才能进行校正。
49.在处理s103中，优选地，基于第一预估旋转矩阵、第二预估旋转矩阵和第三预估旋转矩阵，通过线性插值获得输出旋转向量包括：构建过约束线性系统，基于第一预估旋转矩阵、第二预估旋转矩阵和第三预估旋转矩阵，通过线性插值获得输出旋转向量。例如可以可以通过奇异值分解求解约束线性系统，最小奇异值对应的右单位奇异向量为最小二乘解。由于在校准的初始阶段可能没有足够的运动，因此在校准过程的早期，所得到的输出旋转向量的值可能不准确。随着新的旋转的到来，估计的外方位将收敛到它们的稳定值，这可以通过计算欧拉角在局部周期内的方差来检查，从而得到一个较为准确的输出旋转向量的值。
50.在处理s103中，由于得到了搭载有摄像机、惯性传感装置和激光雷达的无人机或车辆经过上述步骤得到的在第一时刻和第二时刻之间的输出旋转向量，即可根据初始位置等信息对无人机或车辆进行旋转定位，这里的旋转定位，可以是定位无人机或车辆经过旋转后的位置，也可以是准确描述无人机或车辆等装置的旋转程度等，根据具体需要进行设置即可，此处不过多赘述。
51.本发明提供一种多传感器融合的旋转定位方法，包括选取第一时刻和第二时刻；获取摄像机、惯性传感器、激光雷达在第一时刻和第二时刻之间的预估旋转矩阵；基于预估旋转矩阵，通过线性插值获得输出旋转向量，并基于输出旋转向量进行旋转定位。通过本发明的旋转定位方法，使得基于多传感器的旋转标定不再需要精确的时间同步，也不需要传感器位置的初始信息以及在场景中放置标记和做配准。这种方法相比传统方法能够提供更精确的旋转定位。
52.作为一种优选的实现方式，如图2所示，，获取摄像机在第一时刻和第二时刻之间的第一预估旋转矩阵包括：
53.s1021：获取摄像机在第一时刻的第一投影点坐标和在第二时刻的第二投影点坐标；
54.s1022：基于第一投影点坐标和第二投影点坐标，获得第一预估旋转矩阵。
55.在处理s1022中，基于第一投影点坐标和第二投影点坐标，获得第一预估旋转矩阵的实现方式可以是基于第一投影点坐标和第二投影点坐标，建立针孔成像模型，并基于该针孔成像模型，获得第一预估旋转矩阵。这里的具体实现方式可以是如下所示：
56.p1和p2分别是第一时刻和第二时刻拍摄图像上的第一投影点坐标和第二投影点坐标，k是本质矩阵，e是新定义的本质矩阵，r和t是旋转和平移向量。使用针孔成像模型，可以得到表达式：
57.p1＝kp
58.p2＝k(rp+t)
59.对上述公式应用适当的变换，我们得到对极约束：
60.p
2t
k-t
ek-1
p1＝0
[0061][0062]
假定本质矩阵是已知的，给定一系列特征点，能估计出本质矩阵。让xi＝k-1pi，考虑一对点x1＝[u1 v1 1]t，x2＝[u2 v2 1]t，根据对极约束有：
[0063][0064]
根据上式，可以利用八点法求解e。一旦有了八对点，就可以求解e，然后利用奇异值分解得到旋转矩阵r和平移向量t，这样就可以得到第一预估旋转矩阵。
[0065]
作为一种优选的实现方式，如图3所示，获取惯性传感器在第一时刻和第二时刻之间的第二预估旋转矩阵包括：
[0066]
s1023：获取惯性传感器在第一时刻和第二时刻之间的线性加速度、角速度和重力加速度；
[0067]
s1024：基于线性加速度、角速度和重力加速度，通过积分获得第二预估旋转矩阵。
[0068]
处理s1023和处理s1024的优选的实现方式可以是如下所示：
[0069]
由于惯性传感器往往具有高达每秒数百个样本的高采样率，这带来了许多姿态状态，并且需要以高速率执行状态估计。这个问题可以通过整合两个时刻之间的观测值来解决。为了获得第二时刻j时世界坐标系中的位置、速度和姿态，惯性积分方程可以写成：
[0070][0071][0072][0073]
其中，是线性加速度，是惯性测量单元框架中时间t时的角速度。gw是世界框架中的重力加速度。和分别是惯性传感器在世界坐标系中的位置、速度和方向。如果在最后一个姿态的惯性测量单元框架中进行积分，等式可以重新排列成以下形式：
[0074][0075][0076][0077]
在上述等式中，积分是在惯性测量单元框架中进行的，相对于该框架的无人机或车辆的状态可以是已知的。因此，可以在没有初始条件要求的情况下集成惯性测量单元的测量。如果提取来自上述公式的惯性观测的积分，则获得从第一时刻i到第二时刻j的位置、速度和姿态的变化：
[0078][0079][0080][0081]
根据上述公式，即可获得上述的第二预估旋转矩阵。这样的第二预估旋转矩阵至少包括惯性传感器自第一时刻到第二时刻位置、速度和姿态的变化。
[0082]
作为一种优选的实现方式，如图4所示，获取激光雷达在第一时刻和第二时刻之间的第三预估旋转矩阵的实现方式可以是：
[0083]
s1025：获取激光雷达在第一时刻的第一投影点集和在第二时刻的第二投影点集；
[0084]
s1026：基于第一投影点集和第二投影点集之间的对应关系，获得第三预估旋转矩阵。
[0085]
在处理s1025中，优选地，提取激光雷达在第一时刻和第二时刻之间的投影点云，将其重新投影至第二时刻，获得第二投影点集。这里具体的实现方式可以是：
[0086]
将扫描定义为激光雷达完成一次扫描覆盖。使用m，m∈z+，来表示一次扫描。距离测量是在不同的时间接收的，运动估计中的误差会导致点云的配准错误。假设角速度和线速度在扫描期间是常数，那么可以对扫描中不同时间接收的点进行插值变换。设t为当前时间，t
m+1
为扫描m+1的开始时间。将定义为[t
m+1
，t]中激光雷达的变换。给定点pi，pi∈p
m+1
，ti是pi的时间戳，设是[t
m+1
，ti]之间的变换。[t
m+1
，ti]可通过线性插值估算：
[0087][0088]
将「
m+1
定义为从p
m+1
中提取的边点集和平面点集，将「
m+1
中的点重新投影到扫描开始处，即可获得点集。我们可以在「
m+1
和之间建立几何约束，如下所示：
[0089][0090]
其中是pi的坐标，pi∈「
m+1
。是的坐标，的坐标，
[0091]
在扫描m结束时，在该扫描期间接收的点云被重新投影到时间戳t
m+1
，将重新投影的点云表示为的点云表示为是在中提取的边缘点和平面点的集合。利用一定的规则寻找和之间的对应关系并计算对应距离，可以导出一个函数：
[0092][0093]
对于「
m+1
中的每个点，得到一个非线性函数：
[0094][0095]
公式(3.4)可以通过非线性迭代通过将d向零最小化来解决。
[0096]
优选地，处理s103中，如图5所示，基于第一预估旋转矩阵、第二预估旋转矩阵和第三预估旋转矩阵，通过线性插值获得输出旋转向量的优选地实现方式可以是：
[0097]
s1031：获取时间伸缩阈值；
[0098]
s1032：基于时间伸缩阈值对第一时刻和第二时刻进行伸缩，得到期望时间区间；
[0099]
s1033：基于第一预估旋转矩阵、第二预估旋转矩阵和第三预估旋转矩阵，在期望时间区间内通过线性插值获得输出旋转向量。
[0100]
优选地，基于第一预估旋转矩阵、第二预估旋转矩阵和第三预估旋转矩阵，通过线性插值获得输出旋转向量包括：构建过约束线性系统，基于第一预估旋转矩阵、第二预估旋转矩阵和第三预估旋转矩阵，通过线性插值获得输出旋转向量。
[0101]
其具体实现方式可以是：
[0102]
由于传感器是异步的，即它们在不同的时间采样，因此上面获得的相对旋转处于不同的时间间隔。因此，在假设短时间内匀速运动的情况下，可以采用线性插值来获得相同时间间隔内的相对旋转。
[0103]
在时间间隔中获取激光雷达旋转，并且是与[ti，tj]最接近的间隔。所以在相似的时间间隔内有不同传感器的相对旋转。处理s1031中获取时间伸缩阈值的方式可以是根据经验设置一个常数，也可以是定义一个分数，确定相对旋转需要延伸或收缩多少：
[0104][0105]
其中t0和t1是相对旋转的原始开始和结束时间，t
′0和t
′1是期望的开始和结束时间。该分数可以是正的或负的，分别对应于相对旋转的延伸或收缩。此外，我们定义相对旋转插值如下：
[0106][0107]
其中exp()是从轴角表示so(3)到旋转矩阵so(3)的指数映射，log()是从旋转矩阵s0(3)到轴角表示s0(3)的对数映射。
[0108]
在获得了时间伸缩阈值之后，可以通过对准来自惯性测量单元、相机和激光雷达的三个旋转序列来校准外部方位。我们假设可以跟踪足够多的特征，这样我们就可以在图像fi和fj之间使用八点算法估计相机旋转下等式适用于任何i和j(i＜j)：
[0109][0110]
线性系统的每一行都有相同的形式：
[0111][0112]
为了恢复旋转矩阵可以用单位四元数替换上述矩阵：
[0113][0114][0115]
其中是四元数乘法运算符。单位四元数定义为：
[0116][0117]
函数定义为：
[0118][0119][0120]
通过使用增量旋转序列，可以构建过约束线性系统：
[0121][0122]
重。线性系统将通过引入的测量值重建，我们能够使用先前估计的旋转来计算残差。残差定义为：
[0123][0124]
||
·
||是向量的范数。权重可以定义为：
[0125][0126]
其中s是放大影响的因子。结合上述等式，我们可以得到过度约束的线性系统：
[0127][0128]
上述过约束线性系统可以通过奇异值分解求解，最小奇异值对应的右单位奇异向量为最小二乘解。由于在校准的初始阶段可能没有足够的运动，因此在校准过程的早期，我们估计的值可能不准确。随着新的旋转的到来，估计的外方位将收敛到它们的稳定值，这可以通过计算欧拉角在局部周期τ内的方差来检查。和分别对应于局部周期τ内的偏航、俯仰和滚转的方差。如果和η为阈值，则校准成功。
[0129]
上述对本说明书特定实施例进行了描述。其它实施例在所附权利要求书的范围内。在一些情况下，在权利要求书中记载的动作或步骤可以按照不同于实施例中的顺序来执行并且仍然可以实现期望的结果。另外，在附图中描绘的过程不一定要求示出的特定顺序或者连续顺序才能实现期望的结果。在某些实施方式中，多任务处理和并行处理也是可以的或者可能是有利的。
[0130]
本技术还提供一种计算机可读介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如上所描述的多传感器融合的旋转定位方法。该计算机可读介质包括永久性和非永久性、可移动和非可移动，媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(pram)、静态随机存取存储器(sram)、动态随机存取存储器(dram)、其他类型的随机存取存储器(ram)、只读存储器(rom)、电可擦除可编程只读存储器(eeprom)、快闪记忆体或其他内存技术、只读光盘只读存储器(cd-rom)、数字多功能光盘(dvd)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
[0131]
以上描述仅为本技术的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本技术中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本技术中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。