[发明专利]基于RGB-D相机的栈板位姿计算方法

权利要求书

1.一种基于RGB-D相机的栈板位姿计算方法，其特征在于，具体包括以下步骤：

(1)根据叉车叉臂建立叉车坐标系，RGB-D相机获取叉车图像并建立图像坐标系，获取叉车坐标系x轴和y轴在图像坐标系上的灭点，基于灭点和叉车车臂顶点获取的直线计算旋转矩阵，获取叉车坐标系的两个不同点以及图像坐标系对应的坐标，通过最小二乘、平移矩阵和旋转矩阵得到叉车坐标系到RGB-D相机坐标系转换公式；

(2)从深度图中获取点云信息并将其从相机坐标系转换至叉车坐标系，获取栈板截面点云并设为模板点云，建立点云坐标系，计算叉车坐标系相对于点云坐标系的姿态；

(3)获取当前帧的上一帧预测点云，将模板点云转移到当前帧的上一帧，利用三自由度ICP计算预测帧到当前帧的转移矩阵，更新模板点云到当前帧点云姿态，计算当前帧点云相对于叉车坐标系的姿态；

(4)优化步骤(3)获取的姿态。

2.根据权利要求1所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(1)具体包括以下步骤：

(1a)建立叉车坐标系，以叉臂中心与车头交汇处为坐标原点，叉臂指向为y轴正方向，垂直地面向上为z轴正方向，x方向由右手定则确定；RGB-D相机获取叉车图像并建立图像坐标系；

(1b)获取叉车坐标系y轴在图像坐标系上的灭点；

(1c)根据y轴的灭点计算旋转矩阵R＝[r₁ r₂ r₃]的第二列r₂；

(1d)获取叉车叉臂顶点的直线，利用x方向在直线上的灭点和旋转矩阵R的第二列r₂正交计算得到R的第一列r₁，旋转矩阵R的第三轮r₃则通过r₁和r₂的叉积得到；

(1e)通过叉车实际尺寸获取在叉车坐标系下的两个不同的点P₁ P₂，以及对应点在图像上的坐标p₁ p₂，通过最小二乘、平移矩阵和旋转矩阵得到叉车坐标系到RGB-D相机坐标系转换公式。

3.根据权利要求2所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(1b)获取叉车坐标系y轴在图像坐标系上的灭点具体指的是

通过固定在叉车上的RGB-D相机获取一张叉车叉臂图像，通过高斯滤波灰度增强二值化方法对上述图像进行预处理，采用canny边缘检测得到叉臂的轮廓图，拟合出叉臂两端的直线，通过直线方程构造最小二乘，利用SVD分解得到y方向上灭点坐标。

4.根据权利要求2所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(1c)根据y轴的灭点计算旋转矩阵R＝[r₁ r₂ r₃的第二列r₂具体指的是

根据y轴的灭点计算旋转矩阵R＝[r₁ r₂ r₃]的第二列r₂，服从如下公式

其中，[u v 1]^T为获取的y轴上的灭点，K为已知的相机内参矩阵，T为平移矩阵，[x y z1]^T为y轴的灭点在叉车坐标系下的齐次坐标，为[0 1 0 0]^T。

5.根据权利要求2所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(1e)具体包括

通过叉车实际尺寸获取在叉车坐标系下的两个不同的点P₁ P₂，以及对应点在图像上的坐标p₁ p₂，构造最小二乘：Ax＝b

其中x＝[t_x t_y t_z]^T为平移矩阵，f_x，f_y为相机在对应方向的焦距，c_x，c_y为图像坐标系到像素坐标系的偏移量。