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

一种地下矿用车辆的反应式导航控制方法
【技术领域】
[0001] 本发明设及导航技术领域，特别是指一种地下矿用车辆的反应式导航控制方法。
【背景技术】
[0002] 随着技术的发展，现在地下采矿行业中采用的采矿装备都已经实现了无轨化和液 压化，并正在朝着大型化、智能化方向发展。现有的地下矿用车辆大多为无轨较接式结构， 因此较接式车辆的自主导航控制技术是实现地下金属矿设备智能化与自动化的关键。
[0003] 现有的地下矿用车辆的自主导航技术可W分为W下两种；反应式导航和绝对式导 航；
[0004] 其中绝对式导航需要在导航系统中预先定义一组路径（即路径规划）并且得知每 一时刻车体的绝对坐标（即车体定位）。该种导航方式需要预先进行大量工作W对提前规 划合理的行驶路径，并需要结合车辆的运行特性。同时为了保证导航的精确性W防止发生 碰撞，在行驶路径上必须预制大量的专用定位设备，并在车上需要加装定位传感器来采集 定位信息，W实现对车辆的精确定位。由此可见该种导航方法需要大量的前期投入，成本高 且可靠性不好。
[0005] 其中反应式导航是一种主流的地下矿用车辆自主导航技术矿山设备的自主导航 关键技术，反应式导航系统不需要所处位置的环境模型，而是通过车身上的传感器感知周 围环境。当井下车辆通过巷道时，车辆不需要知道自己在井下的绝对位置，只需知道自己相 对于巷道壁的距离及其角度就能自主行走。因此反映式导航也称为相对式导航，具有前瞻 性和极好的适应性，有利于车辆的高速运行。
[0006] 现有技术中对于自主导航已经有了很多研究，例如申请号为化200910238066. 3 的中国发明专利申请公开了一种采用绝对式导航技术的地下自主伊运机导航控制方法，该 种方法是根据事先规划好的目标路径与伊运机实际行驶路径对比得到伊运机行驶过程中 的偏差，W偏差量作为反馈进行实时控制。该种技术至少存在W下缺点：
[0007] 导航控制方法未充分考虑较接式车辆的运动学和动力学特性，导航前需要规划好 一条路径，为了使规划的路径合理，规划路径时需要结合车辆的运动学特点，该增加了导航 控制的前期工作量；导航需要车辆准确的定位信息，因此需要在巷道的"关键地点"加装定 位设备，在大型复杂的地下矿井中，"关键地点"的数目非常大，该就需要大量的定位设备， 会大大增加导航的成本，而且由于地下巷道宽度和高度的限制，定位设备的安装会受到很 大程度的制约；导航中提出用"航位推算"的方法来减少定位设备的安装，但"航位推算"又 需要在车上加装相应的传感器来采集地下巷道壁的信息，由于地下巷道壁的凹凸不平，航 位推算所需要的信息本身也会有很大的误差，该会加大航位推算的误差，而且该种误差会 累加，最终推算出来的结果可能不能使用。W上所述导航控制技术的缺点也反映出绝对式 导航本身的不足之处。

【发明内容】

[0008] 针对现有技术中的导航技术存在的诸多问题，本发明实施例提出了一种地下矿用 车辆的反应式导航控制方法。
[0009] 为了达到上述目的，本发明实施例提出了一种地下矿用车辆的反应式导航控制方 法，包括：
[0010] 根据当前时刻的车辆较接点、前桥中点、前桥车速、前桥横摆角速度、前桥转向半 径，后桥中点、后桥车速、后桥横摆角速度、后桥转向半径，建立下一时刻前桥位姿状态向量 估计值模型；
[0011] 建立航向偏离角模型；
[0012] 建立较接车前桥中屯、位置偏差模型；
[0013] 建立转向角控制模型，其中转向角控制模型包括安全预测模型和模糊控制算法；
[0014]建立较接车速度控制模型，并根据较接车速度控制模型控制地下矿用车辆。
[0015] 其中，下一时刻为t+1时刻，且所述前桥位姿状态向量估计值模型为非线性离散 模型，表示为：
[0016]
[0017]其中较接车交接点为H，前桥中点为Pf(XI，yi)，该点与中央较接点H的距离为li， 车速为vf;后桥中点为Pf(X2, 72)点，该点与中央较接点的距离为I2,车速为vr;前车体横 摆角速度为《 1，转向半径为rl，后车体横摆角速度为转向半径为r2;前车体的航向偏 离角为0 1，后车体的航向偏离角为0 2,转向角为A;t时刻前桥中点的位置姿态信息用位 姿状态向量St= [Xi(t)yi(t) 0i(t)]T表示。
[0018] 其中，建立航向偏离角模型具体包括：
[0019] 较接车的航向偏离角和较接车前车体的横摆角速度《 1相关，根据t时刻前车体 横摆角速度《l、t时刻较接车的车速测量值F?、较接车的转向角测量值1?，通过W下方 法通过运动学模型得到航向偏离角模型：
[0020] 较接式车辆在行驶过程中前后车体在中央较接点处的速度相等，即V2+Vh2 = VVhi;
[0021] 较接式车辆前后车体在中央较接点处平行于后桥轴线方向的速度分量相等， 因此较接式车辆前后车体在中央较接点处垂直于后桥轴线方向的速度分量也相等，即 ViSinA二CO2I2+W1I1COSA ;
[0022] 较接式车辆前后车体在中央较接点处垂直于后桥轴线方向的速度分量也相等，即 ^2二VicosA+C0ilisinA，
[0023] 计算较接式车辆的转向角变化速率等于前后车体绕各自瞬时中屯、转动的角速度 之差，即；-d入/dt=W2-W1，W此得到W1二（V isin入+I2CI入 /化)/(Ig+liCos入），
[0024] 根据公式《1=d0 /化得到较接式车辆车体航向偏离角与转向角W及车体参数 之间关系式为：
[00巧]d目/dt二（VisinA+l2dA/dt) / (I2+I1COS火），
[0026]t时刻航向偏离角 0 (t)表示为 0 (t) = 0 (t-l) + ?i(t)XT,
[0027] 其中0 (t-1)为航向偏离角初始值，设初值为0,T为系统采样时间。
[0028] 其中，建立较接车前桥中屯、位置偏差模型具体包括：
[0029] Wyl，y2为内侧、外侧巷道曲线方程，kl，k2分别为巷道内侧与外侧的一定区域 内的弧线曲率平均值，G1为偏离车体角度为0的方向上障碍点到车体前桥中屯、的距离，贝U kl，k2表示为；
[003引贝ijt时刻较接车前桥中屯、位置偏差5 (t)表达为：
[003引5 (t) = [ki(t)-k2(t)]/2ki(t)k2(t)-Gi(t)。
[0034] 其中，建立转向角控制模型包括：
[0035] 车体航向偏离角安全预测值0 (t+1)由当前时刻车体前方障碍信息确定，令检测 角度分辨率为1度，车辆可检测到前方[0,SA° ]巷道壁距车辆前桥中屯、的距离，则可检测 到的障碍点个数为（SA+1)个；
[0036] 利用障碍贡献函数确定每个障碍点的障碍贡献值，W确定其对车辆安全形势的影 响，
[0037] 将车体正前方的区域等分成n个扇形区域，每个扇形区域内的障碍点密度可表示 为化，进而得到n个扇形区域的障碍密度点D=巧1，D2,…，Dn]T
[0038] 利用=次样条差值，得到的障碍密度点W及其对应的区域角度值转化为笛卡尔坐 标系下的一系列的坐标值{(xl,yl)，（x2,y2)，…（xn,yn)}，
[0039] S次样条差值函数峰值中的最小值为S(Xm)，令B(t+1) =xm，则B(t+1)为t+1时 刻安全行车区域所对应的角度，
[0040] 则t+1时刻车体航向偏离角安全预测值0 (t+1)表示为：
[0041] 0 (t+1) =[SAXJi/360-B(t+l)]，
[0042] 其中kl，k2分别为巷道内侧与外侧的弧线曲率，G1为偏离车体角度为0的方向 上障碍点到车体前桥中屯、的距离，
[0043]t+1时刻较接车前桥中屯、位置偏差安全预测值5(t+1)用车体左、右前方两部分 障碍点权重之差表示为：5(t+1)=K[t(t)-R(t)]，其中K为正值系数，
[0044] 得到t+1时刻航向偏离角安全预测值0 (t+1)和前桥中屯、位置偏离角安全预测值 5 (t+1)后，利用模糊算法生成t+1时刻目标转向角A(t+1)，
[0045] 模糊算法的隶属度函数依据车辆在行驶过程中A与5和0的相关关系确定，
[0046]依据隶属度函数将5、0和A均划分为7个模糊子集，
[0047] 5 ={负大，负中，负小，零，正小，正中，正大}，
[0048] 0 ={负大，负中，负小，零，正小，正中，正大}
[0049]A={负大，负中，负小，零，正小，正中，正大}
[0050] 模糊控制策略的推理逻辑为IF前桥中屯、位置偏差d，AND航向偏离角q，T肥N转向 角入，
[0051] 由此得到t+1时刻较接车的目标转向角A(t+1)。
[0052] 其中，建立较接车速度控制模型具体包括：
[0053] 较接车的目标车速V根据车辆当前车速V和前方障碍距离D确定，地下矿用较接 车在行驶过程中分为加速 行驶、匀速行驶、减速行驶、制动停车，本速度控制模型也按照较 接车的实际驾驶情况分为加速行驶模式、匀速行驶模式、减速行驶模式、制动停车模式；
[0054] 根据测出较接车从最高速减速行驶到停车所用时间t减速，并针对不同驾驶模式 的转换分别设定了转换阔值，分别为：
[00巧]t减速阔值=2t减速，
[0056] t匀速阔值=4t减速，
[0057] t加速阔值二化减速，
[0058] 在采集回来的前方障碍物距离信息D后分析找到距离障碍物距离最近的点，并计 算W当前车速下与障碍发生碰撞所需的时间t，并将t与上述设定的时间阔值进行比较来 选择此时的驾驶模式，
[0059] 如果t《t减速阔值，立即制动停车，
[0060] 如果t减速阔值<t《t匀速阔值，减速行驶，
[006。如果t匀速阔值<t《t加速阔值，匀速行驶，
[0062] 如果t>t加速阔值，加速行驶。
[0063] 本发明的上述技术方案的有益效果如下；
[0064] 本发明与现有技术相比有如下优点和效果：
[0065] 本方法建立了较接车的运动学模型，依据较接车的运动学模型建立导航控制模 型，使其导航控制更加精确。同时本方法明确给出了各个误差模型和控制模型的建立方法， 实用性大大增强。本方法不需要进行路径规划，减小了导航前期的工作量，提高了导航的控 制的效率。本方法不需要车辆的定位信息，省去了定位设备的安装调试，在尽量少的设备中 能够实现较接车的导航控制，在复杂的大型矿中能大大减少成本，因为附属设备的减少提 高了导航控制的稳定性和可靠性。本方法利用距离传感器获取车辆前方的障碍信息，使得 车辆控导航控制具有一定的前瞻性。
【附图说明】
[0066] 图1为本发明实施例的反应式导航控制结构示意图；
[0067] 图2为较接式车辆模型示意图；
[0068] 图3为较接式转向示意图；
[0069] 图4为较接式车辆在圆弧巷道中位置信息示意图；
[0070]图5为较接式车辆在直线巷道中的位置信息示意图；
[0071] 图6为前桥中屯、位置偏差隶属度函数不意图；
[0072] 图7为航向偏离角隶属度函数示意图；
[0073] 图8为转向角隶属度函数示意图；
[0074] 图9为模糊算法规则表；
[00巧]图10为编码器采集数表；
[0076] 图11为取样范围内采集数据列表；
[0077] 图12为划分扇形区域示意图；
[0078] 图13为障碍密度S次样条插值示意图；
[0079] 图14为较接式车辆安全行驶参数及控制参数表；
[0080] 图15为较接式模型车车体参数W及控制参数表；
[0081] 图16为仿真结果示意图；
[0082] 图17为本发明实施例的流程图。
【具体实施方式】
[0083] 为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具 体实例进行详细描述。
[0084] 本发明实施例提出了一种地下矿用车辆的反应式导航控制方法，其流程如图17 所示的，包括：
[0085] 根据当前时刻的车辆较接点、前桥中点、前桥车速、前桥横摆角速度、前桥转向半 径，后桥中点、后桥车速、后桥横摆角速度、后桥转向半径，建立下一时刻前桥位姿状态向量 估计值模型；
[0086] 建立航向偏离角模型；
[0087] 建立较接车前桥中屯、位置偏差模型；
[008引建立转向角控制模型，其中转向角控制模型包括安全预测模型和模糊控制算法；
[0089] 建立较接车速度控制模型，并根据较接车速度控制模型控制地下矿用车辆。
[0090] A.建立较接车运动学模型，
[0091] 较接车转向时模型如附图2所示，较接车交接点为H，前桥中点为Pf(XI，yi)，该点 与中央较接点H的距离为li，车速为vf;后桥中点为Pf(X2, 72)点，该点与中央较接点的距 离为12,车速为vr，前车体横摆角速度为《1，转向半径为rl，后车体横摆角速度为《 2,转 向半径为r2,前车体的航向偏离角为0 1，后车体的航向偏离角为0 2,转向角为A。
[0092]t时刻前桥中点的位置姿态信息可W用位姿状态向量St=[Xi(t)yi(t)0i(t)]T表 示，t+1时刻的前桥位姿状态向量估计值可W用非线性离散模型表示为：
[0093]
[0094] B.建立航向偏离角模型，
[0095] 较接车的航向偏离角和较接车前车体的横摆角速度相关，t时刻前车体横摆角速 度《 1根据t时刻较接车的车速测量值FW和较接车的转向角测量值iw经运动学模型得 到，[08引航向偏离角0 (t)如附图3所示，较接式车辆在行驶过程中前后车体在中央较接 点处的速度相等，则有：
[0096] VVh2=Vi+Vhi，
[0097] 忽略较接式车辆行驶过程中的侧滑，其前后车体在中央较接点处平行于后桥轴线 方向的速度分量相等，进而较接式车辆前后车体在中央较接点处垂直于后桥轴线方向的 速度分量也相等，即ViSinA= ?2l2+?iliC〇s入，
[0098] 较接式车辆前后车体在中央较接点处垂直于后桥轴线方向的速度分量也相等， 良P:乂2 二ViCos入+?ilisin入，
[0099] 较接式车辆的转向角变化速率等于前后车体绕各自瞬时中屯、转动的角速度之差， 良P;-d入/dt= ?2_W1，
[0100] W此得到W1=(ViSinA+12(1A/dt) /Q2+I1COS入）
[0101] 又《i=d0/dt，得到较接式车辆车体航向偏离角与转向角W及车体参数之间关 系式为：
[010引d日/dt=(Visin入+I2CI入/dt) / (I2+I1COS入），
[0103] 当前时刻航向偏离角0 (t)可表示为：0 (t) = 0 (t-l) + ?i(t)XT，
[0104] 其中0 (t-1)为航向偏离角初始值，T为系统采样时间。
[0105] C.建立车体前桥中屯、位置偏差模型，
[0106] 较接式车辆在圆弧巷道中位置信息如附图4所示，yl，y2为内侧、外侧巷道曲线方 程，kl，k2分别为巷道内侧与外侧的一定区域内的弧线曲率平均值，G1为偏离车体角度为 0的方向上障碍点到车体前桥中屯、的距离，则kl，k2表示为：
[0109] 则t时刻较接车前桥中屯、位置偏差5 (t)可表达为：
[0110] 5(t) = [ki(t)-k2(t)]/^i(t)k2(t)-Gi(t)，
[0111] 当巷道为直线巷道时，可简化为图5,此时巷道内侧与外侧的一定区域内的弧线曲 率平均值kl，k2都为0，
[0112] 其中hl，h2，h3分别为偏离车体前桥0°，90°及180°方向上障碍点到车体前桥 中屯、的距离，0为车体航偏离角，GO为巷道宽度，
[0113] 由附图5中的几何关系可得到：
[0114] G〇(t) =hi(t)+h3(t)]cos0(t)，
[0115] Gi(t) =I12(t)sin日（t)，
[0116] t时刻中屯、位置偏差d可表示为；
[0117] 5 (t) =G〇(t)/2-Gi(t)〇 [011引D.建立转向角控制模型，
[0119] 如附图1所示，航行角控制模型包括全预测模型和模糊控制算法，其中安全预测 模型的输入量为t时刻的航向偏离角和前桥中屯、位置偏差，输出量为t+1时刻的航向偏离 角和前桥中屯、位置偏差，模糊控制算法的输入量为t+1时刻的航向偏离角和前桥中屯、位置 偏差，经算法得到的结果即为转向角控制模型最终输出量较接车的转向角入，
[0120] a.建立安全预测模型，利用安全预测模型得到下一时刻较接车的安全航向偏离角 和安全中屯、位置偏差，
[0121] 安全预测模型中下一时刻允许的车体航向偏离角0 (t+1)可由当前时刻车体正 前方障碍点信息确定，
[0122] 如图5所示车体正前方0~SA的角度，半径为R的阴影区域为障碍点对前车体有 影响的区域，其中d为一个障碍点到车体前桥中屯、的距离。对于障碍点的采样角度间隔为 r，即0~SA的角度范围内共采样（SA+1)个点，
[0123] 为表示障碍点对车体的影响，定义障碍贡献值为：
[0124]
[0125] 其中dj.为障碍点到前（或后）桥中屯、的距离，
[012引j= [0，1，…，SA]
[0127]为弥补系统误差，令Re G [1，3]，
[012引将车体正前方的区域划分成n等分扇形区域，每个扇形区域占有角度为S即；n=SA/。，
[0129] 则每个扇形区域内的障碍点密度表示为：
[0130] 其中
[0131] 通过上述计算得到较接式车体正前方n个扇形区域的障碍密度点的集合为：
[013 2] D =巧1，〇2，…，Dn]T，
[0133] 由于该n个障碍密度点是非连续点，则得到的障碍密度曲线为非连续曲线，为了 使得障碍密度曲线连续，且一阶与二阶导数连续，因此选择=次样条插值处理n个障碍密 度点，
[0134] 将得到的障碍密度点W及其对应的区域角度值转化为笛卡尔坐标系下的一系列 的坐标值{(Xi，yi)，（X2, y2)，…，（X。，yj}，
[013引其中；[Xi，X2，…，Xn]T= 31[0，20，...n0]t/180，
[0136] [yi，72，…，yn]T=巧1，〇2，???DniT，
[0137] S为S次样条曲线方程，选择边界条件；x。" =y。'，x" n=y"。从而得到S次样 条插值函数S(x)，
[013引当S' (Xz) =0且S" (Xz)声0时S(xz)为函数的峰值，其中Z G [1，2,…，n]， 设函数峰值中的最小值为S(xm)，令B(t+1) =xm，则B(t+1)为下一时刻安全行车区域所对 应的角度，
[0139]t+1时刻车体航向偏离角0则表示为：
[0140] 0 (t+1) = [SAX Ji/360-B(t+l)]，
[0141] 车体正前方右半部分障碍点权重为
[0142] 车体正前方左半部分障碍点权重为
[0143] 车体前桥中屯、位置偏差5 (t+1)用车体当前正前方左右两部分障碍点权重之差 表示为；S(t+1) =K[L(t)-R(t)]，
[0144] 其中k为正值系数。
[0145]b.得到t+1时刻航向偏离角安全预测值0 (t+1)和前桥中屯、位置偏离角安全预测 值5 (t+1)后，利用模糊算法生成t+1时刻目标转向角A(t+1)，
[0146] 模糊算法的隶属度函数依据车辆在行驶过程中人与5和0的相关关系确定，
[0147]依据隶属度函数将5、0和A均划分为7个模糊子集，
[014引 5 ={负大，负中，负小，零，正小，正中，正大}
[014引0 ={负大，负中，负小，零，正小，正中，正大}
[0150] A ={负大，负中，负小，零，正小，正中，正大}
[0151] 模糊控制策略的推理逻辑为IF前桥中屯、位置偏差S，AND航向偏离角0，T肥N转 向角入，
[0152] 由此得到t+1时刻较接车的目标转向角A(t+1)。
[0153]E.建立较接车速度控制模型，
[0154] 较接车下一时刻车速V根据车辆当前车速真实测量值rm和前方障碍距离D确 定，
[0155] 地下矿用较接车在行驶过程中分为加速行驶、匀速行驶、减速行驶、制动停车，本 速度控制模型也按照较接车的实际驾驶情况分为加速行驶模式、匀速行驶模式、减速行驶 模式、制动停车模式，
[0156] 速度控制模型建立过程如下：
[0157] 1)测出较接车从最高速减速行驶到停车所用时间t减速，在实际驾驶中，为了保 证安全，驾驶员从一种驾驶模式转换到另一种驾驶模式时，一般都会提前动作，因此本速度 控制模型针对不同驾驶模式的转换分别设定了转换阔值，分别为：
[015引t减速阔值=1. 5t减速，
[0159]t匀速阔值=2t减速，
[0160]t加速阔值=2. 5t减速，
[0161]2)在采集回来的前方障碍物距离信息D后分析找到距离障碍物距离最近的点，并 计算W当前车速下与障碍发生碰撞所需的时间t，并将t与上述设定的时间阔值进行比较 来选择此时的驾驶模式，
[0162] 如果t《t减速阔值，立即制动停车，
[0163] 如果t减速阔值<t《t匀速阔值，减速行驶，
[0164] 如果t匀速阔值<t《t加速阔值，匀速行驶，
[0165] 如果t>t加速阔值，加速行驶，
[0166] 经过上述判断过程，得到车辆当下的驾驶模式。
[0167] 下面结合实施例对本发明做进一步说明，
[016引在本实施例中W某型较接式AGV为控制对象，在巷道进行反应式导航控制。实施 例中较接车车身安装有某型号激光雷达、转角传感器、速度编码器等，较接车的结构参数如 图6所示，由转角传感器采集较接式车体转向角1，编码器采集车轮转速，将转速转化为车 体速度V，激光雷达扫描前车体的障碍区域采集障碍点信息dj.，实施步骤如下：
[0169] 1.建立转向控制模型，
[0170] 实施例中控制系统的采样时间为T= 20ms，对于选取的该时刻，较接式模型车车 体上安装的传感器采集数据如附图7所示，
[0171]该时刻激光雷达的角分辨率为r，扫描范围为-5。~185°，共191个点，对于 该时刻由采集的数据得到的曲线可得曲线的曲率为0,即该时刻巷道壁为直巷道，因此确定 车体前桥中屯、位置偏差仅需采集角度为0°，90°W及180° =点的数据即可，为了弥补= 点数据的不精确性，故实验中选择了S点10°范围内的数据值，即0°的数据值由-5°~ 5°的范围内的数据平均确定，90°的数据值由85°~90°的范围内的数据平均值确定W 及180°的数据值由175°~85°的范围内的数据平均值确定，列出该S点10°范围内的 数据值如附图8所示，
[0172] (1)利用航向偏离角模型和前桥中屯、位置偏差模型建立的输出量建立安全预测模 型，
[0173] 利用=次样条差值得到航向偏离角和前桥中屯、位置偏差，选取激光雷达扫描的范 围为0~180°，即SA= 180°，障碍点采集个数是181，取0 = 5,则n= 36,即在车体正 前方划分出36等分扇形区域，扇形区域分布如图9所示，
[0174] 通过公式公式. 得到36个障碍点密度，对 5 于=次样条插值来说，36个点的插值运算量过大，将36个点分为9组，每组4个障碍点进 行=次样条插值，得到9段平滑的曲线，对于相邻两段曲线之间仍取出4个点进行=次样条 插值，得到=次样条差值函数S(x)即得到一条具有连续二阶导数的平滑曲线，如附图10所 示，
[0175] 将得到的障碍密度点W及其对应的区域角度值转化为笛卡尔坐标系下的一系列 的坐标值{(Xi，yi)，（X2,y2)，…，（X。，yj}，
[017引其中；[Xi，X2，…，又^了二31[o，20，...n0]T/180，
[0177] [yi，72，…，yn]T=巧1，〇2，???DniT，
[017引设S为S次样条曲线方程，选择边界条件；x"u=y' 。，X"n=y"。，从而得到 =次样条插值函数S(x)，
[0179] 下一时刻车体航向偏离角为：
[0180] 0 (t+1) =[SAXJi/360-B(t+l)]，
[0181] 车体前桥中屯、位置偏差5 (t+1)用车体当前正前方左右两部分障碍点权重之差 表不，
[0182] 车体正前方右半部分障碍点权重为：
[0183]
[0184] 车体正前方左半部分障碍点权重为：
[0185]
[018引则 5 (t+1) =K[L(t)-R(t)]，
[0187]其中K为正值系数。
[018引（2)利用模糊控制算法得到较接车的实际控制量，
[0189]得到t+1时刻航向偏离角安全预测值0 (t+1)和前桥中屯、位置偏离角安全预测值 5 (t+1)后，利用模糊算法生成t+1时刻目标转向角A(t+1)，
[0190] 模糊算法的隶属度函数依据车辆在行驶过程中A与5和0的相关关系确定，
[0191] 附图12所示，转向角1的隶属度函数如附图13所示，
[0192]模糊算法的推理逻辑为；IF前桥中屯、位置偏差山AND航向偏离角0，T肥N向角 入，通过对驾驶经验的统计得到模糊控制规则如表14所示，
[0193] 航向偏差角、车体前桥中屯、位置偏差通过模糊控制算法得到安全，采用重屯、法来 进行反模糊化，取中屯、偏移位移量化因子取2,偏离角量化因子取2,输出的比例因子取5， 该时刻的得到的输入、输出变量如附图15所示。
[0194]E.建立较接车速度控制模型，
[0195] 地下矿用较接车在行驶过程中分为加速行驶、匀速行驶、减速行驶、制动停车，本 速度控制模型也按照较接车的实际驾驶情况分为加速行驶模式、匀速行驶模式、减速行驶 模式、制动停车模式，具体实施步骤如下：
[0196] (1)测量实施例中较接车从静止W最大加速度行驶至最大速度，匀速行驶一段时 间，然后减速行驶至停止，为了精确，重复测量取均值，得到较接车的速度V和时间t的关 系，本实施例中较接车的最大速度Vmax= 2. 5m/s，较接车从静止到最大速度的时间t加速 =4. 5s，较接车从最高车速减速至停止所用的时间为t减速=7. 4s，
[0197] 在实际驾驶中，为了保证安全，驾驶员从一种驾驶模式转换到另一种驾驶模式时， 一般都会提前动作，因此本速度控制模型针对不同驾驶模式的转换分别设定了安全转换时 间阔值，分别为：
[ 019引 t减速阔值=1. 5t减速，
[0199]t匀速阔值=2t减速，
[0200] t加速阔值=2. 5t减速，
[0201] 2)在采集回来的前方障碍物距离信息D后分析找到距离障碍物距离最近的点，并 计算W当前车速下与障碍发生碰撞所需的时间t，并将t与上述设定的时间阔值进行比较 来选择此时的驾驶模式，
[020引如果t《t减速阔值，立即制动停车，
[020引如果t减速阔值<t《t匀速阔值，减速行驶，
[0204] 如果t匀速阔值<t《t加速阔值，匀速行驶，
[0205] 如果t>t加速阔值，加速行驶，
[0206] 经过上述判断过程，实现车辆驾驶模式的切换。
[0207] 在确定控制流程和模糊算法的规则后，本导航控制系统可在在电脑中进行仿真实 验，仿真实验WVisualStudio为开发环境，搭建了一套可动态加载巷道地图、采集虚拟传 感数据、生成车辆运行轨迹的仿真软件，对仿真软件中的车辆加入本发明提出的控制策略。 [020引仿真较接车结构参数如图6所示，仿真中较接车行驶性能参数设定位Vmax= 2. 5m/s，t减速=7. 4s，仿真巷道分别设置为L、Z、S形，巷道宽度设置为2. 5倍的车宽，在 实验过程中，选取适当的采样时间，忽略较接车的侧滑等因素的影响，本导航控制方法能够 实现较接车辆在巷道中导航控制，仿真实验结果如附图16所示。
[0209] 实验结果表明，本发明中的导航方法能够顺利完成较接车辆在巷道中的导航控制 工作，在直巷道、弯曲巷道中较接车辆能够W最佳行驶模式行驶，在无障碍的情况下，车辆 可w最大速度前进，在转弯时能够减速前行顺利通过弯道，前方不可通行时会自动停车。 [0210]W上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员 来说，在不脱离本发明所述原理的前提下，还可W做出若干改进和润饰，该些改进和润饰也 应视为本发明的保护范围。
【主权项】
1. 一种地下矿用车辆的反应式导航控制方法，其特征在于，包括： 根据当前时刻的车辆铰接点、前桥中点、前桥车速、前桥横摆角速度、前桥转向半径，后 桥中点、后桥车速、后桥横摆角速度、后桥转向半径，建立下一时刻前桥位姿状态向量估计 值模型； 建立航向偏离角模型； 建立铰接车前桥中心位置偏差模型； 建立转向角控制模型，其中转向角控制模型包括安全预测模型和模糊控制算法； 建立铰接车速度控制模型，并根据铰接车速度控制模型控制地下矿用车辆。2. 根据权利要求1所述的地下矿用车辆的反应式导航控制方法，其特征在于，下一时 刻为t+Ι时刻，且所述前桥位姿状态向量估计值模型为非线性离散模型，表示为：其中铰接车交接点为H，前桥中点为Pf (Xl，yi)，该点与中央铰接点H的距离为I1，车速 为￥；^后桥中点为？1^2,72)点，该点与中央铰接点的距离为1 2，车速为￥1';前车体横摆角速 度为ω 1，转向半径为rl，后车体横摆角速度为ω2,转向半径为r2 ;前车体的航向偏离角为 Θ1，后车体的航向偏离角为Θ 2,转向角为λ ;t时刻前桥中点的位置姿态信息用位姿状态 向量 St= [X1 (Oy1 (t) QJt) ]τ表示。3. 根据权利要求2所述的地下矿用车辆的反应式导航控制方法，其特征在于，建立航 向偏离角模型具体包括： 铰接车的航向偏离角和铰接车前车体的横摆角速度ω 1相关，根据t时刻前车体横摆 角速度ω 1、t时刻铰接车的车速测量值铰接车的转向角测量值AW通过以下方法 \ J 通过运动学模型得到航向偏离角模型： 铰接式车辆在行驶过程中前后车体在中央铰接点处的速度相等，即VfVh2= V JVhl; 铰接式车辆前后车体在中央铰接点处平行于后桥轴线方向的速度分量相等，因此铰 接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等，即V 1Sin λ = ω 212+ ω J1Cos λ ; 铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等，即V2= V1Cos λ + ω J1Sin λ， 计算铰接式车辆的转向角变化速率等于前后车体绕各自瞬时中心转动的角速度之差， 艮P :_d λ /dt = ω 2- ω 丄，以此得到 ω 丄=(V pin λ +l2d λ /dt) / (!^+I1Cos λ )， 根据公式ω1= d θ/dt得到铰接式车辆车体航向偏离角与转向角以及车体参数之间 关系式为： d Θ /dt = (V1Sin λ +l2d λ /dt) / (!^+I1Cos λ )， t时刻航向偏离角θ⑴表示为θ⑴=θ (t-D + coJi^XT， 其中θ (t-i)为航向偏离角初始值，设初值为0, T为系统采样时间。4. 根据权利要求3所述的地下矿用车辆的反应式导航控制方法，其特征在于，建立铰 接车前桥中心位置偏差模型具体包括： 以y I，y2为内侧、外侧巷道曲线方程，kI，k2分别为巷道内侧与外侧的一定区域内的弧 线曲率平均值，Gl为偏离车体角度为Θ的方向上障碍点到车体前桥中心的距离，则kl，k2 表示为：则t时刻铰接车前桥中心位置偏差δ (t)表达为： δ (t) = IX 〇5. 根据权利要求4所述的地下矿用车辆的反应式导航控制方法，其特征在于，建立转 向角控制模型包括： 车体航向偏离角安全预测值Θ (t+Ι)由当前时刻车体前方障碍信息确定，令检测角度 分辨率为1度，车辆可检测到前方[〇, SA° ]巷道壁距车辆前桥中心的距离，则可检测到的 障碍点个数为（SA+1)个； 利用障碍贡献函数确定每个障碍点的障碍贡献值，以确定其对车辆安全形势的影响， 将车体正前方的区域等分成η个扇形区域，每个扇形区域内的障碍点密度可表示为 Dn，进而得到η个扇形区域的障碍密度点D = [Dl，D2,…，Dn] T 利用三次样条差值，得到的障碍密度点以及其对应的区域角度值转化为笛卡尔坐标系 下的一系列的坐标值{(xl, yl)，（x2, y2)，…（xn, yn)}， 三次样条差值函数峰值中的最小值为S (Xm)，令B (t+1) = xm，则B (t+1)为t+1时刻安 全行车区域所对应的角度， 则t+Ι时刻车体航向偏离角安全预测值Θ (t+Ι)表示为： Θ (t+1) = [SAX Ji/360-B(t+l)]， 其中kl，k2分别为巷道内侧与外侧的弧线曲率，Gl为偏离车体角度为Θ的方向上障 碍点到车体前桥中心的距离， t+Ι时刻铰接车前桥中心位置偏差安全预测值δ (t+Ι)用车体左、右前方两部分障碍 点权重之差表示为：S (t+Ι) = κ [L(t)-R(t)]，其中κ为正值系数， 得到t+Ι时刻航向偏离角安全预测值Θ (t+Ι)和前桥中心位置偏离角安全预测值 δ (t+Ι)后，利用模糊算法生成t+Ι时刻目标转向角λ (t+Ι)， 模糊算法的隶属度函数依据车辆在行驶过程中λ与δ和Θ的相关关系确定， 依据隶属度函数将S、Θ和λ均划分为7个模糊子集， S ={负大，负中，负小，零，正小，正中，正大}， Θ ={负大，负中，负小，零，正小，正中，正大} λ ={负大，负中，负小，零，正小，正中，正大} 模糊控制策略的推理逻辑为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加速阈值，加速行驶。
【专利摘要】本发明提供了一种地下矿用车辆的反应式导航控制方法，包括：根据当前时刻的车辆铰接点、前桥中点、前桥车速、前桥横摆角速度、前桥转向半径，后桥中点、后桥车速、后桥横摆角速度、后桥转向半径，建立下一时刻前桥位姿状态向量估计值模型；建立航向偏离角模型；建立铰接车前桥中心位置偏差模型；建立转向角控制模型，其中转向角控制模型包括安全预测模型和模糊控制算法；建立铰接车速度控制模型，并根据铰接车速度控制模型控制地下矿用车辆。
【IPC分类】G05D1/02
【公开号】CN104881025
【申请号】CN201510171279
【发明人】孟宇, 豆风铅, 刘立, 胡冰川
【申请人】北京科技大学
【公开日】2015年9月2日
【申请日】2015年4月10日