CN112896147A - 用于矿用车辆的双向定位泊车控制方法及装置 - Google Patents

Abstract

本发明提供一种用于矿用车辆的双向定位泊车控制方法，其包含：实时获取矿用车辆的定位数据以及位姿信息，确定矿用车辆的实时位置信息以及实时姿态信息；基于预设泊车地图，结合实时位置信息以及实时姿态信息，确定矿用车辆的实时行驶偏差；依据实时行驶偏差，对矿用车辆进行轨迹跟踪自动控制，以使矿用车辆按照预设泊车地图对应的参考路径以及参考状态完成双向定位泊车。本发明提供通过矿用车辆的实时位置信息以及实时姿态信息，结合预设泊车地图，得到矿用车辆的行驶偏差，通过对行驶偏差进行修正，完成矿用车辆的双向定位泊车过程。本发明实现矿用车辆自动双向泊车，大大节省人力，减小了泊车难度，提高了泊车精度，提高了矿山运输效率。

Description

用于矿用车辆的双向定位泊车控制方法及装置

技术领域

本发明涉及车辆控制领域，具体地说，涉及一种用于矿用车辆的双向定位泊车控制方法及装置。

背景技术

在目前的露天矿山的开采中，由于矿用车辆的泊车点基本上处在卸料点的位置，而卸料点基本上不改变，因此泊车位置比较固定。目前都是司机人为调整车的方向，经过多次位姿调整到达指定泊车点(卸载点)，并且由于矿用车辆体积较大，存在死区较多，因此精准停车难度较大，每次停止的位置总会有偏差，无法保证每次停止的位置都合适。

因此，本发明提供了一种用于矿用车辆的双向定位泊车控制方法及装置。

发明内容

为解决上述问题，本发明提供了一种用于矿用车辆的双向定位泊车控制方法，所述方法包含以下步骤：

实时获取矿用车辆的定位数据以及位姿信息，确定所述矿用车辆的实时位置信息以及实时姿态信息；

基于预设泊车地图，结合所述实时位置信息以及所述实时姿态信息，确定所述矿用车辆的实时行驶偏差；

依据所述实时行驶偏差，对所述矿用车辆进行轨迹跟踪自动控制，以使所述矿用车辆按照所述预设泊车地图对应的参考路径以及参考状态完成双向定位泊车。

根据本发明的一个实施例，通过以下步骤得到所述预设泊车地图：

确定所述矿用车辆的固定泊车点，对所述矿用车辆的双向定位泊车过程进行由起始点至所述固定泊车点的实际模拟操作；

在所述实际模拟操作期间，采集所述矿用车辆的点数据，并对所述点数据进行处理，得到所述参考路径以及所述参考状态，以生成所述预设泊车地图。

根据本发明的一个实施例，所述点数据包含点序号、与单一点序号对应的路径点坐标值、与单一点序号对应的航向角、与单一点序号对应的速度以及与单一点序号对应的档位。

根据本发明的一个实施例，按照预设间隔，对所述点数据进行间隔划分，对经过间隔划分后的点数据进行拟合以及均分操作，得到所述预设泊车地图。

根据本发明的一个实施例，所述实时行驶偏差包含横向行驶偏差，其中，所述横向行驶偏差包含：横向位移偏差以及航向角偏差。

根据本发明的一个实施例，对所述矿用车辆进行轨迹跟踪自动控制的步骤中，还包含以下步骤：

通过以下公式计算得到所述矿用车辆的前馈控制转角：

其中，δ_ff表示所述前馈控制转角，L表示所述矿用车辆的轴距，K_ug表示不足转向梯度，U_x表示纵向车速，g表示重力加速度，R(s)表示曲率，W_f表示前轴载荷，C_f表示前轴轮胎偏刚度，C_r表示后轴轮胎偏刚度，W_r表示后轴载荷；

通过以下公式计算得到所述矿用车辆的预瞄反馈控制转角：

其中，δ_ctrl表示所述预瞄反馈控制转角，k_p表示第一权重，e_la表示预瞄点偏差，e表示横向位移偏差，x_la表示预瞄距离，

表示航向角偏差；

通过以下公式计算得到所述矿用车辆的横摆阻尼补偿转角：

其中，δ_damp表示所述横摆阻尼补偿转角，

表示第二权重；

基于所述前馈控制转角、所述预瞄反馈控制转角以及所述横摆阻尼补偿转角，通过以下公式计算得到所述矿用车辆的目标方向盘转角，以对所述矿用车辆进行轨迹跟踪自动控制：

δ＝δ_ff+δ_ctrl+δ_damp

其中，δ表示所述目标方向盘转角。

根据本发明的一个实施例，所述实时行驶偏差包含纵向行驶偏差，对所述矿用车辆进行轨迹跟踪自动控制的步骤中，包含以下步骤：

提取所述实时位置信息以及所述实时姿态信息中包含的所述矿用车辆的实时车速；

将所述实时车速与所述预设泊车地图中指向的目标车速进行对比，得到速度误差；

将所述速度误差输入纵向控制系统，输出所述矿用车辆的油门或制动开度的控制指令，以对所述矿用车辆进行轨迹跟踪自动控制。

根据本发明的一个实施例，在所述矿用车辆进行轨迹跟踪自动控制时，还包含对所述矿用车辆进行档位切换控制，具体包含以下步骤：

依据所述实时位置信息，判断所述矿用车辆是否行驶至所述预设泊车地图指向的倒车点；

当行驶至所述倒车点，控制所述矿用车辆进行减速操作，直至所述矿用车辆停车；

在所述矿用车辆停车后，依照所述预设泊车地图指向的档位切换目标，对所述矿用车辆进行档位切换操作。

根据本发明的一个实施例，依据差分GPS高精度定位数据确定所述矿用车辆的实时位置信息，具体包含以下步骤：

在所述GPS定位数据不满足预设精度条件时，同时依据航位推算算法和惯导进行局部的航位定位，确定所述矿用车辆的实时位置信息；

为防止GPS信号丢失，应用依赖标志杆的感知算法获得定位数据，通过自适应卡尔曼校准航位推算所述矿用车辆的实时位置信息。

根据本发明的另一个方面，还提供了一种用于矿用车辆的双向定位泊车控制装置，所述装置包含：

实时信息获取模块，其用于实时获取矿用车辆的定位数据以及位姿信息，确定所述矿用车辆的实时位置信息以及实时姿态信息；

行驶偏差计算模块，其用于基于预设泊车地图，结合所述实时位置信息以及所述实时姿态信息，确定所述矿用车辆的实时行驶偏差；

控制模块，其用于依据所述实时行驶偏差，对所述矿用车辆进行轨迹跟踪自动控制，以使所述矿用车辆按照所述预设泊车地图对应的参考路径以及参考状态完成双向定位泊车。

本发明提供的用于矿用车辆的双向定位泊车控制方法及装置通过矿用车辆的实时位置信息以及实时姿态信息，结合预设泊车地图，得到矿用车辆的行驶偏差，通过对行驶偏差进行修正，完成矿用车辆的双向定位泊车过程。本发明是实现无人矿山的重要的一部分，通过实现矿用车辆自动双向泊车功能可以大大节省人力，减小了泊车难度，提高了泊车精度，提高了矿山运输效率，并且适用范围广泛，能够适用于卸料以及后退停止等工况。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例共同用于解释本发明，并不构成对本发明的限制。在附图中：

图1显示了根据本发明的一个实施例的用于矿用车辆的双向定位泊车控制方法流程图；

图2显示了根据本发明的一个实施例的双向人字形泊车示意图；

图3显示了根据本发明的一个实施例的用于矿用车辆的双向定位泊车控制方法中横向控制算法框图；

图4显示了根据本发明的一个实施例的用于矿用车辆的双向定位泊车控制方法中前馈与预瞄反馈的路径跟踪控制原理示意图；

图5显示了根据本发明的一个实施例的用于矿用车辆的双向定位泊车控制方法中修正横向行驶偏差时车辆状态示意图；

图6显示了根据本发明的一个实施例的用于矿用车辆的双向定位泊车控制方法中进行纵向控制的流程图；

图7显示了根据本发明的一个实施例的用于矿用车辆的双向定位泊车控制方法中纵向控制算法框图；

图8显示了根据本发明的一个实施例的用于矿用车辆的双向定位泊车控制方法中进行档位切换操作的流程图；

图9显示了根据本发明的一个实施例的用于矿用车辆的双向定位泊车控制装置结构框图；以及

图10显示了根据本发明的另一个实施例的用于矿用车辆的双向定位泊车控制装置结构框图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，以下结合附图对本发明实施例作进一步地详细说明。

图1显示了根据本发明的一个实施例的用于矿用车辆的双向定位泊车控制方法流程图。

如图1所示，在步骤S101中，实时获取矿用车辆的定位数据以及位姿信息，确定矿用车辆的实时位置信息以及实时姿态信息。

具体来说，依据差分GPS高精度定位数据确定矿用车辆的实时位置信息，在GPS定位数据不满足预设精度条件时，同时依据航位推算算法和惯导进行局部的航位定位，确定矿用车辆的实时位置信息。为防止GPS信号丢失，应用依赖标志杆的感知算法获得定位数据，通过自适应卡尔曼校准航位推算矿用车辆的实时位置信息。

如图1，在步骤S102中，基于预设泊车地图，结合实时位置信息以及实时姿态信息，确定矿用车辆的实时行驶偏差。一般来说，矿用车辆的双向定位泊车路径如图2所示。

具体来说，通过以下方法得到预设泊车地图：首先，确定矿用车辆的固定泊车点，对矿用车辆的双向定位泊车过程进行由起始点至固定泊车点的实际模拟操作。在实际应用中，首先确定矿用车辆的固定泊车点(卸料点)，由经验驾驶员驾驶矿用车辆从离泊车点的某一位置起开始运行，按需求进行双向定位人字形的泊车，路径如图2所示。

其中，在实际模拟操作期间，采集矿用车辆的点数据，并对点数据进行处理，得到参考路径以及参考状态，以生成预设泊车地图。具体来说，通过安装在矿用车辆上的差分GPS以及惯导等硬件单元采集矿用车辆行驶途径的点数据。点数据包含：点序号、路径点坐标值(X，Y)、航向角φ、速度、档位(1：前进挡，0：驻车挡，2：倒挡)。

具体来说，按照预设间隔，对点数据进行间隔划分，对经过间隔划分后的点数据进行拟合以及均分操作，得到预设泊车地图。在实际应用中，预设间隔可以设置为0.5m。

当预设泊车地图确定后，之后当每次矿用车辆运行到预设泊车地图的起点附近时，由驾驶员手动切换至自动泊车模式，矿用车辆便开始自动进行双向定位泊车控制。矿用车辆根据差分GPS和惯导单元以及底层控制器反馈的定位数据以及位姿信息沿着预设泊车地图的参考路径进行轨迹跟踪控制，从而自动控制矿用车辆精准到达泊车点。之后人工退出自动泊车，由人工操作举升卸料等操作。

如图1，在步骤S103中，依据实时行驶偏差，对矿用车辆进行轨迹跟踪自动控制，以使矿用车辆按照预设泊车地图对应的参考路径以及参考状态完成双向定位泊车。

具体来说，实时行驶偏差包含横向行驶偏差以及纵向行驶偏差，可以通过轨迹跟踪控制模块完成对矿用车辆的自动控制，根据上层以及底层反馈的实时信息，控制矿用车辆沿着处理好的预设泊车地图运行，完成双向定位泊车。轨迹跟踪控制模块又分成横向控制部分以及纵向控制部分，其中，横向行驶偏差包含：横向位移偏差以及航向角偏差。

进一步地，可以通过如图3所示的横向控制算法对矿用车辆进行横向控制，规划子系统确定预设泊车地图中指向的目标轨迹(参考路径)，横向控制系统利用前馈+预瞄反馈的路径跟踪控制方法。通过对曲率、位置偏差、航向角偏差等参数的计算，得到矿用车辆的目标转向角，输出前轮转角控制指令，由底层控制器完成目标转向角的执行。

前馈+预瞄反馈的控制原理如图4所示，主要包括转向几何前馈、预瞄反馈和横摆阻尼补偿三部分。首先，根据预设泊车地图给出的参考路径和传感器测量出的矿用车辆状态参数(如车辆坐标，航向角等)计算出车辆当前误差，横向位移偏差，航向角偏差，曲率等。而后根据前馈+预瞄反馈的路径跟踪控制模型的方程，计算出所需要的方向盘转角，用于控制无人矿用车辆进行基于数字地图的双向定位泊车。

具体来说，通过如下方法对矿用车辆进行轨迹跟踪自动控制，以修正横向行驶偏差，图5显示了修正横向行驶偏差时车辆状态示意图：

首先，通过以下公式计算得到矿用车辆的前馈控制转角：

其中，δ_ff表示前馈控制转角，L表示矿用车辆的轴距，K_ug表示不足转向梯度，U_x表示纵向车速，g表示重力加速度，R(s)表示曲率，W_f表示前轴载荷，C_f表示前轴轮胎偏刚度，C_r表示后轴轮胎偏刚度，W_r表示后轴载荷。

然后，通过以下公式计算得到矿用车辆的预瞄反馈控制转角：

其中，δ_ctrl表示预瞄反馈控制转角，k_p表示第一权重，e_la表示预瞄点偏差，e表示横向位移偏差，x_la表示预瞄距离，

表示航向角偏差。

接着，通过以下公式计算得到矿用车辆的横摆阻尼补偿转角：

其中，δ_damp表示横摆阻尼补偿转角，

表示第二权重。

最后，基于前馈控制转角、预瞄反馈控制转角以及横摆阻尼补偿转角，通过以下公式计算得到矿用车辆的目标方向盘转角，以对矿用车辆进行轨迹跟踪自动控制：

δ＝δ_ff+δ_ctrl+δ_damp

其中，δ表示目标方向盘转角。

另外，通过如图6所示的方法对矿用车辆进行轨迹跟踪自动控制，以修正纵向行驶偏差。在步骤S601中，提取实时位置信息以及实时姿态信息中包含的矿用车辆的实时车速。

在步骤S602中，将实时车速与预设泊车地图中指向的目标车速进行对比，得到速度误差。

在步骤S603中，将速度误差输入纵向控制系统，输出矿用车辆的油门或制动开度的控制指令，以对矿用车辆进行轨迹跟踪自动控制。

具体来说，纵向控制系统主要完成矿用车辆的速度控制。速度控制采用的是PID算法，主要是控制矿用车辆按照预设泊车地图期望的速度行驶，即控制油门的开度和制动踏板的制动量，并根据可编程控制器反馈的当前车速信息，完成矿用车辆行驶速度的闭环控制。纵向控制系统将预设泊车地图中的目标速度与实际车速进行对比，将速度误差作为PID的输入，输出为油门/制动开度的控制信息。速度控制框图如图7所示。

另外，在矿用车辆进行轨迹跟踪自动控制时，还包含对矿用车辆进行档位切换控制，如图8所示。

首先，在步骤S801中，依据实时位置信息，判断矿用车辆是否行驶至预设泊车地图指向的倒车点。

然后，在步骤S802中，当行驶至倒车点，控制矿用车辆进行减速操作，直至矿用车辆停车。

最后，在步骤S803中，在矿用车辆停车后，依照预设泊车地图指向的档位切换目标，对矿用车辆进行档位切换操作。

具体来说，纵向控制系统还可以完成档位切换控制。当车辆运行到倒车点附近时，在读取预设泊车地图中的档位信息有1到-1的跳变时，开始执行换挡工作，首先控制矿用车辆减速至停车，后控制档位切换至倒挡。档位切换完毕后，矿用车辆继续沿着倒退路径进行循迹行驶至泊车点。当矿用车辆精准停稳至泊车点后，控制档位切换至驻车档。由人工退出自动泊车模式，并人工进行矿用车辆的举升卸料等操作。

图9显示了根据本发明的一个实施例的用于矿用车辆的双向定位泊车控制装置结构框图。如图9所示，双向定位泊车控制装置900包含实时信息获取模块901、计算及行驶偏差计算模块902以及控制模块903。

实时信息获取模块901用于实时获取矿用车辆的定位数据以及位姿信息，确定矿用车辆的实时位置信息以及实时姿态信息。

行驶偏差计算模块902用于基于预设泊车地图，结合实时位置信息以及实时姿态信息，确定矿用车辆的实时行驶偏差。

控制模块903用于依据实时行驶偏差，对矿用车辆进行轨迹跟踪自动控制，以使矿用车辆按照预设泊车地图对应的参考路径以及参考状态完成双向定位泊车。

图10显示了根据本发明的另一个实施例的用于矿用车辆的双向定位泊车控制装置结构框图。装置包含定位设备以及轨迹跟踪控制模块。

运用如图10所示的装置时的具体实施流程如下：首先，确定矿用车辆的固定泊车点(卸料点)，由经验驾驶员驾驶矿用车辆从离泊车点的某一位置起开始运行，按需求进行双向定位人字形的泊车，如图2所示。矿用车辆运行的同时，安装在矿用车辆上的差分GPS以及惯导等硬件单元开始采集矿用车辆行驶途径的点数据，数据结构如下：点序号、路径点坐标值(X，Y)、航向角

速度、档位(1：前进挡，0：驻车挡，2：倒挡)。矿用车辆到达泊车点后停止采集。之后对采集的点数据进行处理，以0.5m为间隔重新拟合和均分地图上的点信息，生成用于矿用车辆下一步进行自动双向定位泊车的新的数字地图(预设泊车地图)，包括上述六项属性。

当数字地图确定后，之后当每次矿用车辆运行到数字地图的起点附近时，由驾驶员手动切换至自动泊车，矿用车辆便开始自动进行双向定位泊车控制。矿用车辆根据差分GPS和惯导单元以及底层控制器反馈的定位数据以及位姿信息沿着数字地图的参考路径进行轨迹跟踪控制，从而自动控制矿用车辆精准到达泊车点。之后人工退出自动泊车，由人工操作举升卸料等操作。

综上，本发明提供的用于矿用车辆的双向定位泊车控制方法及装置通过矿用车辆的实时位置信息以及实时姿态信息，结合预设泊车地图，得到矿用车辆的行驶偏差，通过对行驶偏差进行修正，完成矿用车辆的双向定位泊车过程。本发明是实现无人矿山的重要的一部分，通过实现矿用车辆自动双向泊车功能可以大大节省人力，减小了泊车难度，提高了泊车精度，提高了矿山运输效率，并且适用范围广泛，能够适用于卸料以及后退停止等工况。

应该理解的是，本发明所公开的实施例不限于这里所公开的特定结构、处理步骤或材料，而应当延伸到相关领域的普通技术人员所理解的这些特征的等同替代。还应当理解的是，在此使用的术语仅用于描述特定实施例的目的，而并不意味着限制。

说明书中提到的“一个实施例”或“实施例”意指结合实施例描述的特定特征、结构或特性包括在本发明的至少一个实施例中。因此，说明书通篇各个地方出现的短语“一个实施例”或“实施例”并不一定均指同一个实施例。

虽然本发明所公开的实施方式如上，但所述的内容只是为了便于理解本发明而采用的实施方式，并非用以限定本发明。任何本发明所属技术领域内的技术人员，在不脱离本发明所公开的精神和范围的前提下，可以在实施的形式上及细节上作任何的修改与变化，但本发明的专利保护范围，仍须以所附的权利要求书所界定的范围为准。

Claims (10)

1.一种用于矿用车辆的双向定位泊车控制方法，其特征在于，所述方法包含以下步骤：

实时获取矿用车辆的定位数据以及位姿信息，确定所述矿用车辆的实时位置信息以及实时姿态信息；

基于预设泊车地图，结合所述实时位置信息以及所述实时姿态信息，确定所述矿用车辆的实时行驶偏差；

依据所述实时行驶偏差，对所述矿用车辆进行轨迹跟踪自动控制，以使所述矿用车辆按照所述预设泊车地图对应的参考路径以及参考状态完成双向定位泊车。

2.如权利要求1所述的方法，其特征在于，通过以下步骤得到所述预设泊车地图：

确定所述矿用车辆的固定泊车点，对所述矿用车辆的双向定位泊车过程进行由起始点至所述固定泊车点的实际模拟操作；

在所述实际模拟操作期间，采集所述矿用车辆的点数据，并对所述点数据进行处理，得到所述参考路径以及所述参考状态，以生成所述预设泊车地图。

3.如权利要求2所述的方法，其特征在于，所述点数据包含点序号、与单一点序号对应的路径点坐标值、与单一点序号对应的航向角、与单一点序号对应的速度以及与单一点序号对应的档位。

4.如权利要求2-3中任一项所述的方法，其特征在于，按照预设间隔，对所述点数据进行间隔划分，对经过间隔划分后的点数据进行拟合以及均分操作，得到所述预设泊车地图。

5.如权利要求1-4中任一项所述的方法，其特征在于，所述实时行驶偏差包含横向行驶偏差，其中，所述横向行驶偏差包含：横向位移偏差以及航向角偏差。

6.如权利要求5所述的方法，其特征在于，对所述矿用车辆进行轨迹跟踪自动控制的步骤中，还包含以下步骤：

通过以下公式计算得到所述矿用车辆的前馈控制转角：

其中，δ_ff表示所述前馈控制转角，L表示所述矿用车辆的轴距，K_ug表示不足转向梯度，U_x表示纵向车速，g表示重力加速度，R(s)表示曲率，W_f表示前轴载荷，C_f表示前轴轮胎偏刚度，C_r表示后轴轮胎偏刚度，W_r表示后轴载荷；

通过以下公式计算得到所述矿用车辆的预瞄反馈控制转角：

其中，δ_ctrl表示所述预瞄反馈控制转角，k_p表示第一权重，e_la表示预瞄点偏差，e表示横向位移偏差，x_la表示预瞄距离，

表示航向角偏差；

通过以下公式计算得到所述矿用车辆的横摆阻尼补偿转角：

其中，δ_damp表示所述横摆阻尼补偿转角，

表示第二权重；

基于所述前馈控制转角、所述预瞄反馈控制转角以及所述横摆阻尼补偿转角，通过以下公式计算得到所述矿用车辆的目标方向盘转角，以对所述矿用车辆进行轨迹跟踪自动控制：

δ＝δ_ff+δ_ctrl+δ_damp

其中，δ表示所述目标方向盘转角。

7.如权利要求1-6中任一项所述的方法，其特征在于，所述实时行驶偏差包含纵向行驶偏差，对所述矿用车辆进行轨迹跟踪自动控制的步骤中，包含以下步骤：

提取所述实时位置信息以及所述实时姿态信息中包含的所述矿用车辆的实时车速；

将所述实时车速与所述预设泊车地图中指向的目标车速进行对比，得到速度误差；

将所述速度误差输入纵向控制系统，输出所述矿用车辆的油门或制动开度的控制指令，以对所述矿用车辆进行轨迹跟踪自动控制。

8.如权利要求1-7中任一项所述的方法，其特征在于，在所述矿用车辆进行轨迹跟踪自动控制时，还包含对所述矿用车辆进行档位切换控制，具体包含以下步骤：

依据所述实时位置信息，判断所述矿用车辆是否行驶至所述预设泊车地图指向的倒车点；

当行驶至所述倒车点，控制所述矿用车辆进行减速操作，直至所述矿用车辆停车；

在所述矿用车辆停车后，依照所述预设泊车地图指向的档位切换目标，对所述矿用车辆进行档位切换操作。

9.如权利要求1-8中任一项所述的方法，其特征在于，依据差分GPS高精度定位数据确定所述矿用车辆的实时位置信息，具体包含以下步骤：

在所述GPS定位数据不满足预设精度条件时，同时依据航位推算算法和惯导进行局部的航位定位，确定所述矿用车辆的实时位置信息；

为防止GPS信号丢失，应用依赖标志杆的感知算法获得定位数据，通过自适应卡尔曼校准航位推算所述矿用车辆的实时位置信息。

10.一种用于矿用车辆的双向定位泊车控制装置，其特征在于，所述装置包含：

实时信息获取模块，其用于实时获取矿用车辆的定位数据以及位姿信息，确定所述矿用车辆的实时位置信息以及实时姿态信息；

行驶偏差计算模块，其用于基于预设泊车地图，结合所述实时位置信息以及所述实时姿态信息，确定所述矿用车辆的实时行驶偏差；

控制模块，其用于依据所述实时行驶偏差，对所述矿用车辆进行轨迹跟踪自动控制，以使所述矿用车辆按照所述预设泊车地图对应的参考路径以及参考状态完成双向定位泊车。

Images