[发明专利]一种果园树干三维检测定位方法及装置

权利要求书

1.一种果园树干三维检测定位方法，其特征在于，包括如下步骤：

S100、通过图像坐标系、相机坐标系、雷达坐标系与世界坐标系之间的转换关系，建立果树的树干三维检测定位模型；

S200、通过彩色摄像头获取树干彩色图像，根据所述树干彩色图像获得检测目标的图像坐标信息；

S300、提供激光雷达循环获取两个目标点的扫描数据，根据所述激光雷达返回的所述目标点的距离和角度信息，得到所述目标点的雷达坐标信息；

S400、将所述雷达坐标信息转换为图像坐标信息，并结合所述检测目标的图像坐标信息，判断所述两个目标点是否来自同一目标树干；

S500、若所述两个目标点不是来自同一目标树干，则执行步骤S300-S400，重新寻找另外两个目标点重复确认，直至所有目标点检测完毕；

S600、若所述两个目标点来自同一目标树干，则基于所述两个目标点的雷达坐标信息计算所述目标树干的中心点坐标；

S700、根据所述中心点坐标及检测目标的图像坐标信息计算得到所述目标树干的树干区域；

其中，步骤S100中，建立果树的树干三维检测定位模型包括如下步骤：

S101、相机建模，通过彩色摄像头获取树干彩色图像，并对所述树干彩色图像数据进行世界坐标系到图像坐标系的转换；

S102、雷达建模，利用激光雷达获取目标点的雷达扫描数据，并对所述雷达扫描数据进行雷达坐标系到世界坐标系的转换；

S103、关联所述图像坐标系和雷达坐标系；以及

S104、树干中心建模，对激光雷达数据进行雷达坐标系到世界坐标系的转换，所述树干中心建模包括：

P₁,P₂为雷达返回的目标点，P₀为目标树干的中心点，过P₀向P₁,P₂的连线作垂线交于P₃，过P₃向P₀与m轴的垂线做垂线交于点P₄，设P₁(x₁,y₁),P₂(x₂,y₂)，将该雷达坐标转换为图像坐标点：

P₁,P₂连线的斜率为P₀P₃的斜率为则所述目标树干的中心点P₀的所述雷达坐标系的坐标为：

其中，

将所述雷达坐标系的坐标点P₀(x₀,y₀)转换到所述世界坐标系；

并采用如下公式计算所述目标树干的树干区域：

设所述目标树干的宽高在所述树干彩色图像中分别为w,h，则所述目标树干的真实宽高2R,H为：

其中，d₁为P₁点返回的距离值，X，Y为P₁点在物理成像平面的坐标，R为树干半径，H为树干高度。