基于SVD-Unscented卡尔曼滤波的掘进机行进调度纠偏研究

吉晓冬瞿圆媛符世琛张敏骏吴淼

1.中国矿业大学(北京)机电与信息工程学院,北京 100083;2.北京建筑大学机电与车辆工程学院,北京 100044;3.清华大学机械工程系,北京 100084

纵轴悬臂式掘进机(下文简称“掘进机”)是煤矿井下掘进工作面的主要设备之一[1]。在巷道掘进过程中,恶劣的地质环境、狭小地下空间的位姿感知误差等因素,都会导致机身渐渐偏离规划中心线,截割面出现超挖、欠挖等现象,从而影响巷道成形质量,降低掘进效率。目前掘进机纠偏仍很大程度依靠有经验的操作员调度完成,掘进现场作业、机器人化智能发展的进程缓慢[2-3]。

机器人化掘进的实现取决于掘进机的位姿检测、自主纠偏、远程通信与控制、自主截割等关键环节的智能化实现与融合[4]。其中,基于机身位姿信息的自主纠偏,即掘进机的行进调度问题是保证巷道截割质量的重要前提。掘进机的纠偏行进调度涉及两个环节:一是纠偏控制模型的建立;二是机身动态位姿检测的实现。

针对履带车辆的纠偏控制研究,部分文献侧重于完善履带车运动学或动力学建模,以获得更准确有效的机身移动模型表征;也有部分文献更侧重于优化控制策略的使用,以获得更智能有效的动态控制效果。如李岩等[5-6]在履带式移动机器人动力学分析的基础上提出了充分考虑履带与地面之间滑动摩擦力对运动参数影响的滑动操纵模型[7],说明轨迹沿程打滑率是履带车运动参数调整的重要依据。后者相关理论以模型预测控制(MPC)这一基于反馈信息的多目标滚动优化模型为典型,如Wang、Lenain 等[8-9]先后采用不同的履带打滑估算方法模拟实际打滑因素的不确定性,将模型预测控制用于农业履带式运载设备的轨迹跟踪控制;Burke[10]运用MPC 模型结合陀螺仪的测量信息实现打滑率的自适应估计,由此完成某移动机器人的轨迹跟踪控制。

由于掘进机井下工作环境复杂恶劣,且通信能力有限,井上通用可行的机身位姿测量方案往往难以直接移植到井下。同时,工作模式的不同,用于同在井下作业的采煤机其位姿测量方案也并不适用于井下掘进机。在掘进机动态位姿检测研究方面,针对煤巷掘进机位姿检测、定向掘进技术分别论证了一系列自动测量系统或方法,但其在实际工况下的应用并未得到有效验证[11]。

上述研究从不同角度为掘进机的纠偏与轨迹跟踪提供参考,而系统地针对掘进机井下纠偏行进调度跟踪控制策略的研究有限。王福忠等[12]针对掘进机的液压行走驱动进行了建模,并针对行走轨迹偏离巷道中线的问题提出了模糊控制策略;张敏骏等[13]研究了掘进机在不同掘进环境下的打滑率并提出基于神经网络PID 算法的自主纠偏驱动控制策略;张旭辉等[14]运用单目视觉测量技术,并基于履带车运动学分析为掘进机设计了定向掘进控制器。

综上所述,目前在掘进机自主纠偏上有一定的研究,但缺乏对掘进机的位姿测量误差与执行过程误差的反馈控制研究。

掘进机在履带行进的执行过程中普遍存在各类传递误差,用于位姿纠偏的实时位姿测量也不可避免存在观测误差,故而机身行进调度仍然具有显著的非确定性。本文以掘进机行走位姿与设定的目标位姿之间的偏差模型为基础,以超宽带(UWB)测距的位姿测量实验为依据,提出一套考虑测量及执行误差的、基于位姿估计的掘进机行进调度跟踪控制策略。

1 掘进机行进调度需求及建模基础

巷道的掘进通过执行调度行进、截割头掏槽、断面截割、刷帮扫底、煤岩辅运等基础工序循环进行。每完成一段掘进量会迅速执行锚杆支护,此时,掘进机会根据巷道走向进行合理的纠偏,保证巷道走势和成型质量。

1.1 控制律掘进机行进调度规划

为实现智能化高效掘进,需引入机身位姿自主感知环节。此环节一般通过在机身搭载定位系统实现,如薛光辉等[15]提出的激光标靶系统,符世琛等[16]提出的UWB 测试系统,张旭辉等[14]应用的单目图像处理技术等。在每段掘进量开始或结束间隙,机载定位系统会测量当前的机身质点位置坐标,判断是否与设计的巷道中心线有所偏移,进而规划行进轨迹并调整工作参数,为掘进机机身的行进调度纠偏。

机身行进调度包含轨迹规划与轨迹跟踪。在机器人跟踪控制领域,动态轨迹规划与跟踪执行经常同步进行。鉴于方法适用性差异,本文着重讨论已有规划轨迹下的跟踪策略,暂不涉及动态轨迹规划问题。

1.2 基于位姿偏差的纠偏控制模型

在履带式车辆的行进运动学建模研究中,韩庆珏等[17]针对某深海履带车运用了路径偏差模型[18],以路径跟踪的位姿偏差在控制迭代中有限且趋于零为目标,理论验证了基于位姿偏差模型的履带车控制的可行性。张敏骏[13]、张旭辉[14]等针对悬臂式掘进机的行进建模亦采用了该位姿偏差模型作为运动学建模基础,其原理简述如下:

如图1所示,以机身当前转向瞬心到机身中心线的垂足作为分析点(设为机身坐标系xOy的原点),图1中虚曲线表示该点的移动轨迹,为清楚示意机身的移动过程,采用了三角形代替机身简化矩形。(X,Y)、(x,y)分别为大地坐标系与机身坐标系下的质点坐标;参数b为左右履带之间的宽度;v为机身行走速度;φ为机身转向角度(由左右驱动轮转速决定);ω为机身转向角速度;θ为机身方向角;脚标c 表示对应参数的当前值;脚标d 表示对应参数的规划值;脚标e 表示对应参数其当前数值与规划值之间的偏差。

图1 掘进机调度跟踪时位姿变化示意Fig.1 Illustration of the variance of the position and orientation of the body of the road-header when tracking

定义质点当前位姿pc=(Xc,Yc,θc),规划或期望位姿pd=(Xd,Yd,θd);质点在机身坐标系xOy中的位姿偏差pe=(xe,ye,θe),可表示为

对pe取微分[20-21],得

掘进机在一定初始偏差下的位姿纠偏过程就是通过逐步调整参数vc、ωc,最终使得位姿偏差pe在有限的步数内有界且趋于0。显然由式(1)、式(2)描述的偏差模型是一个两输入非线性系统,现基于Back-stepping 设计滑模切换函数:

当xe=0 时,考察Lyapunov 函数

由Lyapunov 稳定判据可知,若满足Ly≥0 且≤0,则ye可在有限的调整周期内收敛为0。同时,考虑实际情况下,即sinθe与θe同号。由此可取yevd=-θe,如此只要满足:xe→0,θe→-yevd,则ye→0。

针对由式(1)、式(2)描述的偏差模型,可设计切换函数:

通过设计滑模控制器,使si→0,(i=1,2),即实现xe收敛到0 且θe收敛到-vdye,从而ye→0,θe→0。对s1、s2均取指数趋近律,令=-kisi(i=1,2),其中ki为正系数。对式(4)取微分可得

将式(4)代入式(5)可得

进一步获得控制律u=(vc,ωc)T的计算式:

由式(7)可知,控制律中的机身转向角速度指令ωc由机身位姿偏差(xe,ye,θe)、目标速度vd、目标转向角速度ωd结合任意正系数k2计算获得;机身速度指令vc由机身位姿偏差(xe,ye,θe)、转向角速度指令ωc结合任意正系数k1计算获得。

1.3 控制律简化及收敛性证明

为方便工业应用,在保证pe有界且趋于0 的前提下,可将式(7)适当简化。考虑机身转向角速度ωc较其行进速度vc略小,且实际运行时vdxe在数值上远小于1;θe与sinθe同号。基于此,将式(7)简化为

式中,kx、ky、kθ取值为正,在此分别称为偏差xe、ye、θe的伴随系数或权值。

由Lyapunov 稳定判据可知,若能取合适的Lyapunov 函数L(xe,ye,θe)≥0,上述控制律(8)能使(xe,ye,θe)≤0,则pe有界且

将式(8)代入式(9)可得

若kky=1,则,控制律式(8)能使机身位姿偏差pe逐渐收敛为0。

2 基于滤波估计的行进调度控制策略

通过运动学分析可知,机身轨迹跟踪调度的实质是对履带驱动轮转速进行合理的控制,但实际操作时存在机械结构的执行误差、位姿检测的测量误差。本文将上述误差分别称为过程误差和观测误差,采用噪声环境下的多状态联合估计问题所常用的非线性滤波法,实现掘进机轨迹跟踪调度在误差影响下对参考控制指令的调整。

2.1 过程误差

由控制律u=(vc,ωc)T指令产生的实际机动效果与配置值不可避免存在差异,履带打滑现象是主要原因之一。履带车行进时,履带与地面之间常发生相对滑动,通常用打滑率来表征滑动对履带理论速度带来的影响,数值上以滑动速度与牵引速度的比值来表示,滑动速度与履带运动参数有关。用iL、iR分别表示左右履带打滑率:

式中,ωL、ωR分别为左右履带驱动轮的角速度;r为履带驱动轮半径;rωL、rωR分别为没有纵向打滑时两边履带的线速度;b为左右履带之间的宽度;为履带车旋转角速度;为机身沿x轴方向的速度。

式中,α为质心处转向半径切线方向与机身前进方向的夹角。

α产生的原因,在于刚体旋转时瞬心与刚体上各质点的连线(转向半径)并不都垂直于机身的前进方向。

机身行走速度v由与其圆周运动的线速度合成,但因机身的转向角速度往往较慢,可忽略圆周运动线速度部分,近似认为v≈;式(12)也说明,通过调整左右履带驱动轮的转速,可达到调整机身前进速度v和转动角速度ω(ω=)的目的。反之,根据控制律计算的前进速度与转动角速度,在考虑打滑因素的情况下,实际对应的左右驱动轮角速度为

本文考虑打滑率的影响,从数值统计的角度讨论履带驱动轮实际转速与设定值的差异。根据Bekker 理论,履带驱动力计算如下[13,21]:

式中,B为履带宽度;x为接触点距履带前端的距离;τmax为最大剪切应力;系数Kr为残余剪切应力与最大剪切应力τmax的比值;Kω为出现最大剪切应力时对应的剪切位移;i(x)为履带打滑率,与履带触地长度有关;C为剪切强度;CD为土壤常数;φ为土壤内摩擦角;μ为地面阻力系数;P为法向应力。

土壤的压力-沉降关系如下:

式中,KC为土壤内聚变形量;Kφ为摩擦变形模量;z为沉降量;n为沉降指数。

韩庆珏[20]通过对深海集矿机履带驱动力与打滑率关系的研究指出:当履带接地长度一定时,履带车在特定软底质上行驶时存在最佳打滑率,其使地面能支撑的履带驱动力具有最大值,即对式(14)求导可得到最佳打滑率的理论值,以此作为掘进机打滑率估算的基础。张敏骏等[13]计算了几种典型土壤环境下的打滑率估算值。本文根据这些数值,假设掘进机履带工作打滑率服从边界值为[0.1,0.35]的均匀分布,可为状态估计所涉及的变量分布提供数据依据。

2.2 观测误差

针对掘进机的位姿测量,符世琛[11,16]揭示了基于超宽带(UWB)测距的定位算法机制,建立了UWB 定位算法误差分布分析方法,阐明了惯性导航系统(INS)姿态解算机理,建立了一种基于UWB标定与INS 实时测量的井下关键作业装备自主导航新方法。本文采用了美国Time Domain 公司P440 超宽带测距模块以及国内Action 公司开发的微机械陀螺仪及轴编码器,搭建完成了掘进机自主导航模拟平台,如图2所示。

图2 掘进机自主导航模拟平台Fig.2 Experimental platform for the autonomous navigation of the road-header

通过导航系统位姿检测现场的一系列实验,获取完整实测数据并实现二维运动轨迹实时显示。数据表明,在78 m 模拟实验巷道场地范围内,系统X轴(横向)偏向误差控制在10 cm 左右,Y轴(掘进方向)偏向误差小于2 cm,机身定位点X、Y轴误差如图3所示。方向角精度可保持在5°以内,俯仰角及横滚角误差可保持在1°左右[11,16]。

图3 机身定位点X、Y 轴误差分布Fig.3 The position errors distribution along the X and Y axes,respectively,of the marker on the body

由测距结果进行姿态解算,可获得掘进机的位姿参数,其与调度目标轨迹上对应的位姿之差即为上文所述的状态参数pe=(xe,ye,θe)T。基于此,本文假设参数xe、ye的测量误差均服从均值为0、标准差为≈3.5 cm 的正态分布;而方向角偏差参数θe则服从均值为0、标准差为5°的正态分布。

2.3 基于状态估计的调度跟踪控制

定义机身状态向量Q=(xe,ye,θe,ω*L,ω*R)T,pe=(xe,ye,θe)T为机身位姿跟踪偏差,可由机身车载定位系统实时测取,通过式(8)、式(13)可估算满足轨迹跟踪的履带左右轮转速调度需求,即(ωL,ωR)T。

考虑打滑,(ω*L,ω*R)T=[ωL(1-iL),ωR(1-iR)]T为实际转速,运用式(1)、式(2),可预测机身的位姿跟踪结果。可见,由式(1)、式(2)、式(8)、式(13)所描述的机身调度轨迹跟踪模型可由以下非线性系统表示

式中,εk为控制实施的过程误差;δk+1为位姿偏差的观测误差。

鉴于偏差与位姿的关系,式(15)中δk+1相当于机身质点的位姿测量误差。针对这类噪声环境下的多状态联合估计问题,非线性滤波法被广泛运用于改善预测效果。本文提出基于 SVD-Unscented 卡尔曼滤波的状态估计即为一种典型的非线性滤波法,可以针对性地处理履带行进打滑导致的指令执行误差和位姿测量误差。控制结构框架如图4所示。

图4 基于SVD-Unscented 卡尔曼滤波的掘进机行进调度纠偏控制结构框图Fig.4 Structure of the path tracking for boom road-header using SVD-Unscented Kalman Filtering

2.4 SVD-Unscented 卡尔曼滤波

为方便解释,将式(15)所描述的非线性系统改写为

Unscented 卡尔曼滤波(UKF)用于状态估计的主要思想在于直接采用非线性变换逼近系统参数的概率分布,而非逼近系统的线性化模型。其核心之一是基于Unscented 变换(UT 变换)的无导数运算估计算法的运用:对于一个N维的自变量x经某非线性变换f(x)后获得随机向量y,记Px|N×N为x的协方差矩阵,为x的均值,为获得y的统计量,运用Sigma 点抽样法形成2N+1 个Sigma 点向量矩阵χ:

式中,λ为尺度参数;α为很小的正常数,决定周围sigma 点的扩散;κ为二阶尺度参数,一般在状态估计时设为0, 在参数估计时设为3-N;为加权平方根矩阵的第i列。

一般情况下,Px为正定对称矩阵,对其进行特征值分解,设Px=QΛQ-1(Q为N×N方阵,其列向量为Px的特征向量;Λ是由对应特征值组成的对角矩阵),进而注意,式(17)成立的前提是Px为正定对称矩阵。然而,在实际执行过程中经常会发生协方差矩阵病态的情况,即Px不满足正定对称矩阵的条件。

由于正定对称矩阵的特征值分解等价于SVD分解:Px=UΛVT(其中U=V=Q,QT-Q=I,Λ同上),故

本文基于SVD 分解数值计算方面的鲁棒性优势,采用式(18)计算协方差矩阵Px的开方,而非直接开方,称为SVD-Unscented 卡尔曼滤波。

基于SVD-Unscented 卡尔曼滤波的状态估计方法如下:

(1) 初始化。构造包含状态量、过程误差以及观测误差的增广状态向量。

(2) 确定Sigma 点集。根据式(17)、式(18)取点向量矩阵χk。

(3) 时间更新。利用系统非线性函数f(x)产生,并计算与。

(4) 状态更新。

以式(21)中的估计值作为状态量xk的更新,以式(20)中的作为观测量yk+1的更新,代入控制函数式(8),完成控制输入u的纠正与更新,即实现基于SVD-Unscented 卡尔曼滤波的状态估计及路径纠偏控制。

3 掘进机调度跟踪控制仿真分析

本节对偏离目标巷道的掘进机调度跟踪控制进行仿真分析,以评估所提出纠偏方案的合理性和可行性。

本文暂不涉及轨迹规划,故预设以如图5所示轨迹点作为机身中心位置的规划轨迹。轨迹参数包括期望的坐标与对应位置上的航向角(转向角速度)变化历程。取图5所示S 型轨迹目的,在于考查左、右转向调度时的控制效果。假设掘进机机身初始位姿为,其中(1,0)为机身的中心坐标,为机身航向角。机身初始位姿与调度轨迹的第一个点所配置的坐标及航向角一致。其他仿真参数见表1。

图5 规划调度轨迹坐标、航向角及转向角速度Fig.5 The planned trajectory

表1 仿真参数Table 1 Data used in simulation

为初步验证由式(1)、式(2)、式(8)、式(13)所描述的控制系统的有效性:

首先,进行不涉及过程误差及测量误差处理的行进调度仿真。图6展示了调度过程中机身中点的坐标变化情况、位姿偏差变化情况以及由控制指令u=(vc,ωc)T换算配置的左右驱动轮转速变化情况。从图6可以看出,调度仿真达到了轨迹跟随的目的:

图6 不考虑噪声时的调度跟踪Fig.6 The tracking results without noises

(1) 空心点表示实际机身移动轨迹,与实心圆点所示的规划轨迹重合度极高。

(2) 机身中心位姿偏差(Xe,Ye,θe)在有限的步数内均趋于零值;有个别突变点出现在对应轨迹中航向角变化速率突变的位置,但其下一步调度即调整回归,属于合理现象。

(3) 调度过程中左右履带驱动轮的转速差值变化较小,个别拐点单个驱动轮的转速数值变化亦较为平缓,说明调度过程较平稳。

其次,考虑引入过程误差及测量误差,但不采用滤波措施下的调度跟踪情况。通过随机加入符合统计规律的打滑率与位姿测量误差,观察调度跟踪仿真效果。图7展示了调度过程中机身中点的坐标、位姿偏差以及由控制指令u=(vc,ωc)T换算配置的左右驱动轮转速等变化情况。

图7对比图6可知,仿真调度轨迹逐渐与期望轨迹重合,但机身中心位姿偏差未能稳定地趋近于零;履带驱动轮的转速变化起伏较大。

图7 噪声影响下的调度跟踪Fig.7 The tracking results considering noises

最后,针对不可避免的过程误差与观测误差,采用图4所展示的SVD-Unscented 卡尔曼滤波来估计机身相对于规划路径的实际偏差,用以修正当前调度步骤下的参考控制指令,更好地跟踪规划路径、完成调度纠偏任务。

调度仿真结果如图8所示。可以看出,机身移动轨迹与规划轨迹非常接近,对应的左右履带驱动轮转速差值变化较小,除个别拐点外,单个驱动轮的转速数值变化在小范围内变化,说明调度过程平稳。对应的位姿偏差曲线显示机身位姿偏差Ye与θe均渐趋于0,但是偏差Xe则没能如期望的那样同步衰减到零值附近,而是在0.25 m 上下浮动。其原因在于:回顾图1及式(1),大地坐标系XOY中的坐标差Xd-Xc与Yd-Yc分别向机身坐标系xOy的两轴投影,当θc接近0°时,两者在机身坐标系x轴上的分量和多于在y轴上的分量和,即只要坐标差Xd-Xc或Yd-Yc不为零,xe必然大于ye;反之当θc接近90°时,只要坐标差Xd-Xc或Yd-Yc不为零,ye必然大于xe。误差存在时,Xd-Xc或Yd-Yc一般不为0,故而当θc接近0°时,位姿偏差xe不会趋于0。

图8 基于SVD-Unscented 卡尔曼滤波的调度跟踪Fig.8 The tracking results using SVD-Unscented Kalman Filtering

4 结 论

(1) 通过分析机身行进过程中机构执行误差、观测误差,基于理论及实验分析总结了履带打滑率、位姿测量误差的统计规律。针对这两类误差,提出将SVD-Unscented 卡尔曼滤波估计引入控制策略以修正参考控制指令,能有效减小误差对跟踪控制带来的影响。所提出的SVD-Unscented 卡尔曼滤波算法避免了实际计算中遭遇因协方差矩阵病态而导致运算中断。

(2) 提出的基于位姿偏差模型的控制策略结构简单、调度有效,能使机身在有限的调度周期内实现向目标巷道的平稳过渡,位姿偏差收敛性良好。在考虑误差的情况下,采用SVD-Unscented 卡尔曼滤波对实际的机身位姿偏差进行滤波估计并修正控制指令,其调度跟踪效果极佳且重复性良好,实现过程简单高效,能为井下有限空间内的掘进机自动纠偏提供参考。