CN109920006B - 一种青饲机自动抛送***位姿变换矩阵的标定方法 - Google Patents

Abstract

一种青饲机自动抛送***位姿变换矩阵的标定方法，包括如下步骤：S1、在相机视野内设置多个标记点，并使其在标定过程中相对于机械臂转盘保持不动；S2、转动机械臂一个位姿，获取该位姿时的机械臂的欧拉角信息，获取该位姿时的相机中的彩图和深度点云图；S3、根据彩图和深度点云图，找出各标记点中心在相机坐标系下的三维信息；S4、重复步骤S2至S3，得到多个位姿时的各标记点中心在相机坐标系下的三维信息和机械臂的欧拉角。S5、根据S4步骤中得到的在相机坐标系下的三维信息，解算不同位姿时的相机坐标系之间的位姿变换矩阵；S6、根据S4步骤中得到的机械臂的欧拉角，解算不同位姿时机械臂坐标系之间的位姿变换矩阵。

Description

一种青饲机自动抛送***位姿变换矩阵的标定方法

技术领域

本发明涉及机器视觉与农业自动化技术领域，尤其是涉及一种青饲机自动抛送***位姿变换矩阵的标定方法。

背景技术

青饲机是一种常用农业收获机械，主要用来收获青绿牧草、燕麦、甜菜茎叶等低矮青饲作物。青饲机大体由前方割台、机身喂入装置及切碎抛送装置组成。前方割台为一个装有多把甩刀旋转切碎器用于高速收割作物，机身收集作物并将其切碎，然后由抛送装置抛出，青饲机抛出的物料由后方或侧方同向行驶的运输车承接。具有自动抛送***的青饲机，机身收集作物并将其切碎，然后由固定在机械臂上的RGB-D相机识别和定位车斗位姿，然后基于相机和机械臂坐标变换矩阵，得出车斗在机械臂坐标系下的位姿，自动调整机械臂实现物料的精准抛投。RGB-D相机(后文中简称相机，本申请中的相机都是指RGB-D相机)是一种可以同时获得颜色信息和深度信息的相机。

青饲机自动抛送***利用相机实现对运输车车斗进行自动识别与定位，其一个前提是需要知道青饲机相机坐标系和机械臂坐标系之间的位姿变换矩阵。求解上述位姿变换矩阵的过程称作标定。具有自动抛送***的青饲机需要出厂之前对位姿变换矩阵进行标定，目前主要是通过人工测量标定物中的标记点在机械臂坐标系下的三维信息，用算法获取在相机中的三维信息，然后对同一个标记点在两坐标系下的三维信息进行3D-3D位姿估计方法解算出两坐标系的位姿变换矩阵。这种基于人工测量的方法费时费力，并且标定误差较大。

发明内容

本发明的目的是针对现有技术存在的不足，提出了一种青饲机自动抛送***位姿变换矩阵的标定方法。

本发明采用的技术方案如下：

姿态传感器安装固定在青饲机的机械臂上；

制作一块背景为灰色的标记板，标记板上包含四个圆形或矩阵的红色标记点，任意两个标记点之间距离差大于80cm。

青饲机自动抛送***位姿变换矩阵的标定过程包含步骤如下：

步骤1：放置标定板于青饲机侧面或后面且位于相机视野内，并使其在标定过程中保持不动；

步骤2：转动机械臂至一个位姿；

步骤3：通过姿态传感器，获取该位姿下的机械臂的欧拉角信息；

步骤4：获取该位姿下相机的彩图和深度点云图；

步骤5：通过颜色滤波方法从彩图中分离出标记点集；

步骤6：把分离出来的标记点集通过聚类算法得出每个标记点中心的像素坐标，把所有标记点中心映射到点云图中，找出各标记点中心在相机坐标系下的三维信息；

重复步骤2-6共四次，设上述四个不同机械臂位姿分别为第一位姿A、第二位姿B、第三位姿C和第四位姿D，获得四组不同机械臂位姿下的各标记点中心在相机坐标系下的三维信息和机械臂位姿的欧拉角。所有位姿下都满足各标记点都在相机视野内，且任意两个位姿之间机械臂在水平或竖直方向转动角度之差大于10度；

步骤7：根据4个不同机械臂位姿下的各标记点中心在相机坐标系下的三维信息，解算各两个不同位姿时相机坐标系之间的位姿变换矩阵Hc；每一个位姿时都有一个相机坐标系，即每一个位姿时都包含一个机械臂坐标系和一个相机坐标系。

其中的相机坐标系是指以相机为原点的三维坐标系。

在一个优选实例中，解算位于第一位姿A和第二位姿B时的相机坐标系之间的位姿变换矩阵H_cAB、位于第一位姿A和第三位姿C时的相机坐标系之间的位姿变换矩阵H_cAC、位于第一位姿A和第四位姿D时的相机坐标系之间的位姿变换矩阵H_cAD。

步骤8：基于4个不同机械臂位姿的欧拉角，解算各两个不同位姿时机械臂坐标系之间的位姿变换矩阵Hg，每一个位姿时都有一个机械臂坐标系，即每一个位姿时都包含一个机械臂坐标系和一个相机坐标系。

其中的机械臂坐标系是指以机械臂的转盘圆心为原点的三维坐标系，且与姿态传感器坐标系平行。

在一个优选实例中，解算位于第一位姿A和第二位姿B时的机械臂坐标系之间的位姿变换矩阵H_gAB、位于第一位姿A和第三位姿C时的机械臂坐标系之间的位姿变换矩阵H_gAC、位于第一位姿A和第四位姿D时的机械臂坐标系之间的位姿变换矩阵H_gAD。

步骤9：采用Tsai-Lenz标定算法，解得相机坐标系与机械臂坐标系之间的位姿变换矩阵，相机坐标系和机械臂坐标系是相对固定的，他两者之间的位姿变换矩阵不随机械臂的位姿的改变而发生变化，所以最终得到的是一个固定的位姿变换矩阵；在一个优选实例中根据H_cAD、H_cAC、H_cAD和H_gAB、H_gAC、H_gAD解算得到位姿变换矩阵H_cg。

本发明的技术方案的技术效果在于，通过读取机械臂多个位姿下姿态传感器的欧拉角及标定点在相机坐标系下的三维坐标信息，解算出青饲机自动抛送***的相机坐标系与机械臂坐标系之间位姿变换矩阵。与现有的基于人工测量的标定方法相比，本发明操作简单快速、标定精度高、自动化程度高、实现成本低。

附图说明

图1为本发明实施例的实施环境示意图。

图2为本发明实施例的标定板示意图。

图3为本发明实施例的机械臂在第一位姿A和第二位姿B时的俯视角度示意图。

图4为本发明实施例的机械臂在第三位姿C和第四位姿D时的俯视角度示意图。

图5为本发明实施例的机械臂在第一位姿A、第二位姿B、第三位姿C和第四位姿时的俯视角度示意图。

图6为本发明实施例的机械臂在第一位姿A、第二位姿B、第三位姿C和第四位姿时的侧面示意图。

图7为本发明实施例的标定过程流程图。

具体实施方式

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

如图1所示，相机和姿态传感器固定在青饲机1的机械臂2上，通过相机4采集标记板5的彩图和深度点云图，通过姿态传感器3采集机械臂的欧拉角，图中6为喷头。

如图2所示，制作一块标记板，尺寸为1.0m*1.5m，标记板背景为灰色，标记板四个角位有4个直径为5cm的圆形红色的标记点，各个标记点的间距如图2所示。

具体示定过程包含如下几个步骤：

步骤1：放置标定板于青饲机后面，并使其在标定的过程中保持不动，标定板和机械臂转盘的直线距离为3m且与车体保持平行；

步骤2：转动机械臂至第一位姿A；

步骤3：通过姿态传感器获取该位姿下的机械臂的欧拉角信息；

步骤4：获取当前位姿时的相机的彩图和深度点云图；

步骤5：基于标定板背景与标记点颜色特征的差异，通过颜色滤波的方法，滤掉彩图中的无关点，从彩图中分离出标记点集。标记点集是指标记点在彩图信息中像素坐标的集合；

步骤6：把分离出来的标记点集通过K-means聚类算法获得出四个标记点的中心的像素坐标。基于四个标记点的中心的像素坐标进行映射到深度点云图中，找出四个标记点的中心在相机坐标系下的三维信息；

如图3至6所示，本实施例共有四个位姿A、B、C、D。第一位姿A的机械臂与车体平行；第二位姿B与第一位姿A高度相同但水平方向转动角度差15度，第三位姿C、第四位姿D比第一位姿A竖直方向转动角度差10度且第四位姿D与第三位姿C沿水平方向转动角度差20度；

重复步骤2至6共三次，再获取第二位姿B、第三位姿C和第四位姿D时的标记点在相机坐标系下的三维信息和机械臂的欧拉角，共获得了四组不同机械臂位姿时的标记点在相机坐标系下的三维信息和机械臂的欧拉角；

步骤7：基于四组不同机械臂位姿下的标记点中心在相机坐标系下的三维信息，用3D-3D的位姿估计方法解算位于第一位姿A和第二位姿B时的相机坐标系之间的位姿变换矩阵H_cAB、位于第一位姿A和第三位姿C时的相机坐标系之间的位姿变换矩阵H_cAC、位于第一位姿A和第四位姿D时的相机坐标系之间的位姿变换矩阵H_cAD；

假设标记板上的四个标记点在第一位姿A时在相机坐标系下的3D点集合表示为：P＝{p₁，p₂，p₃，p₄}，在第二位姿B时在相机坐标系下的3D点集合为：P’＝{p′₁，p′₂，p′₃，p′₄}，需要找到一个欧式变换R_cAB，T_cAB，使得

P＝R_cABP’+T_cAB (1)；

可用SVD方法求解公式(1)中的R_cAB，T_cAB；

(1)计算两组点集合的质心位置p，p’；

(2)计算每个点的去质心坐标；

q_i＝p_i-p，q′_i＝p′_i-p′ (3)

(3)计算W是一个3×3的矩阵；

(4)对W进行SVD分解，得：

W＝UεV^T (5)

其中，ε为奇异值组成的对角矩阵，对角线元素从大到小排列，而U和V为对角矩阵。

当W满秩时，解算R_cAB，T_cAB。

R_cAB＝UVT (6)

T_cAB＝p-R_cABp′ (7)

由R_cAB，T_cAB可组成位于第一位姿A和第二位姿B时的相机坐标系之间变换的位姿变换矩阵H_cAB：

同理可以获得：

步骤8：基于四组不同机械臂位姿时机械臂的欧拉角，用欧拉角与旋转矩阵的转换方法，解算位于第一位姿A和第二位姿B时的机械臂坐标系之间的位姿变换矩阵H_gAB、位于第一位姿A和第三位姿C时的机械臂坐标系之间的位姿变换矩阵H_gAC、位于第一位姿A和第四位姿D时的机械臂坐标系之间的位姿变换矩阵H_gAD。

第一位姿A的机械臂坐标系与第二位姿B的机械臂坐标系的转换也需要找一个位姿变换矩阵来实现。由于已知第二位姿B机械臂坐标系和第一位姿A机械臂坐标系都是由虚拟机械臂坐标系(姿态传感器的欧拉角为零时机械臂的位姿，即姿态O)以原点不变旋转得到，把这两个旋转都可以分解为绕Z-Y-X旋转，旋转角度即姿态传感器两个位姿的欧拉角的值。

设坐标系Ⅰ经原点不变旋转得到坐标系Ⅱ，坐标系Ⅰ和Ⅱ存在如下关系：

R＝R_ZR_YR_X (14)，

ⅠH＝Ⅱ (16)，

其中γ为坐标系Ⅰ旋转到坐标系Ⅱ偏航角，α为Ry坐标系Ⅰ旋转到坐标系Ⅱ俯仰角，β为Rx坐标系Ⅰ旋转到坐标系Ⅱ横滚角，R为坐标系Ⅰ和坐标系Ⅱ之间的旋转矩阵，H为坐标系Ⅰ和坐标系Ⅱ之间的位姿变换矩阵。

由于第一位姿A和第二位姿B都是由位姿O经过旋转得到的，所以存在如下关系：

OH_OA＝A (17)，

OH_OB＝B (18)，

其中，O表示姿态传感器欧拉角为零时的机械臂坐标系，A表示第一位姿A的机械臂坐标系，B表示第二位姿B的机械臂坐标系，H_OA为位姿O机械臂坐标系到第一位姿A机械臂坐标系的位姿变换矩阵，H_OB为位姿O机械臂坐标系到第二位姿B机械臂坐标系的位姿变换矩阵。

第一位姿A机械臂坐标系与第二位姿B机械臂坐标系之间的位姿变换矩阵H_gAB。

同理，

第一位姿A机械臂坐标系与第三位姿C机械臂坐标系之间的位姿变换矩阵H_gAC。

第一位姿A机械臂坐标系与第四位姿D机械臂坐标系之间的位姿变换矩阵H_gAD。

步骤9：采用Tsai-Lenz标定算法，基于H_cAB、H_cAC、H_cAD和H_gAB、H_gAC、H_gAD，求解相机坐标系与机械臂坐标系之间的位姿变换矩阵H_cg。

由第一位姿A和第二位姿B得：

H_gABH_cg＝H_cgH_cAB (23)，

同理由由第一位姿A和第三位姿C得：

H_gACH_cg＝H_cgH_cAC (26)，

同理由由第一位姿A和第四位姿D得：

H_gADH_cg＝H_cgH_cAD (29)，

根据公式(25)、(28)、(31)得：

由以上述方程组就可求出相机坐标系与机械臂坐标系的位姿变换矩阵H_cg。

本实施例中一共有三种位姿变换矩阵，一种是两个不同位置之间的相机坐标系的位姿变换矩阵(H_cAB、H_cAC、H_cAD)，一种是两个不同位姿之间的机械臂坐标系位姿变换矩阵(H_gAB、H_gAC、H_gAD)，一种是相机坐标系和机械臂坐标系之间的位置变换矩阵H_cg，由3个Hc和3个Hg才能解出1个H_cg。

青饲机自动抛送***基于相机坐标系与机械臂坐标系的位姿变换矩阵H_cg，将车斗在相机坐标系下的信息转换为在机械臂坐标系下的信息，对车斗进行自动识别与定位。

Claims (9)

1.一种青饲机自动抛送***位姿变换矩阵的标定方法，其特征在于，基于安装在机械臂上的相机和姿态传感器进行，包括如下步骤：

S1、在相机视野内设置多个标记点，并使其在标定过程中相对于机械臂转盘保持不动；

S2、转动机械臂一个位姿，获取该位姿时的机械臂的欧拉角信息，获取该位姿时的相机中的彩图和深度点云图；

S3、根据彩图和深度点云图，找出各标记点中心在相机坐标系下的三维信息；

S4、重复步骤S2至S3，得到多个位姿时的各标记点中心在相机坐标系下的三维信息和机械臂的欧拉角；

S5、根据S4步骤中得到的在相机坐标系下的三维信息，解算不同位姿时的相机坐标系之间的位姿变换矩阵；其中，位姿包括第一位姿A和第二位姿B，解算位于第一位姿A和第二位姿B时的相机坐标系之间的位姿变换矩阵H_cAB包括如下步骤：

找到一个欧式变换R_cAB，T_cAB，使得下列公式(1)成立，

P＝R_cABP'+T_cAB (1)，

其中P为在第一位姿A时相机坐标系下的四个标记点的集合，表示为P＝{p₁,p₂,p₃,p₄}，P'为在第二位姿B时相机坐标系下的四个标记点的集合，表示为P'＝{p'₁,p'₂,p'₃,p'₄}；

使用SVD方法计算公式(1)中的R_cAB，T_cAB，具体如下：

计算两组集合的质心位置p、p'，

计算每个标记点的去质心坐标，

q_i＝p_i-p,q'_i＝p'_i-p' (3)；

计算W是一个3×3的矩阵，

对W进行SVD分解，得：

W＝UεV^T (5)，

其中，ε为奇异值组成的对角矩阵，对角线元素从大到小排列，U和V为对角矩阵；

当W满秩时，解算得到R_cAB，T_cAB，

R_cAB＝UV^T (6)，

T_cAB＝p-R_cABp' (7)；

根据R_cAB，T_cAB可组成位姿A与位姿B时的相机坐标系之间的位姿变换矩阵H_cAB，

S6、根据S4步骤中得到的机械臂的欧拉角，解算不同位姿时机械臂坐标系之间的位姿变换矩阵；

S7、采用Tsai-Lenz标定算法，根据不同位姿时的相机坐标系之间的位姿变换矩阵和机械臂坐标系之间的位姿变换矩阵，解算得到相机坐标系与机械臂坐标系之间的位姿变换矩阵。

2.根据权利要求1所述的青饲机自动抛送***位姿变换矩阵的标定方法，其特征在于，所述S3步骤中找出各标记点中心在相机坐标系下的三维信息，具体包括如下步骤：

S31、通过颜色滤波方法从彩图中分离出标记点集；

S32、把分离出来的标记点集通过聚类算法得出每个标记点中心的像素坐标，把所有标记点中心映射到深度点云图中，找出各标记点中心在相机坐标系下的三维信息。

3.根据权利要求1所述的青饲机自动抛送***位姿变换矩阵的标定方法，其特征在于，其中S1步骤中设置多个标记点具体是包括，制作一块背景为灰色的标记板，标记板上包含四个圆形或矩阵的红色标记点，使任意两个标记点之间距离差大于80cm，标定板与青饲机车体保持平行。

4.根据权利要求3所述的青饲机自动抛送***位姿变换矩阵的标定方法，其特征在于，所述位姿为四个，具体为第一位姿A、第二位姿B、第三位姿C和第四位姿D，各个位姿时都满足各标记点在相机视野内，且任意两个位姿之间机械臂在水平或竖直方向转动角度之差大于10度。

5.根据权利要求4所述的青饲机自动抛送***位姿变换矩阵的标定方法，其特征在于，第一位姿A的机械臂与车体平行；第二位姿B与第一位姿A高度相同但水平方向转动角度差15度，第三位姿C、第四位姿D比第一位姿A竖直方向转动角度差10度且第四位姿D与第三位姿C沿水平方向转动角度差20度。

6.根据权利要求5所述的青饲机自动抛送***位姿变换矩阵的标定方法，其特征在于：所述步骤S5中解算不同位姿时的相机坐标系之间的位姿变换矩阵；

根据四个不同机械臂位姿时的各标记点中心在相机坐标系下的三维信息，解算位于第一位姿A和第二位姿B时的相机坐标系之间的位姿变换矩阵H_cAB、位于第一位姿A和第三位姿C时的相机坐标系之间的位姿变换矩阵H_cAC、位于第一位姿A和第四位姿D时的相机坐标系之间的位姿变换矩阵H_cAD。

7.根据权利要求6所述的青饲机自动抛送***位姿变换矩阵的标定方法，其特征在于：所述步骤S6中解算不同位姿时的机械臂坐标系之间的位姿变换矩阵包括：

根据四个不同机械臂位姿时机械臂的欧拉角，用欧拉角与旋转矩阵的转换方法，解算位于第一位姿A和第二位姿B时的机械臂坐标系之间的位姿变换矩阵H_gAB、位于第一位姿A和第三位姿C时的机械臂坐标系之间的位姿变换矩阵H_gAC、位于第一位姿A和第四位姿D时的机械臂坐标系之间的位姿变换矩阵H_gAD。

8.根据权利要求7所述的青饲机自动抛送***位姿变换矩阵的标定方法，其特征在于：解算位于第一位姿A和第二位姿B时的机械臂坐标系之间的位姿变换矩阵H_gAB的步骤具体包括：

第一位姿A和第二位姿B存在如下关系：

OH_OA＝A (17)，

OH_OB＝B (18)，

其中，O表示姿态传感器欧拉角为零时的机械臂坐标系，A表示第一位姿A的机械臂坐标系，B表示第二位姿B的机械臂坐标系，H_OA为位姿O机械臂坐标系到第一位姿A机械臂坐标系的位姿变换矩阵，H_OB为位姿O机械臂坐标系到第二位姿B机械臂坐标系的位姿变换矩阵；

根据公式17－19计算第一位姿A的机械臂坐标系与第二位姿B的机械臂坐标系之间的位姿变换矩阵H_gAB，公式如下：

9.根据权利要求8所述的青饲机自动抛送***位姿变换矩阵的标定方法，其特征在于：求解相机坐标系与机械臂坐标系之间的位姿变换矩阵H_cg的步骤具体包括：

建立以下公式：

H_gABH_cg＝H_cgH_cAB (23)，

根据公式(23)-(25)获得：

Images