CN104881025B - 一种地下矿用车辆的反应式导航控制方法 - Google Patents
一种地下矿用车辆的反应式导航控制方法 Download PDFInfo
- Publication number
- CN104881025B CN104881025B CN201510171279.4A CN201510171279A CN104881025B CN 104881025 B CN104881025 B CN 104881025B CN 201510171279 A CN201510171279 A CN 201510171279A CN 104881025 B CN104881025 B CN 104881025B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- speed
- articulator
- model
- car body
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种地下矿用车辆的反应式导航控制方法,包括:根据当前时刻的车辆铰接点、前桥中点、前桥车速、前桥横摆角速度、前桥转向半径,后桥中点、后桥车速、后桥横摆角速度、后桥转向半径,建立下一时刻前桥位姿状态向量估计值模型;建立航向偏离角模型;建立铰接车前桥中心位置偏差模型;建立转向角控制模型,其中转向角控制模型包括安全预测模型和模糊控制算法;建立铰接车速度控制模型,并根据铰接车速度控制模型控制地下矿用车辆。
Description
技术领域
本发明涉及导航技术领域,特别是指一种地下矿用车辆的反应式导航控制方法。
背景技术
随着技术的发展,现在地下采矿行业中采用的采矿装备都已经实现了无轨化和液压化,并正在朝着大型化、智能化方向发展。现有的地下矿用车辆大多为无轨铰接式结构,因此铰接式车辆的自主导航控制技术是实现地下金属矿设备智能化与自动化的关键。
现有的地下矿用车辆的自主导航技术可以分为以下两种;反应式导航和绝对式导航;
其中绝对式导航需要在导航***中预先定义一组路径(即路径规划)并且得知每一时刻车体的绝对坐标(即车体定位)。这种导航方式需要预先进行大量工作以对提前规划合理的行驶路径,并需要结合车辆的运行特性。同时为了保证导航的精确性以防止发生碰撞,在行驶路径上必须预制大量的专用定位设备,并在车上需要加装定位传感器来采集定位信息,以实现对车辆的精确定位。由此可见这种导航方法需要大量的前期投入,成本高且可靠性不好。
其中反应式导航是一种主流的地下矿用车辆自主导航技术矿山设备的自主导航关键技术,反应式导航***不需要所处位置的环境模型,而是通过车身上的传感器感知周围环境。当井下车辆通过巷道时,车辆不需要知道自己在井下的绝对位置,只需知道自己相对于巷道壁的距离及其角度就能自主行走。因此反映式导航也称为相对式导航,具有前瞻性和极好的适应性,有利于车辆的高速运行。
现有技术中对于自主导航已经有了很多研究,例如申请号为ZL200910238066.3的中国发明专利申请公开了一种采用绝对式导航技术的地下自主铲运机导航控制方法,这种方法是根据事先规划好的目标路径与铲运机实际行驶路径对比得到铲运机行驶过程中的偏差,以偏差量作为反馈进行实时控制。这种技术至少存在以下缺点:
导航控制方法未充分考虑铰接式车辆的运动学和动力学特性,导航前需要规划好一条路径,为了使规划的路径合理,规划路径时需要结合车辆的运动学特点,这增加了导航控制的前期工作量;导航需要车辆准确的定位信息,因此需要在巷道的“关键地点”加装定位设备,在大型复杂的地下矿井中,“关键地点”的数目非常大,这就需要大量的定位设备,会大大增加导航的成本,而且由于地下巷道宽度和高度的限制,定位设备的安装会受到很大程度的制约;导航中提出用“航位推算”的方法来减少定位设备的安装,但“航位推算”又需要在车上加装相应的传感器来采集地下巷道壁的信息,由于地下巷道壁的凹凸不平,航位推算所需要的信息本身也会有很大的误差,这会加大航位推算的误差,而且这种误差会累加,最终推算出来的结果可能不能使用。以上所述导航控制技术的缺点也反映出绝对式导航本身的不足之处。
发明内容
针对现有技术中的导航技术存在的诸多问题,本发明实施例提出了一种地下矿用车辆的反应式导航控制方法。
为了达到上述目的,本发明实施例提出了一种地下矿用车辆的反应式导航控制方法,包括:
根据当前时刻的车辆铰接点、前桥中点、前桥车速、前桥横摆角速度、前桥转向半径,后桥中点、后桥车速、后桥横摆角速度、后桥转向半径,建立下一时刻前桥位姿状态向量估计值模型;
建立航向偏离角模型;
建立铰接车前桥中心位置偏差模型;
建立转向角控制模型,其中转向角控制模型包括安全预测模型和模糊控制算法;
建立铰接车速度控制模型,并根据铰接车速度控制模型控制地下矿用车辆。
其中,下一时刻为t+1时刻,且所述前桥位姿状态向量估计值模型为非线性离散模型,表示为:
其中TS为车辆转向时间,为车辆的转向角速度;铰接车交接点为H,前桥中点为Pf(xf,yf),该点与中央铰接点H的距离为l1,该点纵向车速为vf;后桥中点为Pr(xr,yr)点,该点与中央铰接点的距离为l2,车速为vr;前车体横摆角速度为ω1,转向半径为r1,后车体横摆角速度为ω2,转向半径为r2;前车体的航向偏离角为θ1,后车体的航向偏离角为θ2,转向角为λ(t);t时刻前桥中点的位置姿态信息用位姿状态向量St=[xf(t) yf(t) θ1(t)]T表示。
其中,建立航向偏离角模型具体包括:
铰接车的航向偏离角和铰接车前车体的横摆角速度ω1相关,根据t时刻前车体横摆角速度ω1、t时刻铰接车的车速测量值铰接车的转向角测量值通过以下方法通过运动学模型得到航向偏离角模型:
铰接式车辆在行驶过程中前后车体在中央铰接点处的速度相等因此铰接式车辆前后车体在中央铰接点处平行于后桥轴线方向的速度分量相等,因此铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等,即vfsinλ=ω2l2+ω1l1cosλ;
铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等,即vr=vfcosλ+ω1l1sinλ,
计算铰接式车辆的转向角变化速率等于前后车体绕各自瞬时中心转动的角速度之差,即:-dλ/dt=ω2-ω1,以此得到ω1=(vfsinλ+l2(dλ/dt)/(l2+l1cosλ)),
根据公式ω1=dθ/dt得到铰接式车辆车体航向偏离角与转向角以及车体参数之间关系式为:dθ/dt=(vfsinλ+l2(dλ/dt))/(l2+l1cosλ),
t时刻航向偏离角θ(t)表示为θ(t)=θ(t-1)+ω1(t)×T,
其中θ(t-1)为航向偏离角初始值,设初值为0,T为***采样时间。
其中,建立铰接车前桥中心位置偏差模型具体包括:
以y1,y2为内侧、外侧巷道曲线方程,k1,k2分别为巷道内侧与外侧的一定区域内的弧线曲率平均值,G1为偏离车体角度为θ的方向上障碍点到车体前桥中心的距离,则k1,k2表示为:
则t时刻铰接车前桥中心位置偏差δ(t)表达为:
δ(t)=[k1(t)-k2(t)]/2k1(t)k2(t)-G1(t)。
其中,建立转向角控制模型包括:
车体航向偏离角安全预测值θ(t+1)由当前时刻车体前方障碍信息确定,令检测角度分辨率为1度,车辆可检测到前方[0,SA°]巷道壁距车辆前桥中心的距离,则可检测到的障碍点个数为(SA+1)个;
利用障碍贡献函数确定每个障碍点的障碍贡献值,以确定其对车辆安全形势的影响,将车体正前方的区域等分成n个扇形区域,每个扇形区域内的障碍点密度可表示为Dn,进而得到n个扇形区域的障碍密度点的集合D=[D1,D2,…,Dn]T其中T表示转置;利用三次样条差值,将得到的障碍密度点以及其对应的区域角度值转化为笛卡尔坐标系下的一系列的坐标值{(x1,y1),(x2,y2),…,(xn,yn)},
三次样条差值函数峰值中的最小值为S(xm),令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)。
其中,建立铰接车速度控制模型具体包括:
铰接车的目标车速Vdes根据车辆当前车速和前方障碍距离D确定,地下矿用铰接车在行驶过程中分为加速行驶、匀速行驶、减速行驶、制动停车,本速度控制模型也按照铰接车的实际驾驶情况分为加速行驶模式、匀速行驶模式、减速行驶模式、制动停车模式;
根据测出铰接车从最高速减速行驶到停车所用时间t减速,并针对不同驾驶模式的转换分别设定了转换阈值,分别为:
t减速阈值=1.5t减速,
t匀速阈值=2t减速,
t加速阈值=2.5t减速,
在采集回来的前方障碍物距离信息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,前桥中点为Pf(xf,yf),该点与中央铰接点H的距离为l1,车速为vf;后桥中点为Pr(xr,yr)点,该点与中央铰接点的距离为l2,车速为vr,前车体横摆角速度为ω1,转向半径为r1,后车体横摆角速度为ω2,转向半径为r2,前车体的航向偏离角为θ1,后车体的航向偏离角为θ2,转向角为λ。
t时刻前桥中点的位置姿态信息可以用位姿状态向量St=[x1(t)y1(t)θ1(t)]T表示,t+1时刻的前桥位姿状态向量估计值可以用非线性离散模型表示为:
B.建立航向偏离角模型,
铰接车的航向偏离角和铰接车前车体的横摆角速度相关,t时刻前车体横摆角速度ω1根据t时刻铰接车的车速测量值和铰接车的转向角测量值经运动学模型得到,[085]航向偏离角θ(t)如附图3所示,铰接式车辆在行驶过程中前后车体在中央铰接点处的速度相等,
忽略铰接式车辆行驶过程中的侧滑,其前后车体在中央铰接点处平行于后桥轴线方向的速度分量相等,进而铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等,即vfsinλ=ω2l2+ω1l1cosλ,
铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等,即:vr=vfcosλ+ω1l1sinλ,
铰接式车辆的转向角变化速率等于前后车体绕各自瞬时中心转动的角速度之差,即:-dλ/dt=ω2-ω1,
以此得到ω1=(vfsinλ+l2(dλ/dt)/(l2+l1cosλ))
又ω1=dθ/dt,得到铰接式车辆车体航向偏离角与转向角以及车体参数之间关系式为:dθ/dt=(vfsinλ+l2(dλ/dt))/(l2+l1cosλ),
当前时刻航向偏离角θ(t)可表示为:θ(t)=θ(t-1)+ω1(t)×T,
其中θ(t-1)为航向偏离角初始值,T为***采样时间。
C.建立车体前桥中心位置偏差模型,
铰接式车辆在圆弧巷道中位置信息如附图4所示,y1,y2为内侧、外侧巷道曲线方程,k1,k2分别为巷道内侧与外侧的一定区域内的弧线曲率平均值,G1为偏离车体角度为θ的方向上障碍点到车体前桥中心的距离,则k1,k2表示为:
则t时刻铰接车前桥中心位置偏差δ(t)可表达为:
δ(t)=[k1(t)-k2(t)/2k1(t)k2(t)-G1(t),
当巷道为直线巷道时,可简化为图5,此时巷道内侧与外侧的一定区域内的弧线曲率平均值k1,k2都为0,
其中h1,h2,h3分别为偏离车体前桥0°,90°及180°方向上障碍点到车体前桥中心的距离,θ为车体航偏离角,G0为巷道宽度,
由附图5中的几何关系可得到:
G0(t)=[h1(t)+h3(t)]cosθ(t),
G1(t)=h2(t)sinθ(t),
t时刻中心位置偏差δ可表示为:
δ(t)=G0(t)/2-G1(t)。
D.建立转向角控制模型,
如附图1所示,航行角控制模型包括全预测模型和模糊控制算法,其中安全预测模型的输入量为t时刻的航向偏离角和前桥中心位置偏差,输出量为t+1时刻的航向偏离角和前桥中心位置偏差,模糊控制算法的输入量为t+1时刻的航向偏离角和前桥中心位置偏差,经算法得到的结果即为转向角控制模型最终输出量铰接车的转向角λ,
a.建立安全预测模型,利用安全预测模型得到下一时刻铰接车的安全航向偏离角和安全中心位置偏差,
安全预测模型中下一时刻允许的车体航向偏离角θ(t+1)可由当前时刻车体正前方障碍点信息确定,
如图5所示车体正前方0~SA的角度,半径为R的阴影区域为障碍点对前车体有影响的区域,其中d为一个障碍点到车体前桥中心的距离。对于障碍点的采样角度间隔为1°,即0~SA的角度范围内共采样(SA+1)个点,
为表示障碍点对车体的影响,定义障碍贡献值为:
其中dj为障碍点到前(或后)桥中心的距离,
j=[0,1,…,SA]
为弥补***误差,令Re∈[1,3],
将车体正前方的区域划分成n等分扇形区域,每个扇形区域占有角度为σ即:n=SA/σ,
则每个扇形区域内的障碍点密度表示为:
其中
通过上述计算得到铰接式车体正前方n个扇形区域的障碍密度点的集合为:
D=[D1,D2,…,Dn]T,
由于该n个障碍密度点是非连续点,则得到的障碍密度曲线为非连续曲线,为了使得障碍密度曲线连续,且一阶与二阶导数连续,因此选择三次样条插值处理n个障碍密度点,
将得到的障碍密度点以及其对应的区域角度值转化为笛卡尔坐标系下的一系列的坐标值{(x1,y1),(x2,y2),…,(xn,yn)},
其中:[x1,x2,…,xn]T=π[σ,2σ,…nσ]T/180,
[y1,y2,…,yn]T=[D1,D2,…Dn]T,
S为三次样条曲线方程,选择边界条件:x0″=y0″,xn″=yn″从而得到三次样条插值函数S(x),
当S′(xz)=0且S″(xz)≠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)],
其中κ为正值系数。
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所示,由转角传感器采集铰接式车体转向角λ,编码器采集车轮转速,将转速转化为车体速度V,激光雷达扫描前车体的障碍区域采集障碍点信息dj,实施步骤如下:
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所示,
通过公式公式与得到36个障碍点密度,对于三次样条插值来说,36个点的插值运算量过大,将36个点分为9组,每组4个障碍点进行三次样条插值,得到9段平滑的曲线,对于相邻两段曲线之间仍取出4个点进行三次样条插值,得到三次样条差值函数S(x)即得到一条具有连续二阶导数的平滑曲线,如附图10所示,
将得到的障碍密度点以及其对应的区域角度值转化为笛卡尔坐标系下的一系列的坐标值{(x1,y1),(x2,y2),…,(xn,yn)},
其中:[x1,x2,…,xn]T=π[σ,2σ,…nσ]T/180,
[y1,y2,…,yn]T=[D1,D2,…Dn]T,
设S为三次样条曲线方程,选择边界条件:x0″=y0″,x″n=y″n,从而得到三次样条插值函数S(x),
下一时刻车体航向偏离角为:
θ(t+1)=[SA×π/360-B(t+1)],
车体前桥中心位置偏差δ(t+1)用车体当前正前方左右两部分障碍点权重之差表示,
车体正前方右半部分障碍点权重为:
车体正前方左半部分障碍点权重为:
则δ(t+1)=κ[L(t)-R(t)],
其中κ为正值系数。
(2)利用模糊控制算法得到铰接车的实际控制量,
得到t+1时刻航向偏离角安全预测值θ(t+1)和前桥中心位置偏离角安全预测值δ(t+1)后,利用模糊算法生成t+1时刻目标转向角λ(t+1),
模糊算法的隶属度函数依据车辆在行驶过程中λ与δ和θ的相关关系确定,
附图12所示,转向角λ的隶属度函数如附图13所示,
模糊算法的推理逻辑为:IF前桥中心位置偏差δ,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 (2)
1.一种地下矿用车辆的反应式导航控制方法,其特征在于,包括:
根据当前时刻的车辆铰接点、前桥中点、前桥车速、前桥横摆角速度、前桥转向半径,后桥中点、后桥车速、后桥横摆角速度、后桥转向半径,建立下一时刻前桥位姿状态向量估计值模型;
建立航向偏离角模型;
建立铰接车前桥中心位置偏差模型;
建立转向角控制模型,其中转向角控制模型包括安全预测模型和模糊控制算法;
建立铰接车速度控制模型,并根据铰接车速度控制模型控制地下矿用车辆;
其中,下一时刻为t+1时刻,且所述前桥位姿状态向量估计值模型为非线性离散模型,表示为:
其中铰接车铰接点为H,前桥中点为Pf(xf,yf),该点与中央铰接点H的距离为l1,该点纵向车速为vf;后桥中点为Pr(xr,yr)点,该点与中央铰接点的距离为l2,车速为vr;前车体横摆角速度为ω1,转向半径为r1,后车体横摆角速度为ω2,转向半径为r2;前车体的航向偏离角为θ1,后车体的航向偏离角为θ2,转向角为λ(t);t时刻前桥中点的位置姿态信息用位姿状态向量St=[xf(t) yf(t) θ1(t)]T表示;
其中,建立航向偏离角模型具体包括:
铰接车的航向偏离角和铰接车前车体的横摆角速度ω1相关,根据t时刻前车体横摆角速度ω1、t时刻铰接车的车速测量值铰接车的转向角测量值通过以下方法通过运动学模型得到航向偏离角模型:
铰接式车辆在行驶过程中前后车体在中央铰接点处的速度相等,因此铰接式车辆前后车体在中央铰接点处平行于后桥轴线方向的速度分量相等,因此铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等,即vfsinλ=ω2l2+ω1l1cosλ,
铰接式车辆前后车体在中央铰接点处垂直于后桥轴线方向的速度分量也相等,即vr=vfcosλ+ω1l1sinλ,
计算铰接式车辆的转向角变化速率等于前后车体绕各自瞬时中心转动的角速度之差,即:-dλ/dt=ω2-ω1,以此得到ω1=(vfsinλ+l2(dλ/dt)/(l2+l1cosλ)),根据公式ω1=dθ/dt得到铰接式车辆车体航向偏离角与转向角以及车体参数之间关系式为:dθ/dt=(vfsinλ+l2(dλ/dt))/(l2+l1cosλ),
t时刻航向偏离角θ(t)表示为θ(t)=θ(t-1)+ω1(t)×T,
其中θ(t-1)为航向偏离角初始值,设初值为0,T为***采样时间;
其中,建立铰接车前桥中心位置偏差模型具体包括:
以y1,y2为内侧、外侧巷道曲线方程,k1,k2分别为巷道内侧与外侧的一定区域内的弧线曲率平均值,G1为偏离车体角度为θ的方向上障碍点到车体前桥中心的距离,则k1,k2表示为:
则t时刻铰接车前桥中心位置偏差δ(t)表达为:
δ(t)=[k1(t)-k2(t)]/2k1(t)k2(t)-G1(t);
其中,建立转向角控制模型包括:
车体航向偏离角安全预测值θ(t+1)由当前时刻车体前方障碍信息确定,令检测角度分辨率为1度,车辆可检测到前方[0,SA°]巷道壁距车辆前桥中心的距离,则可检测到的障碍点个数为(SA+1)个;
利用障碍贡献函数确定每个障碍点的障碍贡献值,以确定其对车辆安全形势的影响,
将车体正前方的区域等分成n个扇形区域,每个扇形区域内的障碍密度点可表示为Dn,进而得到n个扇形区域的障碍密度点的集合D=[D1,D2,…,Dn]T,其中T表示转置;利用三次样条差值,将得到的障碍密度点以及其对应的区域角度值转化为笛卡尔坐标系下的一系列的坐标值{(x1,y1),(x2,y2),…,(xn,yn)};
三次样条差值函数峰值中的最小值为S(xm),令B(t+1)=xm,则B(t+1)为t+1时刻安全行车区域所对应的角度,
则t+1时刻车体航向偏离角安全预测值θ(t+1)表示为:
θ(t+1)=[SA×π/360-B(t+1)],
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)。
2.根据权利要求1所述的地下矿用车辆的反应式导航控制方法,其特征在于,建立铰接车速度控制模型具体包括:
铰接车的目标车速Vdes根据车辆当前车速和前方障碍距离D确定,地下矿用铰接车在行驶过程中分为加速行驶、匀速行驶、减速行驶、制动停车,本速度控制模型也按照铰接车的实际驾驶情况分为加速行驶模式、匀速行驶模式、减速行驶模式、制动停车模式;
根据测出铰接车从最高速减速行驶到停车所用时间t减速,并针对不同驾驶模式的转换分别设定了转换阈值,分别为:
t减速阈值=1.5t减速,
t匀速阈值=2t减速,
t加速阈值=2.5t减速,
在采集回来的前方障碍距离D后分析找到距离障碍物距离最近的点,并计算以当前车速下与障碍发生碰撞所需的时间t,并将t与上述设定的时间阈值进行比较来选择此时的驾驶模式,
如果t≤t减速阈值,立即制动停车,
如果t减速阈值<t≤t匀速阈值,减速行驶,
如果t匀速阈值<t≤t加速阈值,匀速行驶,
如果t>t加速阈值,加速行驶。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510171279.4A CN104881025B (zh) | 2015-04-10 | 2015-04-10 | 一种地下矿用车辆的反应式导航控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510171279.4A CN104881025B (zh) | 2015-04-10 | 2015-04-10 | 一种地下矿用车辆的反应式导航控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104881025A CN104881025A (zh) | 2015-09-02 |
CN104881025B true CN104881025B (zh) | 2018-11-27 |
Family
ID=53948555
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510171279.4A Active CN104881025B (zh) | 2015-04-10 | 2015-04-10 | 一种地下矿用车辆的反应式导航控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104881025B (zh) |
Families Citing this family (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105631409A (zh) * | 2015-12-21 | 2016-06-01 | 北京科技大学 | 一种通过信标识别确定车辆位置姿态的方法 |
US10602654B2 (en) * | 2016-05-30 | 2020-03-31 | Kubota Corporation | Auto traveling work vehicle |
CN107450308B (zh) * | 2017-06-21 | 2020-05-05 | 合肥中导机器人科技有限公司 | 存储设备、机器人 |
CN108535279B (zh) * | 2018-03-09 | 2021-05-25 | 成都圭目机器人有限公司 | 一种基于污水管线检测机器人的检测方法 |
CN108548544A (zh) * | 2018-03-30 | 2018-09-18 | 济南大学 | 一种现代物流中agv小车的导航模糊控制方法 |
CN108569336B (zh) * | 2018-04-26 | 2020-08-04 | 武汉理工大学 | 在动力学约束下基于车辆运动学模型转向控制方法 |
CN109190185A (zh) * | 2018-08-10 | 2019-01-11 | 安徽库讯自动化设备有限公司 | 一种具有参数分析功能的agv小车的运行状态调控方法 |
CN109165830A (zh) * | 2018-08-10 | 2019-01-08 | 安徽库讯自动化设备有限公司 | 一种agv小车运行状态的智能化调控*** |
CN109116735A (zh) * | 2018-08-31 | 2019-01-01 | 烟台兴业机械股份有限公司 | 一种地下矿山无轨设备的自动行驶控制*** |
CN109374099B (zh) * | 2018-11-23 | 2023-12-08 | 北京科技大学 | 一种铲运机高精度动态智能称重*** |
CN110286683B (zh) * | 2019-07-15 | 2020-07-24 | 北京科技大学 | 一种履带式移动机器人的自主行驶路径跟踪控制方法 |
CN110825077A (zh) * | 2019-09-30 | 2020-02-21 | 淮北幻境智能科技有限公司 | 一种车辆自动驾驶安全管理方法及*** |
CN111538331A (zh) * | 2020-04-24 | 2020-08-14 | 北京科技大学 | 一种地下无人铰接车的反应式导航方法 |
CN113619605B (zh) * | 2021-09-02 | 2022-10-11 | 盟识(上海)科技有限公司 | 一种地下矿用铰接车自动驾驶方法与*** |
CN114715273A (zh) * | 2022-04-27 | 2022-07-08 | 西安优迈智慧矿山研究院有限公司 | 基于双imu传感器的铰接式矿卡转向角度测量方法、***及装置 |
CN116088520A (zh) * | 2023-02-07 | 2023-05-09 | 睿羿科技(山东)有限公司 | 一种无人驾驶铰链车辆的精准方向控制方法 |
CN117519190B (zh) * | 2023-11-30 | 2024-04-26 | 上海联适导航技术股份有限公司 | 一种新的铰接车辆控制方法 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101713999B (zh) * | 2009-11-18 | 2011-09-07 | 北京矿冶研究总院 | 地下自主铲运机的导航控制方法 |
FI121762B (fi) * | 2010-05-10 | 2011-03-31 | Sandvik Mining & Constr Oy | Menetelmä ja laitteisto kaivosajoneuvon paikantamisen järjestämiseksi |
CN103197675B (zh) * | 2013-03-13 | 2016-05-25 | 北京矿冶研究总院 | 地下铲运机自主行驶和避障运动控制及目标路径规划方法 |
-
2015
- 2015-04-10 CN CN201510171279.4A patent/CN104881025B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN104881025A (zh) | 2015-09-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104881025B (zh) | 一种地下矿用车辆的反应式导航控制方法 | |
CN113386795B (zh) | 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策*** | |
CN111338346B (zh) | 一种自动驾驶控制方法、装置、车辆及存储介质 | |
CN109375632B (zh) | 自动驾驶车辆实时轨迹规划方法 | |
Stahl et al. | Multilayer graph-based trajectory planning for race vehicles in dynamic scenarios | |
CN110329263B (zh) | 自动驾驶车辆自适应换道轨迹规划方法 | |
Khodayari et al. | A historical review on lateral and longitudinal control of autonomous vehicle motions | |
AU2006274420B2 (en) | Guidance, navigation, and control system for a vehicle | |
US5545960A (en) | Autonomous mobile machine, and system and method for controlling a mobile machine | |
JP2021524410A (ja) | ドライブエンベロープの決定 | |
CN107867290A (zh) | 一种考虑运动障碍物的汽车紧急避撞分层式控制方法 | |
Zhao et al. | Dynamic motion planning for autonomous vehicle in unknown environments | |
CN109684702B (zh) | 基于轨迹预测的行车风险辨识方法 | |
CN109345020A (zh) | 一种完全信息下的无信号交叉口车辆驾驶行为预测模型 | |
CN112577506B (zh) | 一种自动驾驶局部路径规划方法和*** | |
CN114906173B (zh) | 一种基于两点预瞄驾驶员模型的自动驾驶决策方法 | |
CN113619605B (zh) | 一种地下矿用铰接车自动驾驶方法与*** | |
CN112527000A (zh) | 矿井下智能驾驶的局部路径规划方法及*** | |
CN108594637A (zh) | 两轮自平衡车平面点对点自主运动控制方法 | |
CN115167440B (zh) | 一种基于虚拟领航-领航-跟随的多机器人编队控制方法 | |
CN110297500A (zh) | 一种给定多航路点下无人机的连续航迹规划方法 | |
CN112241171A (zh) | 一种可绕障碍的轮式机器人直线轨迹跟踪方法 | |
CN114942642A (zh) | 一种无人驾驶汽车轨迹规划方法 | |
Xing et al. | Lane change strategy for autonomous vehicle | |
Chen et al. | An overtaking obstacle algorithm for autonomous driving based on dynamic trajectory planning |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
EXSB | Decision made by sipo to initiate substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |