CN113834488A - 基于结构光阵列远程识别的机器人空间姿态解算方法 - Google Patents

Abstract

本发明公开了基于结构光阵列远程识别的机器人空间姿态解算方法，包括如下步骤：S1，结构光光斑识别，获取机器人上不同的光斑；S2，光斑解析，根据不同光斑构成的几何特征，标定机器人姿态；S3，光斑质心重定位；S4，相对姿态测算；S5，空间姿态解算；本方法通过云台相机远程识别机器人表面的结构光阵列，在相机内参固定的情况下，根据结构光几何参数、云台相机外参解算出机器人空间姿态，可以为IMU测量值进行辅助修正。本方法规避了机器人运动时相机远程成像模糊的问题，且不受作业环境噪声干扰，达到了良好的测算效果。

Description

基于结构光阵列远程识别的机器人空间姿态解算方法

技术领域

本发明涉及计算机图形学、计算机图像处理技术领域，尤其是涉及一种为机器人设计的结构光阵列，通过该结构光阵列对机器人进行远程姿态解算的方法。

背景技术

磁吸附爬壁机器人在探测、维护钢结构物方面有着重要应用，自动化作业是其未来的发展趋势。对磁吸附爬壁机器人进行定位和建图，是实现自动化作业的前提。

空间姿态是机器人进行定位的重要参数，目前一般通过IMU（InertialMeasurement Unit，惯性测量单元）计算获得。然而磁吸附爬壁机器人通常具有磁轮、磁阵列等吸附结构，导致周边地磁场严重畸变、IMU内部电子罗盘失效，因此IMU陀螺仪得到的姿态漂移严重，且随着使用时间变长，累积误差逐渐增大。因此，需要研究一种对机器人进行全局姿态测量的方法，对IMU进行辅助修正。

结构光三维面成像的应用日益广泛，投影结构光空间位置和形状参数的精度与三维测量数据的精度有着直接联系。现有技术中，对投影的结构光的标定大多将设为平面，过于简单，但在实际的情况中，例如爬壁机器人在圆柱形的钢制油罐表面，由于钢制油罐表面为圆柱形，通过相机定位时，存在正对和侧对爬壁机器人的情况。通过单应矩阵能够在一定程度上解决该问题，在计算机视觉中，平面的单应性被定义为一个平面到另外一个平面的投影映射，因此一个二维平面上的点映射到摄像机成像仪上的映射就是平面单应性的例子，单应矩阵约束了同一三维空间点在两个像素平面的二维齐次坐标。

但对爬壁机器人的标定时，可能会用到许多类型的标定方式，例如多种不同的光斑颜色或者光斑形状，因此需要设置不同的LED灯，或者不同形状的LED灯，这提高了爬壁机器人的成本和能耗，同时过多的用于标定的光斑也会提高光斑识别的算力。因此，需要一种能够用最少的类型、最少的点的结构光阵列来标定爬壁机器人，再通过标定的光斑和单应矩阵，通过云台相机对爬壁机器人的姿态进行识别和解析，从而修正IMU误差。

发明内容

为解决现有技术的不足，实现IMU误差修正的目的，本发明采用如下的技术方案：

基于结构光阵列远程识别的机器人空间姿态解算方法，包括如下步骤：

S1，结构光光斑识别，获取机器人上不同的光斑；

S2，光斑解析，根据不同光斑构成的几何特征，标定机器人姿态；

S3，光斑质心重定位；

S4，相对姿态测算，根据相机内参标定结果，可以计算机器人相对于相机的姿态，当前相机坐标系下，获取机器人LED光斑像素坐标的光路表示为O _c L；定义虚拟标准坐标系，预先在一定距离、垂直对准条件下，对机器人采集LED光斑像素坐标的光路表示为O _c ’L’。即使O _c L、O _c ’L’长度与夹角未知，图像I与I’的四对LED点坐标之间仍具有唯一对应关系，根据单应矩阵，获取相机坐标系与虚拟标准坐标系间的旋转矩阵与平移向量；

S5，空间姿态解算，根据旋转矩阵及机器人姿态角，得到机器人在全局坐标系的姿态。

进一步地，光斑为两组，一组用于标定机器人前后，一组用于标定机器人左右。

进一步地，两组光斑构成两条非平行的直线。

进一步地，两个光斑构成一条直线，其中一条直线在另一条直线的中垂线上。光斑设置太多，用于识别光斑的计算量会增大，4个光斑，2组颜色的类别，可以节约色相识别运行的次数，在保证最简洁的标定机器人姿态的同时，最大限度的减少计算量。

进一步地，S1中，在相机低曝光模式下获取图像，可以过滤掉环境中的各类颜色干扰，通过色相识别区分不同的光斑。

进一步地，机器人头部左、右设置同组光斑A、B，尾部设置另一组光斑C、D点，CD在AB的中垂线上，S2中，距离一组光斑中点最近的光斑设为C，同组另一个光斑设为D，随机选取光斑向量DC一侧的光斑，将该光斑与D的矢量与矢量DC，计算矢量外积，若结果小于0，则表明该光斑在DC延长线的左侧，将该光斑设为机器人左侧光斑A，同组另一侧的光斑设为B。

进一步地，S3中，为保证算法速度，光斑识别和坐标计算均在压缩图像中进行，实际坐标按压缩率N来放大，但同时可能造成1~N个像素的误差，则对原图像中，光斑质心周围50像素范围内的点进行灰度权值计算，选取最亮的点作为光斑质心坐标。

进一步地，S4中，单应矩阵公式如下：

n ^T P=d （1）

（2）

公式（1）表示灯光平面L在云台坐标系中的方程，P表示符合该方程的点，n表示平 面法向量，d表示相机坐标系原点

到该灯光平面L的垂直距离；公式（2）中，H表示两图像的 单应矩阵，K表示相机内参矩阵，

、

表示相机坐标系与虚拟标准坐标系间的旋转矩阵 与平移向量。在K、H已知的情况下，可通过奇异值分解求得

和

，由此可得到机器人相 对于云台相机的姿态。

进一步地，相机采用云台相机，包括两自由度云台和长焦工业相机，云台提供俯仰 角、偏航角两个方向的转动角度输出，控制云台相机对准机器人，所述S5中，在虚拟标准坐 标系中，机器人姿态角固定为

，机器人在全局坐标系中的姿态为：

（3）

其中，

表示机器人相对于云台相机的旋转矩阵，

表示云台相机外参，即相机 相对于云台基座的角度，由云台两自由度输出值计算得到，

表示云台相机坐标系相对于 全局坐标系（通常按当地东-北-天方向设置全局坐标系）的姿态，可在确定云台放置方向后 手动获得，也可直接将云台坐标系作为全局坐标系使用，若以云台坐标系为全局坐标系，则 忽略此参数。

进一步地，S1之前，对获取的图像进行预处理，包括图像压缩、图像去噪、HSV（Hue,Saturation, Value，色调，饱和度，明度）转换，其中图像压缩可以提高处理速度，图像去噪可以避免提取到环境干扰中的亮点，HSV转换可以方便后续的颜色提取；S1中，对识别出的光斑区域进行去噪、轮廓提取、质心计算，得到四个光斑的坐标值。

本发明的优势和有益效果在于：

1、本发明适用于磁吸附爬壁机器人定位与建图，可通过云台相机的远程识别，解算出机器人姿态，用于对IMU误差进行辅助修正。

2、本发明以结构光为信标，解决了机器人在运动时会导致长焦工业相机成像模糊的问题，信标模糊不影响结构光的像素坐标精度。

3、本发明在相机低曝光模式下检测结构光，过滤了环境中各类非光源颜色特征的干扰，算法稳定性良好，适合室外作业使用。

附图说明

图1是本发明的示例场景模拟图。

图2a是本发明中结构光阵列在机器人正视姿态下的演示图。

图2b是本发明中结构光阵列在机器人斜视姿态下的演示图。

图3是本发明中云台相机的结构示意图。

图4是本发明的方法流程图。

图5是本发明中相对姿态解算的光路原理图。

图6a是本发明的测试场景中正常视角下的结构光阵列图。

图6b是本发明的测试场景中低曝光模式下识别到的结构光阵列图。

图中：1、长焦工业相机，2、偏航角转动转置，3、俯仰角转动装置。

具体实施方式

以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。

基于结构光阵列远程识别的机器人空间姿态解算方法，硬件设计方面，包括：

1、场景介绍：本方法以一台三轮磁吸附爬壁机器人为例进行演示。如图1所示，将爬壁机器人放置于钢制油罐表面，将云台相机随机放置于油罐旁，机器人可在油罐表面运动。

2、结构光阵列选取和几何参数设计：在机器人表面最高平面固定四个LED灯，并进行编号：A、B为红灯，位于机器人头部左右两侧；C、D为蓝灯，位于机器人尾部。其中，A、B、D三点构成等腰三角形，DC垂直于AB。图2a为相机正对机器人时LED灯的分布；图2b为机器人倾斜时LED灯的效果。

3、云台相机架设：云台相机包括一个两自由度云台、一个长焦工业相机1，如图3所示。其中，云台的俯仰角转动装置3和偏航角转动转置2，可以提供俯仰角、偏航角两个方向的转动角度输出，控制相机对准机器人。云台相机的结构包括但不限于本实施例中的模型。

云台相机获取到机器人图像之后，需要对图像进行处理。算法流程包括：结构光光斑识别、光斑解析、光斑质心重定位、相对姿态测算、空间姿态解算等过程，如图4所示。

1、结构光光斑识别：在相机低曝光模式下获取图像，可以过滤掉环境中的各类颜色干扰。随后可以通过色相识别区分红、蓝光斑。

2、光斑解析：基于几何特征，可以总结出光斑解析规则：距离两红色光斑中点最近的蓝色光斑为C点，另一个为D点；光斑向量DC左侧的红色光斑为A点，另一个为B点。根据以上规则可以实现光斑解析。

3、光斑质心重定位：为保证算法速度，光斑识别和坐标计算均在压缩图像中进行，实际坐标应按压缩率N来放大，但同时可能造成1~N个像素的误差，本实施例中图像像素为2448*2048，可设置N=8。例如，若压缩图像中的光斑中心坐标为（100,200），则需要对原图像中（800,1600）周围50像素范围内的点进行灰度权值计算，选择最亮的点作为光斑质心坐标。

4、相对姿态测算：根据相机内参标定结果，可以计算机器人相对于相机的姿态。如图5所示，定义实际场景下相机坐标系，获得LED像素坐标的光路表示为O _c L；定义虚拟标准坐标系，预先在一定距离、垂直对准条件下对机器人采集LED坐标，光路表示为O _c ’L’。即使O _c L、O _c ’L’长度与夹角未知，图像I与I’的四对LED点坐标之间仍具有唯一对应关系，根据单应矩阵计算方法，可表示为：

n ^T P=d （1）

（2）

公式（1）描述了灯光平面L在云台坐标系中的方程，P为符合该方程的点，n为平面 法向量，d表示相机坐标系原点

到该平面的垂直距离；公式（2）中，H为两图像的单应矩阵，K为相机内参矩阵，

、

为相机坐标系与虚拟标准坐标系间的旋转矩阵与平移向量。在K、H已知的情况下，可通过奇异值分解求得

和

，由此可得到机器人相对于云台相机的 姿态。例如，当机器人姿态如图2b所示时，其姿态为

。

5、空间姿态解算：在虚拟相机坐标系中，机器人姿态角固定为

，则机 器人在全局坐标系中的姿态可推导为：

（3）

其中，

即为机器人相对于云台相机的转换矩阵；

为相机外参，即相机相对于 云台基座的角度，由云台两自由度输出值计算得到；

为云台坐标系相对于全局坐标系 （通常按当地东-北-天方向设置全局坐标系）的姿态，可在确定云台放置方向后手动获得， 也可直接将云台坐标系作为全局坐标系使用。若以云台坐标系为全局坐标系，则忽略此参 数。

本实施例中基于结构光阵列远程识别，对机器人空间姿态进行解算，包括如下步骤：

将机器人放置于油罐壁面上，云台相机随机放置于油罐周围，将云台相机对准机器人，此时根据云台电机输出值记录云台初始角度，作为相机外参。

在上位软件中启动相机，设置为低曝光模式，控制机器人移动。相机获取到视频流后，传递给底层算法，算法主要流程如图4所示。

在对图像进行结构光识别之前，需要进行一系列预处理。其中，图像压缩可以提高处理速度；去噪可以避免提取到环境干扰中的亮点；HSV（Hue, Saturation, Value，色调，饱和度，明度）转换可以方便后续的颜色提取。

色相识别可以简单提取图像中符合颜色要求的像素点，选用红蓝两种光代表4个点可以节约色相识别运行的次数；随后，对提取出的区域进行去噪、轮廓提取、质心计算，得到四个光斑的坐标值，识别效果如图6a、图6b所示。

由于图像压缩，光斑坐标值的精度被降低，因此需要回到原图像，对该坐标附近区域进行质心计算，修正该光斑的坐标，否则其误差大约为1~N个像素。

为进行相对姿态测算，需要将识别到的四个光斑与实际的机器人结构光阵列对应起来。以图6b为例，识别到两个红色光斑（114,74）和（217,90），识别到两个蓝色光斑（145,164）和（137,197）。首先计算两红色光斑坐标平均值为（165.5,82），由于蓝色光斑（145,164）与其欧式距离较短，则将蓝色光斑（145,164）标记为C点，另一个蓝色光斑为D点；随后，随机选取红色光斑（114,74）为A点，计算DC*DA矢量外积，若结果小于0，证明随机选取的A点确实位于DC延长线的左侧，则将该点确定为A点，另一个红色光斑（217,90）确定为B点。最终将四个光斑圈出并打上字母标记。

根据图2a的机器人摆放姿态，预先采集图片，获取到结构光坐标，并定义为图5中的虚拟标准坐标系；根据当前获取的图片和结构光坐标，定义当前的相机坐标系；根据图像单应性原理、公式（1）和（2），计算两坐标系的转换矩阵，即机器人相对于相机的姿态。

根据公式（3），将上述姿态解算为机器人空间姿态，随后向机器人输出姿态。

重复上述过程。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的范围。

Claims (10)

1.基于结构光阵列远程识别的机器人空间姿态解算方法，其特征在于包括如下步骤：

S1，结构光光斑识别，获取机器人上不同的光斑；

S2，光斑解析，根据不同光斑构成的几何特征，标定机器人姿态；

S3，光斑质心重定位；

S4，相对姿态测算，当前相机坐标系下，获取机器人光斑像素坐标的光路；虚拟标准坐标系下，对机器人采集光斑像素坐标的光路，根据单应矩阵，获取相机坐标系与虚拟标准坐标系间的旋转矩阵与平移向量；

S5，空间姿态解算，根据旋转矩阵及机器人姿态角，得到机器人在全局坐标系的姿态。

2.根据权利要求1所述的基于结构光阵列远程识别的机器人空间姿态解算方法，其特征在于所述光斑为两组，一组用于标定机器人前后，一组用于标定机器人左右。

3.根据权利要求2所述的基于结构光阵列远程识别的机器人空间姿态解算方法，其特征在于两组光斑构成两条非平行的直线。

4.根据权利要求3所述的基于结构光阵列远程识别的机器人空间姿态解算方法，其特征在于两个光斑构成一条直线，其中一条直线在另一条直线的中垂线上。

5.根据权利要求1所述的基于结构光阵列远程识别的机器人空间姿态解算方法，其特征在于所述S1中，在相机低曝光模式下获取图像，通过色相识别区分不同的光斑。

6.根据权利要求4所述的基于结构光阵列远程识别的机器人空间姿态解算方法，其特征在于机器人头部左、右设置同组光斑A、B，尾部设置另一组光斑C、D点，CD在AB的中垂线上，S2中，距离一组光斑中点最近的光斑设为C，同组另一个光斑设为D，随机选取光斑向量DC一侧的光斑，将该光斑与D的矢量与矢量DC，计算矢量外积，若结果小于0，则将该光斑设为机器人左侧光斑A，同组另一侧的光斑设为B。

7.根据权利要求1所述的基于结构光阵列远程识别的机器人空间姿态解算方法，其特征在于所述S3中，光斑识别和坐标计算均在压缩图像中进行，实际坐标按压缩率N来放大，则对原图像中，光斑质心周围的点进行灰度权值计算，选取最亮的点作为光斑质心坐标。

8.根据权利要求1所述的基于结构光阵列远程识别的机器人空间姿态解算方法，其特征在于所述S4中，单应矩阵公式如下：

n ^T P=d （1）

（2）

公式（1）表示灯光平面L在云台坐标系中的方程，P表示符合该方程的点，n表示平面法 向量，d表示相机坐标系原点

到该灯光平面L的垂直距离；公式（2）中，H表示单应矩阵，K表 示相机内参矩阵，

、

表示相机坐标系与虚拟标准坐标系间的旋转矩阵与平移向量。

9.根据权利要求1所述的基于结构光阵列远程识别的机器人空间姿态解算方法，其特 征在于所述相机采用云台相机，包括两自由度云台和长焦相机，云台提供俯仰角、偏航角两 个方向的转动角度输出，控制云台相机对准机器人，所述S5中，在虚拟标准坐标系中，机器 人姿态角固定为

，机器人在全局坐标系中的姿态为：

（3）

其中，

表示机器人相对于云台相机的旋转矩阵，

表示云台相机外参，即相机相对 于云台基座的角度，

表示云台相机坐标系相对于全局坐标系的姿态。

10.根据权利要求1所述的基于结构光阵列远程识别的机器人空间姿态解算方法，其特征在于所述S1之前，对获取的图像进行预处理，包括图像压缩、图像去噪、HSV转换，S1中，对识别出的光斑区域进行去噪、轮廓提取、质心计算，得到四个光斑的坐标值。

Images