CN113581198B - 一种基于泊车控制点的无人驾驶矿车泊车控制方法 - Google Patents

Abstract

本发明公开了一种基于泊车控制点的无人驾驶矿车泊车控制方法，具体步骤包括获取矿山的道路信息及挖机的定位信息；根据挖机定位信息确定矿车的转弯控制点和挖机主干道的最近点；利用得到的挖机主干道的最近点确定矿车的交接点和回程交接点；同时根据转弯控制点和矿车交接点确定矿车的倒车控制点。本发明通过获取矿山的道路信息及挖机的定位点信息，在矿车泊车路径过程中选取不同的作用的控制点来准确引导矿车，从而控制矿车在泊车时能够更加精准的停在装料点，解决了因挖矿点逐渐远离主干道导致的倒车距离增加，无法保证倒车过程中遇到障碍物泊车失败的问题，提高泊车的成功率和整体泊车效率。

Description

一种基于泊车控制点的无人驾驶矿车泊车控制方法

技术领域

本发明涉及无人驾驶矿车技术领域，特别涉及一种基于泊车控制点的无人驾驶矿车泊车控制方法。

背景技术

目前，无人驾驶矿车在矿山泊车装料过程中，均是通过在交接点直接倒车行驶到挖机定位点，此种泊车方式会出现多个问题，如：随着挖机不断的作业，挖矿点距离主路越来越远，而倒车的速度非常慢，从而影响了整个装载泊车效率；另外交接点和挖机定位点的连线不在挖机朝向上，虽然挖机朝向上没有障碍物,但不能保证倒车过车中遇到障碍物，从而导致泊车失败，其次倒车距离过远，也会导致泊车不准。

发明内容

本发明的目的克服现有技术存在的不足，为实现以上目的，采用一种基于泊车控制点的无人驾驶矿车泊车控制方法，以解决上述背景技术中提出的问题。

一种基于泊车控制点的无人驾驶矿车泊车控制方法，包括：

获取矿山的道路信息及挖机的定位信息；

根据挖机定位信息确定矿车的转弯控制点和挖机主干道的最近点；

利用得到的挖机主干道的最近点确定矿车的交接点和回程交接点；

同时根据转弯控制点和矿车交接点确定矿车的倒车控制点。

作为本发明的进一步的方案：所述根据挖机定位信息确定矿车的转弯控制点的具体步骤包括：

首先获取挖定位信息的定位点坐标及挖机朝向角；

根据定位点坐标及挖机朝向角，计算转弯控制点坐标；

所述坐标计算公式为：

其中，(x，y，θ)为挖机定位信息，L＝n*d+X为倒车距离，n*d为n个车位d的距离，X为挖机铲臂的长度。

作为本发明的进一步的方案：所述根据挖机定位信息确定挖机主干道的最近点的具体步骤包括：

获取矿山预设的道路信息，以及采集当前挖机的定位信息，其中定位信息包括挖机的经纬度及铲臂的朝向角信息；

根据道路信息的主干道每个点，及挖机的定位点，计算得到主干道每个点到定位点距离；

根据到主干道每个点到定位点距离最近的点作为挖机主干道的最近点；

所述最近点距离计算公式为：

式中，(x,y)为挖机的定位点坐标，(x_i,y_i)为主干道第i点坐标。

作为本发明的进一步的方案：所述利用得到的挖机主干道的最近点确定矿车的交接点和回程交接点的具体步骤包括:

获取挖机主干道的最近点及当前的挖机朝向角；

根据当前挖机朝向角判断挖机朝向的外延有向线段是否与主干道有交点；

若有交点则以此交点为起点回退预设距离S处为交接点；

若无交点则以最近点为起点回退预设距离S处为交接点；

同时以最近点为起点前推预设距离S′处为回程交接点。

作为本发明的进一步的方案：所述根据转弯控制点和矿车交接点确定矿车的倒车控制点的具体步骤包括：

获取转弯控制点及挖机定位点；

以转弯控制点为垂足做转弯控制点到挖机定位点的线段的垂线；

再根据垂足以预设距离S_CB得到垂线上的两点B1、B2为倒车控制点；

所述倒车控制点的计算公式为：

式中，(x_C,y_C)为转弯控制点的坐标，Θ为角度数；

判断以转弯控制点为顶点，到交接点和倒车控制点为两边的角是否为钝角；

若呈锐角舍弃，若呈钝角，确定为最终的倒车控制点。

与现有技术相比，本发明存在以下技术效果：

通过采用上述的技术方案，首先利用在挖机设置有的定位信息采集设备，采集挖机的经纬度及铲臂朝向角。再获取矿山的全局路径的主干道的道路信息。根据挖机的经纬度及铲臂朝向角可以通过计算确定挖机主干道的最近点和转弯控制点，再通过最近点确定泊车的交接点和回程交接点，同时根据转弯控制点和交接点能够得到倒车控制点。再依据上述控制点，完成矿车在装载区的进入、转弯、倒车及回程的泊车装载过程。从而解决了现有因挖矿点逐渐远离主干道导致的倒车距离增加，无法保证倒车过程中遇到障碍物泊车失败的问题，提高泊车的成功率和整体泊车效率。

附图说明

下面结合附图，对本发明的具体实施方式进行详细描述：

图1为本申请公开的一些实施例的无人驾驶矿车泊车控制方法的步骤示意图；

图2为本申请公开的一些实施例的挖机和主干道有交点的示意图；

图3为本申请公开的一些实施例的挖机和主干道无交点的示意图；

图4为本申请公开的一些实施例的倒车控制点的选取示意图。

具体实施方式

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

无人驾驶矿车的行驶过程分为两个阶段，第一个阶段为从山下的卸料点沿着主干道路循迹到矿山装料区上，第二个阶段是在矿山上找到一个合适的交接点，将无人驾驶矿车的循迹控制权交给装载泊车控制(ALP)做路径规划，并通过在路径规划过中设计几个合理的临时控制点来引导矿车模拟人工驾驶，并最终达到装料点。

同时在挖机铲臂的回转中心处装备了惯导接收设备，用于获取当前挖掘机的经纬度信息及铲臂的朝向角。记录当前位置的经纬度及朝向角信息，并反馈至卡调平台。

请参考图1，本发明实施例中，一种基于泊车控制点的无人驾驶矿车泊车控制方法，具体步骤包括：

首先卡调平台将挖机当前位置的经纬度及朝向角信息通过4G网关下发给无人驾驶矿车，无人驾驶矿车上的MDC计算平台接收到挖机当前位置的经纬度及朝向角信息，再结合提前录制的道路信息，计算矿车所在位置达到铲装位置所需要经过的控制点，所述包括交接点A，转弯控制点C，倒车控制点B，挖机距离主干道的最近点E，挖机定位点D和回程交接点F；矿车车辆达到各控制点的顺序依次为：先从交接点A前向行驶至转弯控制点C，然后降低车速行驶至倒车控制点B，在B点停车挂倒挡，倒车行驶至装料点D，装料完成后前向行驶至下山交接点F，最后循迹下山完成整个装料过程。

S1、获取矿山的道路信息及挖机的定位信息；

S2、根据挖机定位信息确定矿车的转弯控制点和挖机主干道的最近点；

所述计算选取转弯控制点的具体步骤包括：

首先获取挖定位信息的定位点坐标及挖机朝向角；

根据定位点坐标及挖机朝向角，计算转弯控制点坐标；

所述坐标计算公式为：

其中，(x，y，θ)为挖机定位信息，L＝n*d+X为倒车距离，n*d为n个车位d的距离，X为挖机铲臂的长度。

具体实施方式中，如图2所示，以D点基准点，在挖机朝向上向外推L1 m的点，此处L1取40m。人工驾驶倒车距离大概有n个车位的距离，根据实际情况，n＝3，每个车位大概10m左右，另外自动驾驶需要考虑挖机臂长X m，当前型号为PC2000挖臂长为10m，故自动驾驶时也可设置相近的倒车距离为L1＝n*d+X，其它型号尺寸的挖机对应的倒车距离可进行类推得出。再代入上述坐标公式中，得到转弯控制点坐标。

所述计算选取挖机主干道的最近点的具体步骤包括：

获取矿山预设的道路信息，以及采集当前挖机的定位信息，其中定位信息包括挖机的经纬度及铲臂的朝向角信息；

根据道路信息的主干道每个点，及挖机的定位点，计算得到主干道每个点到定位点距离；

根据到主干道每个点到定位点距离最近的点作为挖机主干道的最近点；

所述最近点距离计算公式为：

式中，(x,y)为挖机的定位点坐标，(x_i,y_i)为主干道第i点坐标。

具体实施方式中，已知挖机定位点D的经纬度信息，同时主干道路上每个点的经纬度信息已知，可以通过两点之间的距离公式计算出D点(x，y)和主干道路上每一个点(x_i，y_i)的距离，同时选出数值最小，即距离最近的点，得出最近点E。

S3、利用得到的挖机主干道的最近点确定矿车的交接点和回程交接点；具体步骤包括:

获取挖机主干道的最近点及当前的挖机朝向角；

根据当前挖机朝向角判断挖机朝向的外延有向线段是否与主干道有交点；

若有交点则以此交点为起点回退预设距离S处为交接点；

若无交点则以最近点为起点回退预设距离S处为交接点；

同时以最近点为起点前推预设距离S′处为回程交接点。

具体实施方式中，如图2和图3所示，图1是挖机朝向和主路有交接点的情况，图2是挖机朝向上和主路没有交接点的情况。根据挖机朝向上向外扩展一定距离的有向线段和主干道路是否有交点。具体可分为两种情况：

与主干道路有交点：该交点设为P，为保证B点在挖机铲臂的右侧，则∠ACB需为钝角，则以P点为起点往后退(即车辆倒车方向)Sm，此处S根据实际情况取50m的点选做为交接点A。

与主干道路没有交点：因随着挖机作业不断的深入，挖机离主路很远，有向线段可能和主路不会再有交点，则以最近点E为起点往后退(即车辆倒车方向)Sm，此处S根据实际情况取50m的点选做为交接点A。

同时，在装料完成后回到主干道路进行回程交接，具体的，通过根据下山时主干道路的最近点E，与上山方法相同，为了避免上下山会车发生碰撞。以最近点为起点前推预设距离S′处为回程交接点，此处S′根据实际情况可选择60m。

S4、同时根据转弯控制点和矿车交接点确定矿车的倒车控制点。具体步骤包括：

获取转弯控制点及挖机定位点；

以转弯控制点为垂足做转弯控制点到挖机定位点的线段的垂线；

再根据垂足以预设距离S_CB得到垂线上的两点B1、B2为倒车控制点；

所述倒车控制点的计算公式为：

式中，(x_C,y_C)为转弯控制点的坐标，Θ为角度数；

判断以转弯控制点为顶点，到交接点和倒车控制点为两边的角是否为钝角；

若呈锐角舍弃，若呈钝角，确定为最终的倒车控制点。

具体实施方式中，在转弯控制点C作直线DC的垂线，在垂线左右两端各选一点B1和B2，且取CB1和CB2的长度都为S_CBm，此处根据实际可标定为12m，也可进行适当修改，其中倒车控制点B会根据交接点A在主路上的位置来选择是在B1点倒车或者是在B2点倒车，如图4所示倒车点B1或B2，为了模拟人工驾驶习惯，一般情况下，如果挖机在主路的左侧，选择挖机右侧点倒车，挖机在主路的右侧，则选择挖机左侧点倒车。随着交接点A位置不同，∠ACB可能为钝角也有可能为锐角，我们规定∠ACB为钝角时，取此时钝角点B点为倒车点，为了保证B点在挖机右侧时∠ACB为钝角，A应该在挖机朝向与主路交点P的左侧，B1，B2点的计算过程为：

利用AC和CB的夹角∠ACB做余弦定理，若呈锐角舍弃，呈钝角，则选择该侧点为倒车控制点B点。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定，均应包含在本发明的保护范围之内。

Claims (2)

1.一种基于泊车控制点的无人驾驶矿车泊车控制方法，其特征在于，具体步骤包括：

获取矿山的道路信息及挖机的定位信息；

根据挖机定位信息确定矿车的转弯控制点和挖机主干道的最近点；

所述根据挖机定位信息确定矿车的转弯控制点的具体步骤包括：

首先获取挖机定位信息的定位点坐标及挖机朝向角；

根据定位点坐标及挖机朝向角，计算转弯控制点坐标；

所述转弯控制点坐标计算公式为：

其中，(x，y，θ)为挖机定位信息，L＝n*d+X为倒车距离，n*d为n个车位d的距离，X为挖机铲臂的长度；

获取交接点和回程交接点，其具体步骤包括:

获取挖机主干道的最近点及当前的挖机朝向角；

根据当前挖机朝向角判断挖机朝向的外延有向线段是否与主干道有交点；

若有交点则以此交点为起点回退预设距离S处为交接点；

若无交点则以最近点为起点回退预设距离S处为交接点；

同时以挖机主干道的最近点为起点前推预设距离S′处为回程交接点；

根据转弯控制点和交接点确定矿车的倒车控制点，其具体步骤包括：

获取转弯控制点及挖机定位点；

以转弯控制点为垂足做转弯控制点到挖机定位点的线段的垂线；

再在垂线上以距离垂足预设距离S_CB得到垂线上的两点B1、B2；

判断以转弯控制点为顶点，该转弯控制点到确定的交接点和到垂线上的点B1的直线为两边形成的角，以及该转弯控制点到确定的交接点和到垂线上的点B2的直线为两边形成的角是锐角还是钝角；

若呈锐角舍弃，若呈钝角，则形成该钝角所对应距离垂足预设距离S_CB的点确定为最终的倒车控制点；

矿车车辆达到各控制点的顺序依次为：首先从交接点前向行驶至转弯控制点，再降低车速行驶至倒车控制点，在倒车控制点停车挂入倒挡，倒车行驶至装料点，装料完成后前向行驶至回程交接点，最后矿车车辆按照循迹路线下山完成整个装料过程；

其中，交接点为矿车车辆循迹***将控制权交于ALP装载泊车控制***的点，转弯控制点为在挖机朝向上向外达到预设距离的点，挖机主干道的最近点为挖机距离主干道最近的点，回程交接点为ALP装载泊车控制***将控制权交于矿车车辆循迹***的点。

2.根据权利要求1所述一种基于泊车控制点的无人驾驶矿车泊车控制方法，其特征在于，所述根据挖机定位信息确定挖机主干道的最近点的具体步骤包括：

获取矿山预设的道路信息，以及采集当前挖机的定位信息，其中定位信息包括挖机的经纬度及铲臂的朝向角信息；

根据道路信息的主干道每个点，及挖机的定位点，计算得到主干道每个点到定位点距离；

根据到主干道每个点到定位点距离最近的点作为挖机主干道的最近点；

所述最近点距离计算公式为：

式中，(x,y)为挖机的定位点坐标，(x_i,y_i)为主干道第i点坐标。

Images