[发明专利]一种基于二维码的物体空间位姿获取方法

权利要求书

1.一种基于二维码的物体空间位姿获取方法，其特征在于，包括以下步骤：

步骤S1、基于单目摄像头获取目标的实时图像，输入至处理器；

步骤S2、处理器对实时图像进行二值化处理，并找出图像中所有的二维码，获取实时图像的二维码位置坐标信息；

步骤S3、对所述二维码位置坐标进行单应变换，获得二维码的单应变换矩阵；

步骤S4、基于所述单应变换矩阵获取二维码的欧拉角和空间位置；

步骤S5、记录各二维码的相对位置关系，将各二维码的欧拉角转换成轴角，融合各个二维码坐标系，并将融合后的坐标系对应到空间坐标系上；基于已识别的二维码当前位姿，获取融合后坐标系上的位姿，得到物体在空间坐标系下的位姿；

所述步骤S3中获取二维码的单应变换矩阵具体步骤如下：

设定相机内参矩阵为单应变换矩阵

其中，f_x和f_y分别为相机在x和y方向上的焦距，c_x和c_y分别为相机中心与图像传感器中心在x和y方向上的偏移量；基于相机内参矩阵K和单应变换矩阵H，获得旋转矩阵和位置矩阵

相机投影矩阵表示为：P＝K[R|T]，空间点X通过投影矩阵变换为图像上的像素点x，投影关系表示为：x＝PX；设定二维码所在平面为X-Y平面，即Z＝0，中心为坐标原点，则对于二维码平面：

将旋转矩阵R的第三列去掉可得：

其中r₀，r₁分别是R的第一列和第二列；则K[r₀ r₁ T]即为所述单应变换矩阵；

所述步骤S4中获取二维码的欧拉角和空间位置具体步骤如下：

设定旋转矩阵计算四元数q＝(q₀ q₁ q₂ q₃)如下：

由可得：

其中，(α β γ)代表二维码的欧拉角；使用atan2函数代替arctan函数表示如下：

设定其中λ为缩放比例，代表缩放后的单位二维码的角点，则

通过非单位化的单应变换矩阵得到旋转矩阵R和位置矩阵T；

所述步骤S5中，将各二维码的欧拉角转换为轴角具体步骤如下：

angle＝2*arccos(c₁c₂c₃-s₁s₂s₃)

x＝c₁c₂s₃+s₁s₂c₃

y＝s₁c₂c₃+c₁s₂s₃

z＝c₁s₂c₃-s₁c₂s₃

其中：

其中α、β、γ分别为偏航角、横滚角和俯仰角；x、y、z为轴角中轴向量的坐标；angle为轴角中的角度。