一种基于鱼眼视觉和惯性测量的目标定位方法及装置制造方法及图纸

本发明专利技术提供了一种基于鱼眼视觉和惯性测量的目标定位方法及装置，其中，该方法包括：获取目标对象当前帧的鱼眼图像数据和惯性测量数据，鱼眼图像数据为采用鱼眼相机拍摄到的图像数据；根据惯性测量数据获取目标对象在当前帧的第一位姿和第一速度；从当前帧的鱼眼图像数据提取视觉特征；获取局部地图中距离第一位姿第一预设范围内的与视觉特征相对应的第一地图点；利用第一优化函数计算目标对象在最小误差下的第二位姿，其中，误差包括第一地图点在当前帧的鱼眼图像数据的重投影误差。本发明专利技术利用鱼眼相机大视场感知的优势，可以在局部地图中匹配到更多的地图点，提升了定位准确度，并且无需对鱼眼图像数据进行畸变校正，减小了定位时的计算量。

【技术实现步骤摘要】
一种基于鱼眼视觉和惯性测量的目标定位方法及装置
本专利技术涉及同步定位与建图领域，具体涉及一种基于鱼眼视觉和惯性测量的目标定位方法及装置。
技术介绍
随着人工智能技术的飞速发展，自动驾驶领域的研究成为了当前的研究热点，对车辆的精准定位是自动驾驶领域必不可少的研究方向。同时定位与地图创建(SimultaneousLocalizationandMapping,SLAM)是指移动机器人在未知环境中利用机身传感器对环境进行感知的同时构建高精度地图并定位自身位置的技术，是自动驾驶平台实现高精度定位的关键技术之一。目前SLAM定位系统主要分为纯激光雷达方案、纯视觉方案、多种传感器融合方案，其中激光雷达存在成本较高，环境表达弱等问题，纯视觉方案存在图像处理量巨大、受光线条件限制较大等问题，因此如何设计可以提供高精度、数据处理量小的多种传感器融合方案为亟需解决的问题。中国专利CN109520497A公开了一种基于视觉和imu的无人机自主定位方法，其具体公开了：对imu获取到的加速度信息和角速度信息的预积分处理，得到位置信息、速度信息和旋转信息；基于视觉信息求解位姿，将其与imu预积分得到的信息进行配准，更新预积分的值；在滑动窗口中对视觉-惯性位姿估计数据进行联合优化，得到精度较高的位姿。专利技术人发现，由于该方案中采用的是单目视觉数据，而普通单目的观测范围窄，观测的环境信息量少，导致定位精度不高且稳定性差。另外该方案还需要先对该单目视觉数据做畸变校正处理，算法需要处理的数据量大，进而增加了整个系统的计算成本。
技术实现思路
因此，本专利技术要解决的技术问题在于克服现有单目视觉定位方案中定位精度不高，稳定性差的不足，以及现有技术中数据处理量大导致系统计算成本高的缺陷，从而提供一种基于鱼眼视觉和惯性测量的目标定位方法及装置。本专利技术第一方面提供了一种基于鱼眼视觉和惯性测量的目标定位方法，包括：获取目标对象当前帧的鱼眼图像数据和惯性测量数据，当前帧的鱼眼图像数据和惯性测量数据的时间戳相同，鱼眼图像数据为采用鱼眼相机拍摄到的图像数据；根据惯性测量数据和目标对象的前一帧的位姿获取目标对象在当前帧的第一位姿和第一速度，第一位姿包括第一位置和第一姿态；从当前帧的鱼眼图像数据提取视觉特征；获取局部地图中距离第一位姿第一预设范围内的与视觉特征相对应的第一地图点；利用第一优化函数基于第一地图点、第一位姿、第一速度、惯性测量单元的第一偏置计算目标对象在最小误差下的第二位姿，以对第一位姿进行优化，其中，误差包括第一地图点在当前帧的鱼眼图像数据的重投影误差，以及惯性测量变量误差。可选地，在本专利技术提供的基于鱼眼视觉和惯性测量的目标定位方法中，利用第一优化函数基于第一地图点、第一位姿、第一速度、惯性测量单元的第一偏置计算目标对象在最小误差下的第二位姿之后，还包括：根据第二位姿确定第一地图点中与第二位姿相匹配的地图点的数量；若第一地图点的数量小于预设阈值，或第一地图点中与第二位姿相匹配的地图点的数量小于预设阈值，获取局部地图中距离第一位姿第二预设范围内的与视觉特征相对应的第二地图点；利用第一优化函数基于第二地图点、第一位姿、第一速度、惯性测量单元的第一偏置重新计算目标对象在最小误差下的第二第二位姿，以对第一位姿进行优化。可选地，在本专利技术提供的基于鱼眼视觉和惯性测量的目标定位方法中，利用第一优化函数基于第一地图点、第一位姿、第一速度、惯性测量单元的第一偏置计算目标对象在最小误差下的第二位姿的步骤，包括：分别计算每个第一地图点在当前帧的鱼眼图像数据上的重投影误差；计算当前帧的鱼眼图像数据对应的惯性测量数据与前一帧的鱼眼图像数据对应的惯性测量数据之间的惯性测量预积分误差；获取所有第一地图点的重投影误差和惯性测量预积分误差的误差和最小时对应的第二姿态、第二位置、第二速度、第二偏置，将第二姿态和第二位置作为第二位姿。可选地，在本专利技术提供的基于鱼眼视觉和惯性测量的目标定位方法中，第一优化函数为：其中，x表示优化变量，Evision(k,j)表示第k个第一地图点在当前帧j的鱼眼图像数据上的重投影误差，EIMU(i,j)表示当前帧j的鱼眼图像数据对应的惯性测量数据与前一帧i的鱼眼图像数据对应的惯性测量数据之间的惯性测量预积分误差；ρ为误差项核函数，uk为与第k个第一地图点对应的二维图像点，π(*)表示相机坐标系中三维点到图像坐标系中二维点的映射函数；Rcb表示惯性测量单元坐标系到本体坐标系的旋转变换，Rwb表示惯性测量单元测量到的姿态，Rcw表示第二姿态，表示第一地图点，tcb表示惯性测量单元坐标系到本体坐标系的平移变换，Pwb表示惯性测量单元测量到的位置，tcw表示第二位置，Σk表示重投影误差的信息矩阵；eR，eV，eP，eb分别表示第二姿态与第一姿态的姿态误差、第二速度与第一速度的速度误差、第二位置与第一位置的位置误差、第二偏置与第一偏置的偏置误差；ΣI，Σb分别表示运动状态和惯性测量单元偏置测量的信息矩阵。可选地，在本专利技术提供的基于鱼眼视觉和惯性测量的目标定位方法中，若当前帧为一关键帧，基于鱼眼视觉和惯性测量的目标定位方法还包括：根据当前帧的鱼眼图像数据生成第三地图点；利用第二优化函数基于预设长度窗口内的其他关键帧对应的地图点、位置、姿态、速度、偏置，以及当前帧对应的第三地图点、第二姿态、第二位置、第二速度、第二偏置计算预设长度窗口内的其他关键帧中目标对象在最小误差下的优化位姿以及当前帧中目标对象在最小误差下的第三位姿，以对其他关键帧的位姿和当前帧的第二位姿进行优化，其中，误差包括其他关键帧对应的地图点在预设长度窗口内所有关键帧的鱼眼图像数据的重投影误差和第三地图点在预设长度窗口内所有关键帧的鱼眼图像数据的重投影误差。可选地，在本专利技术提供的基于鱼眼视觉和惯性测量的目标定位方法中，利用第二优化函数基于预设长度窗口内的其他关键帧对应的地图点、位置、姿态、速度、偏置，以及当前帧对应的第三地图点、第二姿态、第二位置、第二速度、第二偏置计算预设长度窗口内的其他关键帧中目标对象在最小误差下的优化位姿以及当前帧中目标对象在最小误差下的第三位姿的步骤，包括：分别计算每个地图点在与各地图点存在匹配关系的关键帧的鱼眼图像数据上的重投影误差以及第四地图点在存在匹配关系的关键帧的鱼眼图像数据上的重投影误差，第四地图点为第三地图点优化得到的；分别计算相邻两个关键帧对应的惯性测量数据之间的惯性测量预积分误差；获取所有重投影误差和惯性测量预积分误差的误差和最小时其他关键帧对应的优化位置和优化姿态，以及当前帧对应的第三姿态、第三位置、第三速度、第三偏置、第四地图点，将优化位置和优化姿态作为其他各关键帧对应的优化位姿，将第三姿态和第三位置作为第三位姿；将第四地图点加入局部地图中。可选地，在本专利技术提供的基于鱼眼视觉和惯性测量的目标定位方法中，第二优化函数为：其中，x表示优化变量，l表示窗口长度，Evision(k,j)表示第k个第四地图点在第j帧的鱼眼图像数据上的重投影误差，EIMU(i,j)本文档来自技高网...

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

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


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


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


4.根据权利要求3所述的基于鱼眼视觉和惯性测量的目标定位方法，其特征在于，所述第一优化函数为：









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


5.根据权利要求3所述的基于鱼眼视觉和惯性测量的目标定位方法，其特征在于，若所述当前帧为一关键帧，所述基于鱼眼视觉和惯性测量的目标定位方法还包括：
根据所述当前帧的鱼眼图像数据生成第三地图点；
利用第二优化函数基于预设长度窗口内的其他关键帧对应的地图点、位置、姿态、速度、偏置，以及当前帧对应的第三地图点、第二姿态、第二位置、第二速度、第二偏置计算预设长度窗口内的其他关键帧中目标对象在最小误差下的优化位姿、当前帧中目标对象在最小误差下的第三位姿，以及当前帧中目标对象在最小误差下的第四地图点，以对其他关键帧的位姿、当前帧的第二位姿、当前帧对应的第三地图点进行优化，其中，所述误差包括其他关键帧对应的地图点在预设长度窗口内所有关键帧的鱼眼图像数据的重投影误差和所述第三地图点在预设长度窗口内所有关键帧的鱼眼图像数据的重投影误差；
将所述第四地图点加入所述局部地图中。


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


7.根据权利要求6所述的基于鱼眼视觉和惯性测量的目标定位方法，其特征在于，所述第二优化函数为：









其中，x表示优化变量，l表示窗口长度，Evision(k,j)表示第k个第四地图点在第j帧的鱼眼图像数据上的重投影误差，EIMU(i,j)表示第i帧的鱼眼图像数据对应的惯性测量数据与第j帧的鱼眼图像数据对应的惯性测量数据之间的惯性误差项；ρ为误差项核函数，uk为与第k个第四地图点对应的二维图像点，π(*)表示相机坐标系中三维点到图像坐标系中二维点的映射函数；Rcb表示惯性测量单元坐标系到本体坐标系的旋转变换，Rwb表示惯性测量单元测量到的姿态，Rcw表示第三姿态，表示第四地图点数据，tcb表示惯性测量单元坐标系到本体坐标系的平移变换，Pwb表示惯性测量单元测量到的位置，tcw表示第三位置...

【专利技术属性】
技术研发人员：王冬生，李军，褚文博，林昱，杨文，
申请(专利权)人：国汽北京智能网联汽车研究院有限公司，
类型：发明
国别省市：北京;11