本发明主要涉及挖掘机稳定性自动控制技术领域，具体涉及一种挖掘机铲斗的作业轨迹规划方法、系统、介质及设备。
背景技术：
挖掘机在人为操纵或者无人控制过程中，因为操作判断失误可能引起挖掘机整机发生倾覆、滑移等失去稳定性的风险，若不采取干预措施，将可能造成人员伤害和经济损失。因此在挖掘机作业过程中，实时估计整机稳定性，并参照其进行作业尤为重要。
技术实现要素：
本发明要解决的技术问题就在于：针对现有技术存在的问题，本发明提供一种保障作业稳定性的挖掘机铲斗的作业轨迹规划方法、系统、介质及设备。
为解决上述技术问题，本发明提出的技术方案为：
一种挖掘机铲斗的作业轨迹规划方法，包括步骤：
1)基于挖掘机倾覆力矩和滑移力计算得到最大挖掘力分布；
2)获取挖掘机作业时的实时挖掘阻力；
3)基于最大挖掘力分布和实时挖掘阻力，得到当前作业下的稳定区域和非稳定区域；
4)判断当前轨迹是否会与非稳定区域发生碰撞；若发生碰撞，则停止挖掘机的运行，重新规划作业轨迹。
优选地，在步骤4)中，重新规划作业轨迹的步骤为：
4.1)将稳定区域作为地图引入rrt*算法，以稳定区域作为可行域，非稳定区域作为不可行域；
4.2)以铲斗在运动过程中扫过的面积作为路径代价，在一定范围内进行搜索，按照路径代价最高且不通过非稳定区为原则生成新的作业轨迹。
优选地，在步骤4.2)之后，还包括步骤4.3)，采用多项式对新的作业轨迹进行平滑处理，得到最终的作业轨迹。
优选地，在步骤4.2)中，以搜索起点为原点对铲斗齿尖轨迹进行搜索，将0≤x≤2m，-m≤y≤m的范围作为搜索空间，其中m为铲斗齿尖到斗杆-铲斗的转动节点的距离，搜索终点为(2m,0)。
优选地，步骤1)的具体过程为：
1.1)以可能发生倾覆的转动中心线作为倾覆线，以前倾计算倾覆力矩，得到其稳定力fi1；
fi1≤[g0(xt-xg0)-[g1(xg1-xt) g10(xg10-xt) g2(xg2-xt) g20(xg20-xt) g30(xg30-xt) grk(xrk-xt) grd(xrd-xt) g3(xg3-xt)]]/-[sn(γ)(xj-xt)-cos(γ)(yj-yt)]
以后倾计算倾覆力矩，得到其稳定力fi2；
fi2≤[g0(xg0-xs) g1(xg1-xs) g10(xg10-xs) g2(xg2-xs) g20(xg20-xs) g30(xg30-xs) grk(xrk-xs) grd(xrd-xs) g3(xg3-xs)]/[sin(γ)(xj-xs)-cos(γ)(yj-ys)]
取挖掘机与地面之间的附着系数μ，计算滑移力大小，得到其稳定力fi3；
fi3≤μ(g0 g1 g10 g2 g20 g3 grk grd)/ⅰcos(γ)ⅰ
其中，gi(i＝0,1,2,3)分别为主机、动臂、斗杆和铲斗自身的质量；gi0(i＝1,2,3)分别为动臂、斗杆和铲斗的液压缸重力；grk和grk分别为摇杆和连杆的重力；fik(k＝1,2,3)分别为某种姿态下的挖掘力；相应的，xgi(i＝0,1,2,3)分别为主机、动臂、斗杆和铲斗质心横坐标，xgi0(i＝1,2,3)分别为动臂、斗杆和铲斗液压缸质心的横坐标；xj和yj(j＝t,s)分别为前倾线t和后倾线s的横坐标和纵坐标；xrk和xrd分别为摇杆和连杆的横坐标，γ为挖掘阻力在直角坐标系中的方向角；
1.2)将上述方程化为等式，求解出fi1、fi2和fi3，计算fi＝min{fi1,fi2,fi3}，则fi一定满足上述所有不等式；
1.3)在作业空间内求解离散空间下的fi分布，得到保障整机稳定性的最大挖掘力分布。
优选地，步骤2)的具体过程为:
2.1)计算水平挖掘阻力fh:
fh＝wd(1 cotβcotδ)a1((dgε/2) c*cotφ) gq buried*(d-lsinβ)(gε((1-sinφ)/(1 sinφ))) web(1 tanδcotab)a2((ebgε/2) c*cotφ gq dgε((1-sinφ)/(1 sinφ))) 2sda3((dgε/2) c*cotφ gq buried*(d-lssinβ)(gε((1-sinφ)/(1 sinφ))) 4tanδa4lsd((dgε/2) c*cotφ gq buried*(d-lsinβ)(gε((1-sinφ)/(1 sinφ)))
计算垂直挖掘阻力fv:
fv＝fhcot(β δ)
其中，w,d和l分别为铲斗宽度，深度和斜边长度，g为重力加速度，ε为土壤密度，β为倾角，δ为外摩擦角，φ为内摩擦角，eb为钝边厚度，ab为钝边角，c为内聚力系数，q为装载附加质量，ls为边长，buried取决于铲斗是否处于地面之下，如是则为1，否则为0；ai(i＝1,2,3,4)为几何因子，取决于一个表面相对于一个参考平面的角度；
2.2)fv和fh的合力f则为实时挖掘阻力。
优选地，步骤3)的具体过程为：通过计算的挖掘阻力f，结合最大挖掘力分布进行匹配，使最大挖掘力fi减去挖掘阻力f，取所有大于等于零的点，得到空间内的离散挖掘稳定区域，其它小于零的点则为空间内的离散挖掘非稳定区域。
本发明还公开了一种挖掘机铲斗的作业轨迹规划系统，包括：
第一程序模块，用于基于挖掘机倾覆力矩和滑移力计算得到最大挖掘力分布；
第二程序模块，用于获取挖掘机作业时的实时挖掘阻力；
第三程序模块，用于基于最大挖掘力分布和实时挖掘阻力，得到当前作业下的稳定区域和非稳定区域；
第四程序模块，用于判断当前轨迹是否会与非稳定区域发生碰撞；若发生碰撞，则停止挖掘机的运行，重新规划作业轨迹。
本发明进一步公开了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序在被处理器运行时执行如上所述的挖掘机铲斗的作业轨迹规划方法的步骤。
本发明还公开了一种计算机设备，包括存储器和处理器，所述存储器上存储有计算机程序，所述计算机程序在被处理器运行时执行如上所述的挖掘机铲斗的作业轨迹规划方法的步骤。
与现有技术相比，本发明的优点在于：
(1)本发明在作业前对挖掘机进行基于稳定性的最大挖掘力分析，得到挖掘机的挖掘力分布，再对挖掘机作业时所受的挖掘阻力进行实时估计计算，得到挖掘机作业时的稳定、非稳定区域分布，基于此来进行铲斗作业的轨迹规划，保障挖掘机在作业时始终保持在稳定区域内，避免产生倾覆、滑移等危害整机安全的情况，保障挖掘机作业时的稳定性。
(2)本发明采用rrt*算法，可以不包含系统模型，且不需要栅格地图，采用增量式增长的随机采样算法，随机的在待搜索空间中进行采样，可以保证在短时间内得到符合要求的轨迹，并且能够得到最优的轨迹。
(3)本发明在进行轨迹规划时，将物料量装载作为两个相邻节点之间的路径代价，能够保障后续铲斗装载的物料量最多；同时在约定的搜索空间内进行搜索，保障了计算速度，节约了资源。
附图说明
图1为本发明的方法在实施例的流程图。
具体实施方式
以下结合说明书附图和具体实施例对本发明作进一步描述。
如图1所示，本实施例的挖掘机铲斗的作业轨迹规划方法，包括步骤：
1)基于挖掘机倾覆力矩和滑移力计算得到最大挖掘力分布；
2)获取挖掘机作业时的实时挖掘阻力；
3)基于最大挖掘力分布和实时挖掘阻力，得到当前作业下的稳定区域和非稳定区域；
4)判断当前轨迹是否会与非稳定区域发生碰撞；若发生碰撞，则停止挖掘机的运行，重新规划作业轨迹。
本发明在作业前对挖掘机进行基于稳定性的最大挖掘力分析，得到挖掘机的挖掘力分布，再对挖掘机作业时所受的挖掘阻力进行实时估计计算，得到挖掘机作业时的稳定、非稳定区域分布，基于此来进行铲斗作业的轨迹规划，保障挖掘机在作业时始终保持在稳定区域内，避免产生倾覆、滑移等危害整机安全的情况，保障挖掘机作业时的稳定性。
本发明可在无人挖掘机作业时，进行基于整机稳定性的作业轨迹自动规划；或者应急工况下，作为机器接管形式的保障整机稳定性控制。
在一具体实施例中，步骤1)中的最大挖掘力分布在图形中体现，其利用挖掘机本身参数，即各个部件的质量、质心位置和转动惯量进行计算而得到，具体过程为：
1.1)以可能发生倾覆的转动中心线作为倾覆线，首先以前倾计算倾覆力矩，得到其稳定力大小fi1；
fi1≤[g0(xt-xg0)-[g1(xg1-xt) g10(xg10-xt) g2(xg2-xt) g20(xg20-xt) g30(xg30-xt) grk(xrk-xt) grd(xrd-xt) g3(xg3-xt)]]/-[sin(γ)(xj-xt)-cos(γ)(yj-yt)]
以后倾计算倾覆力矩，得到其稳定力fi2；
fi2≤[g0(xg0-xs) g1(xg1-xs) g10(xg10-xs) g2(xg2-xs) g20(xg20-xs) g30(xg30-xs) grk(xrk-xs) grd(xrd-xs) g3(xg3-xs)]/[sin(γ)(xj-xs)-cos(γ)(yj-ys)]
取一般挖掘机与地面之间的附着系数μ，计算滑移力大小，得到其稳定力大小fi3；
fi3≤μ(g0 g1 g10 g2 g20 g3 grk grd)/ⅰcos(γ)ⅰ
其中，gi(i＝0,1,2,3)分别为主机、动臂、斗杆和铲斗自身的质量；gi0(i＝1,2,3)分别为动臂、斗杆和铲斗的液压缸重力；grk和grk分别为摇杆和连杆的重力；fik(k＝1,2,3)分别为某种姿态下的挖掘力；相应的，xgi(i＝0,1,2,3)分别为主机、动臂、斗杆和铲斗质心横坐标，xgi0(i＝1,2,3)分别为动臂、斗杆和铲斗液压缸质心的横坐标；xj和yj(j＝t,s)分别为前倾线t和后倾线s的横坐标和纵坐标；xrk和xrd分别为摇杆和连杆的横坐标，γ为挖掘阻力在直角坐标系中的方向角；
1.2)将上述方程化为等式，求解出fi1、fi2和fi3各值，计算fi＝min{fi1,fi2,fi3}，则fi一定满足上述所有不等式；
1.3)在作业空间内求解离散空间下的fi分布，得到保障整机稳定性的最大挖掘力分布图。
在一具体实施例中，步骤2)的具体过程为：
2.1)计算水平挖掘阻力fh:
fh＝wd(1 cotβcotδ)a1((dgε/2) c*cotφ) gq buried*(d-lsinβ)(gε((1-sinφ)/(1 sinφ))) web(1 tanδcotab)a2((ebgε/2) c*cotφ gq dgε((1-sinφ)/(1 sinφ))) 2sda3((dgε/2) c*cotφ gq buried*(d-lssinβ)(gε((1-sinφ)/(1 sinφ))) 4tanδa4lsd((dgε/2) c*cotφ gq buried*(d-lsinβ)(gε((1-sinφ)/(1 sinφ)))
计算垂直挖掘阻力fv:
fv＝fhcot(β δ)
其中，w,d和l分别为铲斗宽度，深度和斜边长度，g为重力加速度，ε为土壤密度，β为倾角，δ为外摩擦角，φ为内摩擦角，eb为钝边厚度，ab为钝边角，c为内聚力系数，q为装载附加质量，ls为边长，buried取决于铲斗是否处于地面之下，如是则为1，否则为0；ai(i＝1,2,3,4)为几何因子，取决于一个表面相对于一个参考平面的角度；
2.2)fv和fh的合力f则为计算挖掘阻力。
在一具体实施例中，步骤3)的具体过程为：通过计算的挖掘阻力f，结合最大挖掘力分布图进行匹配，使最大挖掘力fi减去计算挖掘阻力f，取所有大于等于零的点，得到空间内的离散挖掘稳定区域，其它小于零的点则为空间内的离散挖掘非稳定区域。
在一具体实施例中，在步骤4)中，根据在作业前预知的作业轨迹，实时判断下一刻路径(即作业轨迹)是否与非稳定区域发生碰撞；若未发生碰撞则继续按原轨迹运行，若发生碰撞，则停止按原轨迹运行，在当前时刻位置停止，进行路径重搜索，具体过程为：
4.1)将稳定区作为地图引入rrt*算法，以稳定区域作为可行域，非稳定区域作为不可行域；
4.2)在保障挖掘机作业稳定的同时，在一次挖掘过程中装载的物料量最大作为约束，使用非稳定区域作为障碍物进行搜索树搜索；
将物料量装载作为两个相邻节点之间的路径代价，即铲斗在运动过程中扫过的面积，路径代价越高装载的物料量越高，路径代价越低则装载的物料量越少；
其中m为铲斗齿尖到斗杆-铲斗的转动节点的距离，xt和xt 1为当前时刻和下一时刻的横坐标值，yt和yt 1为当前时刻和下一时刻的纵坐标值，ωt和ωt 1为当前时刻和下一时刻的铲斗角度值；
其中为保障计算速度，节约资源，不让搜索树在全局进行搜索，以搜索起点为原点对铲斗齿尖轨迹进行搜索，将0≤x≤2m，-m≤y≤m的范围作为搜索空间，其中m为铲斗齿尖到斗杆-铲斗的转动节点的距离，搜索终点为(2m,0)；
在每次得到新节点后，在其一定范围内进行搜索，按照路径代价最高且不通过非稳定区域为原则选择新的父节点；
4.3)由于rrt*算法是对可行区域的随机采样，因此，虽然得到的路径能满足始终在稳定区内且尽可能大的物料装载量的要求，但路径为一条折线，路径平滑度不足；为了保证路径的平滑度，采用多项式进行路径平滑，改善后的路径则具有很高的平滑度，且不会进入挖掘机非稳定区域，同时满足最大物料装载量的要求，保障运行稳定性。
本发明采用rrt*算法，可以不包含系统模型，且不需要栅格地图，采用增量式增长的随机采样算法，随机的在待搜索空间中进行采样，可以保证在短时间内得到符合要求的轨迹，并且能够得到最优的轨迹。
本发明还公开了一种挖掘机铲斗的作业轨迹规划系统，包括：
第一程序模块，用于基于挖掘机倾覆力矩和滑移力计算得到最大挖掘力分布；
第二程序模块，用于获取挖掘机作业时的实时挖掘阻力；
第三程序模块，用于基于最大挖掘力分布和实时挖掘阻力，得到当前作业下的稳定区域和非稳定区域；
第四程序模块，用于判断当前轨迹是否会与非稳定区域发生碰撞；若发生碰撞，则停止挖掘机的运行，重新规划作业轨迹。
本发明的挖掘机铲斗的作业轨迹规划系统，与上述的作业轨迹规划方法相对应，同样具有如上方法所述的优点。
本发明进一步公开了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序在被处理器运行时执行如上所述的挖掘机铲斗的作业轨迹规划方法的步骤。本发明还公开了一种计算机设备，包括存储器和处理器，所述存储器上存储有计算机程序，所述计算机程序在被处理器运行时执行如上所述的挖掘机铲斗的作业轨迹规划方法的步骤。本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，计算机程序可存储于一个计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、电载波信号、电信信号以及软件分发介质等。存储器可用于存储计算机程序和/或模块，处理器通过运行或执行存储在存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现各种功能。存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(smartmediacard,smc)，安全数字(securedigital,sd)卡，闪存卡(flashcard)、至少一个磁盘存储器件、闪存器件、或其它易失性固态存储器件等。
以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。
1.一种挖掘机铲斗的作业轨迹规划方法，其特征在于，包括步骤：
1)基于挖掘机倾覆力矩和滑移力计算得到最大挖掘力分布；
2)获取挖掘机作业时的实时挖掘阻力；
3)基于最大挖掘力分布和实时挖掘阻力，得到当前作业下的稳定区域和非稳定区域；
4)判断当前轨迹是否会与非稳定区域发生碰撞；若发生碰撞，则停止挖掘机的运行，重新规划作业轨迹。
2.根据权利要求1所述的挖掘机铲斗的作业轨迹规划方法，其特征在于，在步骤4)中，重新规划作业轨迹的步骤为：
4.1)将稳定区域作为地图引入rrt*算法，以稳定区域作为可行域，非稳定区域作为不可行域；
4.2)以铲斗在运动过程中扫过的面积作为路径代价，在一定范围内进行搜索，按照路径代价最高且不通过非稳定区为原则生成新的作业轨迹。
3.根据权利要求2所述的挖掘机铲斗的作业轨迹规划方法，其特征在于，在步骤4.2)之后，还包括步骤4.3)，采用多项式对新的作业轨迹进行平滑处理，得到最终的作业轨迹。
4.根据权利要求2或3所述的挖掘机铲斗的作业轨迹规划方法，其特征在于，在步骤4.2)中，以搜索起点为原点对铲斗齿尖轨迹进行搜索，将0≤x≤2m，-m≤y≤m的范围作为搜索空间，其中m为铲斗齿尖到斗杆-铲斗的转动节点的距离，搜索终点为(2m,0)。
5.根据权利要求1～3中任意一项所述的挖掘机铲斗的作业轨迹规划方法，其特征在于，步骤1)的具体过程为：
1.1)以可能发生倾覆的转动中心线作为倾覆线，以前倾计算倾覆力矩，得到其稳定力fi1；
fi1≤[g0(xt-xg0)-[g1(xg1-xt) g10(xg10-xt) g2(xg2-xt) g20(xg20-xt) g30(xg30-xt) grk(xrk-xt) grd(xrd-xt) g3(xg3-xt)]]/-[sin(γ)(xj-xt)-cos(γ)(yj-yt)]
以后倾计算倾覆力矩，得到其稳定力fi2；
fi2≤[g0(xg0-xs) g1(xg1-xs) g10(xg10-xs) g2(xg2-xs) g20(xg20-xs) g30(xg30-xs) grk(xrk-xs) grd(xrd-xs) g3(xg3-xs)]/[sin(γ)(xj-xs)-cos(γ)(yj-ys)]
取挖掘机与地面之间的附着系数μ，计算滑移力大小，得到其稳定力fi3；
fi3≤μ(g0 g1 g10 g2 g20 g3 grk grd)/ⅰcos(γ)ⅰ
其中，gi(i＝0,1,2,3)分别为主机、动臂、斗杆和铲斗自身的质量；gi0(i＝1,2,3)分别为动臂、斗杆和铲斗的液压缸重力；grk和grk分别为摇杆和连杆的重力；fik(k＝1,2,3)分别为某种姿态下的挖掘力；相应的，xgi(i＝0,1,2,3)分别为主机、动臂、斗杆和铲斗质心横坐标，xgi0(i＝1,2,3)分别为动臂、斗杆和铲斗液压缸质心的横坐标；xj和yj(j＝t,s)分别为前倾线t和后倾线s的横坐标和纵坐标；xrk和xrd分别为摇杆和连杆的横坐标，γ为挖掘阻力在直角坐标系中的方向角；
1.2)将上述方程化为等式，求解出fi1、fi2和fi3，计算fi＝min{fi1,fi2,fi3}，则fi一定满足上述所有不等式；
1.3)在作业空间内求解离散空间下的fi分布，得到保障整机稳定性的最大挖掘力分布。
6.根据权利要求5所述的挖掘机铲斗的作业轨迹规划方法，其特征在于，步骤2)的具体过程为:
2.1)计算水平挖掘阻力fh:
fh＝wd(1 cotβcotδ)a1((dgε/2) c*cotφ) gq buried*(d-lsinβ)(gε((1-sinφ)/(1 sinφ))) web(1 tanδcotab)a2((ebgε/2) c*cotφ gq dgε((1-sinφ)/(1 sinφ))) 2sda3((dgε/2) c*cotφ gq buried*(d-lssinβ)(gε((1-sinφ)/(1 sinφ))) 4tanδa4lsd((dgε/2) c*cotφ gq buried*(d-lsinβ)(gε((1-sinφ)/(1 sinφ)))
计算垂直挖掘阻力fv:
fv＝fhcot(β δ)
其中，w,d和l分别为铲斗宽度，深度和斜边长度，g为重力加速度，ε为土壤密度，β为倾角，δ为外摩擦角，φ为内摩擦角，eb为钝边厚度，ab为钝边角，c为内聚力系数，q为装载附加质量，ls为边长，buried取决于铲斗是否处于地面之下，如是则为1，否则为0；ai(i＝1,2,3,4)为几何因子，取决于一个表面相对于一个参考平面的角度；
2.2)fv和fh的合力f则为实时挖掘阻力。
7.根据权利要求6所述的挖掘机铲斗的作业轨迹规划方法，其特征在于，步骤3)的具体过程为：通过计算的挖掘阻力f，结合最大挖掘力分布进行匹配，使最大挖掘力fi减去挖掘阻力f，取所有大于等于零的点，得到空间内的离散挖掘稳定区域，其它小于零的点则为空间内的离散挖掘非稳定区域。
8.一种挖掘机铲斗的作业轨迹规划系统，其特征在于，包括：
第一程序模块，用于基于挖掘机倾覆力矩和滑移力计算得到最大挖掘力分布；
第二程序模块，用于获取挖掘机作业时的实时挖掘阻力；
第三程序模块，用于基于最大挖掘力分布和实时挖掘阻力，得到当前作业下的稳定区域和非稳定区域；
第四程序模块，用于判断当前轨迹是否会与非稳定区域发生碰撞；若发生碰撞，则停止挖掘机的运行，重新规划作业轨迹。
9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序在被处理器运行时执行如权利要求1～7中任意一项所述的挖掘机铲斗的作业轨迹规划方法的步骤。
10.一种计算机设备，包括存储器和处理器，所述存储器上存储有计算机程序，其特征在于，所述计算机程序在被处理器运行时执行如权利要求1～7中任意一项所述的挖掘机铲斗的作业轨迹规划方法的步骤。
技术总结