一种基于视觉和惯性测量单元的姿态估计方法及装置

技术领域

本发明涉及视觉技术领域，特别涉及一种基于视觉和惯性测量单元的姿态估计方法及装置。

背景技术

Perspective-n-Point(PnP)，也称为姿态估计，其构思是在知道n个三维空间点坐标及其二维投影位置时，如何估计位姿。研究表明，当n<3时，PnP问题有无穷多解，因而PnP问题的研究主要针对3个以上特征点的情况。

目前，PnP问题求解利用的信息只有特征点的二维像素坐标和三维坐标，实践中发现，这种求解算法复杂，精度低，且鲁棒性较差。

发明内容

本发明的目的在于提供一种基于视觉和惯性测量单元的姿态估计方法及装置，以解决上述技术问题。

为达此目的，本发明采用以下技术方案：

提供一种基于视觉和惯性测量单元的姿态估计方法，其改进之处在于，包括：

获取相机的图像数据，以及IMU的测量数据，其中，IMU的测量数据包括设置在相机上的IMU的测量数据和设置在目标物体上的IMU的测量数据；

通过图像数据和IMU的测量数据融合估计目标物体在相机坐标系中的姿态。

进一步地，所述通过图像数据和IMU的测量数据融合估计目标物体在相机坐标系中的姿态，包括：

通过计算物体坐标系到相机坐标系的变换矩阵计算获取目标物体在相机坐标系中的姿态。

进一步地，所述变换矩阵定义为：

上式中，R表示旋转矩阵；

t表示平移向量。

进一步地，所述通过图像数据和IMU的测量数据融合估计目标物体在相机坐标系中的姿态，还包括：

对图像数据和IMU的测量数据进行标定，以提高图像数据和IMU的测量数据的融合精度。

进一步地，所述标定方法，包括：

通过预先设定的阈值判断图像数据是否有效；

当为有效图像数据时，通过迭代无迹卡尔曼滤波对图像数据和IMU的测量数据进行融合。

进一步地，还包括：

获取角度数据，其中，角度数据包括设置在相机上的倾角传感器的测量数据和设置在目标物体上的倾角传感器的测量数据；

通过角度数据提供额外的约束，以提高姿态估计的准确性。

为达此目的，本发明还采用以下技术方案：

一种基于视觉和惯性测量单元的姿态估计装置，其改进之处在于，包括：

获取模块，用于获取相机的图像数据，以及IMU的测量数据，其中，IMU的测量数据包括设置在相机上的IMU的测量数据和设置在目标物体上的IMU的测量数据；

估计模块，用于通过图像数据和IMU的测量数据融合估计目标物体在相机坐标系中的姿态。

进一步地，所述估计模块，还用于：

通过计算物体坐标系到相机坐标系的变换矩阵计算获取目标物体在相机坐标系中的姿态。

进一步地，所述估计模块，还用于：对图像数据和IMU的测量数据进行标定，以提高图像数据和IMU的测量数据的融合精度。

进一步地，其特征在于，还包括：

校准模块，用于获取角度数据，其中，角度数据包括设置在相机上的倾角传感器的测量数据和设置在目标物体上的倾角传感器的测量数据；

通过角度数据提供额外的约束，以提高姿态估计的准确性。

本发明通过将相机与惯性测量单元(IMU)组合，由于IMU的加速度计的分辨率特别高，因此，能够使得算法简单，且提高姿态估计的准确性。

附图说明

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

图1是本发明一实施例提供的基于视觉和惯性测量单元的姿态估计方法的流程图；

图2是本发明一实施例提供的基于视觉和惯性测量单元的姿态估计装置的结构示意图。

具体实施方式

下面结合附图并通过具体实施方式来进一步说明本发明的技术方案。

其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本专利的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

本发明一实施例提供的基于视觉和惯性测量单元的姿态估计方法，如图1所示，包括如下内容：

获取相机的图像数据，以及IMU的测量数据，其中，IMU的测量数据包括设置在相机上的IMU的测量数据和设置在目标物体上的IMU的测量数据，目标物体包括n个特征点，且n个特征点的在物体坐标系中的位置已知，其中，n大于3；

通过图像数据和IMU的测量数据融合估计目标物体在相机坐标系中的姿态。

显然，通过将相机与惯性测量单元(IMU)组合，由于IMU的加速度计的分辨率特别高，因此，能够使得算法简单，且提高姿态估计的准确性。

在一些实施例中，通过图像数据和IMU的测量数据融合估计目标物体在相机坐标系中的姿态，包括：

通过计算物体坐标系到相机坐标系的变换矩阵计算获取目标物体在相机坐标系中的姿态，其中，变换矩阵定义为：

上式中，R表示旋转矩阵；

t表示平移向量，t＝[t

本实施例中，通过IMU的测量数据可以直接获取从物体坐标系到相机坐标系的整个旋转矩阵，因此，只需计算平移向量t，即可获取目标物体在相机坐标系中的姿态，显然，计算过程十分简单。

在一些实施例中，通过图像数据和IMU的测量数据融合估计目标物体在相机坐标系中的姿态，还包括：

对图像数据和IMU的测量数据进行标定，以提高图像数据和IMU的测量数据的融合精度，避免融合过程发散，其标定方法包括：

通过预先设定的阈值判断图像数据是否有效；

当为有效图像数据时，通过迭代无迹卡尔曼滤波对图像数据和IMU的测量数据进行融合。

通过以上方案，能够有效提高标定数据的准确性，进而提高姿态估计的准确性。

在一些实施例中，基于视觉和惯性测量单元的姿态估计方法，还包括：

获取角度数据，其中，角度数据包括设置在相机上的倾角传感器的测量数据和设置在目标物体上的倾角传感器的测量数据；

通过角度数据提供额外的约束，以提高姿态估计的准确性。

基于同样的发明构思，本发明实施例还提供一种基于视觉和惯性测量单元的姿态估计装置，如图2所示，包括：

获取模块1，用于获取相机的图像数据，以及IMU的测量数据，其中，IMU的测量数据包括设置在相机上的IMU的测量数据和设置在目标物体上的IMU的测量数据，目标物体包括n个特征点，且n个特征点的在物体坐标系中的位置已知，其中，n大于3；

估计模块2，用于通过图像数据和IMU的测量数据融合估计目标物体在相机坐标系中的姿态。

显然，通过将相机与惯性测量单元(IMU)组合，由于IMU的加速度计的分辨率特别高，因此，能够使得算法简单，且提高姿态估计的准确性。

在一些实施例中，估计模块，还用于：

通过计算物体坐标系到相机坐标系的变换矩阵计算获取目标物体在相机坐标系中的姿态，其中，变换矩阵定义为：

上式中，R表示旋转矩阵；

t表示平移向量，t＝[t

本实施例中，通过IMU的测量数据可以直接获取从物体坐标系到相机坐标系的整个旋转矩阵，因此，只需计算平移向量t，即可获取目标物体在相机坐标系中的姿态，显然，计算过程十分简单。

在一些实施例中，估计模块，还用于：

对图像数据和IMU的测量数据进行标定，以提高图像数据和IMU的测量数据的融合精度，避免融合过程发散，其标定方法包括：

通过预先设定的阈值判断图像数据是否有效；

当为有效图像数据时，通过迭代无迹卡尔曼滤波对图像数据和IMU的测量数据进行融合。

通过以上方案，能够有效提高标定数据的准确性，进而提高姿态估计的准确性。

在一些实施例中，基于视觉和惯性测量单元的姿态估计装置，还包括：

校准模块3，用于获取角度数据，其中，角度数据包括设置在相机上的倾角传感器的测量数据和设置在目标物体上的倾角传感器的测量数据；

通过角度数据提供额外的约束，以提高姿态估计的准确性。

需要声明的是，上述具体实施方式仅仅为本发明的较佳实施例及所运用技术原理。本领域技术人员应该明白，还可以对本发明做各种修改、等同替换、变化等等。但是，这些变换只要未背离本发明的精神，都应在本发明的保护范围之内。另外，本申请说明书和权利要求书所使用的一些术语并不是限制，仅仅是为了便于描述。