基于视觉和惯性测量单元的姿态估计方法及吊车吊臂位姿估算装置

技术领域

本发明涉及工业危险管控技术领域，具体为一种基于视觉和惯性测量单元的姿态估计方法及吊车吊臂位姿估算装置。

背景技术

近几年电力施工吊车作业队伍和工程量不断扩大。吊车司机很难根据肉眼目测来判断吊车臂与带电设备间距离，往往因距离较近而未及时停止吊车臂而引发人身安全和大范围停电事故，国内外公司已研制相关的吊车防碰撞系统并应用于实际工程。

为了解决吊车、高空作业车等大型机具作业时，可能发生吊车吊臂或高空作业车作业平台与周边带电设备安全距离不足发生触电或与周边设施安全距离不足误碰周边设施等问题。本项目提出一种基于视掼融合算法的位姿估计方法，通过视觉传感器和惯性测量传感器，实时采集吊车和吊臂位置，对吊车吊臂位姿的估计，实现吊车作业过程中的全方位安全管控。

发明内容

本发明的目的在于提供本发明提出一种基于视掼融合算法的位姿估计方法，通过视觉传感器和惯性测量传感器，实时采集吊车和吊臂位置，对吊车吊臂位姿的估计，实现吊车作业过程中的全方位安全管控。

为实现上述目的，本发明提供如下技术方案：吊车吊臂位姿估算装置，包括：

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

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

作为一种优选的技术方案，估计模块还包括坐标转换子模块，并通过坐标转换子模块计算物体坐标系到相机坐标系的变换矩阵计算获取目标物体在相机坐标系中的姿态。

作为一种优选的技术方案，还包括标定子模块，所述标定子模块对图像数据和IMU的测量数据进行标定，以提高图像数据和IMU惯性传感器的测量数据的融合精度。

作为一种优选的技术方案，还包括有校准模块，其用于获取角度数据，其中，角度数据包括设置在相机上的倾角传感器的测量数据和设置在目标物体上的倾角传感器的测量数据；通过角度数据提供额外的约束，以提高姿态估计的准确性。

一种基于视觉和惯性测量单元的姿态估计方法，使用了上述的吊车吊臂位姿估算装置，包括以下步骤：获取相机的图像数据，以及IMU惯性传感器的测量数据，其中，IMU惯性传感器的测量数据包括设置在相机上的IMU的测量数据和设置在目标物体上的IMU惯性传感器的测量数据；

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

作为一种优选的技术方案，通过计算物体坐标系到相机坐标系的变换矩阵计算获取目标物体在相机坐标系中的姿态，且所述变换矩阵定义为：

上式中，R表示旋转矩阵；t表示平移向量。

作为一种优选的技术方案，通过图像数据和IMU的测量数据融合估计目标物体在相机坐标系中的姿态，还包括：对图像数据和IMU的测量数据进行标定，以提高图像数据和IMU的测量数据的融合精度。

作为一种优选的技术方案，标定方法包括：

1)通过预先设定的阈值判断图像数据是否有效；有效则执行步骤2)，无效则直接舍弃该数据；

2)通过迭代无迹卡尔曼滤波对图像数据和IMU的测量数据进行融合。

作为一种优选的技术方案，对图像数据和IMU的测量数据进行标定还包括：获取角度数据，其中，角度数据包括设置在相机上的倾角传感器的测量数据和设置在目标物体上的倾角传感器的测量数据；通过角度数据提供额外的约束，以提高姿态估计的准确性。

作为一种优选的技术方案，采用扩展卡尔曼滤波算法，将视觉姿态估计和IMU姿态估计结果进行融合，得到吊臂在世界坐标系下的位姿估计值与现有技术相比，本发明的有益效果是：

本发明的一种基于视掼融合算法的位姿估计方法，通过视觉传感器和惯性测量传感器，实时采集吊车和吊臂位置，对吊车吊臂位姿的估计，有利于实现吊车作业过程中的全方位安全管控。

附图说明

图1为本发明一种基于视觉和惯性测量单元的姿态估计方法的方法流程图。

具体实施方式

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

请参阅图1，本发明提供一种技术方案：本发明的第一个方面提供基于视惯融合算法的位姿估计方法，包括：

视觉传感器数据获取：利用摄像头等视觉传感器获取吊臂的图像序列，并提取关键点、特征描述子等信息。

视觉姿态估计：利用视觉传感器数据，采用视觉SLAM(SimultaneousLocalization and Mapping)算法或其他相关方法，估计吊臂在相机坐标系下的位姿。

IMU数据获取：利用IMU传感器获取吊臂的加速度和角速度数据。

IMU姿态估计：利用IMU数据，采用滤波算法(如卡尔曼滤波)或其他相关方法，估计吊臂在惯性坐标系下的位姿。

视惯融合算法：将视觉姿态估计和IMU姿态估计结果进行融合，得到吊臂在世界坐标系下的位姿估计值。可以采用融合算法，如扩展卡尔曼滤波(EKF)或其他相关方法，对两组数据进行融合。

位姿更新与优化：根据实际应用需求，可以进行位姿的更新和优化，以提高估计结果的准确性和稳定性。

具体的包括：

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

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

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

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

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

上式中，R表示旋转矩阵；t表示平移向量，t＝(t

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

上式中，R表示旋转矩阵；t表示平移向量，t＝(t

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

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

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

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

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

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

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

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

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

本方案提出一种基于视掼融合算法的位姿估计方法，通过视觉传感器和惯性测量传感器，实时采集吊车和吊臂位置，对吊车吊臂位姿的估计，实现吊车作业过程中的全方位安全管控。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。