CN103197675A - 地下铲运机自主行驶和避障运动控制及目标路径规划方法 - Google Patents

Abstract

本发明公开了一种地下铲运机自主行驶和避障运动控制及目标路径规划方法，根据井下巷道情况，先规划出主目标路径，然后规划出依附于主目标路径的局部避障目标路径，在铲运机控制器中存储主目标路径数据，及局部避障目标路径的起始点、终止点和相对于主目标路径的偏移距离数据，由测量系统的测量数据或由轨迹推算模型，计算出的铲运机横向位置偏差、航向角偏差及航向角偏差变化率，三偏差通过融合规则形成综合偏差反馈信号作为PID校正控制的输入量，该控制指令输入给铲运机的电液比例转向控制系统，驱动车辆转向并跟踪目标路径，实现地下铲运机自主导航和避障运动控制，方便、灵活、可靠。

Description

地下铲运机自主行驶和避障运动控制及目标路径规划方法

技术领域

本发明涉及一种自主行驶和避障控制技术，尤其涉及一种地下铲运机自主行驶和避障运动控制及目标路径规划方法。

背景技术

随着现有矿产资源逐步全面转入地下开采，并向深部和难采矿体发展，开采规模逐渐增大，采矿条件愈来愈恶劣，对人的安全威胁也愈来愈大，遥控采矿技术应运而生。地下铲运机是地下无轨采矿的关键设备，实现地下铲运机在井下巷道内无人操纵、自主行驶，可使操作人员远离井下恶劣、危险的工作环境，保护铲运机司机人员安全、提高采矿作业效率并降低采矿成本。地下铲运机自主行驶首先要解决自身在巷道内的自主行驶及避障运动控制问题及目标路径规划问题。

地下铲运机自主行驶研究在国内刚刚起步，国外在此方面已有较多的研究。如澳大利亚彼得里德利对铲运机运动轨迹和轨迹控制进行了研究，构建了自主铲运机自主行驶控制模型，该自主行驶控制模型在小曲率路径跟踪时误差较小，但在目标路径曲率较大时，跟踪误差较大，不能满足控制精度要求。美国丰约翰对轮式机器人路径跟踪问题进行了研究，但将目标路径局限于直线段和圆弧段，让轮式机器人跟踪每一个直线段或圆弧段，给出跟踪直线段或圆弧段的计算办法，该方法不能对其他形状曲线路径进行跟踪计算，同时该研究也未涉及铰接车辆情况，故无法应用于地下铲运机。北京矿冶研究总院发明了一种基于车辆轨迹精确推算模型的地下铲运机自主行驶控制方法，可较精确计算出铲运机的跟踪轨迹与目标路径之间存在的偏差值，构成综合偏差来控制铲运机转向并跟踪目标路径，该方法没有考虑避障运动控制问题，另外，当跟踪偏差较大时，控制系统的稳定性和控制精度会受到较大影响。所以，需要进一步研究解决地下铲运机在较大偏差情况下，自主行驶控制器的稳定性问题，并提高自主行驶控制精度。

避障运动控制是地下智能铲运机一项必不可少的功能。现有避障技术是用几段圆弧线组成的局部避障目标路径，并与其他目标路径相连接，形成一条连续的目标路径，这样做在理论上是可行的，但在实际应用时会很复杂，比如事先要存储大量的目标路径曲率，不同弧线段可能对应几种不同的曲率需要存储，同时还需要存储不同弧线的切换点位置数据，这些情况带来自主行驶避障控制系统数据管理难度大，而且避障目标路径的规划也很复杂，需要大量计算工作，实际避障控制时需要准确找到不同弧线的切换点，否则会严重影响避障控制效果，所以该方法应用时也很不方便；

另外，在铲运机铲装矿石出矿过程中，除了在巷道中心线位置铲装料堆中部的矿石外，铲运机还需要偏左或偏右不断变换位置去铲装料堆左右两边的矿石，通常每铲运一次，换一个铲取位置，这样铲取效率才高。要规划出每一条偏离巷道主目标路径的路径数据量较大，也比较麻烦；

为了降低井下巷道开拓成本，井下巷道的形状和尺寸一般允许有一定程度的不规则情况存在，这给井下巷道目标路径规划带来困难，并使自主行驶控制器存储目标路径数据量增多，目前也没有解决这一问题的较好方法。

发明内容

本发明的目的是提供一种方便、灵活、可靠的地下铲运机自主行驶和避障运动控制及目标路径规划方法。

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

本发明的地下铲运机自主行驶和避障运动控制及目标路径规划方法，用来控制地下铲运机跟踪规划出的主目标路径和局部避障目标路径，其特征在于，该方法包括以下主要步骤：

A、根据地下巷道情况进行地下铲运机主目标路径和局部避障目标路径规划，在地下铲运机主目标路径和局部避障目标路径规划完成后，沿主目标路径曲线弧长s_m(k)存储主目标路径曲率K_m(k)系列值，沿主目标路径曲线弧长s_m(k)存储局部避障目标路径的起始点s_mbb(i)和终止点s_mbe(i)及局部避障目标路径相对于主目标路径的横向偏离位移δ_b0(i)，主目标路径和局部避障目标路径保存在自主行驶控制器的存储介质中；

上式中：k＝1，2，3，…；i为局部避障目标路径数量，i＝1，2，3，…；

B、用采样周期T作为测量和控制间隔，当车辆沿主目标路径行驶时，用车辆的定位参考点相对于主目标路径的位置P(s_m(k)，δ(k))和航向角偏差β(k)，及车辆转向角α(k)，来表示车辆的位姿；当车辆沿局部避障目标路径行驶时，仍然使用车辆的定位参考点相对于主目标路径的位置P(s_m(k)，δ(k))和航向角偏差β(k)，及车辆转向角α(k)，来表示车辆的位姿；

C、判断铲运机是否到达避障位置，当铲运机没有到达局部避障目标路径位置时，输入自主行驶控制器的横向位置偏差为δ_b(k)，δ_b(k)=δ(k)；当铲运机到达局部避障目标路径位置时，自主行驶控制器输入的横向位置偏差为δ_b(k)，δ_b(k)=δ(k)+δ_b0(i)，δ_b0(i)为局部避障目标路径相对于主目标路径的横向偏离位移；

D、计算铲运机跟踪轨迹航向角变化率计算目标路径投影车速v_m(k)，计算目标路径航向角变化率

计算航向角偏差变化率

计算下一采样时刻航向角偏差β(k+1)，计算下一采样时刻横向位置偏差δ(k+1)及δ_b(k+1)，计算下一采样时刻目标路径曲线弧长s_m(k+1)，由目标路径曲率存储控制子系统得到下一采样时刻目标路径曲率K_m(k+1)及K_mb(k+1)的数值；

E、判断横向位置偏差δ_b(k)的数值，当-ε_δ1≤δ_b(k)≤ε_δ1时，δ_b(k)=δ_b(k)。当δ_b(k)＞ε_δ1时，δ_b(k)=ε_δ1；当δ_b(k)＜-ε_δ1时，δ_b(k)=-ε_δ1，其中ε_δ1的取值范围为，0＜ε_δ1≤1(m);

F、计算铲运机横向位置偏差δ_b(k)、航向角偏差β(k)及航向角偏差变化率

三种偏差融合而成综合偏差反馈信号e(k)，作为PID校正控制器的输入；

G、判断横向位置偏差|δ_b(k)|及航向角偏差|β(k)|的数值，当|δ_b(k)|≤ε_δ2且|β(k)|≤ε_β同时满足时，PID自主行驶控制器才进行综合偏差反馈信号e(k)的PID控制，此时PID的积分环节起作用，输出此时的转向控制电压u(k);当|δ_b(k)|≤ε_δ2和|β(k)|≤ε_β不能同时满足时，PID自主行驶控制器仅进行综合偏差反馈信号的PD控制，此时PID的积分环节不起作用，输出此时的转向控制电压u(k)；

H、转向控制电压u(k)作为铲运机电液比例转向控制系统的控制电压，电液比例转向控制系统驱动转向油缸，实现铲运机自主调整转向角度，使铲运机行驶轨迹自动跟踪目标路径；

I、当采样次数k＝1时，航向角偏差β(k)横向位置偏差δ(k)和目标路径曲线弧长s_m(k)都是实测值或由实测值计算得到，航向角偏差变化率是计算值；当k≥2时，航向角偏差β(k)，横向位置偏差δ(k)，目标路径曲线弧长s_m(k)既可以是实测值也可是推算值，航向角偏差变化率

是计算值。

由上述本发明提供的技术方案可以看出，本发明实施例提供的地下铲运机自主行驶和避障运动控制及目标路径规划方法，该方法将相对定位与绝对定位相结合，来确定铲运机的位姿的功能，具有灵活性和可靠性。

附图说明

图1为本发明实施例中地下铲运机主目标路径、局部避障目标路径、跟踪轨迹及参数说明示意图；

图2为本发明实施例中地下铲运机自主行驶和避障运动控制仿真系统框图；

图3为本发明实施例中地下铲运机主目标路径和局部避障目标路径规划实例图；

图4为地下铲运机跟踪目标路径时跟踪轨迹和车身外廓特征点运动轨迹曲线图；

图5为本发明实施例中当车速变化时，三偏差融合反馈控制与单一横向位置偏差反馈控制效果比较图；

图6为本发明实施例中横向位置偏差有限幅作用时和横向位置偏差无限幅作用时的跟踪效果比较图；

图7为本发明实施例中双门限积分控制与常规积分控制跟踪曲线比较图；

图8为本发明实施例中铲运机等距平行移动出矿工况时目标路径规划及跟踪轨迹图；

图9为本发明实施例中地下铲运机自主行驶和避障控制算法框图；

图10为本发明实施例中地下铲运机在跟踪直线主目标路径及局部大尺寸避障时轨迹曲线图；

图11为本发明实施例中地下铲运机在跟踪圆形主目标路径及局部避障时轨迹曲线图；

图12为本发明实施例中地下铲运机在跟踪正弦曲线主目标路径及局部大尺寸避障时轨迹曲线图。

具体实施方式

下面将对本发明实施例作进一步地详细描述。

本发明的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其较佳的具体实施方式是：

用来控制地下铲运机跟踪规划出的主目标路径和局部避障目标路径，该方法包括以下主要步骤：

A、根据地下巷道情况进行地下铲运机主目标路径和局部避障目标路径规划，在地下铲运机主目标路径和局部避障目标路径规划完成后，沿主目标路径曲线弧长s_m(k)存储主目标路径曲率K_m(k)系列值，沿主目标路径曲线弧长s_m(k)存储局部避障目标路径的起始点s_mbb(i)和终止点s_mbe(i)及局部避障目标路径相对于主目标路径的横向偏离位移δ_b0(i)，主目标路径和局部避障目标路径保存在自主行驶控制器的存储介质中；

上式中：k=12，3，…；i为局部避障目标路径数量，i=1，2，3，…；

B、用采样周期T作为测量和控制间隔，当车辆沿主目标路径行驶时，用车辆的定位参考点相对于主目标路径的位置P(s_m(k)，δ(k))和航向角偏差β(k)，及车辆转向角α(k)，来表示车辆的位姿；当车辆沿局部避障目标路径行驶时，仍然使用车辆的定位参考点相对于主目标路径的位置P(s_m(k)，δ(k))和航向角偏差β(k)，及车辆转向角α(k)，来表示车辆的位姿；

C、判断铲运机是否到达避障位置，当铲运机没有到达局部避障目标路径位置时，输入自主行驶控制器的横向位置偏差为δ_b(k)，δ_b(k)=δ(k);当铲运机到达局部避障目标路径位置时，自主行驶控制器输入的横向位置偏差为δ_b(k)，δ_b(k)=δ(k)+δ_b0(i)，δ_b0(i)为局部避障目标路径相对于主目标路径的横向偏离位移；

D、计算铲运机跟踪轨迹航向角变化率计算目标路径投影车速v_m(k),计算目标路径航向角变化率

计算航向角偏差变化率计算下一采样时刻航向角偏差β(k+1)，计算下一采样时刻横向位置偏差δ(k+1)及δ_b(k+1)，计算下一采样时刻目标路径曲线弧长s_m(k+1)，由目标路径曲率存储控制子系统得到下一采样时刻目标路径曲率K_m(k+1)及K_mb(k+1)的数值；

E、判断横向位置偏差δ_b(k)的数值，当-ε_δ1≤δ_b(k)≤ε_δ1时，δ_b(k)=δ_b(k)。当δ_b(k)＞ε_δ1时，δ_b(k)=ε_δ1；当δ_b(k)＜-ε_δ1时，δ_b(k)=-ε_δ1，其中ε_δ1的取值范围为，0＜ε_δ1≤1(m);

F、计算铲运机横向位置偏差δ_b(k)、航向角偏差β(k)及航向角偏差变化率

三种偏差融合而成综合偏差反馈信号e(k)作为PID校正控制器的输入；

G、判断横向位置偏差|δ_b(k)|及航向角偏差|β(k)|的数值，当|δ_b(k)|≤ε_δ2且|β(k)|≤ε_β同时满足时，PID自主行驶控制器才进行综合偏差反馈信号e(k)的PID控制，此时PID的积分环节起作用，输出此时的转向控制电压u(k);当|δ_b(k)|≤ε_δ2和|β(k)|≤ε_β不能同时满足时，PID自主行驶控制器仅进行综合偏差反馈信号的PD控制，此时PID的积分环节不起作用，输出此时的转向控制电压u(k);

H、转向控制电压u(k)作为铲运机电液比例转向控制系统的控制电压，电液比例转向控制系统驱动转向油缸，实现铲运机自主调整转向角度，使铲运机行驶轨迹自动跟踪目标路径；

I、当采样次数k=1时，航向角偏差β(k)横向位置偏差δ(k)和目标路径曲线弧长s_m(k)都是实测值或由实测值计算得到，航向角偏差变化率

是计算值；当k≥2时，航向角偏差β(k)，横向位置偏差δ(k)，目标路径曲线弧长s_m(k)既可以是实测值也可是推算值，航向角偏差变化率

是计算值。

所述步骤A中，在地下铲运机主目标路径和局部避障目标路径规划过程中，先不考虑避障情况，进行铲运机主目标路径的规划，在主目标路径规划完毕后，再进行与主目标路径等距偏移的局部避障目标路径的规划。

局部避障目标路径的曲率K_mb(k)与主目标路径曲率K_m(k)的关系为：

$K_{\mathrm{mb}}\left(k\right)=\frac{K_m\left(k\right)}{1-K_m\left(k\right){\mathrm{\δ}}_{b0}\left(i\right)}$

跟踪轨迹航向角变化率

由安装在铲运机车体上的陀螺仪航向角

测量数据计算得到，即：

或者由推算计算得到，推算计算跟踪轨迹航向角变化率

的公式为：

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

跟踪主目标路径时，推算计算目标路径投影车速v_m(k)的公式为：

$v_m\left(k\right)=\frac{v_g\left(k\right)\cos \mathrm{\β}\left(k\right)}{1+K_m\left(k\right)\mathrm{\δ}\left(k\right)}$

跟踪局部避障目标路径时，推算计算局部避障目标路径投影车速v_mb(k)的公式为：

v_mb(k)=[1-K_m(k)δ_b0(i)]v_m(k)

跟踪主目标路径及跟踪局部避障目标路径时，推算计算目标路径航向角变化率

的公式相同，均为：

跟踪主目标路径及跟踪局部避障目标路径时，计算航向角偏差变化率

的公式为：

下一采样时刻航向角偏差β(k+1)通过测量数据计算得出，或通过计算下一采样时刻航向角偏差β(k+1)的公式得出：

$\mathrm{\β}(k+1)=\mathrm{\β}\left(k\right)+T\frac{\mathrm{\Δ\β}\left(k\right)}{T}$

下一采样时刻横向位置偏差δ(k+1)及δ_b(k+1)通过测量数据计算得出，或通过计算下一采样时刻横向位置偏差δ(k+1)的公式推算得出：

δ(k+1)=δ(k)+Tv_g(k)sinβ(k)

δ_b(k+1)=δ_b(k)+Tv_g(k)sinβ(k)

计算目标路径弧长s_m(k+1)的公式为：

s_m(k+1)=s_m(k)+v_m(k)T。

主目标路径曲率K_m(k)及局部避障目标路径曲率K_mb(k)与主目标路径弧长s_m(k)一一对应。

计算综合反馈e(k)的公式为：

$e\left(k\right)=K_1{\mathrm{\δ}}_b\left(k\right)+K_2\mathrm{\β}\left(k\right)+K_3\frac{\mathrm{\Δ\β}\left(k\right)}{T}$

其中，δ_b(k)=δ(k)+δ_b0(i)，K₁、K₂和K₃为预先存储的反馈系数，K₁、K₂和K₃的选取范围均在0-1之间，且有K₁+K₂+K₃＝1。

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

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

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

K_d为微分系数，T_i为积分时间常数，T_d为微分时间常数，T为采样周期，ε为积分项的开关系数：

其中，ε_δ2的取值范围为，0＜ε_δ2≤0.3(m)，ε_β的取值范围为，0＜ε_β≤0.3(rad)。

所述采样周期T选取范围为，20≤T≤400ms。

所存储的比例系数K_p的取值范围为，1≤K_p≤200，所述积分系数K_i的取值范围为，0≤K_i≤25,所述微分系数K_d的取值范围为，0≤K_d≤5。

采用本发明方法，将相对定位与绝对定位相结合，来确定铲运机的位姿的功能，具有灵活性和可靠性。结合铲运机电液比例转向控制系统部分的数学模型，可实现整个铲运机自主行驶和避障运动控制系统的数学模型表达，通过仿真计算获得不同目标路径曲线下的跟踪和避障曲线及车身外廓特征点曲线，择优作为主目标路径及局部避障目标路径。

采用本发明的方法，当自主铲运机遇到障碍需要避让时，根据障碍物的尺寸大小，给自主行驶控制器在原有横向位置偏差之外，再额外施加一个一定数值的横向位置避障偏差信号，构成相对于局部避障目标路径的综合偏差反馈信号，经PID校正后输入给铲运机的电液比例转向控制系统，驱动铲运机转向，使铲运机跟踪这一临时的局部偏移目标路径，将铲运机从沿原有主目标路径行驶改变到沿该局部避障目标路径行驶。当避障结束后，去掉额外施加的横向位置避障偏差信号，这时输入给自主行驶控制器的为相对于主目标路径的综合偏差反馈信号，这样自主行驶控制器就自动切换回来，输出针对跟踪主目标路径的控制电压，让自主铲运机回到跟踪主目标路径，由此方便地实现了避障运动控制。

井下巷道宏观上多数由直线段和圆弧线段连接组成，对于直线段，其上每一点的曲率都等于0，对于圆弧线段，其上每一点的曲率均等于一个确定的数值。对于局部有不规则形状的巷道，可能对应由复杂曲线形成的目标路径，此时主目标路径上每一点的曲率都可能不同，使主目标路径的规划变得困难。这时可以将巷道局部不规则形状看成是局部障碍，这样巷道主目标路径仍然采用简单的直线段和圆弧线段，局部变形部分的主目标路径可通过该避障方法来进行修正，由此克服了在这种巷道不规则情况下主目标路径的规划难题。

由于局部避障目标路径是相对于主目标路径来规划的，这样在主目标路径规划时就不用考虑局部避障情况了，当主目标路径规划完成后，再来考虑局部避障问题，即将主目标路径和局部避障目标路径分别来规划，大大简化了主目标路径和局部避障目标路径规划问题，同时方便控制器存储目标路径数据，并方便铲运机运动控制。

该方法还可应用在铲运机铲运矿石出矿作业过程中，方便地实现铲运机铲装作业时，每铲装运输一次，下一次需要换一个铲取位置的运动控制要求，即给自主行驶控制器额外施加不同的给定横向偏差数值，使铲运机行驶轨迹相对于巷道主目标路径有不同程度的轨迹偏移，实现地下铲运机铲矿位置的不断变换。

具体实施例：

在本实施例中，以KCY-2YK地下铲运机为控制对象，进行地下铲运机自主行驶和避障运动控制，及主目标路径和局部避障目标路径规划。具体实施步骤如下：

本发明首先定义地下铲运机运动控制所需要的有关参数，并推导出反映参数关系的方程组。巷道内的地下铲运机主目标路径、局部避障目标路径、跟踪轨迹及相关参数说明示意图见图1。以巷道路面为坐标平面建立X-Y坐标系，选取铲运机前方驱动桥的中点为铲运机的定位参考点，以该点在X-Y坐标平面上的投影点P来代表铲运机的位置。以P点的速度方向（轨迹切线方向）作为车辆的运动方向，其与X轴正方向所成的夹角定义为铲运机的航向角

取从X轴逆时针旋转形成的航向角为正，反之为负。P点的运动轨迹即为铲运机的跟踪轨迹，见图中曲线C。在巷道内预先规划好的P点运动目标路径作为铲运机的主目标路径，见图中曲线A。为了避障，在巷道内局部区域规划出的与主目标路径横向等距偏移的P点运动目标路径为局部避障目标路径，见图中曲线B，其中，横向偏离位移用δ_b0表示，取局部避障目标路径在主目标路径曲线左侧的横向偏离位移为正，反之为负。P点在目标路径上的垂直投影点P_m的速度方向与X轴正方向所成的夹角，定义为目标路径的航向角

由于局部避障目标路径上每一点是等距且平行于主目标路径上的对应点，所以P点在主目标路径上的垂直投影点P_m的速度方向与P点在局部避障目标路径上的垂直投影点P_mb的速度方向是相同的，即主目标路径和局部避障目标路径的航向角均为

主目标路径或局部避障目标路径的航向角

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

该参数反映了自主铲运机运动方向与目标路径预定的行驶方向的偏差。铲运机跟踪轨迹上P点与主目标路径上P_m点的距离定义为铲运机的横向位置偏差δ。取P点在目标路径曲线右侧的横向位置偏差为正，反之为负。该参数可反映出铲运机相对于主目标路径的横向定位情况。铲运机跟踪轨迹上P点与局部避障目标路径上P_mb点的距离定义为铲运机的横向位置偏差δ_b。取P点在局部避障目标路径曲线右侧的横向位置偏差为正，反之为负。该参数可反映出铲运机相对于局部避障目标路径的横向定位情况，并且有δ_b＝δ+δ_b0。为了计算方便，地下铲运机定位P点位置坐标由对应的主目标路径P_m点的曲线弧长S_m和铲运机横向位置偏差δ来表示，即P(S_m，δ)。K_g为跟踪轨迹在P点的曲率，当曲率中心O_g在车辆轨迹曲线前进方向的左侧时定义为正曲率，曲率半径R_g为正，反之为负。K_m为主目标路径在P_m点的曲率，R_m为其曲率半径，曲率中心O_m。K_mb为局部避障目标路径在P_mb点的曲率，局部避障目标路径P_mb点曲率与主目标路径P_m点曲率的关系为

铲运机P点行驶车速为v_g，车速v_g与轨迹曲线C相切，在实际自主行驶控制过程中，车速v_g由安装在车辆底盘传动系统的车速传感器或里程计测量得到。主目标路径上P_m点投影车速为v_m，投影车速v_m与主目标路径曲线A相切，局部避障目标路径上P_mb点投影车速为v_mb，投影车速v_mb与局部避障目标路径曲线B相切。行驶车速与主目标路径投影车速关系为

局部避障目标路径上P_mb点投影车速v_mb与主目标路径上P_m点投影车速v_m的关系为v_mb=(1-K_mδ_b0)v_m。取铲运机运动方向为主目标路径曲线弧长S_m的正方向，S_m=∫v_mdt。铲运机沿局部目标路径运动的曲线弧长为S_mb,S_mb=∫v_mbdt。α为铲运机转向角，即铲运机前后车体的相对转动角，由安装在铲运机前后车体铰接处的转角传感器测量得到。使地下铲运机反时针转弯的转向角α为正，反之为负。转向角和转向角变化速率决定了车辆的转弯半径大小和铲运机行驶轨迹，自主铲运机正是通过对转向角的不断调整来实现跟踪目标路径。忽略铲运机行驶中的侧滑和离心力，铲运机跟踪轨迹航向角

与转向角α、车速v_g的关系为

其中，l₁为铲运机前方驱动桥的中点到铰接中心线的距离，l₂为铲运机后方驱动桥的中点到铰接中心线的距离，KCY-2YK地下铲运机的l₁＝l₂＝1.270m。在铲运机车体上安装有陀螺仪，铲运机跟踪轨迹航向角

也可以由陀螺仪航向角输出值得到，特别是当采用高精度陀螺仪时，所测得的航向角

精度比推算值高。主目标路径或局部避障目标路径的航向角与投影车速v_m及主目标路径曲率K_m的关系均为

相对于主目标路径的横向位置偏差δ与车速v_g和航向角偏差β的关系为

相对于局部目标路径的横向位置偏差δ_b与车速v_g和航向角偏差β的关系为 $\frac{{\mathrm{d\δ}}_b}{\mathrm{dt}}=v_g\sin \mathrm{\β}.$ 由

可得

经过公式推导，反映地下铲运机轨迹参数关系的方程组为：

以KCY-2YK地下铲运机为控制对象，建立动力转向数学模型，得到如下方程组：

$\left\{\begin{array}{c}V=\frac{\frac{K_{q10}}{A_{\mathrm{me}}}\frac{A_v{K}_d{K}_i}{A_u{K}_v}u-\frac{K_t}{A_1{A}_{\mathrm{me}}}(\frac{V_e}{4{\mathrm{\β}}_e{K}_t}s+1)F_f}{(\frac{s^2}{{\mathrm{\ω}}_h^2}+\frac{{2\mathrm{\ζ}}_h}{{\mathrm{\ω}}_h}s+1)}\\ \frac{\mathrm{d\α}}{\mathrm{dt}}=\frac{\frac{L_0+\∫\mathrm{Vdt}}{L_{\mathrm{OA}}{L}_{\mathrm{OB}}}V}{\sqrt{1-{\left[\frac{L_{\mathrm{OA}}^2+L_{\mathrm{OB}}^2-{(L_0+\∫\mathrm{Vdt})}^2}{2L_{\mathrm{OA}}{L}_{\mathrm{OB}}}\right]}^2}}\end{array}\right.$

式中，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_d——比例电磁铁的力电流增益，N/A，K_d=40N/A

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

u——转向控制电压，V，0～10V

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

V_e——转向油缸的等效容积，m³，V_e=0.006m³

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

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

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

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

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

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

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

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

以上面两个方程组为依据，结合三偏差综合反馈及PID校正控制，可形成地下铲运机自主行驶和避障运动控制仿真系统，见图2。由该系统在已知初始偏差的情况下，可推算后续跟踪轨迹偏差，并分析各种给定系数及各种工况下的铲运机轨迹跟踪情况，方便考察铲运机车身外廓特征点的运动轨迹与巷道边壁及巷道内障碍物的位置关系。

目标路径规划是自主铲运机自主行驶与控制的基础。首先进行主目标路径的规划设计，此时不必考虑局部避障情况，待主目标路径规划设计完成后，再进行局部避障目标路径的规划设计。在铲运机跟踪行驶过程中，能够使车身左侧、右侧外廓特征点与巷道边壁及巷道内障碍物的距离保持比较均匀的目标路径就是比较好的目标路径。

井下巷道宏观上多数由直线段和圆弧线段组成。在直线段巷道情况，对应的主目标路径可为直线段，直线段上每一点的曲率K_m均为零；在圆弧线段巷道情况，对应的主目标路径可为圆弧线段，圆弧线段上每一点的曲率K_m均相等，用一个数值代表即可。这样，在这两种目标路径情况下，主目标路径的表达非常简单，方便数据存储和数据管理。但是，井下巷道微观上并不是理想的直线段和圆弧线段，对于局部有不规则形状的巷道，可能对应复杂曲线形成的目标路径，使主目标路径的规划变得困难。这时可以将巷道局部不规则形状看做是局部障碍，此时巷道主目标路径仍然采用简单的直线段和圆弧线段，局部变形部分的主目标路径可通过上述避障方法来进行修正，由此克服了在这种巷道不规则情况下主目标路径的规划难题。

地下铲运机主目标路径和局部避障目标路径规划实例图见图3。根据巷道总体情况以巷道中线规划出主目标路径，包括直线段EF（曲率K_m=0m^-1）、圆弧线段FG（曲率K_m=0.05m^-1）及直线段GH（曲率K_m=0m^-1），E点处S_m=0m,F点处S_m=35m,G点处S_m=66.4m,H点处S_m=814m。假定巷道在该圆弧段处不够规则，铲运机按此圆弧目标路径运行时车身外廓与外侧巷道边壁距离太小，有碰撞危险，故在此处设置与主目标路径等距偏移的局部修正曲线作为局部目标路径，其偏移量为δ_b0(2),图中δ_b0(2)=1m，起点为S_mbb(2)＝40m,终止点为S_mbe(2)＝50m,自主铲运机实际运行到达此处会临时跟踪这一局部修正的目标路径，由此加大车身与巷道边壁距离，避免碰撞。为了避免铲运机与巷道内局部障碍物相碰，设置与主目标路径等距偏移的局部避障目标路径曲线，其偏移量为δ_b0(1)=-1.2m，起点为S_mbb(1)=10m终止点为S_mbe(1)＝20m,自主铲运机实际运行到此处时，会临时跟踪这一局部避障目标路径，绕开障碍物，避免碰撞，铲运机驶过局部避障目标路径后再回到跟踪主目标路径行驶。通过调整局部目标路径的起始点和终止点位置及局部避障目标路径相对于主目标路径的横向偏离位移δ_b0的大小，可调整铲运机的避障跟踪轨迹。另外，由于铲运机的铰接转向特性及车身尺寸结构特点，在转弯时铲运机铲斗外廓边缘顶点轨迹偏向巷道外侧，容易与巷道外壁造成刮蹭，所以在转弯处以巷道中线作为目标路径并不合适，需要根据铲运机铰接机身运动特点来规划目标路径，这些因素在路径规划设计中可一并考虑。通过路径规划仿真试验，可得到较佳的规划路径设计。取初始转向角α₀=0rad，初始横向位置偏差δ₀＝0m,初始航向角偏差β₀=0rad，直线路径处K_m=0(m^-1)，曲线路径处K_m=0.05(m^-1),取车速v_g=2.22m/s,铲运机前方驱动桥的中点到铰接中心线的距离l₁=1.270m，铲运机后方驱动桥的中点到铰接中心线的距离l₂=1.270m,取横向位置偏差反馈系数K₁=0.2，取航向角偏差反馈系数K₂=0.55，取航向角变化率偏差反馈系数K₃=0.25，取PID的比例系数K_p=30，取PID的微分系数K_d=0，取PID的积分系数K_i=0.6，由该仿真系统，得到地下铲运机跟踪目标路径时跟踪轨迹和车身外廓特征点运动轨迹曲线见图4。由图中可见，铲运机在跟踪行驶过程中，车身左侧、右侧外廓特征点（即车身前端左右侧外廓端点、中央铰接处左右侧外廓端点及车身后部左右侧外廓端点）与巷道边壁及巷道内障碍物的距离比较均匀，目标路径规划设计满足要求。

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

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

信标安放过多会增加成本和加大工程量，采用相对定位（即轨迹推算）也是必要的，特别是在巷道中存在避车硐室或巷道十字路口等特殊路段，此时激光扫描数据不完整或缺失，此处根据扫描数据计算出的铲运机跟踪轨迹误差较大，这种情况下，轨迹推算可发挥重要作用。相对定位不需要外部传感器信息就能实现对铲运机位置和航向的估算，并提供较高的短期定位精度，可以实现连续自主式定位。相对定位与绝对定位相结合，取长补短，利用相对定位算法的小范围定位精度高和绝对定位方法的大范围定位精度高的优点，通过信息融合获得较满意的定位结果。

目标路径与地下铲运机跟踪轨迹之间的偏差可定义为横向位置偏差δ、航向角偏差β及航向角偏差变化率

通过反映地下铲运机轨迹参数关系的方程组可知，横向位置偏差δ、航向角偏差β及航向角偏差变化率

都是对铲运机跟踪情况的反映，在一定车速下航向角偏差β是对横向位置偏差δ的预测，而航向角偏差变化率

是对航向角偏差β的预测，只有以横向位置偏差δ为基础，结合航向角偏差β及航向角偏差变化率

综合考虑这些因素设计出的多偏差融合自主行驶控制器，才能实现较理想的铲运机跟踪自主行驶控制。将自主行驶控制器设计成三偏差融合反馈结合PID校正的自主行驶控制器，其中三偏差综合反馈e的计算公式为：

$e=K_1{\mathrm{\δ}}_b+K_2\mathrm{\β}+K_3\frac{\mathrm{d\β}}{\mathrm{dt}}$

式中，δ_b为包括避障工况的横向位置偏差，δ_b=δ+δ_b0，β为航向角偏差，

为航向角偏差变化率，K₁、K₂和K₃分别为横向位置偏差、航向角偏差和航向角偏差变化率反馈系数，K₁、K₂和K₃的选取范围均为0-1,且K₁+K₂+K₃＝1。较佳的K₁、K₂和K₃系数值分别为，0.2≤K₁≤0.3，0.3≤K₂≤0.6，0.2≤K₃≤0.4。

三偏差信息融合反馈形成三种控制作用的相互制约，避免了单一反馈控制的局限性，使得自主行驶控制具有预测性和前瞻性，通过改变K₁、K₂和K₃的相对大小，可以改变三项偏差在反馈中的权重，方便地调节自主行驶控制系统总的响应特性，使自主行驶控制系统既有较快的跟踪速度，又避免过大的超调和振荡，实现快速与稳定的均衡，并且在车速大范围变化时，自主行驶控制系统仍具有良好的适应性和稳定性。

当车速变化时，三偏差融合反馈控制与单一横向位置偏差反馈控制效果比较见图5，此时，横向位置偏差反馈系数K₁=0.2，航向角偏差反馈系数K₂=0.55，航向角变化率偏差反馈系数K₃=0.25，横向偏差初始值δ₀=0.6m航向角偏差初始值β₀=0°，转向角初始值α₀=0°，车速分别为v_g=2km/h，v_g=6km/h，v_g=12km/h。由图5可以看出单一横向位置偏差反馈时对车速变化比较敏感，当车速较小时超调量加大稳定性变差，当车速加大时响应变慢调整时间延长；三偏差融合反馈控制能较好地适应车速的大范围变化，能大大减少控制系统的超调量并缩短调节时间，自主行驶控制的快速性、稳定性和对参数变化的适应性均得到显著提高。

在较小的横向位置偏差δ_b(如δ_b＜1m，时，即横向位置偏差δ_b在小范围变化时，均衡关系比较容易实现。但是，当有数值较大的横向位置偏差δ_b(如δ_b＞1m)存在时，由上式计算出的三偏差综合反馈e的数值较大，这时横向位置偏差δ_b的作用效果过强，而航向角偏差β和航向角偏差变化率的作用相对较弱，导致自主行驶控制系统超调加大、振荡加剧，破坏了三偏差之间的均衡作用，甚至使自主行驶控制系统失去稳定；另外，如果以此时较大的横向位置偏差δ_b来调整自主行驶控制系统参数K₁、K₂和K₃，使其有较好控制效果，那么当存在较小的横向位置偏差δ_b时，自主行驶控制系统的响应就会较慢，控制效果也会变差。即当横向位置偏差δ_b在较大范围(δ_b≤5m，K_mδ＜1)变化时，自主行驶控制系统要么失去快速性，要么失去稳定性，两者很难兼顾。

因此，为了使自主行驶控制效果在横向位置偏差δ_b较大范围变化时，仍有较好的控制效果，对横向位置偏差δ_b进行限幅控制，限幅值ε_δ1的取值范围是0＜ε_δ1≤1(m)。当-ε_δ1≤δ_b(k)≤ε_δ1时，δ_b(k)=δ_b(k)。当δ_b(k)＞ε_δ1时，δ_b(k)=ε_δ1。当δ_b(k)＜-ε_δ1时，δ_b(k)=-ε_δ1。通过调整限幅值ε_δ1的大小来慢速释放较大横向位置偏差δ_b对自主行驶控制器的过大作用量，保持自主行驶控制系统三偏差之间的均衡作用，实现自主行驶控制系统的快速性与稳定性的统一，该方法简单可靠、调整方便，横向位置偏差δ_b较佳的限幅范围是0.4≤ε_δ1≤0.8(m)。

横向位置偏差有限幅作用时和横向位置偏差无限幅作用时的跟踪效果比较见图6。此时，横向位置偏差反馈系数K₁=0.2，航向角偏差反馈系数K₂=0.55，航向角变化率偏差反馈系数K₃=0.25,横向位置偏差δ_b限幅值ε_δ1=0.6m，横向偏差初始值δ₀＝1m，δ₀＝3m，δ₀＝5m，航向角偏差初始值β₀=0°，转向角初始值α₀=0°车速v_g=8km/h。当横向位置偏差无限幅作用时，当δ_b＞1m以上时（如δ₀=3m,δ₀=5m），响应超调加剧，甚至失去稳定性；当横向位置偏差有限幅作用时，自主行驶控制系统稳定性保持良好，无超调和失稳，跟踪效果明显优于横向位置偏差无限幅作用情况。

地下铲运机转向油缸是通过电液比例换向阀来实现驱动控制的，电液比例换向阀的输入电压是PWM控制器的脉宽调制输出，PWM控制器的输入是0～10V左右的控制电压信号u，为了在三偏差综合反馈e与控制电压信号u之间形成某种对应关系，并消除稳态误差，需要对三偏差综合反馈e进行PID校正控制，经过PID校正后的输出信号u作为车辆转向控制指令，输入给铲运机的电液比例转向控制系统，驱动车辆转向并跟踪目标路径，实现地下铲运机自主行驶和避障运动控制。

PID（比例积分微分）校正控制是工程上最常用的控制方案，简单实用，易于调节。比例控制能迅速反应误差，从而减小稳态误差。但是，比例控制不能消除稳态误差，比例放大系数的加大，会引起系统的不稳定。微分控制可以减小超调量，克服振荡，使系统的稳定性提高，同时加快系统的动态响应速度，减小调整时间，从而改善系统的动态性能。积分控制的作用是，只要系统有误差存在，积分控制器就不断地积累，输出控制量，以消除误差。因而，只要有足够的时间，积分控制将能完全消除误差，使系统误差为零，从而消除稳态误差。积分作用太强会使系统超调加大，甚至使系统出现振荡，即PID积分系数（K_i）不能取太大，否则会出现积分饱和现象，即积分器原先存储值过大，后来的反向控制信号已经不能及时消除存储值了，结果反向运动控制没有及时执行，导致产生过大的超调，甚至失去稳定；而积分作用太弱，即PID积分系数（K_i）取值过小，又会使系统消除稳态误差的作用时间延长，降低系统控制精度。

解决办法是，分别设置横向位置偏差δ_b门限值ε_δ2和航向角偏差β门限值ε_β。设置横向位置偏差δ_b门限值ε_δ2，可检测铲运机跟踪轨迹是否已经接近目标路径。当铲运机跟踪轨迹的横向位置偏差δ_b的绝对值大于门限值ε_δ2时，说明此时铲运机跟踪轨迹的横向位置偏差还较大，这时还不必考虑消除稳态跟踪误差，故不必应用积分控制，控制系统仅需要根据三偏差综合反馈e的情况结合PD校正进行控制，从而避免了积分饱和现象，避免了产生过大的超调，同时保持较快的响应速度。当铲运机跟踪轨迹的横向位置偏差δ_b的绝对值小于门限值ε_δ2时，此时铲运机跟踪轨迹的横向位置偏差δ_b较小，铲运机轨迹已接近目标路径，这时由于横向位置偏差δ_b的数值较小，产生的控制作用比较弱，加上系统死区的存在，会使控制系统消除横向位置偏差δ_b的能力减弱，调整时间延长，并产生较大稳态误差。为了避免这种情况，需要加入积分控制作用，通过积累较小控制作用形成较大控制作用，加大了系统消除较小误差的能力，减小调整时间，消除存在的稳态误差。考虑到在铲运机横向位置偏差δ_b较小的同时，可能存在航向角偏差β数值较大的情况，如果这时加入积分作用，会使系统超调加大，振荡加剧。所以，在设置横向位置偏差δ_b门限值ε_δ2的同时，设置航向角偏差β门限值ε_β。仅当双门限要求同时满足时，即|δ_b(k)|≤ε_δ2且|β(k)|≤ε_β,才进行综合偏差反馈信号e的PID控制，否则仅进行PD控制，以保证系统的控制效果，这样可避免产生过大的超调，又使系统有较快的响应和较小的稳态误差。其中，ε_δ2的取值范围为，0＜ε_δ2≤0.3(m)，较佳的门限值ε_δ2的取值范围是0.1＜ε_δ2≤0.2(m);ε_β的取值范围为，0＜ε_β≤0.3(rad)，较佳的门限值ε_β的取值范围是，0.03＜ε_β≤0.2(rad)。

通过横向位置偏差及航向角偏差双门限控制，综合偏差反馈信号e获得较理想PID校正效果。该自主行驶控制和避障运动控制器在存在大轨迹偏差时仍然是稳定的，并且跟踪响应速度快，超调量小，稳定性好。双门限积分控制与常规积分控制跟踪曲线比较见图7，此时横向位置偏差反馈系数K₁＝0.2，航向角偏差反馈系数K₂＝0.55，航向角变化率偏差反馈系数K₃=0.25，横向位置偏差δ_b的限幅值ε_δ1=0.6mPID的比例系数K_p=30,PID的微分系数K_d=0，PID的双门限时积分系数K_i=0.6，常规积分时积分系数K_i=0.1，横向位置偏差δ_b门限值ε_δ2=0.150m，航向角偏差β门限值ε_β＝0.06rad，横向偏差初始值为δ₀=0m,航向角偏差初始值分别为β₀=-20°，β₀=-40°，β₀＝-60°，转向角初始值α₀=0°，车速v_g=8km/h，从图7可以看出双门限积分控制时的跟踪效果明显优于常规积分控制时的跟踪效果。

本发明提供一种地下铲运机自主行驶和避障运动控制方法，该方法也构成目标路径规划方法。在车辆跟踪已知的目标路径时，首先由绝对定位测量系统测得初始横向位置偏差和航向角偏差，再通过该自主行驶控制器计算出航向角偏差变化率，将这三项偏差分别乘以反馈系数K₁、K₂、K₃，迭加之后作为综合反馈，输入给PID，由PID校正控制后，自主行驶控制器计算出车辆的转向控制指令，输入给车辆的电液比例转向控制系统，使车辆转向角发生改变。通过测量数据计算出新的横向位置偏差和航向角偏差，或运用该自主行驶控制器内部构建的车辆跟踪轨迹偏差推算模型，计算出车辆的跟踪轨迹与目标路径之间的偏差值，再用这些偏差值计算出新航向角变化率偏差，三项偏差构成新的综合反馈来计算新的转向控制指令，如此反复，使车辆不断跟踪主目标路径或避障目标路径，实现地下铲运机自主行驶和避障运动控制。当自主铲运机遇到障碍需要避让时，根据障碍物的尺寸数据，给自主行驶控制器在原有偏差之外，再额外给自主行驶控制器施加一个一定数值的横向位置避障偏差信号δ_b0，实际意义就是给自主行驶控制器输入一个等距于原有主目标路径的临时的偏移目标路径作为局部避障目标路径，让自主行驶控制器响应这一输入，这时输入给自主行驶控制器的是相对于局部避障目标路径偏差(δ_b=δ+δ_b0、β、

）构成的综合偏差反馈信号e，输出针对跟踪局部避障目标路径的控制电压，使铲运机跟踪这一临时的、局部的偏移目标路径，将铲运机从沿原有主目标路径行驶改变到沿该局部避障目标路径行驶。当避障结束后，去掉额外施加的横向位置避障偏差信号δ_b0，这时输入给自主行驶控制器的仅有相对于主目标路径偏差（δ、β、

）构成的综合偏差反馈信号e，自主行驶控制器恢复到原有状态，这样自主行驶控制器就自动切换回来，输出仅针对跟踪主目标路径的控制电压u，让自主铲运机回到跟踪主目标路径，由此方便地实现了避障运动控制或铲运机平行运动控制。

井下巷道宏观上多数由直线段和圆弧线段连接组成，对于直线段，其上每一点的曲率都等于0，对于圆弧线段，其上每一点的曲率均等于一个确定的数值。对于局部有不规则形状的巷道，可能对应由复杂曲线形成的目标路径，此时主目标路径上每一点的曲率都可能不同，使主目标路径的规划变得困难。这时可以将巷道局部不规则形状看成是局部障碍，这样巷道主目标路径仍然采用简单的直线段和圆弧线段，局部变形部分的主目标路径可通过该避障方法来进行修正，由此克服了在这种巷道不规则情况下主目标路径的规划难题。

由于局部避障目标路径是相对于主目标路径来规划的，这样在主目标路径规划时就不用考虑局部避障情况了，当主目标路径规划完成后，再来考虑局部避障问题，即将主目标路径和局部避障目标路径分别来规划，大大简化了主目标路径和局部避障目标路径规划问题，同时方便控制器存储目标路径数据，并方便铲运机运动控制。

该避障控制策略还可应用在铲运机铲装矿石出矿作业过程中，方便地实现铲运机铲装作业时，每铲装运输一次，下一次需要换一个铲取位置的运动控制要求，即给自主行驶控制器额外施加不同的给定横向偏差数值，使铲运机行驶轨迹相对于巷道主目标路径有不同程度的轨迹偏移，实现地下铲运机铲矿位置的不断变换。该方法可大大减少铲运机行驶和铲装作业过程中目标路径的规划难度和控制难度。地下铲运机等距平行移动出矿工况时目标路径规划及跟踪轨迹见图8。

将横向位置偏差δ_b经过限幅ε_δ1后再与航向角偏差β、航向角变化率偏差

三种偏差进行融合，形成综合偏差反馈e信号作为PID控制的输入，在PID校正控制中，分别设置横向位置偏差δ_b门限值ε_δ2及航向角偏差β门限值ε_β，仅当双门限ε_δ2和ε_β要求同时满足时才进行综合偏差反馈信号积分，PID校正后的输出信号作为车辆转向控制指令u，输入给铲运机的电液比例转向控制系统，驱动车辆转向并跟踪目标路径，实现地下铲运机自主行驶和避障运动控制。

地下铲运机自主行驶和避障控制算法框图见图9，下面对本发明方法主要步骤作进一步说明：

（1）在地下铲运机主目标路径和局部避障目标路径规划过程中，先不考虑避障情况，进行铲运机主目标路径的规划，在主目标路径规划完毕后，再进行局部避障目标路径的规划。

采用本地下铲运机自主行驶和避障运动控制方法，结合铲运机电液比例转向控制系统部分的数学模型，可实现整个铲运机自主行驶和避障运动控制系统的数学模型表达，通过仿真计算获得不同目标路径曲线下的跟踪和避障曲线及车身外廓特征点运动曲线，择优作为主目标路径及局部避障目标路径。

在地下铲运机主目标路径和局部避障目标路径规划完成后，沿主目标路径曲线弧长s_m(k)(k＝1，2，3，…）存储主目标路径曲率K_m(k)系列值，沿主目标路径曲线弧长s_m(k)存储局部避障目标路径的起始点s_mbb(i)和终止点s_mbe(i)(i为局部避障目标路径数量，i＝1，2，3，…），存储局部避障目标路径相对于主目标路径的横向偏离位移δ_b0(i),主目标路径曲率K_m(k)及局部避障目标路径曲率K_mb(k)与主目标路径弧长s_m(k)一一对应，主目标路径和局部避障目标路径保存在自主行驶控制器的存储介质中。

（2）用采样周期T作为测量和控制间隔，当车辆沿主目标路径行驶时，用车辆的定位参考点相对于主目标路径的位置P(s_m(k)，δ(k))和航向角偏差β(k)，及车辆转向角α(k)，来表示车辆的位姿。当车辆沿局部避障目标路径行驶时，仍用车辆的定位参考点相对于主目标路径的位置P(s_m(k)，δ(k))和航向角偏差β(k)，及车辆转向角α(k)，来表示车辆的位姿；

（3）给定K_p、K_i、K_d数值，给定T数值，给定l₁、l₂数值，给定K₁、K₂、K₃数值，给定ε_δ1、ε_δ2、ε_β数值。

其中，比例系数K_p的取值范围为，1≤K_p≤200，取比例系数K_p=300积分系数

积分系数K_i的取值范围为，0≤K_i≤25，取K_i=2。微分系数微分系数K_d的取值范围为，0≤K_d≤5，取微分系数K_d=0。采样周期T选取范围为，20≤T≤400ms，取采样周期T=0.1s，l₁为铲运机前方驱动桥的中点到铰接中心线的距离，l₁=1.270m，l₂为铲运机后方驱动桥的中点到铰接中心线的距离，l₂＝1.270m。K₁、K₂和K₃为反馈系数，K_l、K₂和K₃的选取范围均为0-1,且K₁+K₂+K₃=1，取K₁=0.2，取K₂=0.55，取K₃=0.25。横向位置偏差δ_b的限幅值ε_δ1的取值范围是0＜ε_δ1≤1(m)，取ε_δ1=0.6m。横向位置偏差δ_b门限值ε_δ2的取值范围为，0＜ε_δ2≤0.3(m)，取ε_δ2=0.15m。航向角偏差β门限值ε_β的取值范围为，0＜ε_β≤0.3(rad)，取ε_β=0.06。针对不同车辆上述可选参数的最佳值可由仿真计算得到，并由实验来最终确定。

（4）输入数据，δ(k)、δ_b0(i)、β(k)、s_m(k)、s_mbb(i)、s_mbe(i)、α(k)、α(k-1)、v_g(k)、K_m(k)。

（5）判断铲运机是否到达避障位置，当铲运机没有到达局部避障目标路径位置时，输入自主行驶控制器的横向位置偏差为δ_b(k)，δ_b(k)=δ(k);当铲运机到达局部避障目标路径位置时，自主行驶控制器输入的横向位置偏差为δ_b(k)，δ_b(k)=δ(k)+δ_b0(i)，δ_b0(i)为局部避障目标路径相对于主目标路径的横向偏离位移；

（6）计算铲运机跟踪轨迹航向角变化率

计算目标路径投影车速v_m(k)计算目标路径航向角变化率

计算航向角偏差变化率计算下一采样时刻航向角偏差β(k+1)，计算下一采样时刻横向位置偏差δ(k+1)及δ_b(k+1)，计算下一采样时刻目标路径曲线弧长s_m(k+1)，由目标路径曲率存储控制子系统得到下一采样时刻目标路径曲率K_m(k+1)及K_mb(k+1)的数值。其中，

1）局部避障目标路径的曲率K_mb(k)与主目标路径曲率K_m(k)的关系为：

$K_{\mathrm{mb}}\left(k\right)=\frac{K_m\left(k\right)}{1-K_m\left(k\right){\mathrm{\δ}}_{b0}\left(i\right)}$

2）跟踪轨迹航向角变化率

可由安装在铲运机车体上的陀螺仪航向角

测量数据计算得到，即

或者由推算计算得到，推算计算跟踪轨迹航向角变化率

的公式为：

3）跟踪主目标路径时，推算计算主目标路径投影车速v_m(k)的公式为：

$v_m\left(k\right)=\frac{v_g\left(k\right)\cos \mathrm{\β}\left(k\right)}{1+K_m\left(k\right)\mathrm{\δ}\left(k\right)}$

跟踪局部避障目标路径时，推算计算局部避障目标路径投影车速v_mb(k)的公式为：

v_mb(k)=[1-K_m(k)δ_b0(i)]v_m(k)

4）跟踪主目标路径及跟踪局部避障目标路径时，推算计算目标路径航向角变化率

的公式相同，均为：

5）跟踪主目标路径及跟踪局部避障目标路径时，计算航向角偏差变化率

的公式为：

6）下一采样时刻航向角偏差β(k+1)可通过测量数据计算得出，也可通过计算下一采样时刻航向角偏差β(k+1)的公式计算得出：

$\mathrm{\β}(k+1)=\mathrm{\β}\left(k\right)+T\frac{\mathrm{\Δ\β}\left(k\right)}{T}$

7）下一采样时刻横向位置偏差δ(k+1)及δ_b(k+1)可通过测量数据计算得出，也可通过计算下一采样时刻横向位置偏差δ(k+1)的公式推算得出：

δ(k+1)=δ(k)+Tv_g(k)sinβ(k)

δ_b(k+1)=δ_b(k)+Tv_g(k)sinβ(k)

8）计算目标路径弧长s_m(k+1)的公式为：

s_m(k+1)=s_m(k)+v_m(k)T

（7）判断横向位置偏差δ_b(k)的数值，当-ε_δ1≤δ_b(k)≤ε_δ1时，δ_b(k)=δ_b(k)。当δ_b(k)＞ε_δ1时，δ_b(k)=ε_δ1。当δ_b(k)＜-ε_δ1时，δ_b(k)=-ε_δ1。

（8）计算铲运机横向位置偏差δ_b(k)、航向角偏差β(k)及航向角偏差变化率

三种偏差融合而成综合偏差反馈信号e(k),作为PID自主行驶控制器的输入，计算综合反馈e(k)的公式为：

$e\left(k\right)=K_1{\mathrm{\δ}}_b\left(k\right)+K_2\mathrm{\β}\left(k\right)+K_3\frac{\mathrm{\Δ\β}\left(k\right)}{T}$

其中，δ_b(k)=δ(k)+δ_b0(i)。

（9）判断横向位置偏差|δ_b(k)|及航向角偏差|β(k)|的数值，当|δ_b(k)|≤ε_δ2且|β(k)|≤ε_β同时满足时，PID自主行驶控制器才进行综合偏差反馈信号e(k)的PID控制（此时PID的积分环节起作用），输出此时的转向控制电压u(k)。当|δ_b(k)|≤ε_δ2和|β(k)|≤ε_β不能同时满足时，PID自主行驶控制器仅进行综合偏差反馈信号的PD控制（此时PID的积分环节不起作用），输出此时的转向控制电压u(k);

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

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

其中，u(k-1)为前一时刻控制电压，Δu(k)为转向控制电压的增量，且Δu(k)=K_p[e(k)-e(k-1)]+εK_ie(k)+K_d[e(k)-2e(k-1)+e(k-2)]，其中，e(k-1)为e(k)前一时刻的综合反馈值，e(k-2)为e(k-1)前一时刻的综合反馈值，ε为积分项的开关系数：

（10）转向控制电压u(k)作为地下铲运机电液比例转向控制系统的控制电压，电液比例转向控制系统驱动转向油缸，实现铲运机自主调整转向角度，使铲运机自动跟踪目标路径；

（11）当采样次数k＝1时，航向角偏差β(k)横向位置偏差δ(k)和目标路径曲线弧长s_m(k)都是实测值（或由实测值计算得到），航向角偏差变化率

是计算值。当k≥2时，航向角偏差β(k)，横向位置偏差δ(k)，目标路径曲线弧长s_m(k)既可以是实测值也可是推算值，航向角偏差变化率

是计算值。

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

下面是应用该自主行驶避障控制和目标路径规划方法，进行地下铲运机自主行驶及避障运动控制仿真计算例。

1）在跟踪直线主目标路径时，考察自主行驶和局部避障运动控制效果

取铲运机前方驱动桥的中点到铰接中心线的距离l₁＝1.270m，取铲运机后方驱动桥的中点到铰接中心线的距离1₂=1.270m,取横向位置偏差反馈系数K₁=0.2，取航向角偏差反馈系数K₂＝0.55，取航向角偏差变化率反馈系数K₃=0.25，取PID的比例系数K_p=30，取PID的微分系数K_d=0，取PID的积分系数K_i=0.6，取横向位置偏差δ_b门限值ε_δ2=0.150m，取航向角偏差β门限值ε_β=0.06rad，取车速v_g=4km/h，取铲运机初始具有横向偏差δ₀=1m，取航向角初始偏差分别为β₀=-30°β₀＝-15°，β₀=0°，β₀＝+15°，β₀＝+30°取初始转向角α₀=0°，目标路径为直线K_m=0m^-1，铲运机从原点（0,0）开始向右运行，路径中包含一处避障路段A，A处避障距离δ_b0=-3m，避障信号作用距离20m，起点为S_mbb(1)＝13m,终止点为S_mbe(1)=33m，地下铲运机在跟踪直线主目标路径及局部大尺寸避障时轨迹曲线见图10，由图中轨迹曲线可见铲运机跟踪与避障控制情况良好，自主行驶控制保持优良的稳定性。

2）在跟踪圆弧曲线主目标路径时，考察自主行驶和局部避障运动控制效果

取车速v_g=4km/h，并取横向偏差初始值δ₀=0m，航向角偏差初始值β₀=0°，转向角初始值α₀=0°，铲运机前方驱动桥的中点到铰接中心线的距离l₁＝1.270m，铲运机后方驱动桥的中点到铰接中心线的距离l₂=1.270m，取横向位置偏差反馈系数K₁=0.2，取航向角偏差反馈系数K₂=0.55，取航向角偏差变化率反馈系数K₃=0.25,取PID的比例系数K_p=30,取PID的微分系数K_d=0，取PID的积分系数K_i=0.6，取横向位置偏差δ_b门限值ε_δ2=0.150m，取航向角偏差β门限值ε_β=0.06rad，取主目标路径的半径R_m=10m,主目标路径曲率K_m=0.1m^-1，目标路径包含两处避障路段A和B，A处避障距离δ_b0=0.6m避障信号作用距离5m（短距离），起点S_mbb(1)＝8m,终止点S_mbe(1)=13m。B处避障距离δ_b0=-0.7m，避障信号作用距离15m（长距离），起点S_mbb(2)＝40m,终止点S_mbe(2)=55m。铲运机从原点（0,0）开始反时针跟踪圆形主目标路径及局部避障目标路径时，地下铲运机在跟踪圆形主目标路径及局部避障时轨迹曲线见图11。从仿真结果可看出，该自主行驶控制器能够同时完成圆弧目标路径的跟踪和沿途局部避障控制功能。

3）在跟踪正弦曲线主目标路径时，考察自主行驶和局部避障运动控制效果

正弦曲线是比较复杂的目标路径，在正弦曲线的每一处曲率都在变化，对于考察自主行驶避障控制器的控制性能，正弦曲线是具有代表性和挑战性的目标路径曲线。在正弦主目标路径下，让铲运机以较高车速运行，并结合局部大尺寸障碍避障情况，考察本自主行驶避障控制器的控制效果。

设主目标路径正弦曲线为y=Asinωx，正弦曲线的曲率K_m与x轴坐标关系方程组为：

$\left\{\begin{array}{c}{K}_m=\frac{{-\mathrm{A\ω}}^2\sin \mathrm{\ωx}}{\sqrt{{(1+A^2{\mathrm{\ω}}^2{\cos }^2\mathrm{\ωx})}^3}}\\ x=\∫v_g\cos \arctan A\mathrm{\ω}\cos \left(\mathrm{\ωx}\right)\mathrm{dt}\end{array}\right.$

取车速v_g=8km/h,并取横向偏差初始值δ₀=0m，航向角偏差初始值β₀=0°，转向角初始值α₀＝0°，铲运机前方驱动桥的中点到铰接中心线的距离l₁=1.270m，铲运机后方驱动桥的中点到铰接中心线的距离l₂＝1.270m,取横向位置偏差反馈系数K₁=0.2，取航向角偏差反馈系数K₂=0.55，取航向角偏差变化率反馈系数K₃=0.25，取PID的比例系数K_p=30，取PID的微分系数K_d=0，取PID的积分系数K_i=0.6，取横向位置偏差δ_b门限值ε_δ2=0.150m，取航向角偏差β门限值ε_β＝0.06rad，目标路径正弦曲线取ω=0.1s^-1，取A=10m，取局部避障偏移距离δ_b0=-5m，避障信号作用行程32m，起点为S_mbb(1)＝4m,终止点为S_mbe(1)=36m，铲运机跟踪正弦曲线路径（包含一处大尺寸避障路段），铲运机从原点（0,0）开始跟踪主目标路径和局部避障目标路径，地下铲运机在跟踪正弦曲线主目标路径及局部大尺寸避障时轨迹曲线图见图12。在较高车速8km/h和大尺寸（5m）避障情况下，该自主行驶控制器仍具有良好的跟踪控制效果和控制稳定性。

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

（1）在主目标路径规划时不必考虑避障问题，在主目标路径规划完成后，再考虑避障路径规划问题，即主目标路径和局部避障目标路径可分别进行规划，该方法极大地方便了主目标路径和局部避障目标路径的规划及优化。本发明避障控制策略还可应用于在铲运机铲装矿石出矿作业过程中，方便地实现铲运机往复铲运作业时，每铲运一次换一个铲取位置的运动控制要求。本发明避障控制策略也可应用于其他场合，如钻机通过车身移动在采场内打成排的装药炮孔等。

（2）针对每一处避障或等距平行运动控制，自主行驶控制器系统仅需要存储横向位置避障偏离位移，及依附于主目标路径上的避障（或等距平行运动控制）开始位置和避障（或等距平行运动控制）结束位置数据，共三个数据，大大降低了自主行驶避障控制系统数据存储量和管理难度，调整和组合这三个数值，就可组成无穷多种避障路径，去满足不同情况的控制要求，既方便了避障路径规划，也方便了铲运机避障时的运动控制，该方法还具有较高的灵活性和可靠性。

（3）三偏差通过融合规则，形成综合偏差反馈信号作为PID校正控制的输入信号，横向位置偏差通过限幅来慢速释放大偏差对自主行驶控制器的过大作用量，横向位置偏差及航向角偏差通过双门限控制，综合偏差反馈信号获得较理想PID校正控制效果。该自主行驶控制和避障运动控制器在轨迹偏差及行驶车速大范围变化时仍然稳定，其工况适应性强，即自主行驶控制系统的鲁棒性好，并且该控制系统跟踪响应速度快，超调量小，稳态精度高。

（4）该自主行驶与避障运动控制方法具有包括避障工况的跟踪轨迹偏差预测功能，可用于主目标路径及局部避障目标路径的规划设计，本避障方法克服了巷道不规则情况下主目标路径的规划难题；依据该方法，在铲运机行驶过程中短时间缺少轨迹偏差实测值时，可推算出相对于主目标路径或局部避障目标路径的跟踪轨迹偏差，使自主行驶不至中断，该方法具有良好的适应性和可靠性。

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

Claims (9)

1.一种地下铲运机自主行驶和避障运动控制及目标路径规划方法，用来控制地下铲运机跟踪规划出的主目标路径和局部避障目标路径，其特征在于，该方法包括以下主要步骤：

A、根据地下巷道情况进行地下铲运机主目标路径和局部避障目标路径规划，在地下铲运机主目标路径和局部避障目标路径规划完成后，沿主目标路径曲线弧长s_m(k)存储主目标路径曲率K_m(k)系列值，沿主目标路径曲线弧长s_m(k)存储局部避障目标路径的起始点s_mbb(i)和终止点s_mbe(i)及局部避障目标路径相对于主目标路径的横向偏离位移δ_b0(i)，主目标路径和局部避障目标路径保存在自主行驶控制器的存储介质中；

上式中：k=1，2，3，…；i为局部避障目标路径数量，i=1，2，3，…；

B、用采样周期T作为测量和控制间隔，当车辆沿主目标路径行驶时，用车辆的定位参考点相对于主目标路径的位置P(s_m(k),δ(k))和航向角偏差β(k)，及车辆转向角α(k)，来表示车辆的位姿；当车辆沿局部避障目标路径行驶时，仍然使用车辆的定位参考点相对于主目标路径的位置P(s_m(k),δ(k))和航向角偏差β(k)，及车辆转向角α(k)，来表示车辆的位姿；

C、判断铲运机是否到达避障位置，当铲运机没有到达局部避障目标路径位置时，输入自主行驶控制器的横向位置偏差为δ_b(k)，δ_b(k)=δ(k)；当铲运机到达局部避障目标路径位置时，自主行驶控制器输入的横向位置偏差为δ_b(k)，δ_b(k)=δ(k)+δ_b0(i)，δ_b0(i)为局部避障目标路径相对于主目标路径的横向偏离位移；

D、计算铲运机跟踪轨迹航向角变化率计算目标路径投影车速v_m(k)，计算目标路径航向角变化率

计算航向角偏差变化率

计算下一采样时刻航向角偏差β(k+1)，计算下一采样时刻横向位置偏差δ(k+1)及δ_b(k+1)，计算下一采样时刻目标路径曲线弧长s_m(k+1)，由目标路径曲率存储控制子系统得到下一采样时刻目标路径曲率K_m(k+1)及K_mb(k+1)的数值；

E判断横向位置偏差δ_b(k)的数值，当-ε_δ1≤δ_b(k)≤ε_δ1时，δ_b(k)=δ_b(k)。当δ_b(k)＞ε_δ1时，δ_b(k)=ε_δ1；当δ_b(k)＜-ε_δ1时，δ_b(k)=-ε_δ1，其中ε_δ1的取值范围为，0＜ε_δ1≤1（m）；

F、计算铲运机横向位置偏差δ_b(k)、航向角偏差β(k)及航向角偏差变化率

三种偏差融合而成综合偏差反馈信号e(k)，作为PID校正控制器的输入；

G、判断横向位置偏差|δ_b(k)|及航向角偏差|β(k)|的数值，当|δ_b(k)|≤ε_δ2且|β(k)|≤ε_β同时满足时，PID自主行驶控制器才进行综合偏差反馈信号e(k)的PID控制，此时PID的积分环节起作用，输出此时的转向控制电压u(k);当|δ_b(k)|≤ε_δ2和|β(k)|≤ε_β不能同时满足时，PID自主行驶控制器仅进行综合偏差反馈信号的PD控制，此时PID的积分环节不起作用，输出此时的转向控制电压u(k)；

H、转向控制电压u(k)作为铲运机电液比例转向控制系统的控制电压，电液比例转向控制系统驱动转向油缸，实现铲运机自主调整转向角度，使铲运机行驶轨迹自动跟踪目标路径；

I、当采样次数k=1时，航向角偏差β(k)，横向位置偏差δ(k)和目标路径曲线弧长s_m(k)都是实测值或由实测值计算得到，航向角偏差变化率

是计算值；当k≥2时，航向角偏差β(k)，横向位置偏差δ(k)，目标路径曲线弧长s_m(k)既可以是实测值也可是推算值，航向角偏差变化率

是计算值。

2.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于，所述步骤A中，在地下铲运机主目标路径和局部避障目标路径规划过程中，先不考虑避障情况，进行铲运机主目标路径的规划，在主目标路径规划完毕后，再进行与主目标路径等距偏移的局部避障目标路径的规划。

3.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于，局部避障目标路径的曲率K_mb(k)与主目标路径曲率K_m(k)的关系为：

$K_{\mathrm{mb}}\left(k\right)=\frac{K_m\left(k\right)}{1-K_m\left(k\right){\mathrm{\δ}}_{b0}\left(i\right)}$

4.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于：

跟踪轨迹航向角变化率

由安装在铲运机车体上的陀螺仪航向角

测量数据计算得到，即：

或者由推算计算得到，推算计算跟踪轨迹航向角变化率

的公式为：

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

跟踪主目标路径时，推算计算目标路径投影车速v_m(k)的公式为：

$v_m\left(k\right)=\frac{v_g\left(k\right)\cos \mathrm{\β}\left(k\right)}{1+K_m\left(k\right)\mathrm{\δ}\left(k\right)}$

跟踪局部避障目标路径时，推算计算局部避障目标路径投影车速v_mb(k)的公式为：

v_mb(k)=[1-K_m(k)δ_b0(i)]v_m(k)

跟踪主目标路径及跟踪局部避障目标路径时，推算计算目标路径航向角变化率的公式相同，均为：

跟踪主目标路径及跟踪局部避障目标路径时，计算航向角偏差变化率

的公式为：

下一采样时刻航向角偏差β(k+1)通过测量数据计算得出，或通过计算下一采样时刻航向角偏差β(k+1)的公式得出：

$\mathrm{\β}(k+1)=\mathrm{\β}\left(k\right)+T\frac{\mathrm{\Δ\β}\left(k\right)}{T}$

下一采样时刻横向位置偏差δ(k+1)及δ_b(k+1)通过测量数据计算得出，或通过计算下一采样时刻横向位置偏差δ(k+1)的公式推算得出：

δ(k+1)=δ(k)+Tv_g(k)sinβ(k)

δ_b(k+1)=δ_b(k)+Tv_g(k)sinβ(k)

计算目标路径弧长s_m(k+1)的公式为：

s_m(k+1)=s_m(k)+v_m(k)T。

5.如权利要求1所述的地下铲运机自主行驶控制方法，其特征在于，主目标路径曲率K_m(k)及局部避障目标路径曲率K_mb(k)与主目标路径弧长s_m(k)一一对应。

6.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于，计算综合反馈e(k)的公式为：

$e\left(k\right)=K_1{\mathrm{\δ}}_b\left(k\right)+K_2\mathrm{\β}\left(k\right)+K_3\frac{\mathrm{\Δ\β}\left(k\right)}{T}$

其中，δ_b(k)=δ(k)+δ_b0(i)，K₁、K₂和K₃为预先存储的反馈系数，K₁、K₂和K₃的选取范围均在0-1之间，且有K₂＋K₂＋K₃＝1。

7.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于，综合反馈e(k)经PID控制器校正后，计算输出转向控制电压u(k)的方法为：

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

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

K_d为微分系数，

T₁为积分时间常数，T_d为微分时间常数，T为采样周期，ε为积分项的开关系数：

其中，ε_δ2的取值范围为，0＜ε_δ2≤0.3（m），ε_β的取值范围为，0＜ε_β≤0.3（rad）。

8.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于，所述采样周期T选取范围为，20≤T≤400ms。

9.如权利要求1所述的地下铲运机自主行驶和避障运动控制及目标路径规划方法，其特征在于，所存储的比例系数K_p的取值范围为，1≤K_p≤200，所述积分系数K_i的取值范围为，0≤K_i≤25，所述微分系数K_d的取值范围为，0≤K_d≤5。

Images