CN101713999A

CN101713999A - 地下自主铲运机的导航控制方法

Info

Publication number: CN101713999A
Application number: CN200910238066A
Authority: CN
Inventors: 石峰; 顾洪枢; 战凯; 冯孝华; 郭鑫; 周俊武; 李建国; 刘立; 冯茂林; 陈树新; 迟洪鹏
Original assignee: Beijing General Research Institute of Mining and Metallurgy
Current assignee: BGRIMM Technology Group Co Ltd; BGRIMM Machinery and Automation Technology Co Ltd
Priority date: 2009-11-18
Filing date: 2009-11-18
Publication date: 2010-05-26
Anticipated expiration: 2029-11-18
Also published as: CN101713999B

Abstract

本发明公开了一种地下自主铲运机的导航控制方法，该方法包括：首先规划出目标路径，在车辆跟踪目标路径时，根据已知的初始偏差，将横向位置偏差、航向角偏差、航向角变化率偏差构成综合反馈，作为导航控制器的输入，结合PID校正控制，计算出车辆的转向控制指令，输入给车辆的电液转向控制系统，使车辆转向角发生改变，再运用该导航控制器内部构建的车辆轨迹精确推算模型，精确计算出车辆的跟踪轨迹与目标路径之间存在的偏差值，再用这些偏差值作为反馈来计算新的转向控制指令，如此反复，使车辆不断跟踪目标路径，实现地下铲运机自主导航。

Description

地下自主铲运机的导航控制方法

技术领域

本发明涉及一种导航控制技术，尤其涉及一种地下自主铲运机的导航控制方法。

背景技术

随着地下矿开采深度增加，采矿条件越来越恶劣，对人的安全威胁也越来越大，遥控采矿、自动化矿山开采技术应运而生。铲运机是地下无轨采矿的关键设备，实现铲运机在井下巷道内无人操纵、自主行驶，可使操作工人远离井下恶劣、危险的工作环境，保护铲运机司机人员安全、提高采矿作业效率并降低采矿成本。铲运机自主行驶首先要解决自身在巷道内的定位导航问题。虽然地面车辆的定位导航研究较多，技术上也比较成熟，但是由于地下条件的特殊性，地面车辆的定位导航技术不适于在地下条件应用，由于井下岩石遮蔽作用，GPS信号就无法传到地下，使得GPS导航技术不能在井下使用。另外，地下铲运机为了适应井下低矮、狭窄、多弯巷道环境，一般将铲运机车身设计成低矮、细长、铰接式车体。如目前井下用量最大的地下2立方铲运机，长度一般为7米左右，宽度为1.7米左右，高度为2.0米左右，而铲运机车身两侧外廓与巷道壁的距离一般仅为0.5-1米，这么长的车体要在狭窄、多弯的巷道内行驶，其难度是可想而知的，即使是经过培训的铲运机司机，驾驶铲运机时刮蹭巷道壁也时有发生，这些都与地表普通车辆的导航情况有所不同，因此，地下自主铲运机要求的定位导航精度往往比地表车辆还要高，难度也更大。

现有技术中，地下铲运机自主导航也有较多的研究，其中一种地下自主铲运机导航方案是采用沿墙壁行走相互作用导航控制方式，即利用铲运机与巷道壁之间的距离和角度实时控制地下自主铲运机，防止其与巷道碰撞，该方法的好处是不必预先规划和存储目标路径，甚至可以在陌生的环境中跟随行走。

上述现有技术至少存在以下缺点：

导航控制过程要求大量的实时的环境测量数据，导致测量系统和数据传输系统负担很重，而且一旦失去环境测量数据，不能进行轨迹推算，导航控制立即中止。

发明内容

本发明的目的是提供一种地下自主铲运机的导航控制方法，采用该方法可大大减少对环境信息数据的依赖，有助于克服环境测量数据和处理数据的困难，有助于导引车辆避障并可靠跟踪目标路径。

本发明的目的是通过以下技术方案实现的：

本发明的地下自主铲运机的导航控制方法，包括步骤：

A、以预先规划好的铲运机运动轨迹作为铲运机的目标路径，以铲运机的定位参考点P的实际运动轨迹为铲运机的跟踪轨迹，沿所述目标路径的曲线弧长s_m(k)存储路径曲率K_m(k)系列值和起始点航向角

其中，k为采样次数，k＝1，2，3，…，用采样周期T作为测量和控制间隔；

以所述目标路径上的航向角

与所述跟踪轨迹航向角

的差值定义为航向角偏差

用所述铲运机的定位参考点P与该点投影于所述目标路径上的点P_m的距离定义为铲运机的横向位置偏差δ；

B、用所述铲运机的定位参考点P投影于所述目标路径的点P_m的曲线弧长s_m(k)和铲运机在该点的横向位置偏差δ(k)作为所述铲运机的相对位置P(s_m(k)，δ(k))；

用所述铲运机的相对位置P(s_m(k)，δ(k))及铲运机在该位置处的航向角偏差β(k)和铲运机转向角α(k)来表示所述铲运机在该位置处的位姿；

所述铲运机为前部与后部铰接的结构，前部与后部的铰接处安装有角位移传感器，所述角位移传感器直接测量所述铲运机转向角α(k)；

C、通过所述铲运机的定位参考点P的车速v_g(k)计算该点在所述目标路径上的投影车速

计算所述目标路径的航向角变化率

和所述跟踪轨迹的航向角变化率

并计算航向角变化率偏差

所述目标路径的航向角变化率

的计算公式为：

所述跟踪轨迹的航向角变化率

的计算公式为：

式中，l₁为铲运机前部驱动桥的中点到铰接中心线的距离，l₂为铲运机后部驱动桥的中点到铰接中心线的距离；

D、由所述航向角变化率偏差

航向角偏差β(k)和横向位置偏差δ(k)三个参数构成综合反馈e(k)，该综合反馈用于铲运机的转向控制；

所述综合反馈

其中，K₁、K₂和K₃为预先存储的反馈系数，K₁、K₂和K₃的选取范围均为1～100。

由上述本发明提供的技术方案可以看出，本发明所述的地下自主铲运机的导航控制方法，由于首先规划出目标路径，在车辆跟踪目标路径时，根据已知的初始偏差，将横向位置偏差、航向角偏差、航向角变化率偏差构成综合反馈，作为导航控制器的输入，用于车辆的转向控制，可大大减少对环境信息数据的依赖，有助于克服环境测量数据和处理数据的困难，有助于导引铲运机避障并可靠跟踪目标路径。

附图说明

图1为本发明的具体实施例中地下铲运机跟踪轨迹、目标路径及参数说明示意图；

图2为本发明的具体实施例中地下铲运机定位导航系统示意图；

图3为本发明的具体实施例中地下铲运机跟踪轨迹仿真图；

图4为本发明的具体实施例中地下铲运机转弯处的路径规划和跟踪轨迹仿真图；

图5为本发明的具体实施例中地下铲运机导航控制器结构框图。

具体实施方式

本发明的地下自主铲运机的导航控制方法，其较佳的具体实施方式如图1所示，包括步骤：

以所述目标路径上的航向角

与所述跟踪轨迹航向角

的差值定义为航向角偏差

计算所述目标路径的航向角变化率

和所述跟踪轨迹的航向角变化率

并计算航向角变化率偏差

所述目标路径的航向角变化率

的计算公式为：

所述跟踪轨迹的航向角变化率

的计算公式为：

D、由所述航向角变化率偏差

所述综合反馈

当铲运机开始运行时，令采样次数k＝1，输入航向角偏差β(k)、横向位置偏差δ(k)和目标路径曲线弧长s_m(k)的直接测量值，铲运机导航系统在此基础上进行推算和控制；

当铲运机运行一段时间后，再次令采样次数k＝1，输入航向角偏差β(k)、横向位置偏差δ(k)和目标路径曲线弧长s_m(k)新的直接测量值，铲运机导航系统在此基础上进行新的推算和控制，用于消除各种累计误差。

所述步骤A中，所述目标路径的路径曲率K_m(k)系列值由铲运机上装备的陀螺仪和激光扫描器扫描巷道后，计算获得。

所述目标路径的路径曲率K_m(k)与曲线弧长s_m(k)一一对应。

所述采样周期T的选取范围为，50≤T≤300毫秒。

所述目标路径为记忆中的或由路径产生模块产生的虚拟路径，保存在铲运机导航控制器的存储介质中。

所述综合反馈e(k)经PID控制器校正后，输出转向控制电压u(k)，该控制电压u(k)用来控制铲运机的电液比例转向控制系统，实现铲运机自主调整转向角度，并自动跟踪目标路径。

下面结合实施例对发明做进一步的描述：

在本实施例中，以ACY-2地下铲运机为控制对象，进行基于轨迹精确推算模型的车辆导航控制。具体实施步骤如下：

如图1所示，以巷道内路面为坐标平面建立X-Y坐标系，选取铲运机前方驱动桥的中点为铲运机的定位参考点，以该点在X-Y坐标平面上的投影点P来代表铲运机的位置，取该点作为定位参考点的好处是，当铲运机行驶过程中，铲运机在该点的速度方向与前面车体纵向轴线方向一致，方便定位计算。以P点的速度方向(轨迹切线方向)作为车辆的参考方向，其与X轴正方向所成的夹角定义为铲运机的航向角

取从X轴逆时针旋转形成的航向角为正，反之为负。P点的运动轨迹即为铲运机的跟踪轨迹，见图中曲线B；预先规划好的理想的P点运动轨迹作为铲运机的目标路径，见图中曲线A。P点在目标路径上的垂直投影点P_m的速度方向与X轴正方向所成的夹角，定义为目标路径的航向角

目标路径上的航向角

与铲运机跟踪轨迹航向角的差值，定义为航向角偏差β，

该参数反映了自主铲运机运动方向与目标路径预定的行驶方向的偏差。铲运机跟踪轨迹上P点与目标路径上P_m点的距离定义为铲运机的横向位置偏差δ，取P点在目标路径曲线右侧的横向位置偏差为正，反之为负，该参数可反映出铲运机在巷道内的横向定位情况。

图1中曲线A为预先规划好的目标路径，曲线B为铲运机实际跟踪轨迹。K_g为跟踪轨迹在P点的曲率，当曲率中心O_g在车辆轨迹曲线前进方向的左侧时定义为正曲率，曲率半径R_g为正，反之为负。K_m为目标路径在P_m点的曲率，R_m为其曲率半径，曲率中心O_m。S_m为目标路径曲线的弧长，取铲运机运动方向为正方向。α为铲运机转向角，即铲运机前后车体的相对转动角。使地下铲运机反时针转弯的转向角α为正，反之为负。转向角和转向角变化速率决定了车辆的转弯半径大小和铲运机行驶轨迹，自主铲运机正是通过对转向角的不断调整来实现跟踪目标路径。忽略铲运机行驶中的侧滑和离心力，铲运机跟踪轨迹航向角与转向角α的关系为

其中，l₁为铲运机前方驱动桥的中点到铰接中心线的距离，l₂为铲运机后方驱动桥的中点到铰接中心线的距离。为了计算方便，地下铲运机定位P点位置坐标由对应的目标路径P_m点的曲线弧长S_m和铲运机横向位置偏差δ来表示，即P(S_m，δ)。铲运机P点行驶车速为v_g，车速v_g与轨迹曲线B相切，其目标路径上P_m点投影车速为v_m，投影车速v_m与轨迹曲线A相切。行驶车速与投影车速关系为目标路径的航向角

与投影车速v_m及目标路径曲率K_m的关系为

横向位置偏差δ与车速v_m和航向角偏差β的关系为

ACY-2地下铲运机转向油缸运动方程为：

V = \frac{\frac{K_{q 10}}{A_{me}} \frac{A_{v} K_{m} K_{i}}{A_{u} K_{v}} U_{i} - \frac{K_{t}}{A_{1} A_{me}} (\frac{V_{e}}{4 β_{e} K_{t}} s + 1) F_{f}}{(\frac{s^{2}}{ω_{h}^{2}} + \frac{2 ζ_{h}}{ω_{h}} s + 1)}

式中，V——铲运机液压转向油缸活塞杆运动速度，m/s

K_q10——转向油缸控制主阀流量增益，m²/s，K_q10＝0.8m²/s

A_v——转向油缸控制主阀阀芯端面积，m²，A_v＝0.0003m²

A_u——液压先导阀阀芯端面积，m²，A_u＝0.00002m²

A_me——平均活塞面积，m²，A_me＝0.0155m²

K_v——转向油缸主阀阀芯对中弹簧刚度系数，N/m，K_v＝28×10⁴N/m

K_m——比例电磁铁的力电流增益，N/A，K_m＝40N/A

K_i——比例放大器放大系数，A/V，K_i＝0.2A/V

U_i——转向控制电压，V，0～5V

K_t——总流量-压力系数，m⁵/N·s，K_t＝1.13×10^-10m⁵/N·s

V_e——转向油缸的等效容积，m³，V_e＝0.0097m³

A₁——转向油缸无杆腔活塞有效面积，m²，A₁＝0.0177m²

β_e——液压系统的有效体积弹性模数，Pa，β_e＝690×10⁶Pa

F_f——转向油缸的负载力，N，取F_f＝50000N

ω_h——液压系统阀控转向油缸固有频率，l/s，ω_h＝88l/s

ζ_h——液压系统阀控缸阻尼比，无因次。ζ_h＝0.185

转向油缸运动速度与铲运机转向角的关系为：

\frac{dα}{dt} = \frac{\frac{L_{0} + &Integral; Vdt}{L_{OA} L_{OB}} V}{\sqrt{1 - {[\frac{L_{OA}^{2} + L_{OB}^{2} - {(L_{0} + &Integral; Vdt)}^{2}}{{2 L}_{OA} L_{OB}}]}^{2}}}

式中，L₀——铲运机转向角α＝0时的转向油缸安装长度，m，L₀＝1.426m

L_OA——铲运机中央铰接点到前车架转向油缸铰接点之间的距离，m，L_OA＝0.2504m

L_OB——铲运机中央铰接点到后车架转向油缸铰接点之间的距离，m，L_OB＝1.452m

由于井下巷道断面不规则，巷道壁凹凸不平，即使铲运机理想地沿规划目标路径行驶，测量到的铲运机到巷道壁距离数据仍然剧烈变化，所以测量到的数据并不能真实地反映铲运机定位参考点到规划路径的距离，在这种情况下用于控制系统稳定性调节的PID将失去作用，使得导航控制陷于困境。另外，井下巷道内经常会沿巷道壁放置通风机、电气开关柜等设施，形成局部障碍，加上铲运机自身的铰接转向行驶特性，使得地下铲运机定位参考点并不总是等距离地沿墙行走，这也加大了导航控制难度。

因此，在地下巷道环境下，有必要采用相对定位与绝对定位相结合的方法来确定铲运机的位姿。在相对定位中应用转角传感器、陀螺仪、里程计对铲运机的位姿进行推算，根据推算值与目标路径的偏差来进行导航控制。在绝对定位中利用激光雷达扫描安放在巷道壁已知位置的信标，来感知外部环境信息，进行准确位置识别和计算，以确定铲运机在巷道内的准确位姿，并给出相对定位所需要的初始位置偏差和初始航向角偏差，消除相对定位过程中航位推测产生的累积误差，恢复其定位精度。

为了使铲运机跟踪控制快速有效，采用三项偏差反馈来全面反映跟踪状况，兼顾了横向位置偏差，航向角偏差和航向角变化率偏差，结合PID校正控制，使得导航系统在各种偏差情况下，均有良好的跟踪控制效果。由于在横向位置偏差的基础上又融合了航向角偏差和航向角变化率偏差，该导航系统具有前瞻性，使得其跟踪轨迹控制精度高，响应快，稳定性好，抗干扰能力强，控制系统的鲁棒性好。

如图2所示，为地下铲运机定位导航系统示意图。

通过每隔一定距离在巷道壁已知位置安放信标的方法(绝对定位)，来提供准确的定位测量数据，用来确定铲运机的准确位姿，即提供初始横向位置偏差和初始航向角偏差，消除相对定位(轨迹推算)过程中的累积误差，来提高铲运机在井下巷道的总体定位精度，特别是在一些关键地点，如转弯处、避障处、起始点、终止点等，更是需要信标的准确定位数据。

由于信标不能安放过多，否则会增加成本和加大工程量，有必要采用相对定位(即航位推测法)方法，相对定位不需要外部传感器信息就能实现对铲运机位置和航向的估算，并提供很高的短期定位精度，可以实现连续自主式定位。采用相对定位算法是在两个信标之间的区域推算出铲运机的位置，计算出铲运机跟踪轨迹与目标路径之间的横向位置偏差和航向角偏差，在外部环境信息缺省的情况下，实现导航控制。由于相对定位推算过程是一个累加过程，下一个位置是从上一个位置推算出来的，因此，误差也是一个累积的过程，随着时间的延长而增加，因此不能单独长时间使用。要与绝对定位相结合，取长补短，利用相对定位算法的小范围定位精度高和绝对定位方法的大范围定位精度高的优点，通过信息融合获得较满意的定位结果。

目标路径规划是自主铲运机导航与控制的基础。本导航控制器具有由已知偏差初始值推算出后续的跟踪轨迹和轨迹偏差的功能，因此可以通过对该导航控制器的数学模型进行仿真，比较不同路径下的铲运机仿真跟踪曲线，来获得理想的目标路径。该导航控制器可采用Matlab，Simulink进行仿真。以井下典型巷道情况，直巷道与直角弯巷道组合情况为例说明目标路径的规划。

取巷道为直巷道+直角弯巷道+直巷道情况，巷道宽度3.6m，期望路径选取巷道的中线，在巷道起始位置，取初始转向角α₀＝0rad，初始横向位置偏差δ₀＝0.2m，初始航向角偏差β₀＝0.2rad，直线路径处K_m＝0(m^-1)，曲线路径处K_m＝0.2(m^-1)，取车速v_g＝1.11m/s，铲运机前方驱动桥的中点到铰接中心线的距离l₁＝1.595m，铲运机后方驱动桥的中点到铰接中心线的距离l₂＝1.270m，取横向位置偏差反馈系数K₁＝1，取航向角偏差反馈系数K₂＝3，取航向角变化率偏差反馈系数K₃＝3，取PID的比例系数K_p＝20，取PID的微分时间常数T_d＝0s，取PID的积分时间常数T_i＝1s。

如图3所示，为地下铲运机跟踪轨迹仿真图。该仿真结果表明，本导航控制器在已知初始横向位置偏差和初始航向角偏差的情况下，依靠精确推算和导航控制功能，可以消除初始存在的偏差，并正确跟踪目标路径。由于铲运机的铰接转向特性及车身尺寸结构特点，在转弯时铲运机铲斗外廓边缘顶点轨迹偏向巷道外侧，容易与巷道外壁造成刮蹭，所以在转弯处以巷道中线作为目标路径并不合适，需要根据铲运机机身运动特点来规划目标路径。

如图4所示，为地下铲运机转弯处的路径规划和跟踪轨迹仿真图，为了使铲运机各边缘顶点轨迹居于巷道中间位置，采用两段内侧圆弧路径来代替巷道中线圆弧路径。其中，转弯处的第一段圆弧目标路径取R＝20m，对应圆心角为10°，第二段相切圆弧目标路径取r＝5.5m，对应圆心角为80°，跟踪此目标路径时，铲运机各外廓边缘顶点轨迹综合起来距巷道两边的间距比较均匀，比起巷道中线作为目标路径时改善很多。

另外，目标路径也可以由铲运机上装备的陀螺仪和激光扫描器扫描巷道获得，即在人工驾驶时，由记录每个Δt间隔内目标路径航向角的变化值

车速v_m(k)，计算得到目标路径的一系列曲率K_m(k)，曲率计算公式为

计算出目标路径曲线长度S_m(k)，S_m(k)＝S_m(k-1)+v_m(k)Δt，K_m(k)与S_m(k)对应存储，得到由操作者经验规划出的目标路径。

如图5所示，为地下铲运机导航控制器结构框图。

在车辆跟踪已知的目标路径时，首先由绝对定位测量系统测得初始横向位置偏差和航向角偏差，再通过该导航控制器计算出航向角变化率偏差，将这三项偏差分别乘以反馈系数K₁、K₂、K₃，迭加之后作为综合反馈，输入给PID，由PID校正控制后，导航控制器计算出车辆的转向控制指令，输入给车辆的电液比例转向控制系统，使车辆转向角发生改变，再运用该导航控制器内部构建的车辆轨迹精确推算模型，精确计算出车辆的跟踪轨迹与目标路径之间存在的偏差值，再用这些偏差值构成综合反馈来计算新的转向控制指令，如此反复，使车辆不断跟踪目标路径，实现地下铲运机自主导航。该导航控制器算法如下。

1、给定K_p、K_i、K_d数值，给定l₁、l₂数值、给定T数值，给定K₁、K₂、K₃数值。其中，比例系数K_p的取值范围为，1≤K_p≤800，取比例系数K_p＝20。积分系数

采样周期T选取范围为，50≤T≤300毫秒，取采样周期T＝0.1s，积分时间常数T_i的取值范围为，0≤T_i≤50，取积分时间常数T_i＝1s，则积分系数K_i＝2。微分系数微分时间常数T_d的取值范围为，0≤T_d≤8，取微分时间常数T_d＝0，则微分系数K_d＝0。l₁为铲运机前方驱动桥的中点到铰接中心线的距离，l₁＝1.595m，l₂为铲运机后方驱动桥的中点到铰接中心线的距离，l₂＝1.270m。K₁、K₂和K₃为反馈系数，K₁、K₂和K₃的选取范围均为1～100，取横向位置偏差反馈系数K₁＝1，航向角偏差反馈系数K₂＝3，航向角变化率偏差反馈系数K₃＝3。针对不同车辆的上述参数最佳值可由实验来确定。

2、设初值e(k-1)＝e(1-2)＝0，u(k-1)＝0

当k＝1时，即起点位置信标1触发，发出铲运机自主导航开始信号。

3、输入转向角当前值，α(k)＝α(1)，输入转向角前一采样时刻的数值，α(k-1)＝α(1-1)

4、输入铲运机车速当前值，v_g(k)＝v_g(1)

5、输入航向角偏差初始值，β(1)＝β₀，β₀为此刻的航向角偏差初始值。

6、输入横向位置偏差初始值，δ(1)＝δ₀，δ₀为此刻的横向位置偏差初始值。

7、根据初始纵向位置s_m(1)，从路径存储器中调出该处对应的曲率K_m(1)。

8、计算目标路径航向角变化率

9、计算跟踪轨迹航向角变化率

10、计算航向角偏差变化率

11、计算综合反馈e(1)：

e (1) = K_{1} δ (1) + K_{2} β (1) + K_{3} \frac{Δβ (1)}{T}

12、综合反馈e(1)经PID控制器校正后，计算输出转向控制电压u(1)：

u(1)＝u(1-1)+Δu(1)

其中，u(1-1)为前一采样周期时刻控制电压，Δu(1)为转向控制电压的增量，且Δu(1)＝K_p[e(1)-e(1-1)]+K_ie(1)+K_d[e(1)-2e(1-1)+e(1-2)]，其中，e(1-1)为e(1)前一采样周期时刻的综合反馈值，e(1-2)为e(1-1)前一采样周期时刻的综合反馈值。

13、为下一时刻作准备

(1)、计算下一采样时刻航向角偏差β(1+1)，为下次推算做准备：

β (1 + 1) = β (1) + T \frac{Δβ (1)}{T}

(2)、计算下一采样时刻横向位置偏差δ(1+1)，为下次推算做准备：

δ(1+1)＝δ(1)+Tv_g(1)sinβ(1)

(3)、计算目标路径弧长s_m(1+1)，为下次推算做准备：

s_m(1+1)＝s_m(1)+v_m(1)T

(4)、根据上面目标路径弧长s_m(1+1)，输出对应该处的曲率K_m(1+1)，为下次推算做准备。

(5)转移存储α(1)→α(1-1)，e(1-1)→e(1-2)，e(1)→e(1-1)，u(1)→u(1-1)。

当2≤k＜n时，其中，采样次数n是由下一个信标决定的，初定一个触发距离，如每相隔5米或10米安放一个定位信标，该距离大小可根据实际跟踪效果来调整，当铲运机在两个信标位置之间行驶时，从上面第3步开始进行下面新的推算和导航控制：

3、输入转向角当前值，α(k)，输入转向角前一采样时刻的数值，α(k-1)

4、输入铲运机车速当前值v_g(k)，

5、输入航向角偏差推算值β(k)，即此刻的航向角偏差上次推算值。

6、输入横向位置偏差推算值δ(k)，即此刻的横向位置上次推算值。

7、输入目标路径上该处对应的曲率K_m(k)，即为由目标路径弧长上次推算对应值。

8、计算目标路径航向角变化率

9、计算跟踪轨迹航向角变化率

10、计算航向角偏差变化率

11、计算综合反馈e(k)：

e (k) = K_{1} δ (k) + K_{2} β (k) + K_{3} \frac{Δβ (k)}{T}

12、综合反馈e(k)经PID控制器校正后，计算输出转向控制电压u(k)的方法为：

u(k)＝u(k-1)+Δu(k)

其中，u(k-1)为前一采样周期时刻控制电压，Δu(k)为转向控制电压的增量，且Δu(2)＝K_p[e(2)-e(2-1)]+K_ie(2)+K_d[e(2)-2e(2-1)+e(2-2)]，其中，e(k-1)为e(k)前一采样周期时刻的综合反馈值，e(k-2)为e(k-1)前一采样周期时刻的综合反馈值，K_p为比例系数，K_i为积分系数，K_d为微分系数，

T_i为积分时间常数，T_d为微分时间常数，T为采样周期。

13、为下一时刻作准备

(1)、计算下一采样时刻航向角偏差β(k+1)，为下次推算做准备：

β (k + 1) = β (k) + T \frac{Δβ (k)}{T}

(2)、计算下一采样时刻横向位置偏差δ(k+1)，为下次推算做准备：δ(k+1)＝δ(k)+Tv_g(k)sinβ(k)

(3)、计算下一采样时刻目标路径弧长s_m(k+1)，为下次推算做准备：s_m(k+1)＝s_m(k)+v_m(k)T

(4)、根据上面目标路径弧长s_m(k+1)，输出对应该处的曲率K_m(k+1)，为下次推算做准备。

(5)转移存储α(k)→α(k-1)，e(k-1)→e(k-2)，e(k)→e(k-1)，u(k)→u(k-1)。

当k＝n时，即铲运机行驶到下一个信标触发位置，重新输入由信标确定的准确定位参数，作为初始的横向位置偏差和初始的航向角偏差，令k＝1，重新输入测量值，消除推算累计误差，重新回到前面k＝1步骤，进行新的轨迹推算和导航控制计算。

如果井下巷道某些局部地段巷道壁表面平整光滑(如良好混凝土支护地段)，可以用激光扫描器直接实时测量出铲运机定位参考点到巷道壁的距离，然后计算出每一采样时刻的航向角偏差和横向位置偏差，用于导航控制，相当于上述导航控制算法中k永远等于1，并去掉其中的下一时刻偏差推算步骤，这时该导航控制算法就成为这种巷道壁条件良好时的地下铲运机导航控制算法。

对于井下铲运机的导航来说，由于是在预先设定的已知的巷道环境中行驶，不是在未知的环境中行驶，所以沿墙壁行走相互作用导航控制方式的优势发挥不出来。由于井下巷道断面不规则，巷道壁凹凸不平，对测量数据的处理较困难，而且井下巷道内经常会沿巷道壁放置通风机、电气开关柜等设施，使得地下铲运机并不是等距离地沿墙行走，导致测量系统和数据传输系统负担更重，所以在井下巷道环境中使用沿墙壁行走导航控制方式难度较大，对计算机系统硬件和软件都有很高要求，目前条件下实现起来很困难。

地下铲运机通常都是在事先指定的巷道内反复行驶，所以目标路径一旦规划好了，就可以使用很长时间。目标路径规划时已经综合考虑到了巷道内各种障碍物、巷道曲率、铲运机轨迹特性等因素，按照目标路径来导引地下铲运机会更加安全、更加高效。

本发明基于轨迹精确推算导航控制方法，当计算步长足够小，并忽略轮胎打滑时，地下铲运机跟踪目标路径的稳态误差趋于零。

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

(1)用该控制器导航时，对地下铲运机跟踪轨迹能够精确推算，因而对外部环境测量数据量的需求大为减少(可减少到几十分之一甚至几百分之一)，极大地减轻测量数据和处理数据的任务量，有助于克服井下巷道环境测量数据和处理测量数据的困难，有助于导引地下铲运机避障并可靠跟踪目标路径。

(2)用该导航方法采用三偏差(横向位置偏差、航向角偏差、航向角变化率偏差)综合反馈，导航系统具有前瞻性，其控制精度高，稳定性好，抗干扰能力强，控制系统的鲁棒性好。该导航控制器还可以作为地下铲运机的路径规划器，规划其目标路径。

该导航控制器采用三项偏差反馈来全面反映跟踪状况，兼顾了横向位置偏差，航向角偏差和航向角变化率偏差，结合PID校正控制，使得导航系统在各种偏差情况下，均有良好的跟踪控制效果。由于在横向位置偏差的基础上又融合了航向角偏差和航向角变化率偏差，该导航系统具有前瞻性，使得其跟踪轨迹控制精度高，稳定性好，抗干扰能力强，控制系统的鲁棒性好，可以应用于各种按预设轨迹运行的车辆的控制。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。