CN111844007B - 授粉机器人机械臂避障路径规划方法、装置 - Google Patents

Abstract

本发明提供一种授粉机器人机械臂避障路径规划方法、装置，所述方法包括：通过深度相机获取深度图像，根据深度图像获取障碍物位姿和机械臂的授粉目标点位姿；通过带线性回归的三维RRT算法获取初始路径；通过A*算法对三维RRT算法进行优化，以获取最优路径；通过人工势场法对最优路径进行轨迹平滑，以获取最终路径根据最终路径对机械臂进行控制。该方法采用带线性回归的三维RRT算法，将RRT三维空间探索的目的性进一步的增强，使探索方向更容易朝着开阔的地方发展，基于A*算法优化的三维RRT算法结合的人工势场法可以很好的避免陷入局部极小值的问题，且轨迹平滑，十分适合机械臂在树枝之间移动，有利于提高机器人授粉的可靠性。

Description

授粉机器人机械臂避障路径规划方法、装置

技术领域

本发明涉及及农业机械技术领域，具体涉及一种授粉机器人机械臂避障路径规划方法和一种授粉机器人机械臂避障路径规划装置。

背景技术

大部分的果树都需要异花授粉才能正常结果，而在授粉树缺乏，天气恶劣的情况下，果树的自然授粉会受到不利的影响，此时就需要进行人工授粉。例如火龙果的授粉，火龙果花朵的自然授粉比较困难，需要人工授粉，且火龙果是典型的夜间开花植物，一般傍晚至凌晨开花，凌晨开始逐渐凋萎，直至阳光照射后完全凋谢。这对人工授粉来说费时费力，还容易损伤花朵。

相关技术中虽然有通过授粉机器人自动进行授粉的技术，但是，对于其授粉机械臂的一般是通过简单的RRT算法进行路径规划，而三维RRT算法容易陷入局部极小值等问题，且可能会产生多条路径，可能导致采取的路径远离最优路径，以及路径一般质量都不是很好，例如可能包含棱角，不够光滑，进而会导致授粉机器人的授粉可靠性。

发明内容

本发明为解决上述技术问题，提供了一种授粉机器人机械臂避障路径规划方法，该方法采用带线性回归的三维RRT(Rapidly Exploring Random Tree，快速扩展随机树)算法，将RRT三维空间探索的目的性进一步的增强，使探索方向更容易朝着开阔的地方发展，基于A*算法优化的三维RRT算法结合的人工势场法可以很好的避免陷入局部极小值的问题，且轨迹平滑，十分适合机械臂在树枝之间移动，且有利于提高机器人的稳定性。

本发明采用的技术方案如下：

本发明第一方面实施例提出了一种授粉机器人机械臂避障路径规划方法，包括以下步骤：通过深度相机获取深度图像，根据所述深度图像获取障碍物位姿和所述机械臂的授粉目标点位姿；根据所述障碍物位姿和所述授粉目标点位姿，通过带线性回归的三维RRT算法获取初始路径；通过A*算法对所述三维RRT算法进行优化，以获取最优路径；通过人工势场法对所述最优路径进行轨迹平滑，以获取最终路径；根据所述最终路径对所述机械臂进行控制

根据本发明的一个实施例，通过深度相机获取深度图像，根据所述深度图像获取障碍物位姿和所述机械臂的授粉目标点位姿，包括：根据所述深度图像识别所述花蕊的顶部中心点和底部坐标点；根据所述顶部中心点和所述底部坐标点基于世界坐标系下的Y轴的数值，判断是否对所述花蕊进行授粉；根据所述深度图像识别花蕊位姿和所述障碍物的位姿，将所述花蕊位姿用四元数位姿表示；根据所述四元数位姿反推得所述机械臂的授粉目标点位姿。

根据本发明的一个实施例，根据所述障碍物位姿和所述授粉目标点位姿，通过带线性回归的三维RRT算法获取初始路径，包括：以机械臂末端中心点所在位置X_init为圆心，以预设长度D为半径，在此三维空间里选取10个随机点作为新增节点X_rand进行扩展；分别判断所述新增节点X_rand与授粉目标点X_goal的距离；如果所述新增节点X_rand与所述授粉目标点X_goal的距离小于预设值，则该述新增节点X_rand停止扩展；如果所述新增节点X_rand与所述授粉目标点X_goal的距离大于或等于预设值，则进一步判断所述新增节点X_rand和随机树上其他任意节点X的距离是否大于所述新增节点X_rand和其父节点X_par的距离；如果是，则将该新增节点X_rand点设为路径规划点X_new；根据所述路径规划点X_new获取初始路径。

根据本发明的一个实施例，通过A*算法对所述三维RRT算法进行优化，以获取最优路径，包括：利用A*算法的估价函数估算每个新增节点X_rand到授粉目标点x_goal的路径估计值；根据路径估计值获取最优路径。

根据本发明的一个实施例，通过人工势场法对所述最优路径进行轨迹平滑，以获取最终路径，包括：以所述路径规划点X_new为引力场，以树枝为斥力场；获取所述引力场产生的引力和斥力场产生的斥力；根据所述引力和所述斥力获取合力；根据所述合力对所述路径规划点X_new进行拟合，以获取最终路径。

根据本发明的一个实施例，根据所述最终路径对所述机械臂进行控制，包括：通过逆运动学反解以计算出所述机械臂各个关节所需要旋转的弧度；根据所述弧度对所述机械臂进行控制。

本发明的第二方面实施例提出了一种授粉机器人机械臂避障路径规划装置，包括：第一获取模块，所述第一获取模块用于通过深度相机获取深度图像，并根据所述深度图像获取障碍物位姿和所述机械臂的授粉目标点位姿；第二获取模块，所述第二获取模块用于根据所述障碍物位姿和所述授粉目标点位姿，通过带线性回归的三维RRT算法获取初始路径；优化模块，所述优化模块用于通过A*算法对所述三维RRT算法进行优化，以获取最优路径；平滑模块，所述平滑模块用于通过人工势场法对所述最优路径进行轨迹平滑，以获取最终路径；控制模块，所述控制模块用于根据所述最终路径对所述机械臂进行控制。

根据本发明的一个实施例，所述第一获取模块具体用于：根据所述深度图像识别所述花蕊的顶部中心点和底部坐标点；根据所述顶部中心点和所述底部坐标点基于世界坐标系下的Y轴的数值，判断是否对所述花蕊进行授粉；根据所述深度图像识别花蕊位姿和所述障碍物的位姿，将所述花蕊位姿用四元数位姿表示；根据所述四元数位姿反推得所述机械臂的授粉目标点位姿。

根据本发明的一个实施例，所述第二获取模块具体用于：以机械臂末端中心点所在位置X_init为圆心，以预设长度D为半径，在此三维空间里选取10个随机点作为新增节点X_rand进行扩展；分别判断所述新增节点X_rand与授粉目标点X_goal的距离；如果所述新增节点X_rand与所述授粉目标点X_goal的距离小于预设值，则该述新增节点X_rand停止扩展；如果所述新增节点X_rand与所述授粉目标点X_goal的距离大于或等于预设值，则进一步判断所述新增节点X_rand和随机树上其他任意节点X的距离是否大于所述新增节点X_rand和其父节点X_par的距离；如果是，则将该新增节点X_rand点设为路径规划点X_new；根据所述路径规划点X_new获取初始路径。

根据本发明的一个实施例，所述优化模块具体用于：利用A*算法的估价函数估算每个新增节点X_rand到授粉目标点x_goal的路径估计值；根据路径估计值获取最优路径。

本发明的有益效果：

本发明采用带线性回归的三维RRT算法，使三维RRT算法新增节点的产生更易向空旷的空间衍生，将RRT算法在树枝空间中探索的目的性进一步的增强；采取最短路径最优的想法，将A*算法加入RRT算法构建随机树，使RRT扩展式随机树朝着最优路径发展，解决了三维RRT算法进行三维空间搜索时的随机性；针对其三维改进RRT算法产生的轨迹不平滑，进而不利于机械臂的轨迹规划等问题，采取使用人工势场法来拟合三维改进RRT算法的子结点和父节点之间的轨迹，解决了产生的轨迹不平滑的技术问题，也有效的避免了人工势场法陷入死区，有利于提高机器人授粉的可靠性。

附图说明

图1是根据本发明一个实施例的授粉机器人机械臂避障路径规划方法的流程图；

图2为本发明一个实施例的深度相机进行三维建模原理示意图；

图3是根据本发明另一个实施例的授粉机器人机械臂避障路径规划方法的流程图；

图4a是采用基本的RRT算法获取的初始路径示意图；

图4b是带线性回归的三维RRT算法获取初始路径示意图；

图5是根据本发明一个实施例的人工势场法的原理示意图；

图6是ROS平台下机械臂避障原理示意图；

图7是根据本发明一个实施例的授粉机器人机械臂避障路径规划装置的方框示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1是根据本发明一个实施例的授粉机器人机械臂避障路径规划方法的流程图。如图1所示，该方法包括以下步骤：

S1，通过深度相机获取深度图像，根据深度图像获取障碍物位姿和机械臂的授粉目标点位姿。

位姿即为物体参考点的位置和物体的姿态的统称，一般采用四元数表示。深度相机可以安装在授粉机器人机械臂的末上端。

S2，根据障碍物位姿和授粉目标点位姿，通过带线性回归的三维RRT算法获取初始路径。

具体地，RRT算法中随机树的新增节点X_rand的选定采用了统计学中的线性回归思想。

S3，通过A*算法对三维RRT算法进行优化，以获取最优路径。

A*(A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法。

S4，通过人工势场法对最优路径进行轨迹平滑，以获取最终路径。

人工势场法路径规划是由Khatib提出的一种虚拟力法(Oussama Khatib，Real-Time obstacle Avoidance for Manipulators and Mobile Robots.Proc of The 1994IEEE.)。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。应用势场法规划出来的路径一般是比较平滑并且安全。

S5，根据最终路径对机械臂进行控制。

具体地，在授粉机器人进行授粉时，可通过在深度相机获取深度图像，然后根据深度图像获取障碍物位姿和机械臂的授粉目标点位姿，根据障碍物位姿和机械臂的授粉目标点位姿，在RRT算法中的新增节点X_rand的选定采用了统计学中的线性回归思想，将RRT三维空间探索的目的性进一步的增强，优化了授粉中由于枝叶宽大而使三维RRT算法陷入局部极小值的问题，使探索方向更容易朝着开阔的地方发展。然后采用A*算法对三维RRT算法进行优化，解决三维RRT算法进行三维空间搜索时的随机性，采取最短路径最优的想法，将启发式估价函数A*加入RRT算法构建随机树，使RRT扩展式随机树朝着最优路径发展。将经过回归函数改进且经A*算法优化过的三维RRT算法所得出的路径用人工势场法进行轨迹平滑后得到最终路径，根据最终路径对机械臂进行控制即可。由此，该方法规划的路径适合机械臂在树枝之间移动，有利于提高机器人授粉的可靠性。

根据本发明的一个实施例，通过深度相机获取深度图像，根据深度图像获取障碍物位姿和机械臂的授粉目标点位姿，可以包括：根据深度图像识别花蕊的顶部中心点和底部坐标点；根据顶部中心点和底部坐标点基于世界坐标系下的Y轴的数值，判断是否对当前花蕊进行授粉；如果是，则根据深度图像识别花蕊位姿和障碍物的位姿，将花蕊位姿用四元数位姿表示；根据四元数位姿反推得机械臂的授粉目标点位姿。

具体地，如图2所示，L代表机械臂，X、Y、Z代表世界坐标系下的三个方向轴，通过深度相机进行三维建模，进行手眼标定，确定深度相机的内参、外参。根据深度图像识别出花蕊的顶部中心点B和其花蕊底部A的坐标点，通过判断这两点的基于世界坐标系下的Y轴的数值(即Ya和Yb)，然后根据Ya和Yb确定是否对当前花蕊进行授粉，如果Ya<Yb，则将当前花蕊作为授粉目标点进行授粉，如果Ya>Yb，则说明当前识别物体不为花蕊，放弃此目标点。

然后利用深度相机识别出当前花蕊和障碍物的位姿，将其三维位姿用四元数来表示。其中，花蕊位姿的四元数为q(x，y，z，w)，其等效旋转轴的方向向量为：

等效旋转角为θ，其中：

且有x²+y²+z²+ω²＝1。最后通过花蕊顶部中心点的四元数位姿反推得其机械臂的授粉目标点位姿。

根据本发明的一个实施例，如图3所示，根据障碍物位姿和授粉目标点位姿，通过带线性回归的三维RRT算法获取初始路径，可以包括：

S201，以起始点X_init为圆心，以预设长度D为半径，在此三维空间里选取10个随机点作为新增节点X_rand进行扩展，其中，起始点为机械臂末端中心点所在位置。

机械臂末端中心点所在位置X_init为起始点。

S202，分别判断新增节点X_rand与授粉目标点X_goal的距离。即判断新增节点X_rand与授粉目标点X_goal的距离是否小于预设值。

S203，如果新增节点X_rand与授粉目标点X_goal的距离小于预设值，则新增节点X_rand停止扩展。

S204，如果新增节点X_rand与授粉目标点X_goal的距离大于或等于预设值，则进一步判断新增节点X_rand和随机树上其他任意节点X的距离是否大于新增节点X_rand和其父节点X_par的距离。

S205，如果是，则将当前新增节点X_rand点设为路径规划点X_new；

S206，根据路径规划点X_new获取初始路径。

具体地，首先以起始点X_init(机械臂末端中心点所在位置)为圆心，以D为半径，在此三维空间里选取10个随机点作为新增节X_rand点进行扩展。然后分别判断10个新增节点X_rand与授粉目标点X_goal的距离。新增节点X_rand与授粉目标点X_goal的距离小于预设值，则三维RRT算法结束，新增节点X_rand停止扩展。如果新增节点X_rand与授粉目标点X_goal的距离大于或等于预设值，则遍历随机树，进一步判断新增节点X_rand和随机树上其他任意节点X的距离是否大于新增节点X_rand和其父节点X_par的距离，如果新增节点X_rand和随机树上其他任意节点X的距离与大于新增节点X_rand和其父节点X_par的距离，即满足：X∈T{Dis(X_par,X_rand)<Dis(X,X_rand)}，令满足上述条件的新增节点X_rand设为路径规划点X_new。

采用带线性回归的三维RRT算法获取初始路径和采用基本的RRT算法获取的初始路径的对比图可参照图4a-4b所示，其中，图4a代表采用基本的RRT算法获取的初始路径示意图，图4b代表带线性回归的三维RRT算法获取初始路径示意图。由图中可以明显看出，采用带线性回归的三维RRT算法获取初始路径更容易朝着开阔的地方发展，探索的目的性进一步的增强。

根据本发明的一个实施例，如图3所示，通过A*算法对三维RRT算法进行优化，以获取最优路径，可以包括：

S301，利用A*算法的估价函数估算每个新增节点X_rand到授粉目标点x_goal的路径估计值。

S302，根据路径估计值获取最优路径。

具体地，将随机树的每一个节点定义一个估价函数：f(X)＝g(X)+h(X)；

其中，g(X)＝Road(X，X_rand)，h(X)＝Dis(X_rand，X_goal)，即g(X)是随机节点X_rand到树中节点X所需的路径代价，h(X)为启发式函数，为新增节点X_rand到授粉目标点x_goal的欧式距离，f(X)为从节点X到随机节点X_rand到目标节点X_goal的路径估计值。

用上述回归函数约束产生符合条件的路径规划点X_new，计算方向向量X_par-X_new与基准向量X_goal-X_init之间的夹角，并保存到数组arr中，将其值由大到小排序，选取靠前的5个点将这几个点作为新的父节点X_par进行以X_par为圆心D为半径的扩展，转步骤S202，找到符合要求的点时，则该节点停止扩展。

在A*算法中建立两个数组，一个为Open数组，一个为Close数组。Open数组表示记录待检测的点，而Close代表已检测完毕的点。将起始点Xinit加入到Open数组中，在其数组中寻找f(x)值最低的点，将其作为当前点；将Open数组中选出的点加入Close数组中；将上述找出的父节点X_par周围的新增节点X_new加如Close数组中，如果节点重复则忽略，并且计算f(x)、g(x)、h(x)的值；判断X_new节点是否属于Open数组，不属于则将节点X_new加入Open数组；通过计算g(x)来判断路径包括该节点是否更加好，若该节点的代价低于其他节点的代价，则将该节点更新成节点X_par；当授粉目标点x_goal已加入Open数组中，则成功搜索出一条路径，否则Open数组为空时，则路径规划失败。

根据本发明的一个实施例，如图3所示，通过人工势场法对最优路径进行轨迹平滑，以获取最终路径，可以包括：

S401，以路径规划点X_new为引力场，以树枝为斥力场，获取引力场产生的引力和斥力场产生的斥力。

S402，根据引力和斥力获取合力。

S403，根据合力对路径规划点X_new进行拟合，以获取最终路径。

具体地，如图5所示，goal1和goal2代表路径规划点，根据步骤S3产生的经回归函数改进且经A*算法优化的三维RRT算法所产生的最优路径，以其每一段的父节点X_par为起始起点，以其每一段的新增节点X_new为引力场，以树枝为斥力场，其引力场由新增节点产生，记为U_at(X)，

其中，k是引力场的比例系数，X表示当前点，X_new表示路径规划点即目标点，p(x,x_new)是X点到目标点X_new的距离。

在X点产生的引力F_at(x)为，

其斥力场由深度相机所拍摄到的所有障碍物共同产生，记为U_Re(x),

其中

表示第i个障碍物在X点产生的斥力场，

其中：_η是斥力场的比例系数，p₀表示障碍物的影响范围，

为X节点到第i个障碍物的距离。在X点产生的斥力F_Re(C)为：F_Re(X)

则在X点势场产生的合力记为F_Total

F_Total(X)＝F_at(X)+F_Re(X)

其中F_Total为机器人的驱动力，这种与基于A*算法优化的三维RRT算法结合的人工势场法可以很好的避免陷入局部极小值的问题，且轨迹平滑，十分适合机械臂在树枝之间移动，且有利于提高机器人的稳定性。

根据本发明的一个实施例，如图3所示，根据最终路径对机械臂进行控制，可以包括：

S501，通过逆运动学反解以计算出所机械臂各个关节所需要旋转的弧度。

S502，根据弧度对机械臂进行控制。

具体地，获取到最终路径后，可以通过逆运动学插件来反推出各个机械臂关节所需要运动的弧度，并且将这些信息传送给机械臂控制器，以实现授粉过程中的避障。

可以理解，本发明提出的授粉机器人机械臂避障路径规划方法可以在ROS(RobotOperating System，机器人操作平台)平台实现，ROS平台包括运动规划器客户端和运动规划器服务器端，具体而言，如图6所示，工作过程为：

通过ROS中的actionlib功能包，通过服务器端与外在的机械臂控制器进行通信，而客户端与服务器端进行任务消息的通信机制。在运动规划期的客户端，需要接受深度相机所识别出的花蕊、障碍物的三维坐标信息，经过回归线性优化的三维RRT算法的计算，得出数条可能的路径，并将这些路径传送给运动规划器服务器端。并且在运动规划器客户端还需要接收运动规划器服务器端反馈的授粉机器人的轨迹运动，其中包括：关节空间的关节角、速度和加速度，并根据授粉机器人的轨迹运动及时的调整运动规划器服务器端所发送给机械臂控制器的关节角、速度和加速度。

在运动规划器服务器端将所确定好的几条轨迹进行A*算法进行最优路径选择，并将三维RRT算法所确定的子结点和父节点之间用人工势场法进行曲线的拟合，使之机械臂的运动轨迹能更加的符合在枝条之间的要求。运动规划器服务器端还需要接受从机械臂传回来的各个关节的现有弧度，并反馈给运动规划器客户端。

将运动规划器服务器端规划的轨迹用ROS自带的IK逆运动学算法计算出机械臂各个关节将要运动的轨迹传送给机械臂控制器，完成避障。

综上所述，根据本发明实施例的授粉机器人机械臂避障路径规划方法，采用带线性回归的三维RRT算法，使三维RRT算法新增节点的产生更易向空旷的空间衍生，将RRT算法在树枝空间中探索的目的性进一步的增强；采取最短路径最优的想法，将A*算法加入RRT算法构建随机树，使RRT扩展式随机树朝着最优路径发展，解决了三维RRT算法进行三维空间搜索时的随机性；针对其三维改进RRT算法产生的轨迹不平滑，进而不利于机械臂的轨迹规划等问题，采取使用人工势场法来拟合三维改进RRT算法的子结点和父节点之间的轨迹，解决了产生的轨迹不平滑的技术问题，也有效的避免了人工势场法陷入死区，有利于提高机器人授粉的可靠性。

与上述的授粉机器人机械臂避障路径规划方法相对应，本发明还提出一种授粉机器人机械臂避障路径规划装置。由于本发明的装置实施例与方法实施例相对应，对于装置实施例未披露的细节可参照上述的方法实施例，本发明中不再进行赘述。

图7是根据本发明一个实施例的授粉机器人机械臂避障路径规划装置的方框示意图。如图7所示，该装置包括：第一获取模块1、第二获取模块2、优化模块3、平滑模块4和控制模块5。

其中，第一获取模块1用于通过深度相机获取深度图像，并根据深度图像获取障碍物位姿和机械臂的授粉目标点位姿；第二获取模块2用于根据障碍物位姿和授粉目标点位姿，通过带线性回归的三维RRT算法获取初始路径；优化模块3用于通过A*算法对三维RRT算法进行优化，以获取最优路径；平滑模块4用于通过人工势场法对所述最优路径进行轨迹平滑，以获取最终路径；控制模块5用于根据最终路径对机械臂进行控制。

根据本发明的一个实施例，第一获取模块1具体用于：根据深度图像识别花蕊的顶部中心点和底部坐标点；根据顶部中心点和底部坐标点基于世界坐标系下的Y轴的数值，判断是否对当前花蕊进行授粉；如果是，则根据深度图像识别花蕊位姿和障碍物的位姿，将花蕊位姿用四元数位姿表示；根据四元数位姿反推得机械臂的授粉目标点位姿。

根据本发明的一个实施例，第二获取模块2具体用于：以起始点X_init为圆心，以预设长度D为半径，在此三维空间里选取10个随机点作为新增节点X_rand进行扩展，其中，起始点为机械臂末端中心点所在位置；分别判断新增节点X_rand与授粉目标点X_goal的距离；如果新增节点X_rand与授粉目标点X_goal的距离小于预设值，则新增节点X_rand停止扩展；如果新增节点X_rand与所述授粉目标点X_goal的距离大于或等于预设值，则进一步判断所述新增节点X_rand和随机树上其他任意节点X的距离是否大于新增节点X_rand和其父节点X_par的距离；如果是，则将该新增节点X_rand点设为路径规划点X_new；根据路径规划点X_new获取初始路径。

根据本发明的一个实施例，优化模块3具体用于：利用A*算法的估价函数估算每个新增节点X_rand到授粉目标点x_goal的路径估计值；根据路径估计值获取最优路径。

根据本发明的一个实施例，平滑模块4具体用于：以路径规划点X_new为引力场，以树枝为斥力场，获取引力场产生的引力和斥力场产生的斥力；根据引力和斥力获取合力；根据合力对路径规划点X_new进行拟合，以获取最终路径。

根据本发明的一个实施例，控制模块5具体用于：通过逆运动学反解以计算出机械臂各个关节所需要旋转的弧度；根据弧度对机械臂进行控制。

根据本发明实施例的授粉机器人机械臂避障路径规划装置，采用带线性回归的三维RRT算法，使三维RRT算法新增节点的产生更易向空旷的空间衍生，将RRT算法在树枝空间中探索的目的性进一步的增强；采取最短路径最优的想法，将A*算法加入RRT算法构建随机树，使RRT扩展式随机树朝着最优路径发展，解决了三维RRT算法进行三维空间搜索时的随机性；针对其三维改进RRT算法产生的轨迹不平滑，进而不利于机械臂的轨迹规划等问题，采取使用人工势场法来拟合三维改进RRT算法的子结点和父节点之间的轨迹，解决了产生的轨迹不平滑的技术问题，也有效的避免了人工势场法陷入死区，有利于提高机器人授粉的可靠性。

在本发明的描述中，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。“多个”的含义是两个或两个以上，除非另有明确具体的限定。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行***、装置或设备(如基于计算机的***、包括处理器的***或其他可以从指令执行***、装置或设备取指令并执行指令的***)使用，或结合这些指令执行***、装置或设备而使用。就本说明书而言，"计算机可读介质"可以是任何可以包含、存储、通信、传播或传输程序以供指令执行***、装置或设备或结合这些指令执行***、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行***执行的软件或固件来实现。如，如果用硬件来实现和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

此外，在本发明各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。

上述提到的存储介质可以是只读存储器，磁盘或光盘等。尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (6)

1.一种授粉机器人机械臂避障路径规划方法，其特征在于，包括以下步骤：

通过深度相机获取深度图像，根据所述深度图像获取障碍物位姿和所述机械臂的授粉目标点位姿；

根据所述障碍物位姿和所述授粉目标点位姿，通过带线性回归的三维RRT算法获取初始路径；

通过A*算法对所述三维RRT算法进行优化，以获取最优路径；

通过人工势场法对所述最优路径进行轨迹平滑，以获取最终路径；

根据所述最终路径对所述机械臂进行控制；

其中，根据所述障碍物位姿和所述授粉目标点位姿，通过带线性回归的三维RRT算法获取初始路径，包括：

以起始点X_init为圆心，以预设长度D为半径，在此三维空间里选取10个随机点作为新增节点X_rand进行扩展，其中，所述起始点为机械臂末端中心点所在位置；

分别判断所述新增节点X_rand与授粉目标点X_goal的距离；

如果所述新增节点X_rand与所述授粉目标点X_goal的距离小于预设值，则所述新增节点X_rand停止扩展；

如果所述新增节点X_rand与所述授粉目标点X_goal的距离大于或等于预设值，则进一步判断所述新增节点X_rand和随机树上其他任意节点X的距离是否大于所述新增节点X_rand和其父节点X_par的距离；

如果是，则将当前新增节点X_rand设为路径规划点X_new；

根据所述路径规划点X_new获取初始路径；

通过A*算法对所述三维RRT算法进行优化，以获取最优路径，包括：

利用A*算法的估价函数估算每个新增节点X_rand到授粉目标点x_goal的路径估计值；

根据路径估计值获取最优路径。

2.根据权利要求1所述的授粉机器人机械臂避障路径规划方法，其特征在于，通过深度相机获取深度图像，根据所述深度图像获取障碍物位姿和所述机械臂的授粉目标点位姿，包括：

根据所述深度图像识别花蕊的顶部中心点和底部坐标点；

根据所述顶部中心点和所述底部坐标点基于世界坐标系下的Y轴的数值，判断是否对当前花蕊进行授粉；

如果是，则根据所述深度图像识别花蕊位姿和所述障碍物的位姿，将所述花蕊位姿用四元数位姿表示；

根据所述四元数位姿反推得所述机械臂的授粉目标点位姿。

3.根据权利要求2所述的授粉机器人机械臂避障路径规划方法，其特征在于，通过人工势场法对所述最优路径进行轨迹平滑，以获取最终路径，包括：

以所述路径规划点X_new为引力场，以树枝为斥力场，获取所述引力场产生的引力和斥力场产生的斥力；

根据所述引力和所述斥力获取合力；

根据所述合力对所述路径规划点X_new进行拟合，以获取最终路径。

4.根据权利要求1所述的授粉机器人机械臂避障路径规划方法，其特征在于，根据所述最终路径对所述机械臂进行控制，包括：

通过逆运动学反解以计算出所述机械臂各个关节所需要旋转的弧度；

根据所述弧度对所述机械臂进行控制。

5.一种授粉机器人机械臂避障路径规划装置，其特征在于，包括：

第一获取模块，所述第一获取模块用于通过深度相机获取深度图像，并根据所述深度图像获取障碍物位姿和所述机械臂的授粉目标点位姿；

第二获取模块，所述第二获取模块用于根据所述障碍物位姿和所述授粉目标点位姿，通过带线性回归的三维RRT算法获取初始路径；

优化模块，所述优化模块用于通过A*算法对所述三维RRT算法进行优化，以获取最优路径；

平滑模块，所述平滑模块用于通过人工势场法对所述最优路径进行轨迹平滑，以获取最终路径；

控制模块，所述控制模块用于根据所述最终路径对所述机械臂进行控制；其中，所述第二获取模块具体用于：

以起始点X_init为圆心，以预设长度D为半径，在此三维空间里选取10个随机点作为新增节点X_rand进行扩展，其中，所述起始点为机械臂末端中心点所在位置；

分别判断所述新增节点X_rand与授粉目标点X_goal的距离；

如果所述新增节点X_rand与所述授粉目标点X_goal的距离小于预设值，则所述新增节点X_rand停止扩展；

如果所述新增节点X_rand与所述授粉目标点X_goal的距离大于或等于预设值，则进一步判断所述新增节点X_rand和随机树上其他任意节点X的距离是否大于所述新增节点X_rand和其父节点X_par的距离；

如果是，则将该新增节点X_rand设为路径规划点X_new；

根据所述路径规划点X_new获取初始路径；

所述优化模块具体用于：

利用A*算法的估价函数估算每个新增节点X_rand到授粉目标点x_goal的路径估计值；

根据路径估计值获取最优路径。

6.根据权利要求5所述的授粉机器人机械臂避障路径规划装置，其特征在于，所述第一获取模块具体用于：

根据所述深度图像识别花蕊的顶部中心点和底部坐标点；

根据所述顶部中心点和所述底部坐标点基于世界坐标系下的Y轴的数值，判断是否对当前花蕊进行授粉；

如果是，则根据所述深度图像识别花蕊位姿和所述障碍物的位姿，将所述花蕊位姿用四元数位姿表示；

根据所述四元数位姿反推得所述机械臂的授粉目标点位姿。

Images