基于动态权重的A*算法的森林作业机器人路径规划方法

基于动态权重的a*算法的森林作业机器人路径规划方法
技术领域
1.本发明属于移动机器人导航和路径规划领域，具体涉及一种基于动态权重的a*算法的森林作业机器人路径规划方法。
技术背景
2.由于我国林业生产经营多在偏远山区，林间环境复杂、运行条件困难等，使得传统农机穿行困难，同时环境信号覆盖率低，对森林作业机器人信号传输和处理有一定的挑战。随着科学技术的发展，对于森林作业机器人，主要的研究方向有北斗卫星定位技术、无线通讯技术、智能控制技术与模式识别等高新技术，通过这些技术，可以解决林业作业过程中目标识别、避障、路径规划、智能控制等科学问题，促进了信息获取、通讯处理、控制应用等林业机械信息学的应用，推动了现代信息及控制技术与农机装备融合发展。而路径规划作为森林作业机器人实现智能化的关键技术。
3.根据是否已知运行环境信息，可将路径规划算法分为全局路径规划算法和局部路径规划算法；在全局路径规划算方法中，启发式a*搜索算法应用地最为广泛，传统的启发式a*搜索算法在面对拥有较多障碍物的复杂环境时，会进行较多无用节点的搜索，导致运算量过大、占用计算内存过多和寻路时间长等问题，并且容易陷入局部极小而无法搜索全局最优解、存在路径转角过大等问题。


技术实现要素：

4.本发明的目的在于提出一种基于动态权重的a*算法的森林作业机器人路径规划方法。
5.实现本发明目的的技术解决方案为：一种基于动态权重的a*算法的森林作业机器人路径规划方法，具体包括如下步骤：s1，使用双目深度相机和多线激光雷达，采集森林作业机器人运行时的森林地图信息，构建二维的栅格地图模型；s2，基于二维栅格地图模型，确定森林作业机器人的起始节点s和目标节点g；s3，循环执行基于搜索优先邻域a*算法，在二维栅格地图模型上搜索路径，获得规划路径；s4，基于动态权重的非均匀有理b样条曲线对规划路径进行平滑处理，得到最终路径。
6.进一步的，s1，使用双目深度相机和多线激光雷达采集森林作业机器人运行时的森林地图信息，构建二维的栅格-拓扑双层地图模型，具体方法为：使用奥比中光astra pro深度相机和镭神16线激光雷达采集森林作业机器人运行时的森林地图信息，包括树木、坡度和坑洼信息，构建二维的栅格地图模型，所述栅格地图模型以左下角为坐标原点，横向为x轴，纵向为y轴；采用位置节点映射矩阵存储栅格地图模型，位置节点映射矩阵表示为
，m为矩阵的行数，n为矩阵的列数，m
×
n为采集到的森林地图的大小，障碍物树木所在的位置节点表示为，即森林作业机器人不可通行，，n
t
为障碍物树木的数量，山坡、坑洼所在的位置节点表示为，为森林作业机器人通过山坡、坑洼时的实际等效路径长度，，nh为山坡、坑洼的数量，栅格地图模型上其他的位置节点表示为，即森林作业机器人通过这些位置节点时的实际路径长度为10，，no为二维栅格地图模型上除了树木、山坡和坑洼之外的位置节点的数量；将位置节点映射矩阵元素用栅格序号表示，其中n=m
×
n，矩阵元素的坐标与序号之间的关系由下式确定：。
7.进一步的，s3，循环执行基于搜索优先邻域a*算法，在二维栅格地图模型上搜索路径，获得规划路径，具体方法为：步骤s3.1，分别建立open和close两个列表，用于储存待检测节点和已检测节点或不需要检测的节点，将起始节点s放入open表中；步骤s3.2，判断open列表是否为空，若open表中无节点，则表示路径规划失败，结束寻路；若open表中有节点，则转至步骤s3.3；步骤s3.3，判断目标节点g是否在open列表中，若是，则转至步骤s3.6；若否，则计算open列表中节点的全局代价函数f(n)值，将具有最小f(n)值的节点从open列表中删除，将其放入close列表中，此时close列表中最后一个节点即为当前节点n；其中，引入动态权重、惩罚函数确定全局代价函数f(n)，表达式为：其中，引入动态权重、惩罚函数确定全局代价函数f(n)，表达式为：其中，引入动态权重、惩罚函数确定全局代价函数f(n)，表达式为：其中，引入动态权重、惩罚函数确定全局代价函数f(n)，表达式为：其中，引入动态权重、惩罚函数确定全局代价函数f(n)，表达式为：其中，引入动态权重、惩罚函数确定全局代价函数f(n)，表达式为：
其中，为全局代价函数；为真实代价函数，表示起始节点s到当前节点n的实际路径长度；为启发函数，表示从当前节点n到目标节点g的估计路径长度，采用欧几里得距离；为起始节点s基于二维栅格地图模型的坐标，为当前节点n基于二维栅格地图模型的坐标，为目标节点g基于二维栅格地图模型的坐标，为当前节点n的父节点基于二维栅格地图模型的坐标，为父节点的父节点基于二维栅格地图模型的坐标；为向上取整函数；分别为动态权重，由起始节点s、当前节点n和目标节点g确定；为起始节点s指向当前节点n的方向向量与起始节点s指向目标节点g的方向向量之间的夹角，为当前节点n指向目标节点g的方向向量与起始节点s指向目标节点g的方向向量之间的夹角；为惩罚函数，由障碍物树木、森林作业机器人转向角度和当前节点n与理想路径之间的距离l(n)确定，所谓理想路径即起始节点s通往目标节点g的直线路径，k1、k2、k3为正的惩罚系数，为森林作业机器人转过角的次数，为父节点指向任意两个拓展节点方向向量之间的最大转折角，为父节点指向任意两个相邻拓展节点方向向量之间的转折角，s为可拓展节点数量的最大值，对于起始节点s，其父节点，，惩罚函数；步骤s3.4，以当前节点n为父节点，根据搜索优先邻域对父节点进行拓展，其中，搜索优先邻域由理想路径确定，具体方法为：基于二维栅格地图模型，求出理想路径的方向向量，进行规范化处理：向量，进行规范化处理：其中，为规范化后的理想路径的方向向量；若等于0，等于0，则结束寻路；若等于0，不等于0，则以当前节点n为父节点，根据理想路径的规范化方向向量依次得到搜索优先邻域中基于父节点的优先拓展节点、、、、；若不等于0，等于0，则搜索优先邻域中基于父节点的优先拓展节点依次为、、、、
；若不等于0，不等于0，则搜索优先邻域中基于父节点的优先拓展节点依次为、、、、；定义搜索优先邻域中障碍物树木计数为，依次判断优先拓展节点映射的矩阵元素值是否等于inf，若是，则障碍物树木计数值加一，即，若否，则值不变；步骤s3.5，判断障碍物树木计数值是否小于5，若是，则将得到的搜索优先邻域中的优先拓展节点放入open列表，并转至步骤s3.3；否则，根据节点拓展关系以当前节点n为父节点，依次拓展除搜索优先邻域外其它的节点，并根据拓展节点映射的矩阵元素值是否等于inf，更新值，将得到的拓展节点放入open列表，并转至步骤s3.3；步骤s3.6，在close列表中根据节点父子关系进行回溯，得到从起始节点s至目标节点g的规划路径，寻路完成。
8.进一步的，s4，基于动态权重的非均匀有理b样条曲线对规划路径进行平滑处理，得到最终路径，具体方法为：步骤s4.1，将规划路径代入非均匀有理b样条曲线中，利用积累弦长法计算参数值，从而得到参数化节点矢量，其中非均匀有理b样条曲线表达式为：值，从而得到参数化节点矢量，其中非均匀有理b样条曲线表达式为：其中：其中：式中：为非均匀有理b样条曲线上的位置向量；ci为非均匀有理b样条曲线的控制节点；为当前控制节点基于二维栅格地图模型的坐标；ui为与控制节点相对应的参数化点，集合为参数化节点矢量；为动态权重，与控制节点相联系；为当前控制节点指向上一个控制节点的方向矢量和指向下一个控制节点的方向矢量之间的夹角；k为基函数的次数；是由参数化节点矢量按德布尔-考克斯递推公式决定的k次规范b样条基函数，其递推公式为：规定，将起始节点s和目标节点g的重复度取为4，对积累弦长参数化法进行规
范化处理，得到参数化节点矢量u：范化处理，得到参数化节点矢量u：范化处理，得到参数化节点矢量u：范化处理，得到参数化节点矢量u：式中，为规划路径中基于二维栅格地图模型的各路径节点的位置信息；步骤s4.2，利用切矢条件确定边界条件，切矢条件满足方程组：式中，，与分别为起始节点s和目标节点g的切矢量；步骤s4.3，采用切矢边界条件得到带权控制节点矩阵，求解全部控制节点，具体方法为：对于三次非均匀有理b样条曲线，起始节点s和目标节点g为规划路径首末端的控制节点，即,，得到如下线性方程组：其中：其中：其中：其中：其中：其中：
求解线性方程组，得到全部控制节点；步骤s4.4，采用de-boor递推公式拟合三次非均匀有理b样条曲线，得到最终路径。
9.一种基于动态权重的a*算法的森林作业机器人路径规划系统，基于所述的森林作业机器人路径规划方法，实现基于动态权重的a*算法的森林作业机器人路径规划。
10.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时，基于所述的森林作业机器人路径规划方法，实现基于动态权重的a*算法的森林作业机器人路径规划。
11.一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时，基于所述的森林作业机器人路径规划方法，实现基于动态权重的a*算法的森林作业机器人路径规划。
12.本发明与现有技术相比，其显著优点在于：1）采用双目深度相机和多线激光雷达进行环境建模，使获得的环境地图更为准确。2）采用基于搜索优先邻域a*算法搜索路径，搜索节点更加合理，减少了不必要的内存消耗，提高了算法搜索效率。3）考虑到森林作业机器人的最小转向半径，采用基于动态权重的非均匀有理b样条曲线对规划路径进行平滑处理，得到的最终路径更可靠且更符合森林作业机器人的行驶轨迹。
附图说明
13.图1为实施例中的总体流程图；图2为实施例中将坡度与坑洼等效为球体部分图；图3为实施例中拓展方向及相应的路径代价图；图4为实施例中森林作业机器人运行环境的二维栅格地图模型图；图5为实施例中基于搜索优先邻域a*算法的路径规划流程图；图6为实施例中各动态权重和距离l(n)示意图；图7为实施例中各转折角示意图；图8为实施例中基于二维栅格地图的搜索优先邻域图；图9为实施例中基于动态权重的非均匀有理b样条曲线的路径优化流程图；图10为实施例中非均匀有理b样条曲线的动态权重示意图。
具体实施方式
14.为了使本技术的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本技术进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本技术，并不用于限定本技术。
15.本发明一种基于动态权重的a*算法的森林作业机器人路径规划方法，实现总体流程图如图1所示，具体包括以下步骤：s1，使用双目深度相机和多线激光雷达采集森林作业机器人运行时的森林地图信息，基于采集到的地图信息构建二维的栅格地图模型，具体包括以下过程：使用奥比中光astra pro深度相机和镭神16线激光雷达采集森林作业机器人运行时的森林地图信息，包括树木、坡度和坑洼等信息，基于以上采集到的信息，构建二维的栅
格地图模型，在该地图模型中以左下角为坐标原点，横向为x轴，纵向为y轴；采用矩阵存储栅格地图模型，栅格地图模型中每一个位置节点映射矩阵中的第i行、第j列的元素，表示为，m为矩阵的行数，n为矩阵的列数，m
×
n为采集到的森林地图的大小，障碍物树木在矩阵中表示为，即森林作业机器人不可通行，，n
t
为障碍物树木的数量；针对山坡、坑洼等较为复杂的环境，将山坡、坑洼等效为球体的一部分，如图2所示，通过计算球体的圆弧长度，可以得到经过该山坡或者坑洼的等效路径长度，它们的等效路径长度是相同的，在本实施例中取，此时等效路径长度为16，即该类位置节点在矩阵中表示为，nh为山坡、坑洼等的数量；栅格地图模型上其他的位置节点在矩阵中表示为，即森林作业机器人通过这些位置节点时的实际路径长度为10，，no为二维栅格地图模型上除了障碍物、山坡和坑洼之外的位置节点的数量；又根据拓展规则，可拓展节点的最大值为8，其拓展方向有8个方向，令沿y轴正向为上，x轴正向为右，则8个拓展方向为上、下、左、右、左上、左下、右上、右下，其中往上、下、左、右等方向拓展时森林作业机器人通过的实际路径长度为10，拓展方向为左上、左下、右上、右下方向时森林作业机器人通过的实际路径长度为即14，如图3所示，即往不同的方向拓展时，真实代价函数根据通过环境的不同，需加上上述的实际路径长度或者等效路径长度；可以得到最终建立的二维栅格地图模型，如图4所示，其中圆圈代表障碍物树木等，不可通行，填充颜色深浅不同的圆表示山坡或坑洼；s2，基于二维栅格地图模型，确定森林作业机器人的起始节点s和目标节点g；s3，循环执行基于搜索优先邻域a*算法，在二维栅格地图模型上搜索路径，获得规划路径，流程图如图5所示，具体包括以下过程：步骤s3.1，分别建立open和close两个列表，用于储存待检测节点和已检测节点或不需要检测的节点，将起始节点s放入open表中；步骤s3.2，判断open列表是否为空，若open表中无节点，则表示路径规划失败，结束寻路；若open表中有节点，则转至步骤s3.3；步骤s3.3，判断目标节点g是否在open列表中，若是，则转至步骤s3.6；若否，则计算open列表中节点的全局代价函数f(n)值，将具有最小f(n)值的节点从open列表中删除，将其放入close列表中，此时close列表中最后一个节点即为当前节点n；其中，引入动态权重、惩罚函数确定全局代价函数f(n)，表达式为：其中，引入动态权重、惩罚函数确定全局代价函数f(n)，表达式为：其中，引入动态权重、惩罚函数确定全局代价函数f(n)，表达式为：其中，引入动态权重、惩罚函数确定全局代价函数f(n)，表达式为：
其中，为全局代价函数；为真实代价函数，表示起始节点s到当前节点n的实际路径长度；为启发函数，表示从当前节点n到目标节点g的估计路径长度，采用欧几里得距离；为起始节点s基于二维栅格地图模型的坐标，为当前节点n基于二维栅格地图模型的坐标，为目标节点g基于二维栅格地图模型的坐标，为当前节点n的父节点基于二维栅格地图模型的坐标，为父节点的父节点基于二维栅格地图模型的坐标；为向上取整函数；分别为动态权重，由起始节点s、当前节点n和目标节点g确定；为起始节点s指向当前节点n的方向向量与起始节点s指向目标节点g的方向向量之间的夹角，为当前节点n指向目标节点g的方向向量与起始节点s指向目标节点g的方向向量之间的夹角，动态权重和距离示意图如图6所示；为惩罚函数，由障碍物树木、森林作业机器人转向角度和当前节点n与理想路径之间的距离l(n)确定，所谓理想路径即起始节点s通往目标节点g的直线路径，k1、k2、k3为正的惩罚系数，为森林作业机器人转过角的次数，为父节点指向任意两个拓展节点方向向量之间的最大转折角，本实施例中取，为父节点指向任意两个相邻拓展节点方向向量之间的转折角，本实施例中取，s为可拓展节点的最大值，本实施例中s=8，各转折角如图7所示；对于起始节点s，其父节点，惩罚函数；步骤s3.4，以当前节点n为父节点，根据搜索优先邻域对父节点进行拓展，其中，搜索优先邻域由理想路径确定，搜索优先邻域示意图如图8所示，具体方法为：基于二维栅格地图模型，求出理想路径的方向向量，进行规范化处理：
其中，为规范化后的理想路径的方向向量；若等于0，等于0，则结束寻路；若等于0，不等于0，则以当前节点n为父节点，根据理想路径的规范化方向向量依次得到搜索优先邻域中基于父节点的优先拓展节点、、、、；若不等于0，等于0，则搜索优先邻域中基于父节点的优先拓展节点依次为、、、、；若不等于0，不等于0，则搜索优先邻域中基于父节点的优先拓展节点依次为、、、、；定义搜索优先邻域中障碍物树木计数为，依次判断优先拓展节点映射的矩阵元素值是否等于inf，若是，则障碍物树木计数值加一，即，若否，则值不变；步骤s3.5，判断障碍物树木计数值是否小于5，若是，则将得到的搜索优先邻域中的优先拓展节点放入open列表，并转至步骤s3.3；若否，则根据节点拓展关系以当前节点n为父节点，依次拓展除搜索优先邻域外其它的节点，并判断拓展节点映射的矩阵元素值是否等于inf，若是，则障碍物树木计数值加1，即，若否，则值不变；将得到的拓展节点放入open列表，并转至步骤s3.3；步骤s3.6，在close列表中根据节点父子关系进行回溯，得到从起始节点s至目标节点g的规划路径，寻路完成；s4，基于动态权重的非均匀有理b样条曲线(nurbs)对规划路径进行平滑处理，得到最终路径，图9为对规划路径平滑处理流程图，具体包括以下过程：步骤s4.1，将得到的规划路径代入非均匀有理b样条曲线(nurbs)中，利用积累弦长法计算参数值，从而得到参数化节点矢量，其中非均匀有理b样条曲线表达式为：
其中：其中：式中：为参数为u时非均匀有理b样条曲线上的位置向量；ci为非均匀有理b样条曲线的控制节点；为当前控制节点i基于二维栅格地图模型的坐标；ui为与控制节点相对应的参数化点，节点的集合为参数化节点矢量；为动态权重，与控制节点相联系；为当前控制节点i指向上一个控制节点i-1的方向矢量和指向下一个控制节点i 1的方向矢量之间的夹角，如图10所示；k为基函数的次数；是由参数化节点矢量按德布尔-考克斯递推公式决定的k次规范b样条基函数，其递推公式为规定，将规划路径中起始节点和目标节点的重复度设置为4，对积累弦长参数化法进行规范化处理，可得参数化节点矢量u：式中，为规划路径中基于二维栅格地图模型的各路径节点的位置信息；步骤s4.2，利用切矢条件确定边界条件，切矢条件满足方程组：式中，，与分别为起始节点s和目标节点g的切矢量，本实施例中规划路径中起始节点s的切矢量为(0，1)，目标节点g的切矢量为(-1，0)；步骤s4.3，采用切矢边界条件可得带权控制节点矩阵，求解线性方程组，得到全部控制节点，具体方法为：对于三次nurbs曲线，起始节点s和目标节点g为规划路径首末端的控制节点，即，可得如下线性方程组：
其中：其中，求解线性方程组，可得全部控制节点；步骤s4.4，采用de-boor递推公式拟合三次非均匀有理b样条曲线，得到最终路径，考虑到森林作业机器人的最小转向半径，得到的最终路径更符合森林作业机器人的行驶路径。
16.本发明还提出一种基于动态权重的a*算法的森林作业机器人路径规划系统，基于所述的森林作业机器人路径规划方法，实现基于动态权重的a*算法的森林作业机器人路径规划。
17.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时，基于所述的森林作业机器人路径规划方法，实现基于动态权重的a*算法的森林作业机器人路径规划。
18.一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时，基于所述的森林作业机器人路径规划方法，实现基于动态权重的a*算法的森林作业机器人路径规划。
19.综上所述，本发明使森林作业机器人在树木等障碍物较多的复杂森林环境下也能得到理想的路径，搜索节点更加合理，减少了不必要的内存消耗，提高了算法搜索效率，考虑到森林作业机器人的最小转向半径，利用基于动态权重的非均匀有理b样条曲线对规划路径进行平滑处理，得到的最终路径更平滑且更符合森林作业机器人的行驶轨迹，极大提
高了机器人的工作效率。
20.以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
21.以上所述实施例仅表达了本技术的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本技术范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本技术构思的前提下，还可以做出若干变形和改进，这些都属于本技术的保护范围。因此，本技术的保护范围应以所附权利要求为准。