CN116301001A - 一种用于果园水果采摘的无人机实时避障系统及方法 - Google Patents
一种用于果园水果采摘的无人机实时避障系统及方法 Download PDFInfo
- Publication number
- CN116301001A CN116301001A CN202310228813.5A CN202310228813A CN116301001A CN 116301001 A CN116301001 A CN 116301001A CN 202310228813 A CN202310228813 A CN 202310228813A CN 116301001 A CN116301001 A CN 116301001A
- Authority
- CN
- China
- Prior art keywords
- point
- path
- obstacle avoidance
- unmanned aerial
- aerial vehicle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 60
- 235000013399 edible fruits Nutrition 0.000 title claims abstract description 37
- 239000002420 orchard Substances 0.000 title claims abstract description 25
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 22
- 238000005070 sampling Methods 0.000 claims description 27
- 230000008569 process Effects 0.000 claims description 10
- 238000009825 accumulation Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000012952 Resampling Methods 0.000 claims description 6
- 238000004590 computer program Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000013461 design Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 230000003068 static effect Effects 0.000 claims description 3
- 230000008520 organization Effects 0.000 description 3
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000008094 contradictory effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种用于果园水果采摘无人机的实时避障系统及方法，该方法包括：根据无人机初始位置和目标位置规划第一路径；无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行；若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为避障路径；若避障路径不可执行，则无人机停止行进，执行悬停避障措施。本发明能够及时检测飞行时的障碍物，准确有效避开障碍物完成自主飞行。
Description
技术领域
本发明涉及无人机技术领域，特别是涉及一种用于果园水果采摘无人机的实时避障系统及方法。
背景技术
随着无人机的发展，无人机的应用领域越来越广泛。现如今，由于农用无人机技术的不断发展，果园用无人机已进入人们的视野。目前果园里水果采摘部分仍依靠人力完成，费时费力。而无人机则可以轻松飞到水果面前，通过在无人机下方安装采摘装置，从而实现水果采摘工作。
目前市面上针对水果采摘的无人机装备目前还处于研发阶段，尚未拥有专门针对无人机水果采摘所进行的作业方法。根据水果采摘无人机的作业特性，无人机需要在果树冠层间进行采摘作业。相较于其他种类无人机的工作环境，水果采摘无人机的果树冠层间环境具有障碍物形状不规则、狭窄空间多、障碍物尺寸较小、动态障碍物多等特点。由于其环境特殊性，这对于无人机平台的避障有了更高的要求：需要检测到更小尺寸的障碍物，比如树枝枝条、电线等；需要及时检测动态障碍物等。
所以，如何在果树冠层空域中，生成一条安全、无碰撞的可用于水果采摘无人机执行的飞行路径，并在执行过程中，有效检测并避开障碍物，完成无人机平台自主飞行，是一个急需解决的问题。
发明内容
本发明的目的是提供一种用于果园水果采摘无人机的实时避障系统及方法，该方法能够及时检测飞行时的障碍物，准确有效避开障碍物完成自主飞行。
一种用于果园水果采摘无人机的实时避障方法，包括：
根据无人机初始位置和目标位置规划第一路径；
无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行；
若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为避障路径；
若避障路径不可执行，则无人机停止行进，执行悬停避障措施。
若避障路径不可执行，则无人机停止行进，执行悬停避障措施之后，还包括利用随机采样路径规划算法规划无人机当前位置到目标位置的第三避障路径，将第一路径替换为第三避障路径，具体为：
构造搜索树并搜索新的扩展点，已经搜索的点作为父节点；
确认当前无人机平台所在位置(xnow，ynow，znow)作为起始点，设置终点为目标作业点坐标(xgoal，ygoal，zgoal)，在当前局部地图上设置起始点(xnow，ynow，znow)初始化为父节点，在当前局部地图状态环境内进行随机采样，生成随机点集Grandom；
找出随机点集Grandom中，距离当前父节点最近的采样点(xnearest，ynearest，znearest)，沿当前父节点到最近采样点(xnearest，ynearest，znearest)的方向，以固定步长K迈出，确定候选子节点nextpoint(xnext，ynext，znext)；
判断候选子节点nextpoint(xnext，ynext，znext)是否与障碍物重合、干涉或与障碍物最近距离小于预设阈值；
若是，则返回重新采样；
若否，则将当前候选子节点设为子节点，添加到搜索树中，判断子节点是否到达目标作业点(xgoal，ygoal，zgoal)附近；
若是，将所有搜索树中节点确认为第三避障路径；
若否，则返回重新采样路径点，直至子节点到达目标作业点(xgoal，ygoal，zgoal)附近；
若采样过程中，规划时间超过预设阈值I，则认为无法到达指定作业目标点，所述预设阈值I指随机采样路径规划算法规划路径所需最大时间阈值，超出该阈值则认为无法构建出所述可执行路径，则无人机停止行进，执行悬停避障措施。
根据无人机初始位置和目标位置规划第一路径包括：
根据目标位置坐标(xgoal，ygoal，zgoal)和无人机初始位置坐标(x0，y0，z0)，以及预设步长K，生成定步数的直线路径点集G：
根据无人机初始位置和目标位置规划第一路径之后，还包括根据第一路径，构建第一路径行进方向的时间累积局部地图，具体为：
通过激光雷达获取的3D点云坐标(xp，yp，zp)，通过无人机当前位置(x0，y0，z0)和姿态(θ，ψ，j)，得到旋转矩阵RZXY，将激光雷达获取的3D点云坐标(xp，yp，zp)转换到世界坐标系下(xw，yw，zw)：
[xw yw zw]T＝RZXY[xp yp zp]T+[x0 y0 z0]
在间隔时间0到T内，不断采集并存储激光雷达采集的点云信息，将其作为无人机平台的局部地图，采用k维二叉树结构来动态组织数据；
每个K维二叉树结构包含两个域，一个存储整个结构内包含的点云总数N，另一个指向根节点；
一个根节点包含三个域，一个存储激光点具体坐标(xw，yw，zw)，另外两个域分别指向下一个子节点；
每一个子节点也包含三个点，一个存储激光点具体坐标(xw，yw，zw)，另外两个域分别指向下一个孙节点；
向k维二叉树结构插入激光点的方式为：更新点云总数N，构建一个新的节点，存储激光点具体坐标(xw，yw，zw)，根据维度划分方式，找到最终对应区间插入该节点，照固定时间间隔T进行构建并覆盖的局部。
根据第一路径，构建第一路径行进方向的时间累积局部地图之后，还包括组织两个k维二叉树结构来做为水果采摘无人机平台的局部地图，具体为：
设置变量点云帧数M和k维二叉树棵树Q，M指间隔时间T内，激光雷达总共采集点云帧数，Q为k维二叉树结构个数；
当激光雷达接收到一帧点云数据时，将点云数据插入第一棵k维二叉树；
当激光雷达接收到帧数>M时，将新接收点云数据插入到第二棵k维二叉树中，第一棵二叉树保持静止；
当激光雷达接收到帧数>M*2时，激光雷达接收到帧数重置归零，同时将第一棵k维二叉树清空，将新接收点云数据组织到第一棵k维二叉树中，此时第二棵k维二叉树保持静止；
在任意时刻保持两棵k维两叉树结构，一个静态，一个处于动态加入激光点数据的过程，两棵k维两叉树结构组织的激光点云作为水果采摘无人机平台的时间累积局部地图。
无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行包括：
对第一路径中的路径点集中的每一个路径点(x，y，z)，在局部地图中查找是否存在此路径点(x，y，z)，以及距离路径点最近的激光点(xnearest，ynearest，znearest)，距离为dnearest，计算方式如下：
若路径点(x，y，z)满足以下条件：存在于当前局部地图中，dnearest＜Dsafe
则认为所跟踪路径上是存在不能通行部分；
若路径点不满足条件，则无人机按照第一路径通行。
若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的第一避障路径，将第一路径替换为第一避障路径包括：
确认当前无人机平台所在位置(xnow，ynow，znow)作为起始点，设置终点为目标作业点坐标(xgoal，ygoal，zgoal)，在当前局部地图上设置起始点(xnow，ynow，znow)为当前扩展点，当前扩展点指当前需要进行领域扩展的坐标航点；
从当前扩展点的领域点开始搜索，基于当前局部地图上的邻域点点集：
其中K为动态步长，根据当前扩展点附近的点云密度确定步长
N为当前局部地图中点云量总数，Nnear为局部地图中，与当前扩展点欧氏距离小于预设阈值的点云量数量，Lmax为单次最大步长，预设阈值为狭窄通道中安全距离阈值Dsafe的两倍；
判断点集中的每一个领域点是否与障碍物重合、干涉或与障碍物最近距离小于预设阈值Dsafe；
若是，则将所有扩展点生成为第一避障路径替换第一路径；
若否，则无人机停止行进，执行悬停避障措施。
若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为第一避障路径之后，还包括计算A*算法规划路径的备选扩展点，构建第二避障路径具体为：
将没有与障碍物重合、干涉或与障碍物最近距离小于预设阈值的邻域点(xnear，ynear，znear)作为备选扩展点；
计算备选扩展点的代价函数F(x)，由备选扩展点与目标作业点的欧式距离H(x)和出发后的移动距离G(x)和导向因子R(x)，导向因子为引导无人机采摘平台规划路径远离高硬度障碍物，计算方式如下，其中n为扩展次数，由人为设定；
导向因子R(x)设计规则如下：
N为当前局部地图中点云量总数，Nnear为局部地图中，与当前扩展点欧氏距离小于预设阈值的点云量数量，预设阈值为狭窄通道中安全距离阈值Dsafe的两倍；
Nhard为当前局部地图中，距备选扩展点最近的激光点p的阈值邻域点集Gp中点云数量，阈值为人为设定，k为影响比例因子，由人为设定；
σθ为距备选扩展点最近的激光点p的阈值领域点集Gp中每一个领域点的夹角θ的方差，具体计算方式如下：
根据备选扩展点最近的激光点p和阈值邻域点集Gp；
以p为角点，连接领域点和点p，形成一系列夹角：θ＝{θ1，θ2，θ3，...，θn}，其中n＝Nhard-1；
计算夹角的方差
将当前扩展点设置已扩展标记，在后续扩展过程中不再作为备选扩展点，以满足欧氏距离和移动距离之和最小的备选扩展点作为下一个待扩展点nextpoint
nextpoint＝arg min(H(x)+G(x)+R(x))
判断待扩展点是否到达目标作业点(xgoal，ygoal，zgoal)附近；
若是，将所有待扩展点生成一条可执行路径，该可执行路径为第二避障路径，将第二避障路径作为避障规划路径替换第一路径；
若否，则令待扩展点nextpoint作为当前扩展点(xnow，ynow，znow)，并返回搜索备选扩展点，直至待扩展点到达目标作业点(xgoal，ygoal，zgoal)或附近；
若扩展搜索中，无备选扩展点，则判断无法规划处可执行路径，则无人机停止行进，执行悬停避障措施。
一种用于果园水果采摘无人机的实时避障系统，包括：
第一路径规划模块，用于根据无人机初始位置和目标位置规划第一路径；
数据处理模块，用于无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行；
避障路径规划模块，用于若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为避障路径；
控制模块，用于若避障路径不可执行，则无人机停止行进，执行悬停避障措施。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现所述的一种用于果园水果采摘无人机的实时避障方法。
本发明采用了A*方法和随机采样路径规划方法相结合进行路径规划的策略，在位置空旷、障碍物较少的情况下，通过A*方法进行路径规划能大大提高无人机到达作业目标点的效率，规划路径长度较短，规划时长较短。而在障碍物较多，前进前方有死胡同区域时，A*方法可能会陷入局部最优，无法搜索得到到达作业目标点的避障路径。此时，利用随机采样路径规划方法，可以从多个方向进行路径点扩展，得到一条可执行路径。以上规划方法的优点是：在果园冠层间环境内，根据环境的特性，兼顾了A*方法的导向搜索特性和随机采样路径规划方法的多向扩展特性，并通过组织最近时间的局部地图提供障碍物信息，实时感知环境障碍物特性从而动态切换规划方式，提高了避障效率。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，标示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明的流程图；
图2为本发明的K维二叉树存储结构的示意图；
图3为本发明的系统架构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明，本发明实施例中所有方向性指示(诸如上、下、左、右、前、后……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
另外，在本发明中涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一种该特征。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。
实施例1
一种用于果园水果采摘无人机的实时避障方法，包括：
S100,根据无人机初始位置和目标位置规划第一路径；
S200,无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行；
S300,若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为避障路径；
S400,若避障路径不可执行，则无人机停止行进，执行悬停避障措施。
本发明采用了A*方法和随机采样路径规划方法相结合进行路径规划的策略，在位置空旷、障碍物较少的情况下，通过A*方法进行路径规划能大大提高无人机到达作业目标点的效率，规划路径长度较短，规划时长较短。而在障碍物较多，前进前方有死胡同区域时，A*方法可能会陷入局部最优，无法搜索得到到达作业目标点的避障路径。此时，利用随机采样路径规划方法，可以从多个方向进行路径点扩展，得到一条可执行路径。以上规划方法的优点是：在果园冠层间环境内，根据环境的特性，兼顾了A*方法的导向搜索特性和随机采样路径规划方法的多向扩展特性，并通过组织最近时间的局部地图提供障碍物信息，实时感知环境障碍物特性从而动态切换规划方式，提高了避障效率。
S400若避障路径不可执行，则无人机停止行进，执行悬停避障措施之后，还包括S500利用随机采样路径规划算法规划无人机当前位置到目标位置的第三避障路径，将第一路径替换为第三避障路径，具体为：
构造搜索树并搜索新的扩展点，已经搜索的点作为父节点；
确认当前无人机平台所在位置(xnow，ynow，znow)作为起始点，设置终点为目标作业点坐标(xgoal，ygoal，zgoal)，在当前局部地图上设置起始点(xnow，ynow，znow)初始化为父节点，在当前局部地图状态环境内进行随机采样，生成随机点集Grandom；
找出随机点集Grandom中，距离当前父节点最近的采样点(xnearest，ynearest，znearest)，沿当前父节点到最近采样点(xnearest，ynearest，znearest)的方向，以固定步长K迈出，确定候选子节点nextpoint(xnext，ynext，znext)；
判断候选子节点nextpoint(xnext，ynext，znext)是否与障碍物重合、干涉或与障碍物最近距离小于预设阈值；
若是，则返回重新采样；
若否，则将当前候选子节点设为子节点，添加到搜索树中，判断子节点是否到达目标作业点(xgoal，ygoal，zgoal)附近；
若是，将所有搜索树中节点确认为第三避障路径；
若否，则返回重新采样路径点，直至子节点到达目标作业点(xgoal，ygoal，zgoal)附近；
若采样过程中，规划时间超过预设阈值I，则认为无法到达指定作业目标点，所述预设阈值I指随机采样路径规划算法规划路径所需最大时间阈值，超出该阈值则认为无法构建出所述可执行路径，则无人机停止行进，执行悬停避障措施。
S100根据无人机初始位置和目标位置规划第一路径包括：
根据目标位置坐标(xgoal，ygoal，zgoal)和无人机初始位置坐标(x0，y0，z0)，以及预设步长K，生成定步数的直线路径点集G：
由于考虑到每一帧的点云数据量是不确定的，且要便于快速查询点云信息，所以考虑采用k维二叉树结构来动态组织数据。
S100根据无人机初始位置和目标位置规划第一路径之后，还包括S110根据第一路径，构建第一路径行进方向的时间累积局部地图，具体为：
通过激光雷达获取的3D点云坐标(xp，yp，zp)，通过无人机当前位置(x0，y0，z0)和姿态(θ，ψ，j)，得到旋转矩阵RZXY，将激光雷达获取的3D点云坐标(xp，yp，zp)转换到世界坐标系下(xw，yw，zw)：
[xw yw zw]T＝RZXY[xp yp zp]T+[x0 y0 z0]
在间隔时间0到T内，不断采集并存储激光雷达采集的点云信息，将其作为无人机平台的局部地图，采用k维二叉树结构来动态组织数据；
每个K维二叉树结构包含两个域，一个存储整个结构内包含的点云总数N，另一个指向根节点；
一个根节点包含三个域，一个存储激光点具体坐标(xw，yw，zw)，另外两个域分别指向下一个子节点；
每一个子节点也包含三个点，一个存储激光点具体坐标(xw，yw，zw)，另外两个域分别指向下一个孙节点；
向k维二叉树结构插入激光点的方式为：更新点云总数N，构建一个新的节点，存储激光点具体坐标(xw，yw，zw)，根据维度划分方式，找到最终对应区间插入该节点，照固定时间间隔T进行构建并覆盖的局部。
S110根据第一路径，构建第一路径行进方向的时间累积局部地图之后，还包括S120组织两个k维二叉树结构来做为水果采摘无人机平台的局部地图，具体为：
设置变量点云帧数M和k维二叉树棵树Q，M指间隔时间T内，激光雷达总共采集点云帧数，Q为k维二叉树结构个数；
当激光雷达接收到一帧点云数据时，将点云数据插入第一棵k维二叉树；
当激光雷达接收到帧数>M时，将新接收点云数据插入到第二棵k维二叉树中，第一棵二叉树保持静止；
当激光雷达接收到帧数>M*2时，激光雷达接收到帧数重置归零，同时将第一棵k维二叉树清空，将新接收点云数据组织到第一棵k维二叉树中，此时第二棵k维二叉树保持静止；
在任意时刻保持两棵k维两叉树结构，一个静态，一个处于动态加入激光点数据的过程，两棵k维两叉树结构组织的激光点云作为水果采摘无人机平台的时间累积局部地图。
S200无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行包括：
对第一路径中的路径点集中的每一个路径点(x，y，z)，在局部地图中查找是否存在此路径点(x，y，z)，以及距离路径点最近的激光点(xnearest，ynearest，znearest)，距离为dnearest，计算方式如下：
若路径点(x，y，z)满足以下条件：存在于当前局部地图中，dnearest＜Dsafe
则认为所跟踪路径上是存在不能通行部分；
若路径点不满足条件，则无人机按照第一路径通行。
S300若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的第一避障路径，将第一路径替换为第一避障路径包括：
确认当前无人机平台所在位置(xnow，ynow，znow)作为起始点，设置终点为目标作业点坐标(xgoal，ygoal，zgoal)，在当前局部地图上设置起始点(xnow，ynow，znow)为当前扩展点，当前扩展点指当前需要进行领域扩展的坐标航点；
从当前扩展点的领域点开始搜索，基于当前局部地图上的邻域点点集：
其中K为动态步长，根据当前扩展点附近的点云密度确定步长
N为当前局部地图中点云量总数，Nnear为局部地图中，与当前扩展点欧氏距离小于预设阈值的点云量数量，Lmax为单次最大步长，预设阈值为狭窄通道中安全距离阈值Dsafe的两倍；
判断点集中的每一个领域点是否与障碍物重合、干涉或与障碍物最近距离小于预设阈值Dsafe；
若是，则将所有扩展点生成为第一避障路径替换第一路径；
若否，则无人机停止行进，执行悬停避障措施。
S300若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为第一避障路径之后，还包括S310计算A*算法规划路径的备选扩展点，构建第二避障路径具体为：
将没有与障碍物重合、干涉或与障碍物最近距离小于预设阈值的邻域点(xnear，ynear，znear)作为备选扩展点；
计算备选扩展点的代价函数F(x)，由备选扩展点与目标作业点的欧式距离H(x)和出发后的移动距离G(x)和导向因子R(x)，导向因子为引导无人机采摘平台规划路径远离高硬度障碍物，计算方式如下，其中n为扩展次数，由人为设定；
导向因子R(x)设计规则如下：
N为当前局部地图中点云量总数，Nnear为局部地图中，与当前扩展点欧氏距离小于预设阈值的点云量数量，预设阈值为狭窄通道中安全距离阈值Dsafe的两倍；
Nhard为当前局部地图中，距备选扩展点最近的激光点p的阈值邻域点集Gp中点云数量，阈值为人为设定，k为影响比例因子，由人为设定；
σθ为距备选扩展点最近的激光点p的阈值领域点集Gp中每一个领域点的夹角θ的方差，具体计算方式如下：
根据备选扩展点最近的激光点p和阈值邻域点集Gp；
以p为角点，连接领域点和点p，形成一系列夹角：θ＝{θ1，θ2，θ3，...，θn}，其中n＝Nhard-1；
计算夹角的方差
将当前扩展点设置已扩展标记，在后续扩展过程中不再作为备选扩展点，以满足欧氏距离和移动距离之和最小的备选扩展点作为下一个待扩展点nextpoint
nextpoint＝arg min(H(x)+G(x)+R(x))
判断待扩展点是否到达目标作业点(xgoal，ygoal，zgoal)附近；
若是，将所有待扩展点生成一条可执行路径，该可执行路径为第二避障路径，将第二避障路径作为避障规划路径替换第一路径；
若否，则令待扩展点nextpoint作为当前扩展点(xnow，ynow，znow)，并返回搜索备选扩展点，直至待扩展点到达目标作业点(xgoal，ygoal，zgoal)或附近；
若扩展搜索中，无备选扩展点，则判断无法规划处可执行路径，则无人机停止行进，执行悬停避障措施。
所述导向因子为引导无人机采摘平台规划路径远离高硬度障碍物，例如树干，电线杆等；由于树叶、枝条等硬度较小的障碍物对于无人机作业平台的飞行影响较小，而树干、母枝、电线杆等硬度较大障碍物对于无人机作业平台的飞行影响显著，所以设定导向因子引导无人机原理硬度较大的障碍物。根据植被激光点云特性得知，树叶、枝条等软性障碍物的点云密度较小，点云分布稀疏；植被枝干、母枝等硬度较大障碍物的点云密度较大，点云分布集中。
实施例2
一种用于果园水果采摘无人机的实时避障系统，包括：
第一路径规划模块，用于根据无人机初始位置和目标位置规划第一路径；
数据处理模块，用于无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行；
避障路径规划模块，用于若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为避障路径；
控制模块，用于若避障路径不可执行，则无人机停止行进，执行悬停避障措施。
实施例3
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现所述的一种用于果园水果采摘无人机的实时避障方法。
本发明采用了A*方法和随机采样路径规划方法相结合进行路径规划的策略，在位置空旷、障碍物较少的情况下，通过A*方法进行路径规划能大大提高无人机到达作业目标点的效率，规划路径长度较短，规划时长较短。而在障碍物较多，前进前方有死胡同区域时，A*方法可能会陷入局部最优，无法搜索得到到达作业目标点的避障路径。此时，利用随机采样路径规划方法，可以从多个方向进行路径点扩展，得到一条可执行路径。以上规划方法的优点是：在果园冠层间环境内，根据环境的特性，兼顾了A*方法的导向搜索特性和随机采样路径规划方法的多向扩展特性，并通过组织最近时间的局部地图提供障碍物信息，实时感知环境障碍物特性从而动态切换规划方式，提高了避障效率。
以上所述仅是本发明的具体实施方式，使本领域技术人员能够理解或实现本发明。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。
Claims (10)
1.一种用于果园水果采摘无人机的实时避障方法，其特征在于，包括：
根据无人机初始位置和目标位置规划第一路径；
无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行；
若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为避障路径；
若避障路径不可执行，则无人机停止行进，执行悬停避障措施。
2.根据权利要求1所述的一种用于果园水果采摘无人机的实时避障方法，其特征在于，所述若避障路径不可执行，则无人机停止行进，执行悬停避障措施之后，还包括利用随机采样路径规划算法规划无人机当前位置到目标位置的第三避障路径，将第一路径替换为第三避障路径，具体为：
构造搜索树并搜索新的扩展点，已经搜索的点作为父节点；
确认当前无人机平台所在位置(xnow,ynow,znow)作为起始点，设置终点为目标作业点坐标(xgoal,ygoal,zgoal)，在当前局部地图上设置起始点(xnow,ynow,znow)初始化为父节点，在当前局部地图状态环境内进行随机采样，生成随机点集Grandom；
找出随机点集Grandom中，距离当前父节点最近的采样点(xnearest,ynearest,znearest)，沿当前父节点到最近采样点(xnearest,ynearest,znearest)的方向，以固定步长K迈出，确定候选子节点nextpoint(xnext,ynext,znext)；
判断候选子节点nextpoint(xnext,ynext,znext)是否与障碍物重合、干涉或与障碍物最近距离小于预设阈值；
若是，则返回重新采样；
若否，则将当前候选子节点设为子节点，添加到搜索树中，判断子节点是否到达目标作业点(xgoal,ygoal,zgoal)附近；
若是，将所有搜索树中节点确认为第三避障路径；
若否，则返回重新采样路径点，直至子节点到达目标作业点(xgoal,ygoal,zgoal)附近；
若采样过程中，规划时间超过预设阈值I，则认为无法到达指定作业目标点，所述预设阈值I指随机采样路径规划算法规划路径所需最大时间阈值，超出该阈值则认为无法构建出所述可执行路径，则无人机停止行进，执行悬停避障措施。
3.根据权利要求1所述的一种用于果园水果采摘无人机的实时避障方法，其特征在于，所述根据无人机初始位置和目标位置规划第一路径包括：
根据目标位置坐标(xgoal,ygoal,zgoal)和无人机初始位置坐标(x0,y0,z0)，以及预设步长K，生成定步数的直线路径点集G：
4.根据权利要求3所述的一种用于果园水果采摘无人机的实时避障方法，其特征在于，所述根据无人机初始位置和目标位置规划第一路径之后，还包括根据第一路径，构建第一路径行进方向的时间累积局部地图，具体为：
通过激光雷达获取的3D点云坐标(xp,yp,zp)，通过无人机当前位置(x0,y0,z0)和姿态(θ,ψ,j)，得到旋转矩阵RZXY，将激光雷达获取的3D点云坐标(xp,yp,zp)转换到世界坐标系下(xw,yw,zw)：
[xw yw zw]T＝RZXY[xp yp zp]T+[x0 y0 z0]
在间隔时间0到T内，不断采集并存储激光雷达采集的点云信息，将其作为无人机平台的局部地图，采用k维二叉树结构来动态组织数据；
每个K维二叉树结构包含两个域，一个存储整个结构内包含的点云总数N，另一个指向根节点；
一个根节点包含三个域，一个存储激光点具体坐标(xw,yw,zw)，另外两个域分别指向下一个子节点；
每一个子节点也包含三个点，一个存储激光点具体坐标(xw,yw,zw)，另外两个域分别指向下一个孙节点；
向k维二叉树结构插入激光点的方式为：更新点云总数N，构建一个新的节点，存储激光点具体坐标(xw,yw,zw)，根据维度划分方式，找到最终对应区间插入该节点，照固定时间间隔T进行构建并覆盖的局部。
5.根据权利要求4所述的一种用于果园水果采摘无人机的实时避障方法，其特征在于，所述根据第一路径，构建第一路径行进方向的时间累积局部地图之后，还包括组织两个k维二叉树结构来做为水果采摘无人机平台的局部地图，具体为：
设置变量点云帧数M和k维二叉树棵树Q，M指间隔时间T内，激光雷达总共采集点云帧数，Q为k维二叉树结构个数；
当激光雷达接收到一帧点云数据时，将点云数据插入第一棵k维二叉树；
当激光雷达接收到帧数>M时，将新接收点云数据插入到第二棵k维二叉树中，第一棵二叉树保持静止；
当激光雷达接收到帧数>M*2时，激光雷达接收到帧数重置归零，同时将第一棵k维二叉树清空，将新接收点云数据组织到第一棵k维二叉树中，此时第二棵k维二叉树保持静止；
在任意时刻保持两棵k维两叉树结构，一个静态，一个处于动态加入激光点数据的过程，两棵k维两叉树结构组织的激光点云作为水果采摘无人机平台的时间累积局部地图。
6.据权利要求1所述的一种用于果园水果采摘无人机的实时避障方法，其特征在于，所述无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行包括：
对第一路径中的路径点集中的每一个路径点(x,y,z)，在局部地图中查找是否存在此路径点(x,y,z)，以及距离路径点最近的激光点(xnearest,ynearest,znearest)，距离为dnearest，计算方式如下：
若路径点(x,y,z)满足以下条件：存在于当前局部地图中，dnearest<Dsafe
则认为所跟踪路径上是存在不能通行部分；
若路径点不满足条件，则无人机按照第一路径通行。
7.据权利要求1所述的一种用于果园水果采摘无人机的实时避障方法，其特征在于，所述若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的第一避障路径，将第一路径替换为第一避障路径包括：
确认当前无人机平台所在位置(xnow,ynow,znow)作为起始点，设置终点为目标作业点坐标(xgoal,ygoal,zgoal)，在当前局部地图上设置起始点(xnow,ynow,znow)为当前扩展点，当前扩展点指当前需要进行领域扩展的坐标航点；
从当前扩展点的领域点开始搜索，基于当前局部地图上的邻域点点集：
其中K为动态步长，根据当前扩展点附近的点云密度确定步长
N为当前局部地图中点云量总数，Nnear为局部地图中，与当前扩展点欧氏距离小于预设阈值的点云量数量，Lmax为单次最大步长，预设阈值为狭窄通道中安全距离阈值Dsafe的两倍；
判断点集中的每一个领域点是否与障碍物重合、干涉或与障碍物最近距离小于预设阈值Dsafe；
若是，则将所有扩展点生成为第一避障路径替换第一路径；
若否，则无人机停止行进，执行悬停避障措施。
8.据权利要求1所述的一种用于果园水果采摘无人机的实时避障方法，其特征在于，所述若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为第一避障路径之后，还包括计算A*算法规划路径的备选扩展点，构建第二避障路径具体为：
将没有与障碍物重合、干涉或与障碍物最近距离小于预设阈值的邻域点(xnear,ynear,znear)作为备选扩展点；
计算备选扩展点的代价函数F(x)，由备选扩展点与目标作业点的欧式距离H(x)和出发后的移动距离G(x)和导向因子R(x)，导向因子为引导无人机采摘平台规划路径远离高硬度障碍物，计算方式如下，其中n为扩展次数，由人为设定；
导向因子R(x)设计规则如下：
N为当前局部地图中点云量总数，Nnear为局部地图中，与当前扩展点欧氏距离小于预设阈值的点云量数量，预设阈值为狭窄通道中安全距离阈值Dsafe的两倍；
Nhard为当前局部地图中，距备选扩展点最近的激光点p的阈值邻域点集Gp中点云数量，阈值为人为设定，k为影响比例因子，由人为设定；
σθ为距备选扩展点最近的激光点p的阈值领域点集Gp中每一个领域点的夹角θ的方差，具体计算方式如下：
根据备选扩展点最近的激光点p和阈值邻域点集Gp；
以p为角点，连接领域点和点p，形成一系列夹角：θ＝{θ1,θ2,θ3,...,θn}，其中n＝Nhard-1；
计算夹角的方差
将当前扩展点设置已扩展标记，在后续扩展过程中不再作为备选扩展点，以满足欧氏距离和移动距离之和最小的备选扩展点作为下一个待扩展点nextpoint
nextpoint＝arg min(H(x)+G(x)+R(x))
判断待扩展点是否到达目标作业点(ygoal,ygoal,zgoal)附近；
若是，将所有待扩展点生成一条可执行路径，该可执行路径为第二避障路径，将第二避障路径作为避障规划路径替换第一路径；
若否，则令待扩展点nextpoint作为当前扩展点(xnow,ynow,znow)，并返回搜索备选扩展点，直至待扩展点到达目标作业点(ygoal,ygoal,zgoal)或附近；
若扩展搜索中，无备选扩展点，则判断无法规划处可执行路径，则无人机停止行进，执行悬停避障措施。
9.一种用于果园水果采摘无人机的实时避障系统，其特征在于，包括：
第一路径规划模块，用于根据无人机初始位置和目标位置规划第一路径；
数据处理模块，用于无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行；
避障路径规划模块，用于若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为避障路径；
控制模块，用于若避障路径不可执行，则无人机停止行进，执行悬停避障措施。
10.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至8中任一项所述的一种用于果园水果采摘无人机的实时避障方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310228813.5A CN116301001A (zh) | 2023-03-09 | 2023-03-09 | 一种用于果园水果采摘的无人机实时避障系统及方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310228813.5A CN116301001A (zh) | 2023-03-09 | 2023-03-09 | 一种用于果园水果采摘的无人机实时避障系统及方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116301001A true CN116301001A (zh) | 2023-06-23 |
Family
ID=86821834
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310228813.5A Pending CN116301001A (zh) | 2023-03-09 | 2023-03-09 | 一种用于果园水果采摘的无人机实时避障系统及方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116301001A (zh) |
-
2023
- 2023-03-09 CN CN202310228813.5A patent/CN116301001A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109509210B (zh) | 障碍物跟踪方法和装置 | |
| CN113625721B (zh) | 一种未知空间自主探索规划方法 | |
| CN111694356B (zh) | 一种行驶控制方法、装置、电子设备及存储介质 | |
| US11044842B2 (en) | Path planning for area coverage | |
| EP1504277B1 (en) | Real-time target tracking of an unpredictable target amid unknown obstacles | |
| US8498788B2 (en) | Method and system for determining a planned path of a vehicle | |
| CN114812581B (zh) | 一种基于多传感器融合的越野环境导航方法 | |
| EP1522823B1 (en) | Modular path planner | |
| EP3731052B1 (en) | Control device, work machine and control program | |
| CN115164907A (zh) | 基于动态权重的a*算法的森林作业机器人路径规划方法 | |
| CN114119920A (zh) | 三维点云地图构建方法、系统 | |
| Aguiar et al. | Localization and mapping on agriculture based on point-feature extraction and semiplanes segmentation from 3D LiDAR data | |
| Ye et al. | Path planning for mobile robots in unstructured orchard environments: An improved kinematically constrained bi-directional RRT approach | |
| Velasquez et al. | Multi-sensor fusion based robust row following for compact agricultural robots | |
| CN116880497A (zh) | 一种自动化农业机械的全覆盖路径规划方法、装置和设备 | |
| Fassbender et al. | Landmark-based navigation in large-scale outdoor environments | |
| CN111580530B (zh) | 一种定位方法、装置、自主移动设备及介质 | |
| CN116136417B (zh) | 一种面向越野环境的无人驾驶车辆局部路径规划方法 | |
| CN117406771A (zh) | 一种基于四旋翼无人机的高效自主探索方法、系统及设备 | |
| CN116301001A (zh) | 一种用于果园水果采摘的无人机实时避障系统及方法 | |
| Stelzer et al. | Trail-Map: A scalable landmark data structure for biologically inspired range-free navigation | |
| Saarinen et al. | Best-first branch and bound search method for map based localization | |
| CA3184001A1 (en) | Multi-agent map generation | |
| Tormagov et al. | Motion Control of Autonomous Wheeled Robots in Precision Agriculture | |
| Inoue et al. | Autonomous Navigation and Obstacle Avoidance in an Orchard Using Machine Vision Techniques for a Robotic Mower |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |