一种用于果园水果采摘的无人机实时避障系统及方法技术方案

本发明专利技术提供一种用于果园水果采摘无人机的实时避障系统及方法，该方法包括：根据无人机初始位置和目标位置规划第一路径；无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行；若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为避障路径；若避障路径不可执行，则无人机停止行进，执行悬停避障措施。本发明专利技术能够及时检测飞行时的障碍物，准确有效避开障碍物完成自主飞行。有效避开障碍物完成自主飞行。有效避开障碍物完成自主飞行。

【技术实现步骤摘要】
一种用于果园水果采摘的无人机实时避障系统及方法


[0001]本专利技术涉及无人机
，特别是涉及一种用于果园水果采摘无人机的实时避障系统及方法。

技术介绍

[0002]随着无人机的发展，无人机的应用领域越来越广泛。现如今，由于农用无人机技术的不断发展，果园用无人机已进入人们的视野。目前果园里水果采摘部分仍依靠人力完成，费时费力。而无人机则可以轻松飞到水果面前，通过在无人机下方安装采摘装置，从而实现水果采摘工作。
[0003]目前市面上针对水果采摘的无人机装备目前还处于研发阶段，尚未拥有专门针对无人机水果采摘所进行的作业方法。根据水果采摘无人机的作业特性，无人机需要在果树冠层间进行采摘作业。相较于其他种类无人机的工作环境，水果采摘无人机的果树冠层间环境具有障碍物形状不规则、狭窄空间多、障碍物尺寸较小、动态障碍物多等特点。由于其环境特殊性，这对于无人机平台的避障有了更高的要求：需要检测到更小尺寸的障碍物，比如树枝枝条、电线等；需要及时检测动态障碍物等。
[0004]所以，如何在果树冠层空域中，生成一条安全、无碰撞的可用于水果采摘无人机执行的飞行路径，并在执行过程中，有效检测并避开障碍物，完成无人机平台自主飞行，是一个急需解决的问题。

技术实现思路

[0005]本专利技术的目的是提供一种用于果园水果采摘无人机的实时避障系统及方法，该方法能够及时检测飞行时的障碍物，准确有效避开障碍物完成自主飞行。
[0006]一种用于果园水果采摘无人机的实时避障方法，包括：
[0007]根据无人机初始位置和目标位置规划第一路径；
[0008]无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行；
[0009]若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为避障路径；
[0010]若避障路径不可执行，则无人机停止行进，执行悬停避障措施。
[0011]若避障路径不可执行，则无人机停止行进，执行悬停避障措施之后，还包括利用随机采样路径规划算法规划无人机当前位置到目标位置的第三避障路径，将第一路径替换为第三避障路径，具体为：
[0012]构造搜索树并搜索新的扩展点，已经搜索的点作为父节点；
[0013]确认当前无人机平台所在位置(x
now
，y
now
，z
now
)作为起始点，设置终点为目标作业点坐标(x
goal
，y
goal
，z
goal
)，在当前局部地图上设置起始点(x
now
，y
now
，z
now
)初始化为父节点，在当前局部地图状态环境内进行随机采样，生成随机点集G
random
；
[0014]找出随机点集G
random
中，距离当前父节点最近的采样点(x
nearest
，y
nearest
，z
nearest
)，沿当前父节点到最近采样点(x
nearest
，y
nearest
，z
nearest
)的方向，以固定步长K迈出，确定候选子节点nextpoint(x
next
，y
next
，z
next
)；
[0015]判断候选子节点nextpoint(x
next
，y
next
，z
next
)是否与障碍物重合、干涉或与障碍物最近距离小于预设阈值；
[0016]若是，则返回重新采样；
[0017]若否，则将当前候选子节点设为子节点，添加到搜索树中，判断子节点是否到达目标作业点(x
goal
，y
goal
，z
goal
)附近；
[0018]若是，将所有搜索树中节点确认为第三避障路径；
[0019]若否，则返回重新采样路径点，直至子节点到达目标作业点(x
goal
，y
goal
，z
goal
)附近；
[0020]若采样过程中，规划时间超过预设阈值I，则认为无法到达指定作业目标点，所述预设阈值I指随机采样路径规划算法规划路径所需最大时间阈值，超出该阈值则认为无法构建出所述可执行路径，则无人机停止行进，执行悬停避障措施。
[0021]根据无人机初始位置和目标位置规划第一路径包括：
[0022]根据目标位置坐标(x
goal
，y
goal
，z
goal
)和无人机初始位置坐标(x0，y0，z0)，以及预设步长K，生成定步数的直线路径点集G：
[0023][0024]根据无人机初始位置和目标位置规划第一路径之后，还包括根据第一路径，构建第一路径行进方向的时间累积局部地图，具体为：
[0025]通过激光雷达获取的3D点云坐标(x
p
，y
p
，z
p
)，通过无人机当前位置(x0，y0，z0)和姿态(θ，ψ，j)，得到旋转矩阵R
ZXY
，将激光雷达获取的3D点云坐标(x
p
，y
p
，z
p
)转换到世界坐标系下(x
w
，y
w
，z
w
)：
[0026][0027][x
w y
w z
w
]T
＝R
ZXY
[x
p y
p z
p
]T
+[x
0 y
0 z0][0028]在间隔时间0到T内，不断采集并存储激光雷达采集的点云信息，将其作为无人机平台的局部地图，采用k维二叉树结构来动态组织数据；
[0029]每个K维二叉树结构包含两个域，一个存储整个结构内包含的点云总数N，另一个指向根节点；
[0030]一个根节点包含三个域，一个存储激光点具体坐标(x
w
，y
w
，z
w
)，另外两个域分别指向下一个子节点；
[0031]每一个子节点也包含三个点，一个存储激光点具体坐标(x
w
，y
w
，z
w
)，另外两个域分别指向下一个孙节点；
[0032]向k维二叉树结构插入激光点的方式为：更新点云总数N，构建一个新的节点，存储激光点具体坐标(x
w
，y
w
，z
w
)，根据维度划分方式，找到最终对应区间插入该节点，照固定时间间隔T进行构建并覆盖的局部。
[0033]根据第一路径，构建本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种用于果园水果采摘无人机的实时避障方法，其特征在于，包括：根据无人机初始位置和目标位置规划第一路径；无人机飞行时判断第一路径是否存在不能通行的部分，若不存在则按照第一路径通行；若第一路径存在不能通行的部分，则利用基于植被点云密度引导的A*算法规划无人机当前位置到目标位置的避障路径，将第一路径替换为避障路径；若避障路径不可执行，则无人机停止行进，执行悬停避障措施。2.根据权利要求1所述的一种用于果园水果采摘无人机的实时避障方法，其特征在于，所述若避障路径不可执行，则无人机停止行进，执行悬停避障措施之后，还包括利用随机采样路径规划算法规划无人机当前位置到目标位置的第三避障路径，将第一路径替换为第三避障路径，具体为：构造搜索树并搜索新的扩展点，已经搜索的点作为父节点；确认当前无人机平台所在位置(x
now
,y
now
,z
now
)作为起始点，设置终点为目标作业点坐标(x
goal
,y
goal
,z
goal
)，在当前局部地图上设置起始点(x
now
,y
now
,z
now
)初始化为父节点，在当前局部地图状态环境内进行随机采样，生成随机点集G
random
；找出随机点集G
random
中，距离当前父节点最近的采样点(x
nearest
,y
nearest
,z
nearest
)，沿当前父节点到最近采样点(x
nearest
,y
nearest
,z
nearest
)的方向，以固定步长K迈出，确定候选子节点nextpoint(x
next
,y
next
,z
next
)；判断候选子节点nextpoint(x
next
,y
next
,z
next
)是否与障碍物重合、干涉或与障碍物最近距离小于预设阈值；若是，则返回重新采样；若否，则将当前候选子节点设为子节点，添加到搜索树中，判断子节点是否到达目标作业点(x
goal
,y
goal
,z
goal
)附近；若是，将所有搜索树中节点确认为第三避障路径；若否，则返回重新采样路径点，直至子节点到达目标作业点(x
goal
,y
goal
,z
goal
)附近；若采样过程中，规划时间超过预设阈值I，则认为无法到达指定作业目标点，所述预设阈值I指随机采样路径规划算法规划路径所需最大时间阈值，超出该阈值则认为无法构建出所述可执行路径，则无人机停止行进，执行悬停避障措施。3.根据权利要求1所述的一种用于果园水果采摘无人机的实时避障方法，其特征在于，所述根据无人机初始位置和目标位置规划第一路径包括：根据目标位置坐标(x
goal
,y
goal
,z
goal
)和无人机初始位置坐标(x0,y0,z0)，以及预设步长K，生成定步数的直线路径点集G：4.根据权利要求3所述的一种用于果园水果采摘无人机的实时避障方法，其特征在于，所述根据无人机初始位置和目标位置规划第一路径之后，还包括根据第一路径，构建第一路径行进方向的时间累积局部地图，具体为：通过激光雷达获取的3D点云坐标(x
p
,y
p
,z
p
)，通过无人机当前位置(x0,y0,z0)和姿态(θ,ψ,j)，得到旋转矩阵R
ZXY
，将激光雷达获取的3D点云坐标(x
p
,y
p
,z
p
)转换到世界坐标系下(x
w
,y
w
,z
w
)：
[x
w y
w z
w
]
T
＝R
ZXY
[x
p y
p z
p
]
T
+[x
0 y
0 z0]在间隔时间0到T内，不断采集并存储激光雷达采集的点云信息，将其作为无人机平台的局部地图，采用k维二叉树结构来动态组织数据；每个K维二叉树结构包含两个域，一个存储整个结构内包含的点云总数N，另一个指向根节点；一个根节点包含三个域，一个存储激光点具体坐标(x
w
,y
w
,z
w
)，另外两个域分别指向下一个子节点；每一个子节点也包含三个点，一个存储激光点具体坐标(x
w
,y
w
,z
w
)，另外两个域分别指向下一个孙节点；向k维二叉树结构插入激光点的方式为：更新点云总数N，构建一个新的节点，存储激光点具体坐标(x
w
,y
w
,z
w
)，根据维度划分方式，找到最终对应区间插入该节点，照固定时间间隔T进行构建并覆盖的局部。5.根据权利要求4所述的一种用于果园水果采摘无人机的实时避障方法，其特征在于，所述根据第一路径，构建第一路径行进方向的时间累积局部地图之后，还包括组织两个k维二叉树结构来做为水果采摘无人机平台的局部地图，具体为：设置变量点云帧数M和k维二叉树棵树Q，M指间隔时间T内，激光雷达总共采集点云帧数，Q为k维二叉树结构个数；当激光雷达接收到一帧点云数据时，将点云数据插入第一棵k维二叉树；当激光雷达接收到帧数>M时，将新接收点云数据插入到第二棵k维二叉树中，第一棵二叉树保持静止；当激光雷达接收到帧数>M*2时，激光雷达接收到帧数重置归零，同时将第一棵k维二叉...

【专利技术属性】
技术研发人员：李君，贾宇航，李灯辉，施琳琳，周浩波，周峥琦，巫凯旋，林佩怡，姚中威，李钊，
申请(专利权)人：华南农业大学，
类型：发明
国别省市：