一种基于鱼眼视觉和惯性测量的目标定位方法及装置与流程

本发明涉及同步定位与建图领域，具体涉及一种基于鱼眼视觉和惯性测量的目标定位方法及装置。

背景技术：

随着人工智能技术的飞速发展，自动驾驶领域的研究成为了当前的研究热点，对车辆的精准定位是自动驾驶领域必不可少的研究方向。同时定位与地图创建(simultaneouslocalizationandmapping,slam)是指移动机器人在未知环境中利用机身传感器对环境进行感知的同时构建高精度地图并定位自身位置的技术，是自动驾驶平台实现高精度定位的关键技术之一。目前slam定位系统主要分为纯激光雷达方案、纯视觉方案、多种传感器融合方案，其中激光雷达存在成本较高，环境表达弱等问题，纯视觉方案存在图像处理量巨大、受光线条件限制较大等问题，因此如何设计可以提供高精度、数据处理量小的多种传感器融合方案为亟需解决的问题。

中国专利cn109520497a公开了一种基于视觉和imu的无人机自主定位方法，其具体公开了：对imu获取到的加速度信息和角速度信息的预积分处理，得到位置信息、速度信息和旋转信息；基于视觉信息求解位姿，将其与imu预积分得到的信息进行配准，更新预积分的值；在滑动窗口中对视觉-惯性位姿估计数据进行联合优化，得到精度较高的位姿。发明人发现，由于该方案中采用的是单目视觉数据，而普通单目的观测范围窄，观测的环境信息量少，导致定位精度不高且稳定性差。另外该方案还需要先对该单目视觉数据做畸变校正处理，算法需要处理的数据量大，进而增加了整个系统的计算成本。

技术实现要素：

因此，本发明要解决的技术问题在于克服现有单目视觉定位方案中定位精度不高，稳定性差的不足，以及现有技术中数据处理量大导致系统计算成本高的缺陷，从而提供一种基于鱼眼视觉和惯性测量的目标定位方法及装置。

本发明第一方面提供了一种基于鱼眼视觉和惯性测量的目标定位方法，包括：获取目标对象当前帧的鱼眼图像数据和惯性测量数据，当前帧的鱼眼图像数据和惯性测量数据的时间戳相同，鱼眼图像数据为采用鱼眼相机拍摄到的图像数据；根据惯性测量数据和目标对象的前一帧的位姿获取目标对象在当前帧的第一位姿和第一速度，第一位姿包括第一位置和第一姿态；从当前帧的鱼眼图像数据提取视觉特征；获取局部地图中距离第一位姿第一预设范围内的与视觉特征相对应的第一地图点；利用第一优化函数基于第一地图点、第一位姿、第一速度、惯性测量单元的第一偏置计算目标对象在最小误差下的第二位姿，以对第一位姿进行优化，其中，误差包括第一地图点在当前帧的鱼眼图像数据的重投影误差，以及惯性测量变量误差。

可选地，在本发明提供的基于鱼眼视觉和惯性测量的目标定位方法中，利用第一优化函数基于第一地图点、第一位姿、第一速度、惯性测量单元的第一偏置计算目标对象在最小误差下的第二位姿之后，还包括：根据第二位姿确定第一地图点中与第二位姿相匹配的地图点的数量；若第一地图点的数量小于预设阈值，或第一地图点中与第二位姿相匹配的地图点的数量小于预设阈值，获取局部地图中距离第一位姿第二预设范围内的与视觉特征相对应的第二地图点；利用第一优化函数基于第二地图点、第一位姿、第一速度、惯性测量单元的第一偏置重新计算目标对象在最小误差下的第二第二位姿，以对第一位姿进行优化。

可选地，在本发明提供的基于鱼眼视觉和惯性测量的目标定位方法中，利用第一优化函数基于第一地图点、第一位姿、第一速度、惯性测量单元的第一偏置计算目标对象在最小误差下的第二位姿的步骤，包括：分别计算每个第一地图点在当前帧的鱼眼图像数据上的重投影误差；计算当前帧的鱼眼图像数据对应的惯性测量数据与前一帧的鱼眼图像数据对应的惯性测量数据之间的惯性测量预积分误差；获取所有第一地图点的重投影误差和惯性测量预积分误差的误差和最小时对应的第二姿态、第二位置、第二速度、第二偏置，将第二姿态和第二位置作为第二位姿。

可选地，在本发明提供的基于鱼眼视觉和惯性测量的目标定位方法中，第一优化函数为：

其中，x表示优化变量，evision(k,j)表示第k个第一地图点在当前帧j的鱼眼图像数据上的重投影误差，eimu(i,j)表示当前帧j的鱼眼图像数据对应的惯性测量数据与前一帧i的鱼眼图像数据对应的惯性测量数据之间的惯性测量预积分误差；ρ为误差项核函数，u^k为与第k个第一地图点对应的二维图像点，π(*)表示相机坐标系中三维点到图像坐标系中二维点的映射函数；rcb表示惯性测量单元坐标系到本体坐标系的旋转变换，rwb表示惯性测量单元测量到的姿态，rcw表示第二姿态，表示第一地图点，tcb表示惯性测量单元坐标系到本体坐标系的平移变换，pwb表示惯性测量单元测量到的位置，tcw表示第二位置，σk表示重投影误差的信息矩阵；er，ev，ep，eb分别表示第二姿态与第一姿态的姿态误差、第二速度与第一速度的速度误差、第二位置与第一位置的位置误差、第二偏置与第一偏置的偏置误差；σi，σb分别表示运动状态和惯性测量单元偏置测量的信息矩阵。

可选地，在本发明提供的基于鱼眼视觉和惯性测量的目标定位方法中，若当前帧为一关键帧，基于鱼眼视觉和惯性测量的目标定位方法还包括：根据当前帧的鱼眼图像数据生成第三地图点；利用第二优化函数基于预设长度窗口内的其他关键帧对应的地图点、位置、姿态、速度、偏置，以及当前帧对应的第三地图点、第二姿态、第二位置、第二速度、第二偏置计算预设长度窗口内的其他关键帧中目标对象在最小误差下的优化位姿以及当前帧中目标对象在最小误差下的第三位姿，以对其他关键帧的位姿和当前帧的第二位姿进行优化，其中，误差包括其他关键帧对应的地图点在预设长度窗口内所有关键帧的鱼眼图像数据的重投影误差和第三地图点在预设长度窗口内所有关键帧的鱼眼图像数据的重投影误差。

可选地，在本发明提供的基于鱼眼视觉和惯性测量的目标定位方法中，利用第二优化函数基于预设长度窗口内的其他关键帧对应的地图点、位置、姿态、速度、偏置，以及当前帧对应的第三地图点、第二姿态、第二位置、第二速度、第二偏置计算预设长度窗口内的其他关键帧中目标对象在最小误差下的优化位姿以及当前帧中目标对象在最小误差下的第三位姿的步骤，包括：分别计算每个地图点在与各地图点存在匹配关系的关键帧的鱼眼图像数据上的重投影误差以及第四地图点在存在匹配关系的关键帧的鱼眼图像数据上的重投影误差，第四地图点为第三地图点优化得到的；分别计算相邻两个关键帧对应的惯性测量数据之间的惯性测量预积分误差；获取所有重投影误差和惯性测量预积分误差的误差和最小时其他关键帧对应的优化位置和优化姿态，以及当前帧对应的第三姿态、第三位置、第三速度、第三偏置、第四地图点，将优化位置和优化姿态作为其他各关键帧对应的优化位姿，将第三姿态和第三位置作为第三位姿；将第四地图点加入局部地图中。

可选地，在本发明提供的基于鱼眼视觉和惯性测量的目标定位方法中，第二优化函数为：

其中，x表示优化变量，l表示窗口长度，evision(k,j)表示第k个第四地图点在第j帧的鱼眼图像数据上的重投影误差，eimu(i,j)表示第i帧的鱼眼图像数据对应的惯性测量数据与第j帧的鱼眼图像数据对应的惯性测量数据之间的惯性误差项；ρ为误差项核函数，u^k为与第k个第四地图点对应的二维图像点，π(*)表示相机坐标系中三维点到图像坐标系中二维点的映射函数；rcb表示惯性测量单元坐标系到本体坐标系的旋转变换，rwb表示惯性测量单元测量到的姿态，rcw表示第三姿态，表示第四地图点数据，tcb表示惯性测量单元坐标系到本体坐标系的平移变换，pwb表示惯性测量单元测量到的位置，tcw表示第三位置，σk表示重投影误差的信息矩阵；er，ev，ep，eb分别表示第三姿态与第二姿态的姿态误差、第三速度与第二速度的速度误差、第三位置与第二位置的位置误差、第三偏置与第二偏置的偏置误差；σi，σb分别表示运动状态和惯性测量单元偏置测量的信息矩阵。

可选地，在本发明提供的基于鱼眼视觉和惯性测量的目标定位方法中，若其他关键帧中存在与当前帧的鱼眼图像数据相匹配的关键帧，基于鱼眼视觉和惯性测量的目标定位方法还包括：利用第三优化函数基于第i个关键帧和第i+1个关键帧对应的第三姿态和第三位置计算在第i个关键帧中目标对象在最小误差下的第四姿态和第四位置，将第四姿态和第四位置作为第四位姿，以对第三位姿进行优化；若存在未确定第四位姿的关键帧，将i的值加1，并返回利用第三优化函数基于第i个关键帧和第i+1个关键帧对应的第三姿态和第三位置计算在第i个关键帧中目标对象在最小误差下的第四姿态和第四位置的步骤。

可选地，在本发明提供的基于鱼眼视觉和惯性测量的目标定位方法中，第三优化函数为：

其中，其中，x表示优化变量，ekf(i,i+1)为相邻两个关键帧的位姿约束，δri(i+1)为关键帧i与关键帧i+1的第四姿态的相对旋转矩阵，即δtij为关键帧i与关键帧i+1的第四位置的相对位移，即σk为相邻两个关键帧相对位姿误差项的信息矩阵。

可选地，本发明提供的基于鱼眼视觉和惯性测量的目标定位方法还包括：利用第四优化函数基于各关键帧对应的第四地图点、第四位置、第四姿态、第三速度、第三偏置计算各关键帧中目标对象在最小误差下的第五位姿，以对第四位姿进行优化，其中，误差包括第四地图点在与各第四地图点对应的关键帧的鱼眼图像数据的重投影误差；将第四优化函数的函数值最小时对应的第五地图点数据的集合确定为优化地图。

可选地，在本发明提供的基于鱼眼视觉和惯性测量的目标定位方法中，利用第四优化函数基于所述各关键帧对应的第四地图点、第四位置、第四姿态、第三速度、第三偏置计算各关键帧中目标对象在最小误差下的第五位姿的步骤，包括：分别计算每个第五地图点在与各第五地图点对应的关键帧的鱼眼图像数据上的重投影误差，第五地图点是第四地图点优化得到的；计算各关键帧的鱼眼图像数据对应的惯性测量数据与前一个关键帧的鱼眼图像数据对应的惯性测量数据之间的惯性测量预积分误差；获取所有第五地图点的重投影误差和惯性测量预积分误差的误差和最小时对应的第五姿态、第五位置、第四速度、第四偏置、第五地图点，将第五姿态和第五位置作为第五位姿。

可选地，在本发明提供的基于鱼眼视觉和惯性测量的目标定位方法中，第四优化函数为：

其中，x表示优化变量，evision(k,j)表示第k个第五地图点数据在第j帧视觉图像数据上的重投影误差，eimu(i,j)表示第i帧的鱼眼图像数据对应的惯性测量数据与第j帧的鱼眼图像数据对应的惯性测量数据之间的惯性误差项；ρ为误差项核函数，u^k为与第k个第五地图点数据匹配的二维图像点，π(*)表示相机坐标系中三维点到图像坐标系中二维点的映射函数；rcb表示惯性测量单元坐标系到本体坐标系的旋转变换，rwb表示惯性测量单元测量到的姿态，rcw表示第五姿态，表示第五地图点数据，tcb表示惯性测量单元坐标系到本体坐标系的平移变换，pwb表示惯性测量单元测量到的位置，tcw表示第五位置，σk表示重投影误差的信息矩阵，er，ev，ep，eb分别表示第五姿态与第四姿态的姿态误差、第四速度与三速度的速度误差、第五位置与第四位置的位置误差、第四偏置与第三偏置的偏置误差；σi，σb分别表示运动状态和惯性测量单元偏置测量的信息矩阵。

可选地，在本发明提供的基于鱼眼视觉和惯性测量的目标定位方法中，获取目标对象当前帧的鱼眼图像数据和惯性测量数据的步骤，包括：获取目标对象的当前帧的鱼眼图像数据和当前帧的鱼眼图像数据的时间戳；若不存在时间戳与当前帧的鱼眼数据相同的原始惯性测量数据，获取原始惯性测量数据中时间戳与当前帧的鱼眼图像数据的时间戳向前相邻的第一原始惯性测量数据和向后相邻的第二原始惯性测量数据；获取第一原始惯性测量数据与当前帧的鱼眼图像数据的时间差；根据第一原始惯性测量数据、第二原始惯性测量数据、第一原始惯性测量数据的时间戳、第二原始惯性测量数据的时间戳、第一原始惯性测量数据与当前帧的鱼眼图像数据的时间差获取惯性测量数据。

本发明第二方面提供了一种基于鱼眼视觉和惯性测量的目标定位装置，包括：数据获取模块，用于获取目标对象当前帧的鱼眼图像数据和惯性测量数据，当前帧的鱼眼图像数据和惯性测量数据的时间戳相同，鱼眼图像数据为采用鱼眼相机拍摄到的图像数据；第一姿态获取模块，用于根据惯性测量数据和目标对象的前一帧的位姿获取目标对象在当前帧的第一位姿和第一速度，第一位姿包括第一位置和第一姿态；视觉特征提取模块，用于从当前帧的鱼眼图像数据提取视觉特征；第一地图点获取模块，用于获取局部地图中距离第一位姿第一预设范围内的与视觉特征相对应的第一地图点；第一位姿优化模块，用于利用第一优化函数基于第一地图点、第一位姿、第一速度、惯性测量单元的第一偏置计算目标对象在最小误差下的第二位姿，以对第一位姿进行优化，其中，误差包括第一地图点在当前帧的鱼眼图像数据的重投影误差，以及惯性测量变量误差。

本发明第三方面提供了一种计算机设备，包括：至少一个处理器；以及与至少一个处理器通信连接的存储器；其中，存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，从而执行如本发明第一方面提供的基于鱼眼视觉和惯性测量的目标定位方法。

本发明第四方面提供了一种计算机可读存储介质，计算机可读存储介质存储有计算机指令，计算机指令用于使计算机执行如本发明第一方面提供的基于鱼眼视觉和惯性测量的目标定位方法。

本发明技术方案，具有如下优点：

本发明提供的基于鱼眼视觉和惯性测量的目标定位方法及装置，先获取目标对象当前帧的鱼眼图像数据和惯性测量数据，其中，鱼眼图像数据是通过鱼眼相机拍摄到的，然后根据惯性测量数据获取目标对象在当前帧的第一位姿，然后从当前帧的鱼眼图像数据提取视觉特征，获取与视觉特征相对应的第一地图点，最后利用第一优化函数计算目标对象在最小误差下的第二位姿，其中，误差包括第一地图点在当前帧的鱼眼图像数据的重投影误差。首先，本发明中获取鱼眼图像数据时，是通过鱼眼相机获取的，鱼眼相机大视场感知的特点可以在局部地图中获取更多的地图点，从而强化视觉特征对位姿的约束，达到提升定位准确度的目的，其次，本发明中图像特征提取步骤和利用第一优化函数计算的重投影误差步骤都是直接在原始鱼眼图像上进行，无需对鱼眼图像图像数据进行畸变校正，从而减小了需要处理的数据量，降低了计算成本。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1-图9为本发明实施例中基于鱼眼视觉和惯性测量的目标定位方法的具体示例的流程图；

图10为本发明实施例中基于鱼眼视觉和惯性测量的目标定位装置的具体示例的原理框图；

图11为发明实施例中提供的计算机设备的原理框图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需要说明的是，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。

此外，下面所描述的本发明不同实施方式中所涉及的技术特征只要彼此之间未构成冲突就可以相互结合。

实施例1

本发明实施例提供了一种基于鱼眼视觉和惯性测量的目标定位方法，如图1所示，包括：

步骤s11：获取目标对象当前帧的鱼眼图像数据和惯性测量数据，当前帧的鱼眼图像数据和惯性测量数据的时间戳相同。鱼眼图像数据为采用安装在目标对象上的鱼眼相机拍摄到的图像数据，惯性测量数据为通过安装在目标对象上的惯性测量单元(inertialmeasurementunit,imu)测量得到的，惯性测量数据包括陀螺仪和加速度计测量得到的数据：

其中，表示imu坐标系下加速度计的测量值，即测量加速度；rbw为世界坐标系w到imu坐标系b的旋转矩阵；aw表示世界坐标系下imu真实的加速度；gw表示重力加速度；b^a表示加速度计的偏置；η^a表示加速度计的测量噪声；表示imu坐标系下陀螺仪的测量值，即测量角速度；ωb表示imu坐标系下陀螺仪的真实角速度；b^g表示陀螺仪的偏置；η^g表示陀螺仪的测量噪声。

步骤s12：根据惯性测量数据和目标对象的前一帧的位姿获取目标对象在当前帧的第一位姿和第一速度，第一位姿包括第一位置和第一姿态。

具体地，求解第一姿态时，先对当前帧对应的惯性测量数据进行预积分：

其中，表示目标对象在当前帧j相对于上一帧i的姿态增量，表示目标对象在当前帧j相对于上一帧i的速度增量，表示目标对象在当前帧j相对于上一帧i的位置增量，表示目标对象在当前帧j的速度，表示目标对象在上一帧i的速度，δtij表示目标对象在当前帧j和上一帧i的时间戳的差，表示目标对象在当前帧j的位置，表示目标对象在上一帧i的位置；

然后基于预积分得到的位置增量、姿态增量、上一帧的位置、上一帧的姿态获取当前帧的第一位姿。

在一具体实施例中，由于对当前帧的第一位姿的计算需要依赖于上一帧的位姿，但是第一次通过惯性测量单元获取位姿时，没有上一帧的位姿，因此在第一次通过惯性测量单元获取位姿前，需要利用纯视觉vo与imu预积分技术初始化imu的角速度偏置、加速度偏置、重力加速度、初始速度：

首先，通过如下目标函数利用非线性最小二乘方法对imu陀螺仪偏置进行迭代优化估计，得到角速度偏置：

其中，δri,i+1为i与i+1时间段内的imu姿态预积分；exp表示姿态从李代数到李群表示的映射；为姿态预积分的雅克比；根据姿态传递关系可得rwb＝rwcrcb，rwc由纯视觉vo得到；根据旋转矩阵的正交性可得rbw＝(rwb)^t，通过上述公式可优化求解出角速度偏置的最优估计值。

然后，通过如下公式计算imu尺度和重力加速度：

β(i)＝-0.5(δt12²δt23+δt23²δt12)i3×3,(9)

上述关系式是利用相邻三个视觉vo的关键帧、视觉与惯性位姿相似变换关系得到的，其中，pwc表示当前位姿下相机光心在世界坐标系中的位置；rwc表示当前位姿下相机坐标系相对于世界坐标系的姿态；pcb表示imu坐标系相对于相机坐标系的位移，由于通常相机与imu传感器安装的相对位置固定，所以pcb可通过标定得到。设置重力加速度幅值g＝9.81m/s²，对于n个连续的视觉vo关键帧可以建立n-2个上述方程，因此，至少需要连续四个关键帧可求解尺度s和重力加速度g的粗略解。

其次，通过如下公式计算加速度偏置：

其中，rwi为重力方向从惯性系b到世界系的旋转矩阵；g为重力模长，δθxy为rwi的右扰动，j为雅克比矩阵。

最后，通过计算得到的角速度偏置、加速度偏置尺度、重力加速度和公式(5)求解出当前状态下的运动速度。

步骤s13：从当前帧的鱼眼图像数据提取视觉特征。

步骤s14：获取局部地图中距离第一位姿第一预设范围内的与视觉特征相对应的第一地图点。

在一具体实施例中，先根据鱼眼相机模型和第一位姿将三维地图点投影至二维图像平面，然后以第一位姿的二维特征点为中心按照第一预设范围创建搜索区域，然后将搜索区域中与视觉特征相匹配的二维点相对应的三维地图点确定为第一地图点。

步骤s15：利用第一优化函数基于第一地图点、第一位姿、第一速度、惯性测量单元的第一偏置计算目标对象在最小误差下的第二位姿，以对第一位姿进行优化，其中，误差包括第一地图点在当前帧的鱼眼图像数据的重投影误差,以及惯性测量变量误差，第一偏置包括第一加速度偏置和第一角速度偏置。

在一具体实施例中，利用第一优化函数基于第一地图点、第一位姿、第一速度、第一偏置计算目标对象在最小误差下的第二位姿，是指将第一位姿、第一速度、第一偏置和第一地图点代入第一优化函数中迭代计算函数值，第一优化函数的函数值最小时的位姿也就是目标对象在最小误差下的第二位姿。

首先，本发明中获取鱼眼图像数据时，是通过鱼眼相机获取的，鱼眼相机大视场感知的特点可以在局部地图中获取更多的地图点，从而强化视觉特征对位姿的约束，达到提升定位准确度的目的，其次，本发明中图像特征提取步骤和利用第一优化函数计算的重投影误差步骤都是直接在原始鱼眼图像上进行，无需对鱼眼图像图像数据进行畸变校正，从而减小了需要处理的数据量，降低了计算成本。

在一可选实施例中，如图2所示，上述步骤s15具体包括：

步骤s151：分别计算每个第一地图点在当前帧的鱼眼图像数据上的重投影误差。

在一具体实施例中，通过误差项核函数、局部地图中第一地图点在图像上的二维投影点、图像坐标系中与第一地图点匹配的二维图像点，以及第一位置的迭代值、第一姿态的迭代值计算每个第一地图点在当前帧的鱼眼图像数据上的重投影误差。

步骤s152：计算当前帧的鱼眼图像数据对应的惯性测量数据与前一帧的鱼眼图像数据对应的惯性测量数据之间的惯性测量预积分误差。

在一具体实施例中，通过误差项核函数、第一位置的迭代值与第一位置的误差、第一姿态的迭代值与第一姿态的误差、第一速度的迭代值与第一速度的误差、第一偏置的迭代值与第一偏置的误差计算惯性测量预积分误差。

步骤s153：获取所有第一地图点的重投影误差和惯性测量预积分误差的误差和最小时对应的第二姿态、第二位置、第二速度、第二偏置，将第二姿态和第二位置作为第二位姿。

具体地，误差和最小时对应的第二姿态、第二位置、第二速度、第二偏置为所有第一地图点的重投影误差和惯性测量预积分误差的误差和最小时对应的第一姿态的迭代值、第一位置的迭代值、第一速度的迭代值、第一偏置的迭代值。

在一可选实施例中，第一优化函数为：

其中，x表示优化变量，evision(k,j)表示第k个第一地图点在当前帧j的鱼眼图像数据上的重投影误差，eimu(i,j)表示当前帧j的鱼眼图像数据对应的惯性测量数据与前一帧i的鱼眼图像数据对应的惯性测量数据之间的惯性测量预积分误差；ρ为误差项核函数，误差项核函数能够降低异常值对优化结果的影响，可选用但不限于huber、柯西核函数等，u^k为与第k个第一地图点对应的二维图像点，π(*)表示相机坐标系中三维点到图像坐标系中二维点的映射函数；rcb表示惯性测量单元坐标系到本体坐标系的旋转变换，rwb表示惯性测量单元测量到的姿态，rcw表示第二姿态，表示第一地图点，tcb表示惯性测量单元坐标系到本体坐标系的平移变换，pwb表示惯性测量单元测量到的位置，tcw表示第二位置，σk表示重投影误差的信息矩阵；er，ev，ep，eb分别表示第二姿态与第一姿态的姿态误差、第二速度与第一速度的速度误差、第二位置与第一位置的位置误差、第二偏置与第一偏置的偏置误差；σi，σb分别表示运动状态和惯性测量单元偏置测量的信息矩阵。

在上述公式(17)中，相机坐标系中三维点到图像坐标系中二维点的映射函数为：

fx,fy,cx,cy,α,β为eucm鱼眼相机模型内参数，可通过事先标定得到，其中，fx,fy分别指图像坐标系中x与y方向的焦距，cx,cy分别指图像坐标系中图像光心位置的x与y方向坐标值，α,β分别为鱼眼相机模型中两个必要的关联变量。

在一可选实施例中，如图3所示，在执行上述步骤s15后，还包括：

步骤s16：根据第二位姿确定第一地图点中与第二位姿相匹配的地图点的数量。

在一具体实施例中，在通过第一优化函数对第一位姿进行优化得到第二位姿后，根据第二位姿对现有的与视觉特征相匹配的第一地图点中的错误项进行剔除，导致最终与第二位姿相匹配的地图点的数量会有所下降。

步骤s17：判断第一地图点的数量是否小于预设阈值，或第一地图点中与第二位姿相匹配的地图点的数量是否小于预设阈值，若都不小于预设阈值，不执行任何操作，若其中一项小于预设阈值，执行如下步骤：

步骤s18：匹配局部地图中距离第一位姿第二预设范围内的与视觉特征相对应的第二地图点，利用第一优化函数基于第二地图点、第一位姿、第一速度、惯性测量单元的第一偏置重新计算目标对象在最小误差下的第二位姿，以对第一位姿重新优化。对第一位姿进行重新优化的详细描述见上述步骤s151-步骤s153,以及上述公式(16)-公式(18)。

在一具体实施例中，当第一地图点的数量小于预设阈值，或第一地图点中与第二位姿相匹配的地图点的数量小于预设阈值时，判定目标对象位姿跟踪失败，因此需要获取更多的与视觉特征相对应的地图点重新求解目标对象的位姿。

在一具体实施例中，预设阈值可根据鱼眼图像数据的分辨率确定，分辨率越高，可设置越大的预设阈值，分辨率越低，可设置越小的预设阈值，例如，当分辨率较高时，可以将预设阈值设置为30、40等，当分辨率较低时，可以将预设阈值设置为10、15等。

在一可选实施例中，如图4所示，在执行上述步骤s153后，本发明实施例提供的基于鱼眼视觉和惯性测量的目标定位方法还包括：

步骤s19：判断当前帧的鱼眼图像数据是否为一关键帧。

在一具体实施例中，判断当前帧的鱼眼图像数据是否为一关键帧，实际是判断当前帧的鱼眼图像数据是否满足生成新关键帧的生成条件。条件太松会增加系统的计算负担，条件太紧会降低定位的精度。因此，需要根据传感器配置和应用环境进行设定，在本发明实施例中，若当前帧中跟踪上的局部地图点小于95％，且优化的内点数在15个以上，且满足以下条件之一时，判定当前帧的鱼眼图像数据为一关键帧：

a.局部地图优化线程处于空闲状态；

b.距离上次生成关键帧时间超过3s；

c.距离上次生成关键帧移动超过5m。

以上成为关键帧的条件仅为举例说明，在实际应用中可根据实际需求进行调整。

若当前帧不为关键帧，不执行任何操作。

若当前帧为一关键帧，执行如下步骤：

步骤s20：根据当前帧的鱼眼图像数据生成第三地图点。在本发明实施例中，第三地图点是根据eucm鱼眼相机模型生成的，生成方法如下：

pc＝[mx,my,mz]^t(23)

其中，pw表示第三地图点；λ表示地图点的深度；pc表示新地图点在相机坐标系中的方向向量表示；rij与tij表示第i个关键帧与第j个关键帧的相对姿态与位置。(u,v)表示匹配的二维图像点坐标。该过程直接利用原始鱼眼图像中像素点生成第三地图点，不需要进行畸变校正，进而可以减小计算成本。

步骤s21：利用第二优化函数基于预设长度窗口内的其他关键帧对应的地图点、位置、姿态、速度、偏置，以及当前帧对应的第三地图点、第二姿态、第二位置、第二速度、第二偏置计算预设长度窗口内的其他关键帧中目标对象在最小误差下的优化位姿、当前帧中目标对象在最小误差下的第三位姿，以及当前帧中目标对象在最小误差下的第四地图点，以对其他关键帧的位姿、当前帧的第二位姿、当前帧对应的第三地图点进行优化，其中，误差包括其他关键帧对应的地图点在预设窗口内所有关键帧鱼眼图像数据的重投影误差和第三地图点在预设窗口内所有关键帧鱼眼图像数据的重投影误差。

步骤s22：将第四地图点加入局部地图中。

在本发明实施例中，步骤s21是指将预设长度窗口内的其他关键帧对应的位置、姿态、速度、偏置进行迭代后得到的值、当前帧对应的第三地图点、第二姿态、第二位置、第二速度、第二偏置进行迭代后得到的值，以及其他关键帧对应的地图点代入第二优化函数中计算函数值，函数值最小时的当前帧对应的地图点为第四地图点，函数值最小时与其他帧对应的位姿也就是其他帧的优化位姿，函数值最小时与当前帧对应的位姿也就是第三位姿。

在本发明实施例提供的基于鱼眼视觉和惯性测量的目标定位方法中，确定当前帧的鱼眼图像数据后，通过滑动窗口对当前帧对应的第三地图点和第二位姿进行优化，并对其他关键帧对应的位姿进行优化。相比传统对局部窗口内所有与关键帧的关联地图点进行优化的方法，本发明实施例提出的方法在保证其他关键帧对应的地图点在历史中进行过优化的前提下，只对当前帧对应的地图点进行优化，降低了每次优化的数据量，提高了对于滑动窗口中位姿与局部地图的优化效率。

在一可选实施例中，如图5所示，在本发明实施例中，上述步骤s21具体包括：

步骤s211:分别计算每个地图点在与各地图点存在匹配关系的关键帧的鱼眼图像数据上的重投影误差以及第四地图点在存在匹配关系的关键帧的鱼眼图像数据上的重投影误差，第四地图点为第三地图点优化得到的。

在一具体实施例中，通过误差项核函数、局部地图中所有其他三维地图点在图像上的二维投影点、第四地图点在图像上的二维投影点、图像坐标系中与各地图点匹配的二维图像点，以及滑动窗口内所有关键帧的位置的迭代值、姿态的迭代值计算每个地图点在其他各关键帧的鱼眼图像数据上的重投影误差，第四地图点为第三地图点的迭代值；

步骤s212:分别计算相邻两个关键帧对应的惯性测量数据之间的惯性测量预积分误差。

在一具体实施例中，通过误差项核函数、第二位置的迭代值与第二位置的误差、第二姿态的迭代值与第二姿态的误差、第二速度的迭代值与第二速度的误差、第二偏置的迭代值与第二偏置的误差计算惯性测量预积分误差。

步骤s213:获取所有重投影误差和惯性测量预积分误差的误差和最小时其他关键帧对应的优化位置和优化姿态，以及当前帧对应的第三姿态、第三位置、第三速度、第三偏置、第四地图点，将优化位置和优化姿态作为其他各关键帧对应的优化位姿，将第三姿态和第三位置作为第三位姿。

具体地，误差和最小时对应的第三姿态、第三位置、第三速度、第三偏置、第四地图点、优化位置、优化姿态为所有地图点的重投影误差和惯性测量预积分误差的误差和最小时对应的第二姿态的迭代值、第二位置的迭代值、第二速度的迭代值、第二偏置的迭代值、第三地图点的迭代值，以及其他各关键帧对应的位置和姿态的迭代值。

在一可选实施例中，第二优化函数为：

其中，x表示优化变量，l表示窗口长度，evision(k,j)表示第k个第四地图点在第j帧的鱼眼图像数据上的重投影误差，eimu(i,j)表示第i帧的鱼眼图像数据对应的惯性测量数据与第j帧的鱼眼图像数据对应的惯性测量数据之间的惯性误差项；ρ为误差项核函数，u^k为与第k个第四地图点对应的二维图像点，π(*)表示相机坐标系中三维点到图像坐标系中二维点的映射函数；rcb表示惯性测量单元坐标系到本体坐标系的旋转变换，rwb表示惯性测量单元测量到的姿态，rcw表示第三姿态，表示第四地图点数据，tcb表示惯性测量单元坐标系到本体坐标系的平移变换，pwb表示惯性测量单元测量到的位置，tcw表示第三位置，σk表示重投影误差的信息矩阵；er，ev，ep，eb分别表示第三姿态与第二姿态的姿态误差、第三速度与第二速度的速度误差、第三位置与第二位置的位置误差、第三偏置与第二偏置的偏置误差；σi，σb分别表示运动状态和惯性测量单元偏置测量的信息矩阵。

对公式(26)的描述参见上述对公式(17)的描述。

在一可选实施例中，如图6所示，在上述步骤s22之后，本发明实施例提供的基于鱼眼视觉和惯性测量的目标定位方法还包括：

步骤s23：判断其他关键帧中是否存在与当前帧的鱼眼图像数据相匹配的关键帧，若不存在，不执行任何操作，做存在，执行如下步骤：

步骤s24：利用第三优化函数基于第i个关键帧和第i+1个关键帧对应的第三姿态和第三位置计算在第i个关键帧中目标对象在最小误差下的第四姿态和第四位置，将第四姿态和第四位置作为第四位姿，以对第三位姿进行优化。

步骤s25：判断是否存在未确定第四位姿的关键帧，若不存在，不执行任何操作，若存在，将i的值加1，并返回上述步骤s24。

当其他关键帧中存在与当前关键帧的鱼眼图像数据相匹配的关键帧时，说明其他关键帧与当前帧形成了闭环，与各关键帧对应的位姿构成了位姿链，上述步骤s24-步骤s25利用闭环约束，将误差传递到所有位姿链的关键帧上，该过程能够消除闭环区域内定位的积累误差，提高关键帧位姿的估计精度。

在一可选实施例中，第三优化函数为：

其中，x表示优化变量，ekf(i,i+1)为相邻两个关键帧的位姿约束，δri(i+1)为关键帧i与关键帧i+1的第四姿态的相对旋转矩阵，即δtij为关键帧i与关键帧i+1的第四位置的相对位移，即σk为相邻两个关键帧相对位姿误差项的信息矩阵，第四姿态和第四位置是通过第三姿态和第三位置迭代得到的。

在一可选实施例中，如图7所示，在执行上述步骤s25之后，还包括：

步骤s26：利用第四优化函数基于各关键帧对应的第四地图点、第四位置、第四姿态、第三速度、第三偏置计算各关键帧中目标对象在最小误差下的第五位姿，以对第四位姿进行优化，其中，误差包括第四地图点在与各第四地图点对应的关键帧的鱼眼图像数据的重投影误差。

在一具体实施例中，利用第四优化函数基于各关键帧对应的第四地图点、第四位姿、第三速度、第三偏置计算目标对象在最小误差下的第五位姿，是指将第四位姿、第三速度、第三偏置进行迭代后得到的值代入第四优化函数中计算函数值，第四优化函数的值最小时的位姿也就是目标对象在最小误差下的第五位姿，第四优化函数的值最小时的地图点也就是目标对象在最小误差下的第五地图点。

步骤s27：将第四优化函数的函数值最小时对应的第五地图点数据的集合确定为优化地图。

步骤s26-步骤s27是指利用关键帧间约束以及关键帧与地图点间约束优化全局所有状态信息，包括每个关键帧的位置、姿态、速度、地图点。

在一可选实施例中，如图8所示，上述步骤s26具体包括：

步骤s261：分别计算每个第五地图点在与各第五地图点对应的关键帧的鱼眼图像数据上的重投影误差，第五地图点是第四地图点优化得到的。

在一具体实施例中，通过误差项核函数、图像坐标系中与第五地图点匹配的二维图像点、第五地图点在图像坐标系中投影的二维图像点，以及第四位置的迭代值、第四姿态的迭代值计算每个第五地图点在当前帧的鱼眼图像数据上的重投影误差，第五地图点是通过第四地图点迭代得到的。

步骤s262：计算各关键帧的鱼眼图像数据对应的惯性测量数据与前一个关键帧的鱼眼图像数据对应的惯性测量数据之间的惯性测量预积分误差。

在一具体实施例中，通过误差项核函数、第四位置的迭代值与第四位置的误差、第四姿态的迭代值与第四姿态的误差、第三速度的迭代值与第三速度的误差、第三偏置的迭代值与第三偏置的误差计算惯性测量预积分误差。

步骤s263：获取所有第五地图点的重投影误差和惯性测量预积分误差的误差和最小时对应的第五姿态、第五位置、第四速度、第四偏置、第五地图点，将第五姿态和第五位置作为第五位姿。

具体地，误差和最小时对应的第五姿态、第五位置、第四速度、第四偏置、第五地图点为所有第四地图点迭代后生成的第五地图点的重投影误差和惯性测量预积分误差的误差和最小时对应的第五姿态的迭代值、第五位置的迭代值、第四速度的迭代值、第四偏置的迭代值、第四地图点的迭代值。

在一可选实施例中，第四优化函数为：

其中，x表示优化变量，evision(k,j)表示第k个第五地图点数据在第j帧视觉图像数据上的重投影误差，eimu(i,j)表示第i帧的鱼眼图像数据对应的惯性测量数据与第j帧的鱼眼图像数据对应的惯性测量数据之间的惯性误差项；ρ为误差项核函数，u^k为与第k个第五地图点数据匹配的二维图像点，π(*)表示相机坐标系中三维点到图像坐标系中二维点的映射函数；rcb表示惯性测量单元坐标系到本体坐标系的旋转变换，rwb表示惯性测量单元测量到的姿态，rcw表示第五姿态，表示第五地图点数据，tcb表示惯性测量单元坐标系到本体坐标系的平移变换，pwb表示惯性测量单元测量到的位置，tcw表示第五位置，σk表示重投影误差的信息矩阵，er，ev，ep，eb分别表示第五姿态与第四姿态的姿态误差、第四速度与三速度的速度误差、第五位置与第四位置的位置误差、第四偏置与第三偏置的偏置误差；σi，σb分别表示运动状态和惯性测量单元偏置测量的信息矩阵。

在一可选实施例中，如图9所示，上述步骤s11具体包括：

步骤s111：获取目标对象的当前帧的鱼眼图像数据和当前帧的鱼眼图像数据的时间戳。在本发明实施例中，鱼眼相机实时获取鱼眼图像数据，当对目标对象的定位速度通常小于鱼眼相机获取鱼眼图像数据的速度时，会对部分鱼眼图像数据进行舍弃，保障了每次对目标对象的定位都是基于实时的鱼眼图像数据进行定位。

步骤s112：判断是否存在时间戳与当前帧的鱼眼数据相同的原始惯性测量数据，若存在，执行步骤s12，若不存在，执行如下步骤：

步骤s113：获取原始惯性测量数据中时间戳与当前帧的鱼眼图像数据的时间戳向前相邻的第一原始惯性测量数据和向后相邻的第二原始惯性测量数据。

步骤s114：获取第一原始惯性测量数据与当前帧的鱼眼图像数据的时间差。

步骤s115：根据第一原始惯性测量数据、第二原始惯性测量数据、第一原始惯性测量数据的时间戳、第二原始惯性测量数据的时间戳、第一原始惯性测量数据与当前帧的鱼眼图像数据的时间差获取惯性测量数据：

其中，ti表示第一原始惯性测量数据的时间戳，tj表示第二原始惯性测量数据的时间戳，vi表示第一原始惯性测量数据，vj表示第二原始惯性测量数据，δti表示第一原始惯性测量数据与当前帧的鱼眼图像数据的时间差。

由于鱼眼相机和惯性测量单元是实时获取数据的，但是对目标对象进行定位的速度通常低于鱼眼相机获取鱼眼图像数据的速度，因此会存在部分鱼眼图像数据舍弃的情况，另外如果视觉与惯性传感器没有做硬件同步触发功能，都会导致鱼眼图像数据与惯性测量数据的时间戳无法保持一致，本发明实施例提供的基于鱼眼视觉和惯性测量的目标定位方法中通过原始惯性测量数据中时间戳与当前帧的鱼眼图像数据的时间戳向前相邻的第一原始惯性测量数据和向后相邻的第二原始惯性测量数据获取与鱼眼图像数据时间戳相同的惯性测量数据，使得对当前时间戳下目标对象的定位更准确。

在一可选实施例中，本发明实施例提供的基于鱼眼视觉和惯性测量的目标定位方法实现过程分为三大线程：

上述步骤s11-步骤s18属于第一线程，第一线程用于实现对目标对象的实时定位，其中，第一线程包括测量数据处理、位姿跟踪和重定位，上述步骤s11、步骤s13属于测量数据处理，步骤s12、步骤s14步骤s15属于位姿跟踪，步骤s18属于重定位。在执行上述步骤s17后，若判定跟踪成功，可直接输出第二位姿，若跟踪失败，需执行步骤s18后输出第二位姿。

上述步骤s19-步骤s22属于第二线程，第二线程用于实现局部地图优化，局部地图优化是指对所有关键帧的位姿进行优化，以及对当前帧对应的地图点进行优化，第二线程还用于对惯性测量单元进行初始化。

上述步骤s23-步骤s27属于第三线程，第三线程用于实现回环检测及全局优化，回环检测是指通过当前关键帧与历史关键帧存在的地图点匹配关系判断是否来到曾经到过的场景，完成对位姿的回环检测。全局优化是指通过执行上述步骤s24-步骤s25将闭环区域内定位的积累误差传递到所有位姿链的关键帧上，执行上述步骤s26-步骤s27完成对各关键帧对应的位姿及地图点进行全局优化的过程。

实施例2

本发明实施例提供了一种基于鱼眼视觉和惯性测量的目标定位装置，如图10所示，包括：

数据获取模块11，用于获取目标对象当前帧的鱼眼图像数据和惯性测量数据，当前帧的鱼眼图像数据和惯性测量数据的时间戳相同，鱼眼图像数据为采用鱼眼相机拍摄到的图像数据，详细描述见上述实施例1中对步骤s11的描述。

第一姿态获取模块12，用于根据惯性测量数据和目标对象的前一帧的位姿获取目标对象在当前帧的第一位姿和第一速度，第一位姿包括第一位置和第一姿态，详细描述见上述实施例1中对步骤s12的描述。

视觉特征提取模块13，用于从当前帧的鱼眼图像数据提取视觉特征，详细描述见上述实施例1中对步骤s13的描述。

第一地图点获取模块14，用于获取局部地图中距离第一位姿第一预设范围内的与视觉特征相对应的第一地图点，详细描述见上述实施例1中对步骤s14的描述。

第一位姿优化模块15，用于利用第一优化函数基于第一地图点、第一位姿、第一速度、惯性测量单元的第一偏置计算目标对象在最小误差下的第二位姿，以对第一位姿进行优化，其中，误差包括第一地图点在当前帧的鱼眼图像数据的重投影误差，以及惯性测量变量误差，详细描述见上述实施例1中对步骤s15的描述。

本发明提供的基于鱼眼视觉和惯性测量的目标定位装置，在获取鱼眼图像数据时，是通过鱼眼相机获取的，鱼眼相机大视场感知的特点可以在局部地图中获取更多的地图点，从而强化视觉特征对位姿的约束，达到提升定位准确度的目的，并且，本发明中图像特征提取步骤和利用第一优化函数计算的重投影误差步骤都是直接在原始鱼眼图像上进行，无需对鱼眼图像图像数据进行畸变校正，从而减小了需要处理的数据量，降低了计算成本。

实施例3

本发明实施例提供一种计算机设备，如图11所示，该计算机设备主要包括一个或多个处理器31以及存储器32，图11中以一个处理器31为例。

该计算机设备还可以包括：输入装置33和输出装置34。

处理器31、存储器32、输入装置33和输出装置34可以通过总线或者其他方式连接，图11中以通过总线连接为例。

处理器31可以为中央处理器(centralprocessingunit，cpu)。处理器31还可以为其他通用处理器、图形处理器(graphicsprocessingunit，gpu)、数字信号处理器(digitalsignalprocessor，dsp)、专用集成电路(applicationspecificintegratedcircuit，asic)、现场可编程门阵列(field-programmablegatearray，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等芯片，或者上述各类芯片的组合。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。存储器32可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据基于鱼眼视觉和惯性测量的目标定位装置的使用所创建的数据等。此外，存储器32可以包括高速随机存取存储器，还可以包括非暂态存储器，例如至少一个磁盘存储器件、闪存器件、或其他非暂态固态存储器件。在一些实施例中，存储器32可选包括相对于处理器31远程设置的存储器，这些远程存储器可以通过网络连接至基于鱼眼视觉和惯性测量的目标定位装置。输入装置33可接收用户输入的计算请求(或其他数字或字符信息)，以及产生与基于鱼眼视觉和惯性测量的目标定位装置有关的键信号输入。输出装置34可包括显示屏等显示设备，用以输出计算结果。

实施例4

本发明实施例提供一种计算机可读存储介质，该计算机可读存储介质存储计算机指令，所述计算机存储介质存储有计算机可执行指令，该计算机可执行指令可执行上述任意方法实施例中的基于鱼眼视觉和惯性测量的目标定位方法。其中，所述存储介质可为磁碟、光盘、只读存储记忆体(read-onlymemory，rom)、随机存储记忆体(randomaccessmemory，ram)、快闪存储器(flashmemory)、硬盘(harddiskdrive，缩写：hdd)或固态硬盘(solid-statedrive，ssd)等；所述存储介质还可以包括上述种类的存储器的组合。

显然，上述实施例仅仅是为清楚地说明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。