一种果园树干三维检测定位方法及装置，该方法包括建立果树的树干三维检测定位模型；获取树干彩色图像，根据该树干彩色图像获得检测目标的图像坐标信息；获取两个目标点的雷达扫描数据，根据返回的该目标点的距离和角度信息，得到该目标点的雷达坐标信息；将该雷达坐标信息转换为图像坐标信息，并根据该检测目标的图像坐标信息，判断该两个目标点是否来自同一目标树干；若否重新寻找另外两个目标点重复确认；若是则基于该两个目标点的雷达坐标信息计算该目标树干的中心点坐标；根据该中心点坐标及检测目标的图像坐标信息计算得到该目标树干的树干区域。本发明还提供了采用该方法的果园树干三维检测定位装置。
1.一种果园树干三维检测定位方法，其特征在于，包括如下步骤：S100、通过图像坐标系、相机坐标系、雷达坐标系与世界坐标系之间的转换关系，建立果树的树干三维检测定位模型；
S200、通过彩色摄像头获取树干彩色图像，根据所述树干彩色图像获得检测目标的图像坐标信息；
S300、提供激光雷达循环获取两个目标点的扫描数据，根据所述激光雷达返回的所述目标点的距离和角度信息，得到所述目标点的雷达坐标信息；
S400、将所述雷达坐标信息转换为图像坐标信息，并结合所述检测目标的图像坐标信息，判断所述两个目标点是否来自同一目标树干；
S500、若所述两个目标点不是来自同一目标树干，则执行步骤S300-S400，重新寻找另外两个目标点重复确认，直至所有目标点检测完毕；
S600、若所述两个目标点来自同一目标树干，则基于所述两个目标点的雷达坐标信息计算所述目标树干的中心点坐标；
S700、根据所述中心点坐标及检测目标的图像坐标信息计算得到所述目标树干的树干区域；
其中，步骤S100中，建立果树的树干三维检测定位模型包括如下步骤：S101、相机建模，通过彩色摄像头获取树干彩色图像，并对所述树干彩色图像数据进行世界坐标系到图像坐标系的转换；
S102、雷达建模，利用激光雷达获取目标点的雷达扫描数据，并对所述雷达扫描数据进行雷达坐标系到世界坐标系的转换；
S104、树干中心建模，对激光雷达数据进行雷达坐标系到世界坐标系的转换，所述树干中心建模包括：P1,P2为雷达返回的目标点，P0为目标树干的中心点，过P0向P1,P2的连线作垂线交于P3，过P3向P0与m轴的垂线做垂线交于点P4，设P1(x1,y1),P2(x2,y2)，将该雷达坐标转换为图像坐标点：P1,P2连线的斜率为 P0P3的斜率为 则所
将所述雷达坐标系的坐标点P0(x0,y0)转换到所述世界坐标系；
设所述目标树干的宽高在所述树干彩色图像中分别为w,h，则所述目标树干的真实宽高2R,H为：其中，d1为P1点返回的距离值，X，Y为P1点在物理成像平面的坐标，R为树干半径，H为树干高度。
2.如权利要求1所述的果园树干三维检测定位方法，其特征在于，步骤S101中，所述相机建模包括：将所述彩色摄像头的成像过程用小孔模型进行建模，设O-x-y-z为相机坐标系，z轴指向前方，x向右，y向下，O为相机的光心，目标点P在相机坐标系下的坐标设为PC＝[X,Y,Z]T，P'为目标成像点，设坐标为P'＝[X',Y',Z']T，假设物理成像平面到光心的距离为f，即为相机焦距，根据相似三角形关系可得：假设像素坐标系o-u-v固定于物理成像平面，原点o'位于图像左上角，u轴向右与x轴平行，v轴向下与y轴平行，两者在u轴缩放α倍，在v轴缩放β倍，原点平移了[cx,cy]T，则P'与像素坐标、PC与像素坐标的关系如下：其中，f单位为米，α,β单位为像素/米，fx,fy单位为像素，fx为αf的合并表示，fy为βf的合并表示；
其中KC2I为所述彩色摄像头的相机内参矩阵，其相机坐标系下目标坐标PC为世界坐标系下坐标通过位姿变换获得，相机的位姿通过旋转矩阵Rr和平移向量t表达，设目标点P在世界坐标系下坐标为Pw，则可得：其中，Kw2C为世界坐标系到相机坐标系的转换矩阵，KC2I为相机坐标系到图像坐标系的转换矩阵。
3.如权利要求2所述的果园树干三维检测定位方法，其特征在于，所述彩色摄像头的相机内参矩阵通过预先标定获得。
4.如权利要求2或3所述的果园树干三维检测定位方法，其特征在于，步骤S102中，所述雷达建模包括：通过旋转所述相机坐标系中RL和平移tL获得激光雷达坐标系，假设激光雷达返回目标点的数据为(d,θ)，则所述目标点在雷达坐标系m-n-q下的坐标为：所述雷达坐标系到所述世界坐标系的转换关系如下：
其中，KL2W为雷达坐标系到世界坐标系的转换矩阵，PL为所述雷达坐标系的P点坐标，PC为转换到所述相机坐标系的坐标，Pw为转换到所述世界坐标系的坐标。
5.如权利要求4所述的果园树干三维检测定位方法，其特征在于，步骤S103中，采用如下公式关联所述图像坐标系和雷达坐标系：PI＝KC2IKC2L-1PL。
6.一种果园树干三维检测装置，其特征在于，采用上述权利要求1-5中任意一项所述的果园树干三维检测定位方法进行果园中的树干检测和定位，所述果园树干三维检测装置包括彩色摄像头、激光雷达和上位机，所述上位机安装在所述果园除草机器人上，所述彩色摄像头固定于所述果园除草机器人正前方，且所述彩色摄像头水平朝向正前方；所述彩色摄像头与所述上位机通过USB口连接；所述激光雷达水平安装于所述果园除草机器人上方正中央且周围没有遮挡。
7.如权利要求6所述的果园树干三维检测装置，其特征在于，所述激光雷达为单线激光雷达，且距离地面60cm；所述彩色摄像头距离地面50cm。
一种果园树干三维检测定位方法及装置
技术领域
[0001] 本发明涉及农林种植维护保障技术领域，特别是一种融合图像与激光雷达技术的果园树干三维检测定位方法及装置。
背景技术
[0002] 果园智能化机械作业需要需明确果树位置，有效避开障碍物。现有果园环境传感手段有彩色摄像头、单线激光雷达等。彩色摄像头仅能反应图像彩色及纹理等信息，缺少距离信息；单线激光雷达虽能反应障碍物角度和距离信息，但缺少目标类别信息，且雷达数据易产生噪点和遗漏目标点，无法准确估计目标障碍物的大小信息。
发明内容
[0003] 本发明所要解决的技术问题是针对现有技术中单一传感器无法准确检测目标障碍的位置及大小信息的问题，提供一种果园树干三维检测定位方法及装置，为果园作业机械有效避障和智能规划提供技术保障。
[0004] 为了实现上述目的，本发明提供了一种果园树干三维检测定位方法，其中，包括如下步骤：
[0005] S100、通过图像坐标系、相机坐标系、雷达坐标系与世界坐标系之间的转换关系，建立果树的树干三维检测定位模型；
[0006] S200、通过彩色摄像头获取树干彩色图像，根据所述树干彩色图像获得检测目标的图像坐标信息；
[0007] S300、提供激光雷达循环获取两个目标点的扫描数据，根据所述激光雷达返回的所述目标点的距离和角度信息，得到所述目标点的雷达坐标信息；
[0008] S400、将所述雷达坐标信息转换为图像坐标信息，并结合所述检测目标的图像坐标信息，判断所述两个目标点是否来自同一目标树干；
[0009] S500、若所述两个目标点不是来自同一目标树干，则执行步骤S300-S400，重新寻找另外两个目标点重复确认，直至所有目标点检测完毕；
[0010] S600、若所述两个目标点来自同一目标树干，则基于所述两个目标点的雷达坐标信息计算所述目标树干的中心点坐标；
[0011] S700、根据所述中心点坐标及检测目标的图像坐标信息计算得到所述目标树干的树干区域。
[0012] 上述的果园树干三维检测定位方法，其中，步骤S100中，建立果树的树干三维检测定位模型包括如下步骤：
[0013] S101、相机建模，通过彩色摄像头获取树干彩色图像，并对所述树干彩色图像数据进行世界坐标系到图像坐标系的转换；
[0014] S102、雷达建模，利用激光雷达获取目标点的雷达扫描数据，并对所述雷达扫描数据进行雷达坐标系到世界坐标系的转换；
[0015] S103、关联所述图像坐标系和雷达坐标系；以及
[0016] S104、树干中心建模，对激光雷达数据进行雷达坐标系到世界坐标系的转换。
[0017] 上述的果园树干三维检测定位方法，其中，步骤S101中，所述相机建模包括：
[0018] 将所述彩色摄像头的成像过程用小孔模型进行建模，设O-x-y-z为相机坐标系，z轴指向前方，x向右，y向下，O为相机的光心，目标点P在相机坐标系下的坐标设为PC＝[X,Y,Z]T，P'为目标成像点，设坐标为P'＝[X',Y',Z']T，假设物理成像平面到光心的距离为f，即为相机焦距，根据相似三角形关系可得：
[0019]
[0020] 假设像素坐标系o-u-v固定于物理成像平面，原点o'位于图像左上角，u轴向右与x轴平行，v轴向下与y轴平行，两者在u轴缩放α倍，在v轴缩放β倍，原点平移了[cx,cy]T，则P'与像素坐标、PC与像素坐标的关系如下：
[0021]
[0022] 其中，f单位为米，α,β单位为像素/米，fx,fy单位为像素；
[0023] 将上式转换为齐次坐标为：
[0024]
[0025] 其中KC2I为所述彩色摄像头的相机内参矩阵，其相机坐标系下目标坐标PC为世界坐标系下坐标通过位姿变换获得，相机的位姿通过旋转矩阵Rr和平移向量t表达，设目标点P在世界坐标系下坐标为Pw，则可得：
[0026]
[0027] 其中，Kw2C为世界坐标系到相机坐标系的转换矩阵，KC2I为相机坐标系到图像坐标系的转换矩阵。
[0028] 上述的果园树干三维检测定位方法，其中，所述彩色摄像头的相机内参矩阵通过预先标定获得。
[0029] 上述的果园树干三维检测定位方法，其中，步骤S102中，所述雷达建模包括：
[0030] 通过旋转所述相机坐标系中RL和平移tL获得激光雷达坐标系，假设激光雷达返回目标点的数据为(d,θ)，则所述目标点在雷达坐标系m-n-q下的坐标为：
[0031]
[0032] 所述雷达坐标系到所述世界坐标系的转换关系如下：
[0033] Pw＝KL2wPL；
[0034] 其中，PL为所述雷达坐标系的P点坐标，PC为转换到所述相机坐标系的坐标，Pw为转换到所述世界坐标系的坐标。
[0035] 上述的果园树干三维检测定位方法，其中，步骤S103中，采用如下公式关联所述图像坐标系和雷达坐标系：
[0036] PI＝KC2IKC2L-1PL。
[0037] 上述的果园树干三维检测定位方法，其中，步骤S104中，所述树干中心建模包括：
[0038] P1,P2为雷达返回的目标点，P0为目标树干的中心点，过P0向P1,P2的连线作垂线交于P3，过P3向P0与m轴的垂线做垂线交于点P4，设P1(x1,y1),P2(x2,y2)，将该雷达坐标转换为图像坐标点：
[0039] P1,P2连线的斜率为 P0P3的斜率为则所述目标树干的中心点P0的所述雷达坐标系的坐标为：
[0040]
[0041] 其中，
[0042]
[0043]
[0044] 将所述雷达坐标系的坐标点P0(x0,y0)转换到所述世界坐标系。
[0045] 上述的果园树干三维检测定位方法，其中，采用如下公式计算所述目标树干的树干区域：
[0046] 设所述目标树干的宽高在所述树干彩色图像中分别为w,h，则所述目标树干的真实宽高2R,H为：
[0047]
[0048] 其中，d1为P1点返回的距离值，X，Y为P1点在物理成像平面的坐标。
[0049] 为了更好地实现上述目的，本发明还提供了一种果园树干三维检测装置，其中，采用上述的果园树干三维检测定位方法进行果园中的树干检测和定位，所述果园树干三维检测装置包括彩色摄像头、激光雷达和上位机，所述上位机安装在所述果园除草机器人上，所述彩色摄像头固定于所述果园除草机器人正前方，且所述彩色摄像头水平朝向正前方；所述彩色摄像头与所述上位机通过USB口连接；所述激光雷达水平安装于所述果园除草机器人上方正中央且周围没有遮挡。
[0050] 上述的果园树干三维检测装置，其中，所述激光雷达为单线激光雷达，且距离地面60cm；所述彩色摄像头距离地面50cm。
[0051] 本发明的技术效果在于：
[0052] 本发明利用彩色图像获取的2D树干检测结果，利用单线激光雷达扫描数据，返回目标障碍物距离和角度信息。对彩色摄像头预先标定获得相机的内参矩阵，通过图像坐标系、相机坐标系、雷达坐标系与世界坐标系之间的状态空间转换，进行几何关系的推算，建立果树树干三维检测定位模型，实现了果园中的树干检测和定位，并为自主导航提供了保障。
[0053] 以下结合附图和具体实施例对本发明进行详细描述，但不作为对本发明的限定。
附图说明
[0054] 图1为本发明一实施例的果树树干检测结果示意图；
[0055] 图2为本发明一实施例的相机建模示意图；
[0056] 图3为本发明一实施例的雷达建模示意图；
[0057] 图4为本发明一实施例的树干中心建模示意图；
[0058] 图5为本发明一实施例的果树树干3D检测结果示意图；
[0059] 图6为本发明一实施例的果树树干三维检测定位装置工作示意图；
[0060] 图7为本发明一实施例的果树树干3D检测定位工作流程图。
具体实施方式
[0061] 下面结合附图对本发明的结构原理和工作原理作具体的描述：
[0062] 本发明基于机器视觉技术、激光雷达技术，利用摄像头获取视场内图像信息，激光雷达返回目标障碍物距离和角度信息。对彩色摄像头获取的图像进行树干检测，并确定其位置。再利用预先标定的相机内参矩阵，通过图像坐标系、相机坐标系、雷达坐标系与世界坐标系之间的状态空间转换，进行坐标的统一运算。利用空间几何关系推导出检测目标的三维检测位置。
[0063] 本发明的果园树干三维检测装置，包括彩色摄像头、激光雷达和上位机，所述上位机安装在所述果园除草机器人上，所述彩色摄像头固定于所述果园除草机器人正前方，且所述彩色摄像头水平朝向正前方；所述彩色摄像头与所述上位机通过USB口连接；所述激光雷达水平安装于所述果园除草机器人上方正中央且周围没有遮挡。其中，所述激光雷达为单线激光雷达，且距离地面60cm；所述彩色摄像头距离地面50cm。
[0064] 该果园树干三维检测装置采用如下果园树干三维检测定位方法进行果园中的树干检测和定位，具体可包括如下步骤：
[0065] 步骤S100、通过图像坐标系、相机坐标系、雷达坐标系与世界坐标系之间的转换关系，建立果树的树干三维检测定位模型；
[0066] 步骤S200、通过彩色摄像头获取树干彩色图像，根据所述树干彩色图像获得检测目标的图像坐标信息；具体可将摄像头采集的图像数据进行分析，得到果树树干检测结果(树干在图像坐标系中的位置)，如图1所示，图中矩形框为检测结果；
[0067] 步骤S300、提供激光雷达循环获取两个目标点的扫描数据，根据所述激光雷达返回的所述目标点的距离和角度信息，得到所述目标点的雷达坐标信息；
[0068] 步骤S400、将所述雷达坐标信息转换为图像坐标信息，并结合所述检测目标的图像坐标信息，判断所述两个目标点是否来自同一目标树干；
[0069] 步骤S500、若所述两个目标点不是来自同一目标树干，则执行步骤S300-S400，，重新寻找另外两个目标点重复确认，直至所有目标点检测完毕；
[0070] 步骤S600、若所述两个目标点来自同一目标树干，则基于所述两个目标点的雷达坐标信息计算所述目标树干的中心点坐标；
[0071] 步骤S700、根据所述中心点坐标及检测目标的图像坐标信息计算得到所述目标树干的树干区域。
[0072] 其中，步骤S100中，建立果树的树干三维检测定位模型包括如下步骤：
[0073] 步骤S101、相机建模，通过彩色摄像头获取树干彩色图像，并对所述树干彩色图像数据进行世界坐标系到图像坐标系的转换；
[0074] 其中，所述相机建模进一步包括：
[0075] 将所述彩色摄像头的成像过程用小孔模型进行建模，如图2所示，设O-x-y-z为相机坐标系，z轴指向前方，x向右，y向下，O为相机的光心，目标点P在相机坐标系下的坐标设为PC＝[X,Y,Z]T，P'为目标成像点，设坐标为P'＝[X',Y',Z']T，假设物理成像平面到光心的距离为f，即为相机焦距，根据相似三角形关系可得：
[0076]
[0077] 其中负号表示成像为倒像。可以简化模型为：
[0078]
[0079] 由于图像中的像素点是成像平面经过采样和量化的，故假设像素坐标系o-u-v固定于物理成像平面，原点o'位于图像左上角，u轴向右与x轴平行，v轴向下与y轴平行，两者在u轴缩放α倍，在v轴缩放β倍，原点平移了[cx,cy]T，则P'与像素坐标、PC与像素坐标的关系如下：
[0080]
[0081] 其中，f单位为米，α,β单位为像素/米，fx,fy单位为像素；
[0082] 将上式转换为齐次坐标为：
[0083]
[0084] 其中KC2I为所述彩色摄像头的相机内参矩阵，所述彩色摄像头的相机内参矩阵通过预先标定获得，由于相机跟随机器人不断运动，故其相机坐标系下目标坐标PC为世界坐标系下坐标通过位姿变换获得，相机的位姿可通过旋转矩阵Rr和平移向量t表达，设目标点P在世界坐标系下坐标为Pw，则可得：
[0085]
[0086] 其中，Kw2C为世界坐标系到相机坐标系的转换矩阵，KC2I为相机内参矩阵，即相机坐标系到图像坐标系的转换矩阵。
[0087] 步骤S102、雷达建模，利用激光雷达获取目标点的雷达扫描数据，并对所述雷达扫描数据进行雷达坐标系到世界坐标系的转换；
[0088] 其中，所述雷达建模进一步包括：
[0089] 根据激光雷达的安装位置，激光雷达坐标系是相机坐标系通过旋转RL和平移tL后获得。假设激光雷达返回数据为(d,θ)，雷达坐标示意图如图3所示，则该点在雷达坐标系m-n-q下的坐标为：
[0090]
[0091] 可整理为：
[0092]
[0093] 同理可得所述雷达坐标系到所述世界坐标系的转换关系如下：
[0094] Pw＝KL2wPL (1.8)；
[0095] 其中，PL为所述雷达坐标系的P点坐标，PC为转换到所述相机坐标系的坐标，Pw为转换到所述世界坐标系的坐标。
[0096] 步骤S103、关联所述图像坐标系和雷达坐标系，激光雷达扫描的过程中会返回若干个目标点。设雷达坐标系P点坐标为PL，通过式(1.7)可转换到相机坐标系为PC，再利用公式(1.4)转为图像坐标系，即可采用如下公式关联所述图像坐标系和雷达坐标系：
[0097] PI＝KC2IKC2L-1PL (1.9)；以及
[0098] 步骤S104、树干中心建模，对激光雷达数据进行雷达坐标系到世界坐标系的转换。
[0099] 其中，融合几何空间变换与果树2D检测识别同目标树干激光雷达点，所述树干中心建模进一步包括：
[0100] P1,P2为雷达返回的目标点，将树干近似为圆形，半径为R其在雷达坐标系下目标扫描模型如图4所示，P0为目标树干的中心点，过P0向P1,P2的连线作垂线交于P3，过P3向P0与m轴的垂线做垂线交于点P4，设P1(x1,y1),P2(x2,y2)，通过公式(1.9)可将该雷达坐标转换为图像坐标点，并与图1的检测结果结合，判断该两点是否来自同一个目标树干，若是同一个目标树干，则进行下面计算；若不是，重新寻找另外两个点重复确认。
[0101] 将该雷达坐标转换为图像坐标点：
[0102] 由图4易得 P1,P2连线的斜率为 由于P0P3⊥P1P2，故P0P3的斜率为 则所述目标树干的中心点P0的所述雷达坐标系的坐标为：
[0103]
[0104] 其中，
[0105]
[0106]
[0107] 利用公式(1.8)可将所述雷达坐标系的坐标点P0(x0,y0)转换到所述世界坐标系。
[0108] 步骤S700中，可采用如下公式计算所述目标树干的树干区域：
[0109] 基于图1的检测结果可知，所述目标树干的宽高在所述树干彩色图像中分别为w,h，则根据公式(1.2)的映射关系，可获得所述目标树干的真实宽高2R,H为：
[0110]
[0111] 其中，d1为P1点返回的距离值，x'1,y'1为P1点在物理成像平面的坐标。调整目标中心世界坐标系下的高度 最终3D检测结果如图5所示。
[0112] 参见图7，图7为本发明一实施例的果树树干3D检测定位工作流程图。其中，果树树干三维检测定位装置的工作装置及环境如图6所示。本发明一实施例的具体工作过程如下：
[0113] 第1步、开机：启动彩色摄像头、激光雷达和上位机，运行目标检测定位系统；
[0114] 第2步、摄像头初始化：打开摄像头并获取第一帧图像，若获取成功，则进入第3步、若打开摄像头失败，则提示用户检查摄像头连接装置，重新初始化；
[0115] 第3步、激光雷达初始化：检查雷达通信，若初始化成功则进入第4步，若失败，则提示用户检查雷达连接装置，重新初始化；
[0116] 第4步、获取彩色图像和雷达检测数据；
[0117] 第5步、树干的识别与定位：将彩色图像输入检测模型，获得目标的2D检测结果；循环获取一对目标点，根据坐标系间的转换关系和目标2D检测结果，判断该对雷达点对是否来自同一目标，若来自同一目标，则计算其目标中心并回算目标3D检测结果，最后进行3D框中心高度校正；若不是来自同一目标，则重新获取雷达点对，直至所有点检查完毕；
[0118] 第6步、遍历所有目标判断：判断视场内所有检测目标是否都进行了3D检测框计算，若是则进入第7步；若否，则返回第4步，重新获取点对；
[0119] 第7步、结束检测判断：判断是否检测完毕，若检测完毕，则结束程序，退出系统；若没有检测完毕，则返回第4步，继续获取图像数据和雷达数据；
[0120] 第8步、关机，结束系统运行。
[0121] 本发明用于果园除草机器人的果园树干三维检测装置及方法，利用彩色图像获取的2D树干检测结果，利用单线激光雷达扫描数据，返回目标障碍物距离和角度信息。对彩色摄像头预先标定获得相机的内参矩阵，通过图像坐标系、相机坐标系、雷达坐标系与世界坐标系之间的状态空间转换，进行几何关系的推算。建立果树树干三维检测定位模型，实现果园中的树干检测和定位，并为自主导航提供保障。
[0122] 当然，本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。
