CN115790622A - 面向果园车辆导航路线规划的走廊地图构建及导航点生成方法 - Google Patents

Abstract

本发明提出一种面向果园车辆导航路线规划的走廊地图构建及导航点生成方法，包括：树干检测、地图倾斜校正、果树栽植行距求取、走廊地图构建以及导航点生成；本发明基于二维占据栅格地图，将其转化为走廊地图，使得起点到终点只有一条通路，为果园车辆梭式导航路线规划提供环境模型和路径端点。

Description

面向果园车辆导航路线规划的走廊地图构建及导航点生成 方法

技术领域

本发明涉及果园车辆自主导航技术领域，更具体地，涉及面向果园车辆导航路线规划的走廊地图构建及导航点生成方法。

背景技术

果园车辆自主导航技术是智能果园装备基础技术之一，其中，导航路线规划是实现果园车辆自主导航的基础，合理的导航路线规划可以减少果园车辆的总作业路径、降低多余覆盖率、提升作业效率。

对于果树株行排列种植的宜机化标准果园，通常采用车辆梭式行走策略实现全覆盖路径规划。

一种方法是首先采集果园整体环境信息，得到二维占据栅格地图，然后将其转化为走廊地图，使得起点到终点只有一条通路，最后采用全局路径规划算法生成从起点到终点的导航路线。这种方法的难点在于两个方面：

其一：原始果园地图往往存在倾斜，为构建果园走廊地图带来困难。

其二：由于果园走廊地图有较多的换行转弯，直接规划从起点到终点的导航路线较为复杂，并且不能保证生成的导航路线在道路中心线附近。

发明内容

本发明的目的是针对上述问题，本发明提出果园走廊地图构建及导航点生成方法，根据果园二维占据栅格地图构建果园走廊地图，并在换行转弯处生成中间导航点，为果园车辆梭式导航路线规划提供环境模型和路径端点。

本发明的技术方案是：

本发明提供一种面向果园车辆导航路线规划的走廊地图构建及导航点生成方法，该方法包括以下步骤：

S1、树干检测：对果园二维占据栅格地图进行树干检测，获取树干位置坐标；

S2、地图倾斜校正：基于树干位置坐标获取地图的倾斜角，对地图进行倾斜校正；

S3、果树栽植行距求取：基于倾斜校正后的地图获取果树栽植行距，并求取果园外接矩形；

S4、走廊地图构建：基于果园外接矩形向四周扩大行距构建四周墙壁，在树行位置构建内部墙壁；

S5、导航点生成：基于墙壁坐标生成走廊导航点和转弯导航点。

进一步地，所述的S1具体为：

S1.1地图二值化：对果园二维占据栅格地图进行二值化处理，白色像素表示该区域被树干占据，黑色像素表示该区域没有树干，其中，树干内部具有黑色区域；

S1.2孔洞填充：采用孔洞填充算法，将二值图像中树干内部的黑色区域填充为白色；

S1.3树干定位：采用连通域分析算法查找孔洞填充后二值图像中的所有连通域，计算每个连通域的质心坐标，作为树干位置坐标。

进一步地，所述的S2具体为：

S2.1倾斜角求取：基于所有树干位置坐标，获取协方差矩阵中两个特征向量与图像坐标系下横轴的夹角，取绝对值较小的夹角作为地图倾斜角θ；

S2.2倾斜校正：将地图绕其中心点逆时针旋转θ，坐标为(x，y)的像素在旋转后新的坐标为(x′，y′)，其中：(x₀，y₀)是地图中心坐标；

进一步地，所述的S2.1倾斜角求取具体为：

计算所有树干位置坐标的协方差矩阵C；

其中：N是树干个数，p_i是第i个树干的位置向量，x_i、y_i分别是第i个树干的横坐标和纵坐标，m是所有树干的位置均值向量；

对于果树株行排列种植的宜机化标准果园，C的两个特征向量指向树行、树列的排列方向，分别计算前述两个特征向量与图像坐标系横轴的夹角，逆时针为正，顺时针为负，取绝对值较小的夹角作为地图倾斜角θ。

进一步地，所述的S3具体为：

S3.1地图投影：将校正后的地图向横轴方向作垂直投影，得到一维灰度图像，第c个像素的灰度值代表地图第c列白色像素个数。将地图向纵轴方向作水平投影，得到一维灰度图像，第r个像素的灰度值代表地图第r行白色像素个数；

S3.2果园外接矩形求取：提取垂直投影图像上灰度值非0的像素点，记录像素点的坐标范围[x_min,x_max]，提取水平投影图像上灰度值非0的像素点，记录像素点的坐标范围[y_min,y_max]，则果园外接矩形的左上角位置坐标为(x_min,y_min)，宽w＝x_max-x_min，高h＝y_max-y_min；

S3.3树行检测：从垂直投影图像中获取由非零像素组成的线段，线段个数代表果园包含的树行个数N₁，i₁表示树行编号，求取线段中心坐标

S3.4行距求取：根据垂直投影图像中相邻线段的距离求取行距d、：

进一步地，所述的S4具体为：

S4.1四周墙壁构建：令果园外接矩形向四周扩大行距d，构建走廊地图的四周墙壁，用矩形表示，矩形左上角位置坐标为(x_min-d,y_min-d)，宽w′＝w+2d，高h′＝h+2d。

S4.2内部墙壁构建：在树行位置构建墙壁，用线段表示；

树行编号i₁为奇数时，将墙壁与上方墙壁连接，对应线段起点为

终点为

树行编号i₁为偶数时，将墙壁与下方墙壁连接，对应线段起点为

终点为

进一步地，所述的S5具体为：

计算走廊上方导航点坐标和下方导航点坐标；

将走廊末端坐标向未占据区域延长

处作为末端转弯处，生成转弯导航点。

本发明的有益效果：

本发明的方法是基于二维占据栅格地图，将其转化为走廊地图，使得起点到终点只有一条通路，为果园车辆梭式导航路线规划提供环境模型和导航点。

本发明从果园二维占据栅格地图中自动识别树行，在树行所在直线上建立占据栅格，构建果园走廊地图；由于果园走廊地图有较多的换行转弯，为提高路径规划算法的路径搜索效率，在转弯处插入转弯导航点，将规划起点到终点的全局路径问题转化为规划导航点间的局部路径问题。

本发明的其它特征和优点将在随后具体实施方式部分予以详细说明。

附图说明

通过结合附图对本发明示例性实施方式进行更详细的描述，本发明的上述以及其它目的、特征和优势将变得更加明显，其中，在本发明示例性实施方式中，相同的参考标号通常代表相同部件。

图1示出了实施例中机器人仿真平台Gazebo仿真果园环境和果园车辆的示意图。

图2示出了实施例中的二维占据栅格地图。

图3示出了实施例中二维占据栅格地图的二值化结果示意图。

图4示出了实施例中二值图像孔洞填充结果。

图5示出了实施例中树干定位结果示意图。

图6示出了实施例中树行、树列方向以及倾斜角示意图。

图7示出了实施例中倾斜校正、垂直投影和水平投影结果示意图。

图8示出了实施例中生成的走廊地图与导航点示意图。

具体实施方式

下面将参照附图更详细地描述本发明的优选实施方式。虽然附图中显示了本发明的优选实施方式，然而应该理解，可以以各种形式实现本发明而不应被这里阐述的实施方式所限制。

本发明提供一种面向果园车辆导航路线规划的走廊地图构建及导航点生成方法，该方法包括以下步骤：

S1、树干检测：对果园二维占据栅格地图进行树干检测，获取树干位置坐标，具体为：S1.1地图二值化：对果园二维占据栅格地图进行二值化处理，白色像素表示该区域被树干占据，黑色像素表示该区域没有树干，其中，树干内部具有黑色区域；

S1.2孔洞填充：采用孔洞填充算法，将二值图像中树干内部的黑色区域填充为白色；

S1.3树干定位：采用连通域分析算法查找孔洞填充后二值图像中的所有连通域，计算每个连通域的质心坐标，作为树干位置坐标。

S2、地图倾斜校正：基于树干位置坐标获取地图的倾斜角，对地图进行倾斜校正，具体为：

S2.1倾斜角求取：基于所有树干位置坐标，获取协方差矩阵中两个特征向量与图像坐标系下横轴的夹角，取绝对值较小的夹角作为地图倾斜角θ；

首先，计算所有树干位置坐标的协方差矩阵C；

其中：N是树干个数，p_i是第i个树干的位置向量，x_i、y_i分别是第i个树干的横坐标和纵坐标，m是所有树干的位置均值向量；

其次，对于果树株行排列种植的宜机化标准果园，C的两个特征向量指向树行、树列的排列方向，分别计算前述两个特征向量与图像坐标系横轴的夹角，逆时针为正，顺时针为负，取绝对值较小的夹角作为地图倾斜角θ。

S2.2倾斜校正：将地图绕其中心点逆时针旋转θ，坐标为(x，y)的像素在旋转后新的坐标为(x′，y′)，其中：(x₀，y₀)是地图中心坐标；

S3、果树栽植行距求取：基于倾斜校正后的地图获取果树栽植行距，并求取果园外接矩形，具体为：

S3.1地图投影：将校正后的地图向横轴方向作垂直投影，得到一维灰度图像，第c个像素的灰度值代表地图第c列白色像素个数。将地图向纵轴方向作水平投影，得到一维灰度图像，第r个像素的灰度值代表地图第r行白色像素个数；

S3.2果园外接矩形求取：提取垂直投影图像上灰度值非0的像素点，记录像素点的坐标范围[x_min，x_max]，提取水平投影图像上灰度值非0的像素点，记录像素点的坐标范围[y_min，y_max]，则果园外接矩形的左上角位置坐标为(x_min，y_min)，宽w＝x_max-x_min，高h＝y_max-y_min；

S3.3树行检测：从垂直投影图像中获取由非零像素组成的线段，线段个数代表果园包含的树行个数N₁，i₁表示树行编号，求取线段中心坐标

S3.4行距求取：根据垂直投影图像中相邻线段的距离求取行距d：

S4、走廊地图构建：基于果园外接矩形向四周扩大行距构建四周墙壁，在树行位置构建内部墙壁，具体为：

S4.1四周墙壁构建：令果园外接矩形向四周扩大行距d，构建走廊地图的四周墙壁，用矩形表示，矩形左上角位置坐标为(x_min-d，y_min-d)，宽w′＝w+2d，高h′＝h+2d。

S4.2内部墙壁构建：在树行位置构建墙壁，用线段表示；

树行编号i₁为奇数时，将墙壁与上方墙壁连接，对应线段起点为

终点为

树行编号i₁为偶数时，将墙壁与下方墙壁连接，对应线段起点为

终点为

S5、导航点生成：基于墙壁坐标生成走廊导航点和转弯导航点，具体为：

计算走廊上方导航点坐标和下方导航点坐标；

将走廊末端坐标向未占据区域延长

处作为末端转弯处，生成转弯导航点。

具体实施时：

使用机器人仿真平台Gazebo仿真果园环境和果园车辆，如图1所示，包含4个树行，每行有5株果树，行距1.6m，株距0.8m，树干直径12cm，果园车辆选用Husky机器人，传感器选用Rplidar A2二维激光雷达。基于ROS机器人软件开发平台，采用GMapping算法构建果园地图，得到的二维占据栅格地图如图2所示，一个栅格对应一块大小为2cm×2cm的正方形区域，白色表示该区域没有障碍物，黑色表示该区域被障碍物占据，灰色表示未知区域。

采用Matlab R2018a实现本发明方法。图3为二值化结果，白色像素表示该区域被树干占据，黑色像素表示该区域没有树干。图4为二值图像孔洞填充结果，树干内部区域被填充为白色。图5为树干定位结果，图中用红色加号标记出了树干的位置。图6为倾斜角求取结果，蓝色特征向量与横轴的夹角为倾斜角，为-3.4774°。图7为倾斜校正、垂直投影和水平投影结果，垂直投影图像中非零像素灰度值代表对应列的白色像素个数，水平投影图像中非零像素灰度值代表对应行的白色像素个数，红色框为果园外接矩形。图8为最终生成的走廊地图与导航点。

以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。

Claims (7)

1.一种面向果园车辆导航路径规划的走廊地图构建及导航点生成方法，其特征在于，该方法包括以下步骤：

S1、树干检测：对果园二维占据栅格地图进行树干检测，获取树干位置坐标；

S2、地图倾斜校正：基于树干位置坐标获取地图的倾斜角，对地图进行倾斜校正；

S3、果树栽植行距求取：基于倾斜校正后的地图获取果树栽植行距，并求取果园外接矩形；

S4、走廊地图构建：基于果园外接矩形向四周扩大行距构建四周墙壁，在树行位置构建内部墙壁；

S5、导航点生成：基于墙壁坐标生成走廊导航点和转弯导航点。

2.根据权利要求1所述的面向果园车辆导航路线规划的走廊地图构建及导航点生成方法，其特征在于，所述的S1具体为：

S1.1地图二值化：对果园二维占据栅格地图进行二值化处理，白色像素表示该区域被树干占据，黑色像素表示该区域没有树干，其中，树干内部具有黑色区域；

S1.2孔洞填充：采用孔洞填充算法，将二值图像中树干内部的黑色区域填充为白色；S1.3树干定位：采用连通域分析算法查找孔洞填充后二值图像中的所有连通域，计算每个连通域的质心坐标，作为树干位置坐标。

3.根据权利要求1所述的面向果园车辆导航路线规划的走廊地图构建及导航点生成方法，其特征在于，所述的S2具体为：

S2.1倾斜角求取：基于所有树干位置坐标，获取协方差矩阵中两个特征向量与图像坐标系下横轴的夹角，取绝对值较小的夹角作为地图倾斜角θ；

S2.2倾斜校正：将地图绕其中心点逆时针旋转θ，坐标为(x,y)的像素在旋转后新的坐标为(x′,y′)，其中：(x₀,y₀)是地图中心坐标；

4.根据权利要求3所述的面向果园车辆导航路线规划的走廊地图构建及导航点生成方法，其特征在于，所述的S2.1倾斜角求取具体为：

计算所有树干位置坐标的协方差矩阵C；

其中：N是树干个数，p_i是第i个树干的位置向量，x_i、y_i分别是第i个树干的横坐标和纵坐标，m是所有树干的位置均值向量；

对于果树株行排列种植的宜机化标准果园，C的两个特征向量指向树行、树列的排列方向，分别计算前述两个特征向量与图像坐标系横轴的夹角，逆时针为正，顺时针为负，取绝对值较小的夹角作为地图倾斜角θ。

5.根据权利要求1所述的面向果园车辆导航路线规划的走廊地图构建及导航点生成方法，其特征在于，所述的S3具体为：

S3.1地图投影：将校正后的地图向横轴方向作垂直投影，得到一维灰度图像，第c个像素的灰度值代表地图第c列白色像素个数。将地图向纵轴方向作水平投影，得到一维灰度图像，第r个像素的灰度值代表地图第r行白色像素个数；

S3.2果园外接矩形求取：提取垂直投影图像上灰度值非0的像素点，记录像素点的坐标范围[x_min，x_max]，提取水平投影图像上灰度值非0的像素点，记录像素点的坐标范围[y_min，y_max]，则果园外接矩形的左上角位置坐标为(x_min，y_min)，宽w＝x_max-x_min，高h＝y_max-y_min；

S3.3树行检测：从垂直投影图像中获取由非零像素组成的线段，线段个数代表果园包含的树行个数N₁，i₁表示树行编号，求取线段中心坐标

S3.4行距求取：根据垂直投影图像中相邻线段的距离求取行距d：

6.根据权利要求1所述的面向果园车辆导航路线规划的走廊地图构建及导航点生成方法，其特征在于，所述的S4具体为：

S4.1四周墙壁构建：令果园外接矩形向四周扩大行距d，构建走廊地图的四周墙壁，用矩形表示，矩形左上角位置坐标为(x_min-d，y_min-d)，宽w′＝w+2d，高h′＝h+2d。S4.2内部墙壁构建：在树行位置构建墙壁，用线段表示；

树行编号i₁为奇数时，将墙壁与上方墙壁连接，对应线段起点为

终点为

树行编号i₁为偶数时，将墙壁与下方墙壁连接，对应线段起点为

终点为

7.根据权利要求5所述的面向果园车辆导航路线规划的走廊地图构建及导航点生成方法，其特征在于，所述的S5具体为：

计算走廊上方导航点坐标和下方导航点坐标；

将走廊末端坐标向未占据区域延长

处作为末端转弯处，生成转弯导航点。

Images