CN104881025A - 一种地下矿用车辆的反应式导航控制方法 - Google Patents

Abstract

本发明提供了一种地下矿用车辆的反应式导航控制方法，包括：根据当前时刻的车辆铰接点、前桥中点、前桥车速、前桥横摆角速度、前桥转向半径，后桥中点、后桥车速、后桥横摆角速度、后桥转向半径，建立下一时刻前桥位姿状态向量估计值模型；建立航向偏离角模型；建立铰接车前桥中心位置偏差模型；建立转向角控制模型，其中转向角控制模型包括安全预测模型和模糊控制算法；建立铰接车速度控制模型，并根据铰接车速度控制模型控制地下矿用车辆。

Description

一种地下矿用车辆的反应式导航控制方法

技术领域

本发明涉及导航技术领域，特别是指一种地下矿用车辆的反应式导航控制方法。

背景技术

随着技术的发展，现在地下采矿行业中采用的采矿装备都已经实现了无轨化和液压化，并正在朝着大型化、智能化方向发展。现有的地下矿用车辆大多为无轨铰接式结构，因此铰接式车辆的自主导航控制技术是实现地下金属矿设备智能化与自动化的关键。

现有的地下矿用车辆的自主导航技术可以分为以下两种；反应式导航和绝对式导航；

其中绝对式导航需要在导航系统中预先定义一组路径(即路径规划)并且得知每一时刻车体的绝对坐标(即车体定位)。这种导航方式需要预先进行大量工作以对提前规划合理的行驶路径，并需要结合车辆的运行特性。同时为了保证导航的精确性以防止发生碰撞，在行驶路径上必须预制大量的专用定位设备，并在车上需要加装定位传感器来采集定位信息，以实现对车辆的精确定位。由此可见这种导航方法需要大量的前期投入，成本高且可靠性不好。

其中反应式导航是一种主流的地下矿用车辆自主导航技术矿山设备的自主导航关键技术，反应式导航系统不需要所处位置的环境模型，而是通过车身上的传感器感知周围环境。当井下车辆通过巷道时，车辆不需要知道自己在井下的绝对位置，只需知道自己相对于巷道壁的距离及其角度就能自主行走。因此反映式导航也称为相对式导航，具有前瞻性和极好的适应性，有利于车辆的高速运行。

现有技术中对于自主导航已经有了很多研究，例如申请号为ZL200910238066.3的中国发明专利申请公开了一种采用绝对式导航技术的地下自主铲运机导航控制方法，这种方法是根据事先规划好的目标路径与铲运机实际行驶路径对比得到铲运机行驶过程中的偏差，以偏差量作为反馈进行实时控制。这种技术至少存在以下缺点：

导航控制方法未充分考虑铰接式车辆的运动学和动力学特性，导航前需要规划好一条路径，为了使规划的路径合理，规划路径时需要结合车辆的运动学特点，这增加了导航控制的前期工作量；导航需要车辆准确的定位信息，因此需要在巷道的“关键地点”加装定位设备，在大型复杂的地下矿井中，“关键地点”的数目非常大，这就需要大量的定位设备，会大大增加导航的成本，而且由于地下巷道宽度和高度的限制，定位设备的安装会受到很大程度的制约；导航中提出用“航位推算”的方法来减少定位设备的安装，但“航位推算”又需要在车上加装相应的传感器来采集地下巷道壁的信息，由于地下巷道壁的凹凸不平，航位推算所需要的信息本身也会有很大的误差，这会加大航位推算的误差，而且这种误差会累加，最终推算出来的结果可能不能使用。以上所述导航控制技术的缺点也反映出绝对式导航本身的不足之处。

发明内容

针对现有技术中的导航技术存在的诸多问题，本发明实施例提出了一种地下矿用车辆的反应式导航控制方法。

为了达到上述目的，本发明实施例提出了一种地下矿用车辆的反应式导航控制方法，包括：

根据当前时刻的车辆铰接点、前桥中点、前桥车速、前桥横摆角速度、前桥转向半径，后桥中点、后桥车速、后桥横摆角速度、后桥转向半径，建立下一时刻前桥位姿状态向量估计值模型；

建立航向偏离角模型；

建立铰接车前桥中心位置偏差模型；

建立转向角控制模型，其中转向角控制模型包括安全预测模型和模糊控制算法；

建立铰接车速度控制模型，并根据铰接车速度控制模型控制地下矿用车辆。

其中，下一时刻为t+1时刻，且所述前桥位姿状态向量估计值模型为非线性离散模型，表示为：

${\hat{S}}_{t+1}=S_t+T_s\left[\begin{array}{cc}\cos {\mathrm{\θ}}_1\left(t\right)& 0\\ \sin {\mathrm{\θ}}_1\left(t\right)& 0\\ \frac{-\sin \mathrm{\γ}\left(t\right)}{l_1\cos \mathrm{\γ}\left(t\right)+l_2}& \frac{-l_2}{l_1\cos \mathrm{\γ}\left(t\right)+l_2}\end{array}\right]\left[\begin{array}{c}{v}_f\left(t\right)\\ {\stackrel{\·}{\mathrm{\λ}}}_1\left(t\right)\end{array}\right]$

其中铰接车交接点为H，前桥中点为P_f(x₁，y₁)，该点与中央铰接点H的距离为l₁，车速为vf；后桥中点为P_r(x₂，y₂)点，该点与中央铰接点的距离为l₂，车速为vr；前车体横摆角速度为ω1，转向半径为r1，后车体横摆角速度为ω2，转向半径为r2；前车体的航向偏离角为θ1，后车体的航向偏离角为θ2，转向角为λ；t时刻前桥中点的位置姿态信息用位姿状态向量S_t＝[x₁(t)y₁(t)θ₁(t)]^T表示。

其中，建立航向偏离角模型具体包括：

铰接车的航向偏离角和铰接车前车体的横摆角速度ω1相关，根据t时刻前车体横摆角速度ω1、t时刻铰接车的车速测量值铰接车的转向角测量值通过以下方法通过运动学模型得到航向偏离角模型：

铰接式车辆在行驶过程中前后车体在中央铰接点处的速度相等，即V₂+V_h2＝V₁+V_h1；

铰接式车辆前后车体在中央铰接点处平行于后桥轴线方向的速度分量相等,因此铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等，即V₁sinλ＝ω₂l₂+ω₁l₁cosλ；

铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等，即V₂＝V₁cosλ+ω₁l₁sinλ，

计算铰接式车辆的转向角变化速率等于前后车体绕各自瞬时中心转动的角速度之差，即：-dλ/dt＝ω₂-ω₁，以此得到ω₁＝(V₁sinλ+l₂dλ/dt)/(l₂+l₁cosλ)，

根据公式ω₁＝dθ/dt得到铰接式车辆车体航向偏离角与转向角以及车体参数之间关系式为：

dθ/dt＝(V₁sinλ+l₂dλ/dt)/(l₂+l₁cosλ)，

t时刻航向偏离角θ(t)表示为θ(t)＝θ(t-1)+ω₁(t)×T，

其中θ(t-1)为航向偏离角初始值，设初值为0，T为系统采样时间。

其中，建立铰接车前桥中心位置偏差模型具体包括：

以y1，y2为内侧、外侧巷道曲线方程，k1，k2分别为巷道内侧与外侧的一定区域内的弧线曲率平均值，G1为偏离车体角度为θ的方向上障碍点到车体前桥中心的距离，则k1，k2表示为：

$k_1=\left|{y^{\′\′}}_1\right|/\sqrt{{(1+{y_1}^{\′2})}^3},$

$k_2=\left|{y^{\′\′}}_2\right|/\sqrt{{(1+{y_2}^{\′2})}^3},$

则t时刻铰接车前桥中心位置偏差δ(t)表达为：

δ(t)＝[k₁(t)-k₂(t)]/2k₁(t)k₂(t)-G₁(t)。

其中，建立转向角控制模型包括：

车体航向偏离角安全预测值θ(t+1)由当前时刻车体前方障碍信息确定，令检测角度分辨率为1度，车辆可检测到前方[0,SA°]巷道壁距车辆前桥中心的距离，则可检测到的障碍点个数为(SA+1)个；

利用障碍贡献函数确定每个障碍点的障碍贡献值，以确定其对车辆安全形势的影响，

将车体正前方的区域等分成n个扇形区域，每个扇形区域内的障碍点密度可表示为Dn，进而得到n个扇形区域的障碍密度点D＝[D1,D2,…,Dn]T

利用三次样条差值，得到的障碍密度点以及其对应的区域角度值转化为笛卡尔坐标系下的一系列的坐标值{(x1,y1),(x2,y2),…(xn,yn)}，

三次样条差值函数峰值中的最小值为S(x_m)，令B(t+1)＝xm，则B(t+1)为t+1时刻安全行车区域所对应的角度，

则t+1时刻车体航向偏离角安全预测值θ(t+1)表示为：

θ(t+1)＝[SA×π/360-B(t+1)]，

其中k1，k2分别为巷道内侧与外侧的弧线曲率，G1为偏离车体角度为θ的方向上障碍点到车体前桥中心的距离，

t+1时刻铰接车前桥中心位置偏差安全预测值δ(t+1)用车体左、右前方两部分障碍点权重之差表示为：δ(t+1)＝κ[t(t)-R(t)]，其中κ为正值系数，

得到t+1时刻航向偏离角安全预测值θ(t+1)和前桥中心位置偏离角安全预测值δ(t+1)后，利用模糊算法生成t+1时刻目标转向角λ(t+1)，

模糊算法的隶属度函数依据车辆在行驶过程中λ与δ和θ的相关关系确定，

依据隶属度函数将δ、θ和λ均划分为7个模糊子集，

δ＝﹛负大，负中，负小，零，正小，正中，正大﹜，

θ＝﹛负大，负中，负小，零，正小，正中，正大﹜

λ＝﹛负大，负中，负小，零，正小，正中，正大﹜

模糊控制策略的推理逻辑为IF前桥中心位置偏差d，AND航向偏离角q，THEN转向角λ，

由此得到t+1时刻铰接车的目标转向角λ(t+1)。

其中，建立铰接车速度控制模型具体包括：

铰接车的目标车速V根据车辆当前车速V和前方障碍距离D确定，地下矿用铰接车在行驶过程中分为加速行驶、匀速行驶、减速行驶、制动停车，本速度控制模型也按照铰接车的实际驾驶情况分为加速行驶模式、匀速行驶模式、减速行驶模式、制动停车模式；

根据测出铰接车从最高速减速行驶到停车所用时间t减速，并针对不同驾驶模式的转换分别设定了转换阈值，分别为：

t减速阈值＝2t减速，

t匀速阈值＝4t减速，

t加速阈值＝6t减速，

在采集回来的前方障碍物距离信息D后分析找到距离障碍物距离最近的点，并计算以当前车速下与障碍发生碰撞所需的时间t，并将t与上述设定的时间阈值进行比较来选择此时的驾驶模式，

如果t≤t减速阈值，立即制动停车，

如果t减速阈值<t≤t匀速阈值，减速行驶，

如果t匀速阈值<t≤t加速阈值，匀速行驶，

如果t≥t加速阈值，加速行驶。

本发明的上述技术方案的有益效果如下：

本发明与现有技术相比有如下优点和效果：

本方法建立了铰接车的运动学模型，依据铰接车的运动学模型建立导航控制模型，使其导航控制更加精确。同时本方法明确给出了各个误差模型和控制模型的建立方法，实用性大大增强。本方法不需要进行路径规划，减小了导航前期的工作量，提高了导航的控制的效率。本方法不需要车辆的定位信息，省去了定位设备的安装调试，在尽量少的设备中能够实现铰接车的导航控制，在复杂的大型矿中能大大减少成本，因为附属设备的减少提高了导航控制的稳定性和可靠性。本方法利用距离传感器获取车辆前方的障碍信息，使得车辆控导航控制具有一定的前瞻性。

附图说明

图1为本发明实施例的反应式导航控制结构示意图；

图2为铰接式车辆模型示意图；

图3为铰接式转向示意图；

图4为铰接式车辆在圆弧巷道中位置信息示意图；

图5为铰接式车辆在直线巷道中的位置信息示意图；

图6为前桥中心位置偏差隶属度函数示意图；

图7为航向偏离角隶属度函数示意图；

图8为转向角隶属度函数示意图；

图9为模糊算法规则表；

图10为编码器采集数表；

图11为取样范围内采集数据列表；

图12为划分扇形区域示意图；

图13为障碍密度三次样条插值示意图；

图14为铰接式车辆安全行驶参数及控制参数表；

图15为铰接式模型车车体参数以及控制参数表；

图16为仿真结果示意图；

图17为本发明实施例的流程图。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实例进行详细描述。

本发明实施例提出了一种地下矿用车辆的反应式导航控制方法，其流程如图17所示的，包括：

根据当前时刻的车辆铰接点、前桥中点、前桥车速、前桥横摆角速度、前桥转向半径，后桥中点、后桥车速、后桥横摆角速度、后桥转向半径，建立下一时刻前桥位姿状态向量估计值模型；

建立航向偏离角模型；

建立铰接车前桥中心位置偏差模型；

建立转向角控制模型，其中转向角控制模型包括安全预测模型和模糊控制算法；

建立铰接车速度控制模型，并根据铰接车速度控制模型控制地下矿用车辆。

A.建立铰接车运动学模型，

铰接车转向时模型如附图2所示，铰接车交接点为H，前桥中点为P_f(x₁，y₁)，该点与中央铰接点H的距离为l₁，车速为vf；后桥中点为P_r(x₂，y₂)点，该点与中央铰接点的距离为l₂，车速为vr，前车体横摆角速度为ω1，转向半径为r1，后车体横摆角速度为ω2，转向半径为r2，前车体的航向偏离角为θ1，后车体的航向偏离角为θ2，转向角为λ。

t时刻前桥中点的位置姿态信息可以用位姿状态向量S_t＝[x₁(t)y₁(t)θ₁(t)]^T表示，t+1时刻的前桥位姿状态向量估计值可以用非线性离散模型表示为：

${\hat{S}}_{t+1}=S_t+T_s\left[\begin{array}{cc}\cos {\mathrm{\&theta;}}_1\left(t\right)& 0\\ \sin {\mathrm{\&theta;}}_1\left(t\right)& 0\\ \frac{-\sin \mathrm{\&gamma;}\left(t\right)}{l_1\cos \mathrm{\&gamma;}\left(t\right)+l_2}& \frac{-l_2}{l_1\cos \mathrm{\&gamma;}\left(t\right)+l_2}\end{array}\right]\left[\begin{array}{c}{v}_f\left(t\right)\\ {\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}}_1\left(t\right)\end{array}\right].$

B.建立航向偏离角模型，

铰接车的航向偏离角和铰接车前车体的横摆角速度相关，t时刻前车体横摆角速度ω1根据t时刻铰接车的车速测量值和铰接车的转向角测量值经运动学模型得到，[085]航向偏离角θ(t)如附图3所示，铰接式车辆在行驶过程中前后车体在中央铰接点处的速度相等，则有：

V₂+V_h2＝V₁+V_h1，

忽略铰接式车辆行驶过程中的侧滑，其前后车体在中央铰接点处平行于后桥轴线方向的速度分量相等,进而铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等，即V₁sinλ＝ω₂l₂+ω₁l₁cosλ，

铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等，即:V₂＝V₁cosλ+ω₁l₁sinλ，

铰接式车辆的转向角变化速率等于前后车体绕各自瞬时中心转动的角速度之差，即：-dλ/dt＝ω₂-ω₁，

以此得到ω₁＝(V₁sinλ+l₂dλ/dt)/(l₂+l₁cosλ)

又ω₁＝dθ/dt，得到铰接式车辆车体航向偏离角与转向角以及车体参数之间关系式为：

dθ/dt＝(V₁sinλ+l₂dλ/dt)/(l₂+l₁cosλ)，

当前时刻航向偏离角θ(t)可表示为：θ(t)＝θ(t-1)+ω₁(t)×T，

其中θ(t-1)为航向偏离角初始值，T为系统采样时间。

C.建立车体前桥中心位置偏差模型，

铰接式车辆在圆弧巷道中位置信息如附图4所示，y1，y2为内侧、外侧巷道曲线方程，k1，k2分别为巷道内侧与外侧的一定区域内的弧线曲率平均值，G1为偏离车体角度为θ的方向上障碍点到车体前桥中心的距离，则k1，k2表示为：

$k_1=\left|{y^{\&prime;\&prime;}}_1\right|/\sqrt{{(1+{y_1}^{\&prime;2})}^3},$

$k_2=\left|{y^{\&prime;\&prime;}}_2\right|/\sqrt{{(1+{y_2}^{\&prime;2})}^3},$

则t时刻铰接车前桥中心位置偏差δ(t)可表达为：

δ(t)＝[k₁(t)-k₂(t)]/2k₁(t)k₂(t)-G₁(t)，

当巷道为直线巷道时，可简化为图5，此时巷道内侧与外侧的一定区域内的弧线曲率平均值k1，k2都为0，

其中h1，h2，h3分别为偏离车体前桥0°，90°及180°方向上障碍点到车体前桥中心的距离，θ为车体航偏离角，G0为巷道宽度，

由附图5中的几何关系可得到：

G₀(t)＝[h₁(t)+h₃(t)]cosθ(t)，

G₁(t)＝h₂(t)sinθ(t)，

t时刻中心位置偏差d可表示为：

δ(t)＝G₀(t)/2-G₁(t)。

D.建立转向角控制模型，

如附图1所示，航行角控制模型包括全预测模型和模糊控制算法，其中安全预测模型的输入量为t时刻的航向偏离角和前桥中心位置偏差，输出量为t+1时刻的航向偏离角和前桥中心位置偏差，模糊控制算法的输入量为t+1时刻的航向偏离角和前桥中心位置偏差，经算法得到的结果即为转向角控制模型最终输出量铰接车的转向角λ，

a.建立安全预测模型，利用安全预测模型得到下一时刻铰接车的安全航向偏离角和安全中心位置偏差，

安全预测模型中下一时刻允许的车体航向偏离角θ(t+1)可由当前时刻车体正前方障碍点信息确定，

如图5所示车体正前方0～SA的角度，半径为R的阴影区域为障碍点对前车体有影响的区域，其中d为一个障碍点到车体前桥中心的距离。对于障碍点的采样角度间隔为1°，即0～SA的角度范围内共采样(SA+1)个点，

为表示障碍点对车体的影响，定义障碍贡献值为：

$M_j=\mathrm{Re}\&times;(-\ln j\&times;d_j/\underset{j=0}{\overset{\mathrm{SA}}{\mathrm{\&Sigma;}}}{d}_j),$

其中d_j为障碍点到前(或后)桥中心的距离，

j＝[0，1，…，SA]

为弥补系统误差，令Re∈[1，3]，

将车体正前方的区域划分成n等分扇形区域，每个扇形区域占有角度为s即：n＝SA/σ，

则每个扇形区域内的障碍点密度表示为：

其中 $n=[\mathrm{1,2},...,\frac{\mathrm{SA}}{\mathrm{\&sigma;}}],$

通过上述计算得到铰接式车体正前方n个扇形区域的障碍密度点的集合为：

D＝[D₁，D₂，…，D_n]^T，

由于该n个障碍密度点是非连续点，则得到的障碍密度曲线为非连续曲线，为了使得障碍密度曲线连续，且一阶与二阶导数连续，因此选择三次样条插值处理n个障碍密度点，

将得到的障碍密度点以及其对应的区域角度值转化为笛卡尔坐标系下的一系列的坐标值{(x₁，y₁)，(x₂，y₂)，…，(x_n，y_n)}，

其中：[x₁，x₂，…，x_n]^T＝π[σ，2σ，…nσ]^T/180，

[y₁，y₂，…，y_n]^T＝[D₁，D₂，…D_n]^T，

S为三次样条曲线方程，选择边界条件：x₀″＝y₀′，x″_n＝y″_n从而得到三次样条插值函数S(x)，

当S′(x_z)＝0且S″(x_z)≠0时S(xz)为函数的峰值，其中z∈[1,2,…,n]，设函数峰值中的最小值为S(xm)，令B(t+1)＝xm，则B(t+1)为下一时刻安全行车区域所对应的角度，

t+1时刻车体航向偏离角θ则表示为：

θ(t+1)＝[SA×π/360-B(t+1)]，

车体正前方右半部分障碍点权重为：

车体正前方左半部分障碍点权重为：

车体前桥中心位置偏差δ(t+1)用车体当前正前方左右两部分障碍点权重之差表示为：δ(t+1)＝κ[L(t)-R(t)]，

其中k为正值系数。

b.得到t+1时刻航向偏离角安全预测值θ(t+1)和前桥中心位置偏离角安全预测值δ(t+1)后，利用模糊算法生成t+1时刻目标转向角λ(t+1)，

模糊算法的隶属度函数依据车辆在行驶过程中λ与δ和θ的相关关系确定，

依据隶属度函数将δ、θ和λ均划分为7个模糊子集，

δ＝﹛负大，负中，负小，零，正小，正中，正大﹜

θ＝﹛负大，负中，负小，零，正小，正中，正大﹜

λ＝﹛负大，负中，负小，零，正小，正中，正大﹜

模糊控制策略的推理逻辑为IF前桥中心位置偏差δ，AND航向偏离角θ，THEN转向角λ，

由此得到t+1时刻铰接车的目标转向角λ(t+1)。

E.建立铰接车速度控制模型，

铰接车下一时刻车速V根据车辆当前车速真实测量值和前方障碍距离D确定，

地下矿用铰接车在行驶过程中分为加速行驶、匀速行驶、减速行驶、制动停车，本速度控制模型也按照铰接车的实际驾驶情况分为加速行驶模式、匀速行驶模式、减速行驶模式、制动停车模式，

速度控制模型建立过程如下：

1)测出铰接车从最高速减速行驶到停车所用时间t减速，在实际驾驶中，为了保证安全，驾驶员从一种驾驶模式转换到另一种驾驶模式时，一般都会提前动作，因此本速度控制模型针对不同驾驶模式的转换分别设定了转换阈值，分别为：

t减速阈值＝1.5t减速，

t匀速阈值＝2t减速，

t加速阈值＝2.5t减速，

2)在采集回来的前方障碍物距离信息D后分析找到距离障碍物距离最近的点，并计算以当前车速下与障碍发生碰撞所需的时间t，并将t与上述设定的时间阈值进行比较来选择此时的驾驶模式，

如果t≤t减速阈值，立即制动停车，

如果t减速阈值<t≤t匀速阈值，减速行驶，

如果t匀速阈值<t≤t加速阈值，匀速行驶，

如果t≥t加速阈值，加速行驶，

经过上述判断过程，得到车辆当下的驾驶模式。

下面结合实施例对本发明做进一步说明，

在本实施例中以某型铰接式AGV为控制对象，在巷道进行反应式导航控制。实施例中铰接车车身安装有某型号激光雷达、转角传感器、速度编码器等，铰接车的结构参数如图6所示，由转角传感器采集铰接式车体转向角l，编码器采集车轮转速，将转速转化为车体速度V，激光雷达扫描前车体的障碍区域采集障碍点信息d_j，实施步骤如下：

1.建立转向控制模型，

实施例中控制系统的采样时间为T＝20ms，对于选取的该时刻，铰接式模型车车体上安装的传感器采集数据如附图7所示，

该时刻激光雷达的角分辨率为1°，扫描范围为-5°～185°，共191个点，对于该时刻由采集的数据得到的曲线可得曲线的曲率为0，即该时刻巷道壁为直巷道，因此确定车体前桥中心位置偏差仅需采集角度为0°，90°以及180°三点的数据即可，为了弥补三点数据的不精确性，故实验中选择了三点10°范围内的数据值，即0°的数据值由-5°～5°的范围内的数据平均确定，90°的数据值由85°～90°的范围内的数据平均值确定以及180°的数据值由175°～85°的范围内的数据平均值确定，列出该三点10°范围内的数据值如附图8所示，

(1)利用航向偏离角模型和前桥中心位置偏差模型建立的输出量建立安全预测模型，

利用三次样条差值得到航向偏离角和前桥中心位置偏差，选取激光雷达扫描的范围为0～180°，即SA＝180°，障碍点采集个数是181，取σ＝5，则n＝36，即在车体正前方划分出36等分扇形区域，扇形区域分布如图9所示，

通过公式公式 $M_j=\mathrm{Re}\&times;(\ln j\&times;d_j/\underset{j=0}{\overset{\mathrm{SA}}{\mathrm{\&Sigma;}}}{d}_j)$ 与 $D_n=\underset{j=\mathrm{\&sigma;}(n-1)}{\overset{\mathrm{n\&sigma;}-1}{\mathrm{\&Sigma;}}}{M}_j,$ 得到36个障碍点密度，对于三次样条插值来说，36个点的插值运算量过大，将36个点分为9组，每组4个障碍点进行三次样条插值，得到9段平滑的曲线，对于相邻两段曲线之间仍取出4个点进行三次样条插值，得到三次样条差值函数S(x)即得到一条具有连续二阶导数的平滑曲线，如附图10所示，

将得到的障碍密度点以及其对应的区域角度值转化为笛卡尔坐标系下的一系列的坐标值{(x₁，y₁)，(x₂，y₂)，…，(x_n，y_n)}，

其中：[x₁，x₂，…，x_n]^T＝π[σ，2σ,…nσ]T/180，

[y₁，y₂，…，y_n]^T＝[D₁，D₂，…D_n]^T，

设S为三次样条曲线方程，选择边界条件：x″₀＝y′₀，x″_n＝y″_n，从而得到三次样条插值函数S(x)，

下一时刻车体航向偏离角为：

θ(t+1)＝[SA×π/360-B(t+1)]，

车体前桥中心位置偏差δ(t+1)用车体当前正前方左右两部分障碍点权重之差表示，

车体正前方右半部分障碍点权重为：

$R\left(t\right)=\underset{j=0}{\overset{\mathrm{SA}/2}{\mathrm{\&Sigma;}}}{d}_j\left(t\right),$

车体正前方左半部分障碍点权重为：

$L\left(t\right)=\underset{j=\mathrm{SA}/2}{\overset{\mathrm{SA}}{\mathrm{\&Sigma;}}}{d}_j\left(t\right)$

则δ(t+1)＝κ[L(t)-R(t)]，

其中κ为正值系数。

(2)利用模糊控制算法得到铰接车的实际控制量，

得到t+1时刻航向偏离角安全预测值θ(t+1)和前桥中心位置偏离角安全预测值δ(t+1)后，利用模糊算法生成t+1时刻目标转向角λ(t+1)，

模糊算法的隶属度函数依据车辆在行驶过程中λ与δ和θ的相关关系确定，

附图12所示，转向角l的隶属度函数如附图13所示，

模糊算法的推理逻辑为：IF前桥中心位置偏差d，AND航向偏离角θ，THEN向角λ，通过对驾驶经验的统计得到模糊控制规则如表14所示，

航向偏差角、车体前桥中心位置偏差通过模糊控制算法得到安全，采用重心法来进行反模糊化，取中心偏移位移量化因子取2，偏离角量化因子取2，输出的比例因子取5，该时刻的得到的输入、输出变量如附图15所示。

E.建立铰接车速度控制模型，

地下矿用铰接车在行驶过程中分为加速行驶、匀速行驶、减速行驶、制动停车，本速度控制模型也按照铰接车的实际驾驶情况分为加速行驶模式、匀速行驶模式、减速行驶模式、制动停车模式，具体实施步骤如下：

(1)测量实施例中铰接车从静止以最大加速度行驶至最大速度，匀速行驶一段时间，然后减速行驶至停止，为了精确，重复测量取均值，得到铰接车的速度V和时间t的关系，本实施例中铰接车的最大速度Vmax＝2.5m/s，铰接车从静止到最大速度的时间t加速＝4.5s，铰接车从最高车速减速至停止所用的时间为t减速＝7.4s，

在实际驾驶中，为了保证安全，驾驶员从一种驾驶模式转换到另一种驾驶模式时，一般都会提前动作，因此本速度控制模型针对不同驾驶模式的转换分别设定了安全转换时间阈值，分别为：

t减速阈值＝1.5t减速，

t匀速阈值＝2t减速，

t加速阈值＝2.5t减速，

2)在采集回来的前方障碍物距离信息D后分析找到距离障碍物距离最近的点，并计算以当前车速下与障碍发生碰撞所需的时间t，并将t与上述设定的时间阈值进行比较来选择此时的驾驶模式，

如果t≤t减速阈值，立即制动停车，

如果t减速阈值<t≤t匀速阈值，减速行驶，

如果t匀速阈值<t≤t加速阈值，匀速行驶，

如果t≥t加速阈值，加速行驶，

经过上述判断过程，实现车辆驾驶模式的切换。

在确定控制流程和模糊算法的规则后，本导航控制系统可在在电脑中进行仿真实验，仿真实验以Visual Studio为开发环境，搭建了一套可动态加载巷道地图、采集虚拟传感数据、生成车辆运行轨迹的仿真软件，对仿真软件中的车辆加入本发明提出的控制策略。

仿真铰接车结构参数如图6所示，仿真中铰接车行驶性能参数设定位Vmax＝2.5m/s，t减速＝7.4s，仿真巷道分别设置为L、Z、S形，巷道宽度设置为2.5倍的车宽，在实验过程中，选取适当的采样时间，忽略铰接车的侧滑等因素的影响，本导航控制方法能够实现铰接车辆在巷道中导航控制，仿真实验结果如附图16所示。

实验结果表明，本发明中的导航方法能够顺利完成铰接车辆在巷道中的导航控制工作，在直巷道、弯曲巷道中铰接车辆能够以最佳行驶模式行驶，在无障碍的情况下，车辆可以最大速度前进，在转弯时能够减速前行顺利通过弯道，前方不可通行时会自动停车。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (6)

1.一种地下矿用车辆的反应式导航控制方法，其特征在于，包括：

根据当前时刻的车辆铰接点、前桥中点、前桥车速、前桥横摆角速度、前桥转向半径，后桥中点、后桥车速、后桥横摆角速度、后桥转向半径，建立下一时刻前桥位姿状态向量估计值模型；

建立航向偏离角模型；

建立铰接车前桥中心位置偏差模型；

建立转向角控制模型，其中转向角控制模型包括安全预测模型和模糊控制算法；

建立铰接车速度控制模型，并根据铰接车速度控制模型控制地下矿用车辆。

2.根据权利要求1所述的地下矿用车辆的反应式导航控制方法，其特征在于，下一时刻为t+1时刻，且所述前桥位姿状态向量估计值模型为非线性离散模型，表示为：

${\hat{S}}_{t+1}=S_t+T_s\left[\begin{array}{cc}\cos {\mathrm{\&theta;}}_1\left(t\right)& 0\\ \sin {\mathrm{\&theta;}}_1\left(t\right)& 0\\ \frac{-\sin \mathrm{\&gamma;}\left(t\right)}{l_1\cos \mathrm{\&gamma;}\left(t\right)+l_2}& \frac{-l_2}{l_1\cos \mathrm{\&gamma;}\left(t\right)+l_2}\end{array}\right]\left[\begin{array}{c}{v}_f\left(t\right)\\ {\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}}_1\left(t\right)\end{array}\right]$

其中铰接车交接点为H，前桥中点为P_f(x₁，y₁)，该点与中央铰接点H的距离为l₁，车速为vf；后桥中点为P_r(x₂，y₂)点，该点与中央铰接点的距离为l₂，车速为vr；前车体横摆角速度为ω1，转向半径为r1，后车体横摆角速度为ω2，转向半径为r2；前车体的航向偏离角为θ1，后车体的航向偏离角为θ2，转向角为λ；t时刻前桥中点的位置姿态信息用位姿状态向量S_t＝[x₁(t)y₁(t)θ₁(t)]^T表示。

3.根据权利要求2所述的地下矿用车辆的反应式导航控制方法，其特征在于，建立航向偏离角模型具体包括：

铰接车的航向偏离角和铰接车前车体的横摆角速度ω1相关，根据t时刻前车体横摆角速度ω1、t时刻铰接车的车速测量值铰接车的转向角测量值通过以下方法通过运动学模型得到航向偏离角模型：

铰接式车辆在行驶过程中前后车体在中央铰接点处的速度相等，即V₂+V_h2＝V₁+V_h1；

铰接式车辆前后车体在中央铰接点处平行于后桥轴线方向的速度分量相等,因此铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等，即V₁sinλ＝ω₂l₂+ω₁l₁cosλ；

铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等，即V₂＝V₁cosλ+ω₁l₁sinλ，

计算铰接式车辆的转向角变化速率等于前后车体绕各自瞬时中心转动的角速度之差，即：-dλ/dt＝ω₂-ω₁，以此得到ω₁＝(V₁sinλ+l₂dλ/dt)/(l₂+l₁cosλ)，

根据公式ω₁＝dθ/dt得到铰接式车辆车体航向偏离角与转向角以及车体参数之间关系式为：

dθ/dt＝(V₁sinλ+l₂dλ/dt)/(l₂+l₁cosλ)，

t时刻航向偏离角θ(t)表示为θ(t)＝θ(t-1)+ω₁(t)×T，

其中θ(t-1)为航向偏离角初始值，设初值为0，T为系统采样时间。

4.根据权利要求3所述的地下矿用车辆的反应式导航控制方法，其特征在于，建立铰接车前桥中心位置偏差模型具体包括：

以y1，y2为内侧、外侧巷道曲线方程，k1，k2分别为巷道内侧与外侧的一定区域内的弧线曲率平均值，G1为偏离车体角度为θ的方向上障碍点到车体前桥中心的距离，则k1，k2表示为：

$k_1=\left|{y^{\&prime;\&prime;}}_1\right|/\sqrt{{(1+{y_1}^{\&prime;2})}^3},$

$k_2=\left|{y^{\&prime;\&prime;}}_2\right|/\sqrt{{(1+{y_2}^{\&prime;2})}^3},$

则t时刻铰接车前桥中心位置偏差δ(t)表达为：

δ(t)＝[k₁(t)-k₂(t)]/2k₁(t)k₂(t)-G₁(t)。

5.根据权利要求4所述的地下矿用车辆的反应式导航控制方法，其特征在于，建立转向角控制模型包括：

车体航向偏离角安全预测值θ(t+1)由当前时刻车体前方障碍信息确定，令检测角度分辨率为1度，车辆可检测到前方[0,SA°]巷道壁距车辆前桥中心的距离，则可检测到的障碍点个数为(SA+1)个；

利用障碍贡献函数确定每个障碍点的障碍贡献值，以确定其对车辆安全形势的影响，

将车体正前方的区域等分成n个扇形区域，每个扇形区域内的障碍点密度可表示为Dn，进而得到n个扇形区域的障碍密度点D＝[D1,D2,…,Dn]T

利用三次样条差值，得到的障碍密度点以及其对应的区域角度值转化为笛卡尔坐标系下的一系列的坐标值{(x1,y1),(x2,y2),…(xn,yn)}，

三次样条差值函数峰值中的最小值为S(x_m)，令B(t+1)＝xm，则B(t+1)为t+1时刻安全行车区域所对应的角度，

则t+1时刻车体航向偏离角安全预测值θ(t+1)表示为：

θ(t+1)＝[SA×π/360-B(t+1)]，

其中k1，k2分别为巷道内侧与外侧的弧线曲率，G1为偏离车体角度为θ的方向上障碍点到车体前桥中心的距离，

t+1时刻铰接车前桥中心位置偏差安全预测值δ(t+1)用车体左、右前方两部分障碍点权重之差表示为：δ(t+1)＝κ[L(t)-R(t)]，其中κ为正值系数，

得到t+1时刻航向偏离角安全预测值θ(t+1)和前桥中心位置偏离角安全预测值δ(t+1)后，利用模糊算法生成t+1时刻目标转向角λ(t+1)，

模糊算法的隶属度函数依据车辆在行驶过程中λ与δ和θ的相关关系确定，

依据隶属度函数将δ、θ和λ均划分为7个模糊子集，

δ＝﹛负大，负中，负小，零，正小，正中，正大﹜，

θ＝﹛负大，负中，负小，零，正小，正中，正大﹜

λ＝﹛负大，负中，负小，零，正小，正中，正大﹜

模糊控制策略的推理逻辑为IF前桥中心位置偏差d，AND航向偏离角q，THEN转向角λ，

由此得到t+1时刻铰接车的目标转向角λ(t+1)。

6.根据权利要求5所述的地下矿用车辆的反应式导航控制方法，其特征在于，建立铰接车速度控制模型具体包括：

铰接车的目标车速V根据车辆当前车速V和前方障碍距离D确定，地下矿用铰接车在行驶过程中分为加速行驶、匀速行驶、减速行驶、制动停车，本速度控制模型也按照铰接车的实际驾驶情况分为加速行驶模式、匀速行驶模式、减速行驶模式、制动停车模式；

根据测出铰接车从最高速减速行驶到停车所用时间t减速，并针对不同驾驶模式的转换分别设定了转换阈值，分别为：

t减速阈值＝2t减速，

t匀速阈值＝4t减速，

t加速阈值＝6t减速，

在采集回来的前方障碍物距离信息D后分析找到距离障碍物距离最近的点，并计算以当前车速下与障碍发生碰撞所需的时间t，并将t与上述设定的时间阈值进行比较来选择此时的驾驶模式，

如果t≤t减速阈值，立即制动停车，

如果t减速阈值<t≤t匀速阈值，减速行驶，

如果t匀速阈值<t≤t加速阈值，匀速行驶，

如果t≥t加速阈值，加速行驶。