CN112113564B

CN112113564B - 一种基于图像传感器和惯性传感器的定位方法及系统

Info

Publication number: CN112113564B
Application number: CN202010994114.8A
Authority: CN
Inventors: 宋韬; 朱猛猛; 郭帅
Original assignee: University of Shanghai for Science and Technology
Current assignee: Shanghai Jinshangya Technology Co ltd
Priority date: 2020-09-21
Filing date: 2020-09-21
Publication date: 2022-08-02
Anticipated expiration: 2040-09-21
Also published as: CN112113564A

Abstract

本发明公开了一种基于图像传感器和惯性传感器的定位方法及系统，所述定位方法，首先获取定位目标上部署的特征图案基于图像传感器的位姿信息和基于惯性传感器的位姿信息；然后，利用图优化方法对基于图像传感器的位姿信息和基于惯性传感器的位姿信息进行优化，获得定位目标的方位信息。本发明利用基于图像传感器的位姿信息对基于惯性传感器的位姿信息进行优化，克服了惯性传感器随着时间的增长会有飘移，影响定位精度的技术缺陷，同时利用了惯性传感器不受外部环境影响的优点，提供了一种不依赖于外部条件的且不存在累计误差的高精度的定位方法。

Description

一种基于图像传感器和惯性传感器的定位方法及系统

技术领域

本发明涉及定位技术领域，特别涉及一种基于图像传感器和惯性传感器的定位方法及系统。

背景技术

定位技术具有广泛的应用前景，现有的定位技术包括GPS、北斗、GLONASS、惯导、超声波、里程计等。这些现有的定位技术都有一定的局限性，满足不了复杂的定位环境和日益增长的定位精度需求。GPS、北斗等全球定位系统无法应用于室内定位，在小环境中它的定位精度也达不到要求；基于惯性传感器的定位方法主要优点是不依赖外部条件就可实现移动机器人定位，短时精度比较好。缺点就是随着时间的增长会有飘移，比较小的常数误差被积分后都会无限变大；里程计航迹推算定位方法也是存在累积误差，不适合机器人长距离内的定位。

发明内容

本发明的目的是提供一种基于图像传感器和惯性传感器的定位方法及系统，以提供一种不依赖于外部条件的且不存在累计误差的高精度的定位方法。

为实现上述目的，本发明提供了如下方案：

一种基于图像传感器和惯性传感器的定位方法，所述定位方法包括如下步骤：

获取定位目标上部署的特征图案基于图像传感器的位姿信息；

获取定位目标上部署的特征图案基于惯性传感器的位姿信息；

利用图优化方法对基于图像传感器的位姿信息和基于惯性传感器的位姿信息进行优化，获得定位目标的方位信息。

可选的，所述获取定位目标上部署的特征图案基于图像传感器的位姿信息，具体包括：

采用张正友标定法对图像传感器进行标定，获得图像传感器坐标系相对于自然坐标系的旋转矩阵R和平移矩阵t；

将特征图案布置在定位目标的中心位置，通过图像传感器获取定位目标所在区域的区域图像；

采用Harris算法对所述区域图像进行目标检测，获得基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息l_k；

根据基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息，利用公式z_k＝Rl_k+t，计算定位目标上部署的特征图案基于图像传感器在自然坐标系下的位姿信息z_k；

其中，t表示相机坐标系到自然坐标系的平移矩阵，l_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息，R表示相机坐标系到自然坐标系的转换矩阵，z_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息。

可选的，所述获取定位目标基于惯性传感器的位姿信息，具体包括：

对惯性传感器获得的加速度信息和角加速度信息进行二次积分，获得定位目标上部署的特征图案基于惯性传感器的位姿信息。

可选的，所述利用图优化方法对基于图像传感器所获得在自然坐标系下的位姿信息和基于惯性传感器所获得在自然坐标系下的位姿信息进行优化，获得定位目标的方位信息，具体包括：

利用公式e_k＝z_k-x_k，计算基于图像传感器所获得在自然坐标系下的位姿信息和基于惯性传感器所获得在自然坐标系下的位姿信息的差值；

其中，x_k表示k时刻基于惯性传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息，z_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息，e_k表示k时刻基于图像传感器所获得位姿信息和基于惯性传感器所获得的位姿信息的差值；

判断所述差值是否小于预设阈值，得到判断结果；

若所述判断结果表示否，则优化基于惯性传感器所获得在自然坐标系下的位姿信息，返回步骤“利用公式e_k＝z_k-x_k，计算基于图像传感器所获得在自然坐标系下的位姿信息和基于惯性传感器所获得在自然坐标系下的位姿信息的差值”；

若所述判断结果表示是，则输出优化后的基于惯性传感器所获得在自然坐标系下的位姿信息作为定位目标的方位信息。

一种基于图像传感器和惯性传感器的定位系统，所述定位系统包括：

第一位姿信息获取模块，用于获取定位目标上部署的特征图案基于图像传感器所获得在自然坐标系下的位姿信息；

第二位姿信息获取模块，用于获取定位目标基于惯性传感器所获得在自然坐标系下的位姿信息；

位姿信息优化模块，用于利用图优化方法对基于图像传感器所获得在自然坐标系下的位姿信息和基于惯性传感器所获得在自然坐标系下的位姿信息进行优化，获得定位目标的方位信息。

可选的，所述第一位姿信息获取模块，具体包括：

标定子模块，用于采用张正友标定法对图像传感器进行标定，获得图像传感器坐标系相对于自然坐标系的旋转矩阵R和平移矩阵t；

区域图像获取子模块，用于将特征图案布置在定位目标的中心位置，通过图像传感器获取定位目标所在区域的区域图像；

定位目标图案识别子模块，用于采用Harris算法对所述区域图像进行目标检测，获得基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息l_k；

坐标转换子模块，用于根据基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息，利用公式z_k＝Rl_k+t，计算定位目标上部署的特征图案基于图像传感器在自然坐标系下的位姿信息z_k；

可选的，所述第二位姿信息获取模块，具体包括：

二次积分子模块，用于对惯性传感器获得的加速度信息和角加速度信息进行二次积分，获得定位目标基于惯性传感器所获得在自然坐标系下的位姿信息。

可选的，所述位姿信息优化模块，具体包括：

差值计算子模块，用于利用公式e_k＝z_k-x_k，计算基于图像传感器所获得在自然坐标系下的位姿信息和基于惯性传感器所获得在自然坐标系下的位姿信息的差值；

判断子模块，用于判断所述差值是否小于预设阈值，得到判断结果；

位姿信息优化子模块，用于若所述判断结果表示否，则优化基于惯性传感器所获得在自然坐标系下的位姿信息，返回步骤“利用公式e_k＝z_k-x_k，计算基于图像传感器所获得在自然坐标系下的位姿信息和基于惯性传感器所获得在自然坐标系下的位姿信息的差值”；

优化后的位姿信息输出子模块，用于若所述判断结果表示是，则输出优化后的基于惯性传感器所获得在自然坐标系下的位姿信息作为定位目标的方位信息。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

附图说明

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

图1为本发明提供的一种基于图像传感器和惯性传感器的定位方法的流程图；

图2为本发明提供的一种基于图像传感器和惯性传感器的定位方法的原理图；

图3为本发明提供的图优化方法的原理图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对发明作进一步详细的说明。

为实现上述目的，本发明提供了如下方案：

如图1和2所示，本发明提供一种基于图像传感器和惯性传感器的定位方法，所述定位方法包括如下步骤：

步骤101，获取定位目标上部署的特征图案基于图像传感器的位姿信息。

步骤101所述获取定位目标上部署的特征图案基于图像传感器的位姿信息，具体包括：采用张正友标定法对图像传感器进行标定，获得图像传感器坐标系相对于自然坐标系的旋转矩阵R和平移矩阵t；将特征图案布置在定位目标的中心位置，通过图像传感器获取定位目标所在区域的区域图像；采用Harris算法对所述区域图像进行目标检测，获得基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息l_k；根据基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息l_k，利用公式z_k＝Rl_k+t，计算定位目标上部署的特征图案基于图像传感器在自然坐标系下的位姿信息z_k；其中，t表示相机坐标系到自然坐标系的平移矩阵，l_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息，R表示相机坐标系到自然坐标系的转换矩阵，z_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息。

具体的，获取定位目标上部署的特征图案基于图像传感器所获得在自然坐标系下的位姿信息，包括如下步骤：

1)使用固定尺寸的标定板对图像传感器进行静态标定。标定板竖直放置，使用图像传感器在至少十五个不同位姿下静止拍下至少十五张照片，检测出图像中的特征点，计算出图像传感器的内外参数和畸变系数，即张正友标定法。通过标定获取图像传感器坐标系相对于自然坐标系的旋转矩阵R和平移矩阵t。

2)通过相机获取区域内的图像，所述区域内图像通过相机在固定位姿拍摄形成；所述区域图像内包含至少1个特征图案(定位目标图案)；

3)对获得图像进行畸变处理。图像像素坐标系(uOv坐标系)下的无畸变坐标(U，V)，经过径向畸变和切向畸变后落在了uOv坐标系的(Ud，Vd)上。即就是说，真实图像imgR与畸变图像imgD之间的关系为:imgR(U，V)＝imgD(Ud，Vd)。

4)解析区域图像，提取特征图案；使用Harris算法解析区域图像中的角点特征，根据目标图案的角点特征从区域图像中提取出相应的特征图案。

5)根据确定的特征图案，计算图案的位姿信息z_k。估计相机相对于目标图案的相对方位信息的过程包括：基于当前的拍摄规则，推断出相机拍摄时相对于目标图案的相对方位矢量；利用获得的相机拍摄时相对于目标图案的相对方位矢量，以及所获得的相机固定的位姿信息，来确定图案在待定位区域内的当前方位信息。所述定位目标上的特征图案的相关信息对应的储存在数据库中，所述相关信息至少包括所部署图案的基于图像传感器在自然坐标系下的位姿信息；

步骤102，获取定位目标基于惯性传感器所获得在自然坐标系下的位姿信息。

步骤102所述获取定位目标基于惯性传感器的位姿信息，具体包括：对惯性传感器获得的加速度信息和角加速度信息进行二次积分，获得定位目标基于惯性传感器所获得在自然坐标系下的位姿信息。

具体的，获取定位目标当前的基于惯性传感器所获得在自然坐标系下的位姿信息x_k的过程包括：

1)对惯性传感器进行标定，包括：计算出零偏，比例因子，交轴耦合等误差。

2)获取惯性传感器的加速度和角加速度信息。

3)计算特征图案当前的基于惯性传感器所获得在自然坐标系下的位姿信息x_k。对获取的惯性传感器信息进行二次积分分别得到位移和角度信息，根据位移和角度计算出定位目标基于惯性传感器所获得在自然坐标系下的位姿信息x_k。

步骤103，利用图优化方法对基于图像传感器的位姿信息和基于惯性传感器的位姿信息进行优化，获得定位目标的方位信息。

所述图优化，就是把一个常规优化问题，以图的形式来表述。图是由顶点和边构成的。

本发明利用图像传感器和惯性传感器融合计算移动机器人位姿就是一个迭代优化误差的过程，每一次优化完成后需根据z_k和x_k对惯性传感器参考点进行设置和更新直至误差小于给定阈值。

具体的，步骤103所述利用图优化方法对基于图像传感器的位姿信息和基于惯性传感器的位姿信息进行优化，获得定位目标的方位信息，具体包括：利用公式e_k＝z_k-x_k，计算基于图像传感器的位姿信息和基于惯性传感器的位姿信息的差值；其中，x_k表示k时刻基于惯性传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息，z_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息，e_k表示k时刻基于图像传感器所获得位姿信息和基于惯性传感器所获得的位姿信息的差值；判断所述差值是否小于预设阈值，得到判断结果；若所述判断结果表示否，则优化基于惯性传感器所获得在自然坐标系下的位姿信息，返回步骤“利用公式e_k＝z_k-x_k，计算基于图像传感器所获得在自然坐标系下的位姿信息和基于惯性传感器所获得在自然坐标系下的位姿信息的差值”；若所述判断结果表示是，则输出优化后的基于惯性传感器所获得在自然坐标系下的位姿信息作为定位目标的方位信息。

本发明还提供一种基于图像传感器和惯性传感器的定位系统，所述定位系统包括：

第一位姿信息获取模块，用于获取定位目标上部署的特征图案基于图像传感器所获得在自然坐标系下的位姿信息.

所述第一位姿信息获取模块，具体包括：标定子模块，用于采用张正友标定法对图像传感器进行标定，获得图像传感器坐标系相对于自然坐标系的旋转矩阵R和平移矩阵t；区域图像获取子模块，用于将特征图案布置在定位目标的中心位置，通过图像传感器获取定位目标所在区域的区域图像；定位目标图案识别子模块，用于采用Harris算法对所述区域图像进行目标检测，获得基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息l_k；坐标转换子模块，用于根据基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息，利用公式z_k＝Rl_k+t，计算定位目标上部署的特征图案基于图像传感器在自然坐标系下的位姿信息z_k；其中，t表示相机坐标系到自然坐标系的平移矩阵，l_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息，R表示相机坐标系到自然坐标系的转换矩阵，z_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息。

第二位姿信息获取模块，用于获取定位目标基于惯性传感器所获得在自然坐标系下的位姿信息.

所述第二位姿信息获取模块，具体包括：二次积分子模块，用于对惯性传感器获得的加速度信息和角加速度信息进行二次积分，获得定位目标基于惯性传感器所获得在自然坐标系下的位姿信息。

所述位姿信息优化模块，具体包括：差值计算子模块，用于利用公式e_k＝z_k-x_k，计算基于图像传感器所获得在自然坐标系下的位姿信息和基于惯性传感器所获得在自然坐标系下的位姿信息的差值；其中，x_k表示k时刻基于惯性传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息，z_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息，e_k表示k时刻基于图像传感器所获得位姿信息和基于惯性传感器所获得的位姿信息的差值；判断子模块，用于判断所述差值是否小于预设阈值，得到判断结果；位姿信息优化子模块，用于若所述判断结果表示否，则优化基于惯性传感器所获得在自然坐标系下的位姿信息，返回步骤“利用公式e_k＝z_k-x_k，计算基于图像传感器所获得在自然坐标系下的位姿信息和基于惯性传感器所获得在自然坐标系下的位姿信息的差值”；优化后的位姿信息输出子模块，用于若所述判断结果表示是，则输出优化后的基于惯性传感器所获得在自然坐标系下的位姿信息作为定位目标的方位信息。

本发明提供的定位方案能够提高特定应用场景中的定位精度，弥补现有技术中的不足。

本发明在使用时，只需利用相机和惯性传感器，不需要额外的硬件设备；同时用图优化的方法融合优化两个传感上获得的位姿信息，达到增强定位精度和稳定性的目的。

为了说明本发明的方法和系统效果，本发明还提供一个具体的实施方式。

本发明公开的一种基于图像传感器和惯性传感器的定位方法，包括如下步骤：

所述图像传感器为单目相机，首先要对单目相机进行标定，消除单目相机的系统误差并建立相机坐标系与惯性传感器坐标系之间的位姿关系；

进一步，用单目相机的拍摄功能获取环境图像；

进一步，提取特征图案；

进一步，计算特征图案基于图像传感器的位姿；

所述惯性传感器为三轴加速度计和三轴陀螺仪的组合；首先要对惯性传感器进行标定，消除惯性传感器的系统误差；

进一步，获取惯性传感器的数据，本步骤中数据的获取可以通过蓝牙传输；

进一步，计算特征图案基于惯性传感器的位姿。

本实施例中传感器信息包括：

第一步、对采集的传感器信息进行预处理

采集图像传感器输出的图像信息，对采集的图像信息进行去畸变处理；采集惯性传感器数据，并对加速度计的数据和陀螺仪的数据进行积分处理，获取惯性传感器的惯性增量；

第二步、对图像传感器和惯性传感器获得的特征图案位姿信息通过图优化进行融合

所述图优化方法流程如图3所示：

举例来说，假设移动机器人在运动过程中，相机和惯性传感器记录了n组观测数据，这些数据可以构建出n条边的图，其目标函数可以写成：

e_k表示在原理上表示传感器相关联观测数据的误差，即边，是一个矢量，作为优化变量x_k和z_k符合程度的一个度量；信息矩阵Ω是协方差矩阵的逆，是一个对称矩阵；x_k表示由惯性传感器计算出的位姿构成的顶点；z_k表示由相机计算出的位姿构成的顶点。

在一个相机位姿T_k处对空间点x_k进行了一次观测，得到z_k，那么这条二元边的数学形式即为:

e_k(x_k，T_k，z_k)＝(z_k-C(R_cx_k+t_c))^TΩ_k(z_k-C(R_cx_k-t_c))

其中，C为相机内参，R_c，t_c为旋转和平移矩阵。

对于一个有很多个节点和边的图，构成了一个庞大的优化问题。为了求解优化，需要知道一个初始点和迭代方向。为了数学上的方便，从第k条边e_k(x_k)开始考虑。

假设初始点为

增量为Δx,则边的估计值就变为

误差值从

变为

对误差项进行一阶展开：

J_k是e_k关于x_k的导数，矩阵形式下为雅可比阵。

于是，对于第k条边的目标函数项，有：

把和Δx无关的整理成常数项

把一次项系数写成2b_k，二次项则为H_k(注意到二次项系数其实是Hessian矩阵)。

在x_k发生增量后，目标函数F_k项改变的值即为:

ΔF_k＝2b_kΔx+Δx^TH_kΔx

令ΔF_k对Δx的导数为零，

最终位姿优化问题，变成求解线性方程组：

H_kΔx＝-b_k

本发明使用图像传感器和惯性传感器采集移动机器人的位姿信息，根据图优化方法构建误差函数对所述位姿信息进行校正，获得校正后的位姿信息。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。

本文中应用了具体个例对发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

Claims

1.一种基于图像传感器和惯性传感器的定位方法，其特征在于，所述定位方法包括如下步骤：

所述获取定位目标上部署的特征图案基于图像传感器的位姿信息，具体包括：

其中，t表示相机坐标系到自然坐标系的平移矩阵，l_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在相机坐标系下的位姿信息，R表示相机坐标系到自然坐标系的转换矩阵，z_k表示k时刻基于图像传感器获取的定位目标上部署的特征图案在自然坐标系下的位姿信息；

所述获取定位目标上部署的特征图案基于惯性传感器的位姿信息，具体包括：

对惯性传感器获得的加速度信息和角加速度信息进行二次积分，获得定位目标上部署的特征图案基于惯性传感器的位姿信息；

利用图优化方法对基于图像传感器的位姿信息和基于惯性传感器的位姿信息进行优化，获得定位目标的方位信息；

所述利用图优化方法对基于图像传感器的位姿信息和基于惯性传感器的位姿信息进行优化，获得定位目标的方位信息，具体包括：

利用公式e_k＝z_k-x_k，计算基于图像传感器的位姿信息和基于惯性传感器的位姿信息的差值；

判断所述差值是否小于预设阈值，得到判断结果；

若所述判断结果表示否，则优化基于惯性传感器的位姿信息，返回步骤“利用公式e_k＝z_k-x_k，计算基于图像传感器的位姿信息和基于惯性传感器的位姿信息的差值”；

若所述判断结果表示是，则输出优化后的基于惯性传感器的位姿信息作为定位目标的方位信息。

2.一种基于图像传感器和惯性传感器的定位系统，其特征在于，所述定位系统包括：

第一位姿信息获取模块，用于获取定位目标上部署的特征图案基于图像传感器的位姿信息；

所述第一位姿信息获取模块，具体包括：

第二位姿信息获取模块，用于获取定位目标上部署的特征图案基于惯性传感器的位姿信息；

所述第二位姿信息获取模块，具体包括：

二次积分子模块，用于对惯性传感器获得的加速度信息和角加速度信息进行二次积分，获得定位目标上部署的特征图案基于惯性传感器的位姿信息；

位姿信息优化模块，用于利用图优化方法对基于图像传感器的位姿信息和基于惯性传感器的位姿信息进行优化，获得定位目标的方位信息；

所述位姿信息优化模块，具体包括：

差值计算子模块，用于利用公式e_k＝z_k-x_k，计算基于图像传感器的位姿信息和基于惯性传感器的位姿信息的差值；

位姿信息优化子模块，用于若所述判断结果表示否，则优化基于惯性传感器的位姿信息，返回步骤“利用公式e_k＝z_k-x_k，计算基于图像传感器的位姿信息和基于惯性传感器的位姿信息的差值”；

优化后的位姿信息输出子模块，用于若所述判断结果表示是，则输出优化后的基于惯性传感器的位姿信息作为定位目标的方位信息。