# Path Correction of the Boom Road-Header in Coal Mining Based on State Estimation

Authors: Yuanyuan Qu; Xin Cheng; Minjun Zhang; Shichen Fu; Miao Wu
Journal: Mathematical Problems in Engineering
 (2019)

---

## Abstract

Trajectory planning and tracking control algorithm based on a position and orientation deviation model are proposed to achieve path correction for the mining boom road-header working underground. The proposed strategy is assessed to be feasible and potentially practicable by simulations, from which the following statements are summarized. Firstly, trajectory planning is necessary since different trajectories correspond to different scenarios about undesirable excavation space, slipping level, and power consumption. Secondly, using the proposed tracking control algorithm, the road-header is guided back onto the expected path in limited adjusting steps, with smoothly varying rotation speeds of the driving wheels and regularly reducing pose errors. Lastly, it shows that by implying the SVD-unscented Kalman filtering in the tracking control, the adverse impacts of process and measurement noises are appeased obviously. This research provides an advisable modeling and valuable simulation for the road-header to achieve robotic operation underground.


---

## Body

## 1. Introduction

Boom road-header (or “road-header” for short) is used for underground roadway shaping in mining industry, by drilling and cutting the coal and rocks following a predesigned geological blueprint. It is mainly composed of a framework, cutting arm and shovel plate, belt conveyor, walking mechanism, hydraulic suits, and control system, as shown in Figure1.Figure 1
General outline drawing of a boom road-header.The road-header is expected to walk along the centre line of the predesigned roadway. The ideal excavation procedure is as follows: the yawing angle is judged, namely, the orientation of the vehicle body, at first; the road-header is driven with certain turning angular speed (φ˙) and heading velocity (x˙), and coal mixed with rocks is cut down and conveyed to the rear while displacement of the vehicle completes; and the position and posture information of the vehicle are then updated by estimation or measurements and are used to plan the yawing angle, turning speed, and forward velocity for the next excavation step. Once off-track happens, namely, any of position and orientation fails to follow the predesigned roadway, path correction is required. This paper proposes a solution for path correction of the road-header that occasionally departs from the desired path underground.It can be seen in Figure1 that the road-header walks rely on caterpillar bands; hence, its walking manner is similar with the general caterpillar vehicles to a certain extent. It is a common sense that slipping between the tracks and ground happens with high probability, which would weaken the manoeuvre control effects in practice.To achieve path planning and tracking for vehicles or robots, there are many literatures that can be referred to. To give a concise introduction, these literatures are classified into two classes which can be represented by references [1, 2], respectively: one class worked more on the optimal control strategy such as the model predictive control (MPC) used in [3] for obstacle avoidance; the other group placed more emphasis on dynamic analysis or device assistance to establish a kinematic model of the vehicle as practical as possible. Something in common is that the uncertainties of parameters involved in modeling are considered improving tracking performance of the vehicles. It is shown in [4] that steering actuator fault detection is investigated thoroughly based on the linear parameter-varying (LPV) model of an electric ground vehicle to achieve better control over the vehicle.Similarly, for caterpillar vehicles, based on certain dynamic or kinematic modeling of the vehicles, a control model is deduced to fulfill the task of path planning and tracking. Furthermore, the uncertainties of some critical parameters involved in modeling are concerned from different angles. For example, Wang et al. [5] and Lenain et al. [6] applied a slipping estimation algorithm and an adaptive min-max cost function, respectively, to determine the effects of track slipping for farming-tracked vehicles. Burke [7] used a rate gyroscope and proposed a least squares estimator to compensate slips when applying MPC for path following. The usage of skip-steering model in reference [8, 9] is also a typical tactic to cope with the varying slip ratio caused by the changing physical environments. Endo et al. [10] used a slip-compensating odometry for an outdoor robot, and Hin and Liu [11] applied the path deviation model for the deep-sea tracked vehicle.Nevertheless, for the road-header, a large tracked vehicle working underground in a sealed space, there are very few counterpart literatures for reference. Wang and Zhang [12] used fuzzy control strategy based on the modeling of the hydraulic driving system of the road-header. Yang et al. [13] proposed an optimal path planning in stochastic constraint network scenario for the road-header underground. However, slip compensation was barely discussed in both of the mentioned references. Zhang et al. [14] proposed a self-rectification planning for the road-header, using a PID control and a neural network based on regional grid modeling. Slip rates corresponding to different environments were estimated in this self-rectification planning, but the noises involved in pose measurement and movement processing were deficiently considered.This paper aims to propose a feasible solution towards path correction of the road-header working underground. Facing that, no matter which control algorithm or strategy it would use, it is necessary to figure out the kinematic characters of the road-header at first. Technological tactics based on real-time measurement or estimation about related motion parameters is beneficial for making reliable control law. Besides, because of the noises that exist inevitably in measurement and processing, there is uncertainty over the movement control of the road-header.To study the walking motion of a road-header, dynamic and kinematic modeling is the general and fundamental starting. In terms of dynamic modeling, the hydraulic motor regarding valve control towards rotating speeds of the driving wheels has to be studied, so does the mechanical motion regarding torque towards traction force and acceleration. The modeling of each aforementioned block is complicated along with many influence factors to consider. Aiming to develop a simple but potential solution, this paper puts precise dynamic analysis of the road-header aside for a while but focuses on control system formulation based on kinematic modeling and simulation that considers possible process and measurement noises which were obtained by previous studies on real-time pose detection and track slipping of the working road-header.For an overview, the following Section2 presents the kinematic model of the walking unit of the road-header briefly, as well as a short explanation about the differences between the road-header and general-tracked vehicles or robots in aspect of path following. Section 3 interprets why and how to plan a trajectory heading to the desired path before tracking. In Section 4, the controller based on position and orientation deviation is then designed and proved according to Lyapunov stability criterion. Section 5 explains how the measurement and process noises are managed using SVD-unscented Kalman filtering in the control procedure. A group of simulations are carried out in Section 6 to predict the performance of path correction; particular discussions are given corresponding to different scenarios. Final conclusion is summarized in Section 7.


## 2. Kinematic Model of the Walking Mechanism

Suppose the road-header works on a plane with no tilt, and according to practice, the steering or turning motion of the road-header is usually sluggish. When the vehicle walks straight, there is no lateral friction or force along the tracks. However, when the vehicle turns, there is a common phenomenon called track slip, which is caused by the lateral friction and force over the tracks, leading to the mismatch between the ordered and real values of the walking velocity and the turning angular speed. Slip ratio is used to represent the effect of track slip. It is numerically defined as the ratio of slipping speed to traction speed of the track.Suppose a road-header is turning left,ωL and ωR are the rotating speeds of the driving wheels of the tracks on two sides, respectively; r is the radius of the driving wheels; and apparently, rωL and rωR are the expected traction speeds of the two tracks. Take b as the distance between the left and right tracks; φ˙ as the turning angular speed of the vehicle; and x˙ as the instant velocity of the vehicle body along the x-axis of the vehicle coordinate system xoy. If there are no slips along the tracks, x˙ and φ˙ are expected to be 1/2rωL+rωR and r/bωR−ωL, respectively, while if slip effects are considered, the instant velocity and steering angular speed are as follows:(1)x˙=r2ωL1−iL+ωR1−iR,φ˙=rbωR1−iR−ωL1−iL,y˙=−x˙tanα,where iL and iR are the slip ratios over the left and right track, respectively, and α is the tiny angle between x-axis and the tangent direction of the turning radius asides the mass point underchecked. Obviously, by configuring different sets of ωL and ωR, different forward speed x˙ and steering angular speed φ˙ are resulted. The instant heading velocity v is actually composited by x˙ and y˙, and the linear velocity is caused by the steering motion of the vehicle. Considering both the turning angular speed and the angle α are usually very small, the corresponding linear velocity and y˙ are ignorable compared with the x˙. Hence, the value of v is very close to x˙, namely, v≈x˙.Conversely, if the forward speed and steering angular speed were planned, the corresponding rotating speeds of the left and right driving wheels can be calculated by(2)ωR1−iR=1rx˙+0.5bφ˙,ωL1−iL=1rx˙−0.5bφ˙.The simple kinematic equations (1) and (2) are suitable for most caterpillar vehicles, including the road-header.However, there are some differences between the road-header and other caterpillar vehicles in respect of path correction, which are summarized as follows:(1)
For the road-header, the time period of each displacement is usually longer than that of most caterpillar vehicles on ground. It is more likely to use “adjustment” or “scheduling” instead of “tracking” when illustrating the procedure of path correction in detail. The time interval is uncertain due to different geological environments and is usually more than 5 minutes. Since noise accumulated in such a period is nonignorable, model-based kinematic states updating without any real-time measured information or state estimation are unpersuasive. In other words, noise should be considered in an appropriate way.(2)
For safety, the displacement in each adjusting step is limited to be less than 80 cm. To simplify, this limitation over displacement is transformed to a restraint on walking velocity if the time interval is treated to be constant. Namely, the road-header walks with speed less than 0.8 m/Δt.(3)
Except the natural kinematic restraints of the walking mechanism, considering the sealed space where the road-header locates, the orientation of the road-header can hardly vary randomly when space cost is concerned. The space cost during excavation, namely the undesirable space that was produced around the vehicle during adjustments, should be as less as possible. Therefore, not only the position but also the orientation of the road-header should be well considered in the trajectory planning.(4)
Compared with the size of the road-header, the concerned parameters such as velocity, turning angular speed, position, and orientation deviation are very small so does the variances of them. Therefore, the big-sized body of the vehicle cannot be treated as a mass point in trajectory planning. Also, when grim geology condition is encountered, for example, when huge and tough coal rocks emerge in front, it can never move like a smart car doing obstacle avoidance on the ground. Instead, the current tracking is then replaced by a new one which provides a totally new set of adjustments in plan.


## 3. Trajectory Planning and Movements Scheduling

Notice that the road-header locates underground in a sealed space; it can hardly reset its orientation in situ and move back to the aimed track immediately. More likely, it may need to retreat, adjust yawing angle, and then move forward again, that is, a set of inefficient operation. Alternatively, it can keep moving forward instead of retreating, but with well-planned turning speed (φ˙) and forward velocity (x˙) and walk to the aimed track regularly in steps. That is what trajectory planning does in this paper.There are several trajectories that could converge to the predesigned track, but not all of them are recommended. From an economic perspective, more excavation means more expending; therefore, limited excavation step is required. Secondly, since the body size of the road-header is huge, it is inevitable to produce derived space, i.e., the unexpected excavation space, around the main roadway. An advisable trajectory is the one that produces less-derived space. Lastly, to shape a roadway close to the predesigned one as much as possible, the trajectory is expected to be smoothly converging to the predesigned path.To judge the amount of excavation space during displacement, the top view of the road-header is simplified as a rectangular frame with sizel×b, and then the area swept by the rectangular during movements is judged, which is the shadow area in Figure 2. Suppose the feasible trajectory is composed of n steps of displacement, then each step is resulted by operating certain turning speed (φ˙=ω) and heading velocity (x˙=v). Set the location state of the road-header at step k as sk=Xkh,Ykh,θk,Xkr,Ykr, in which Xkh,Ykh and Xkr,Ykr are the coordinates of the head and rear point of the road-header, respectively, in the geodetic coordinate system XOY; θk is the orientation angle. The area Ak is calculated by(3)Ak=12a→k×d→kr+12a→k+1×d→kh,where a→k and d→kr are vectors Xkh−Xkr,Ykh−Ykr and Xk+1r−Xkr,Yk+1r−Ykr, respectively; and a→k+1 and d→kh are vectors Xk+1h−Xk+1r,Yk+1h−Yk+1r and Xk+1r−Xkr,Yk+1r−Ykr, respectively. It is obtained by geometry deduction that(4)12a→k×d→kr=12vlω1−cosθk+1−θk,12a→k+1×d→kh=12vlωcosθk+1−θk−1+l2sinθk+1−θk.Figure 2
The area swept by the road-header in each displacement.Equations (3) and (4) show that Ak is related to v and ω. Therefore, the trajectory planning is somehow a kind of position and orientation scheduling by the configuration of a set of suggested v and ω that comply with the following conditions:(5)∑k=0nvkωksinθk+1−sinθk=Xd−X0,∑k=0nωk⋅Δt=θd−θ0,min∑k=0nAk,where Xd and X0 are the destination and current X coordinates, respectively, of a certain particle on the vehicle, saying the centroid and θd and θ0 are the destination and current orientation angle, respectively. Each adjustment with certain pair of v and ω produces the location state of the road-header sk, and then the scheduled trajectory is obtained.Equation (5) alone cannot result in a trajectory for practical, some operational requirements need to be considered generally, including ① Whether the head of the road-header being allowed to go over the center line in the process? ② Is the value of constrained to be less than a certain number, saying 6? ③ Is the Y coordinate of the final point on the planned trajectory required to be less or further than a certain value? ④ The value of and are constant or not? Are they limited numerically or not?Suppose the centre line of the aimed roadway locates along with the lineX=2; the road-header is originally at a wrong pose denoted by s0=0.25,−1.3,π/3,1.27,1.3. Figure 3 presents two feasible trajectories for the road-header to follow, with different considerations that are listed in Table 1, where the values of some necessary parameters are also presented.Figure 3
Two feasible trajectories corresponding to different considerations.Table 1
Comparison of the planned Trajectory 1 with Trajectory 2.
The length of the road-headerl=3m; heading speed v<0.8m/Δt
Considerations or requirements
Planned trajectory 1
Planned trajectory 2
Considerations ①
Yes
No
Considerations ②
≤6
≤6
Considerations ③
No special requirements
No special requirements
Considerations ④
v is a constant to be 0.5m/Δt while Δω≤0.04rad/Δt
Bothv and ω are constant: v=0.4m/Δt, ω=0.02rad/ΔtIn Figure3, the road-header is simplified as a line segment composed of three points: the centroid, the front, and rear points. The centre line of the aimed roadway is illustrated by X=2; sk presents the original pose of the vehicle. Both the two curves marked by trajectory 1 and trajectory 2 can guide the vehicle back on to the aimed roadway, but with different considerations. The three marks on the right-hand side of the aimed roadway are the locus of the front point of the road-header when it moves following trajectory 1. It implies that the head of the vehicle is allowed to be over the centre line during path correction, which is also explained in Table 1.The path planning introduced above is offline at the current stage, while in the future real experiments it is expected to be online. The primary solution for online path planning is to set threshold values over the position and orientation errors and check the tracking effects based on the current planned path, so as to trigger the proceeding of path renewal on time.For example, under certain initial condition, the first step is to check if there is a planned trajectory given for tracking. If the answer is no, a new trajectory is planned immediately. Recall that the planned trajectory includes two parts: the transition section which locates between the current position and the aimed roadway and the displacement section which coincides with the aimed roadway. In the subsequent vehicle adjustment, the trajectory would be renewed if any of the following two cases appear: firstly, when the position and orientation errors go beyond the corresponding thresholds and secondly, the path following based on the current planned trajectory was failed.


## 4. Path Tracking of the Road-Header

Although given a scheduled trajectory with a set of position, orientation, and suggested heading velocityv together with turning angular speed ω, it is hard to make the road-header follow the trajectory exactly only by carrying out the referred v and ω step by step. Hence, an alternative practice is to track only the position and orientation in the schedule, but adjust the turning speed ω and moving velocity v accordingly.


### 4.1. Path Tracking Control

The tracking control of the road-header was carried out based on the position and orientation deviation model, which is demonstrated below with the help of Figure4.Figure 4
Tracking control based on the position and orientation deviation model.For a compact illustration like in Figure4, suppose the projective point of the instant steering centre on the centre line of the vehicle is the concerned centroid. A triangle with this centroid as its apex is used to display the pose (position of the centroid and orientation of the vehicle) of the road-header; the dotted triangle represents the scheduled next pose of the road-header.The parameters such asb,v,φ,θ,andω have the same meaning that are introduced in the previous section. x,y and X,Y are the coordinates in the vehicle coordinate system xoy and geodetic coordinate system XOY, respectively.Denote the current pose of the centroid asPc=Xc,Yc,θcT and the destined pose as Pd=Xd,Yd,θdT, and then their difference in the vehicle coordinate system is(6)Pe=xeyeθe=cosθcsinθc0−sinθccosθc0001Xd−XcYd−Ycθd−θc.Taking the differential ofxe:(7)x˙e=X˙d−X˙ccosθc+Y˙d−Y˙csinθc−Xd−Xcθ˙csinθc+Yd−Ycθ˙ccosθc=X˙dcosθc+Y˙dsinθc−X˙ccosθc−Y˙csinθc+yeωc.NoticeX˙ccosθc+Y˙csinθc=x˙c=vc and consider X˙dsinθd+Y˙dcosθd=y˙d≈0; using θd−θe to replace θc, it turns out that(8)x˙e=X˙dcosθd−θe+Y˙dsinθd−θe−vc+yeωc=cosθeX˙dcosθd+Y˙dsinθd+sinθeX˙dsinθd+Y˙dsinθd−vc+yeωc=vdcosθe−vc+yeωc.Similarly, the differential ofPe is obtained as follows:(9)P˙e=x˙ey˙eθ˙e=vdcosθe−vc+yeωc−xeωc+vdsinθeωd−ωc.Based on Equations (6) and (9), a control law updating currant heading velocity vc and steering angular speed ωc is in need to realize Pe⟶0,0,0T within limited steps, which is the path correction in this work. The control law could be obtained by defining switching function and using the backstepping approach:Whenxe⟶0, investigate the Lyapunov function Vy=1/2ye2 by looking at(10)V˙y=yey˙e=ye−xeωc+vdsinθe.IfVy≥0 and V˙y<0, then limn⟶∞ye=0. In practice, the gap between the current and desired heading angle is θe∈−π/2,π/2, which means θe⋅sinθe≥0; therefore, if k0yevd=−θe, where k0 is a positive random coefficient, then V˙y≤0 (V˙y=0 only when θe=0 or ye=0). In other words, if xe⟶0 and θe⟶−k0yevd, then ye⟶0.Design the switch function as(11)S=s1s2=xeθe+k0yevd.Letsi⟶0,i=1,2 obeying exponential reaching law, i.e., s˙i=−kisi in which ki is a positive random coefficient.(12)S˙=s˙1s˙2=−k1s1−k2s2=−k1xe−k2θe+k0yevd=x˙eθ˙e+k0yevd′.Substitute equation (20) into equation (21) and get the controller about vc and ωc as follows:(13)vc=k1xe+vdcosθe+yeωc,1+k0vdxeωc=ωd+k0yev˙d+k0vd2sinθe+k2θe+k0yevd.It could be simplified as the following, by considering that (1)ωc is very small comparing with xe; (2) v˙d≈0 ; and (3) sinθe≈θe.(14)vc=k1xe+vdcosθe,1+k0vdxeωc=ωd+k0vd2+k2sinθe+k0k2yevd.Ifk0 is taken small enough to make k0vdxe≪1, equation (21) is equivalent to(15)vc=k1xe+vdcosθe,ωc=ωd+k0vd2+k2sinθe+k0k2yevd.The control function is finally given by(16)u=vcωc=kxxe+vdcosθeωd+kθsinθe+kyyevd,where kx=k1, ky=k0k2 and kθ=k0vd2+k2; they are random positive coefficients according to the fact that ki,i=1,2 are positive random coefficients, which stand beside the pose errors xe, ye, and θe, respectively.According to the Lyapunov stability criterion, if a suitable Lyapunov functionVxe,ye,θe≥0 is found out and it fulfills V˙xe,ye,θe<0 based on equations (6), (9), and (16), then Pe is limited and limn⟶∞Pe=0.Take the Lyapunov function asV=1/2xe2+1/2ye2+2ksin2θe/2, where k is a random positive number, and consider the differential of V:(17)V˙=xex˙e+yey˙e+2kθ˙esinθe2cosθe2=xevdcosθe−vc+yeωc+ye−xeωc+vdsinθe+kωd−ωcsinθe=−xevc+vdxecosθe+vdyesinθe+kωd−ωcsinθe.Substituting equation (14) into V˙, it is obtained that(18)V˙=−kxxe2+vdyesinθe−kkyyevdsinθe−kkθsin2θe.From equation (18), it is obvious that if kky=1 is taken, then V˙=−kxxe2−kkθsin2θe≤0, where V˙=0 only when xe=0 and θe=0. Therefore, Pe is limited and expected to converge to zero as long as xe⟶0 and θe⟶0 which are driven by the equation (13) and its simplified form, i.e., equation (16).


## 4.1. Path Tracking Control

The tracking control of the road-header was carried out based on the position and orientation deviation model, which is demonstrated below with the help of Figure4.Figure 4
Tracking control based on the position and orientation deviation model.For a compact illustration like in Figure4, suppose the projective point of the instant steering centre on the centre line of the vehicle is the concerned centroid. A triangle with this centroid as its apex is used to display the pose (position of the centroid and orientation of the vehicle) of the road-header; the dotted triangle represents the scheduled next pose of the road-header.The parameters such asb,v,φ,θ,andω have the same meaning that are introduced in the previous section. x,y and X,Y are the coordinates in the vehicle coordinate system xoy and geodetic coordinate system XOY, respectively.Denote the current pose of the centroid asPc=Xc,Yc,θcT and the destined pose as Pd=Xd,Yd,θdT, and then their difference in the vehicle coordinate system is(6)Pe=xeyeθe=cosθcsinθc0−sinθccosθc0001Xd−XcYd−Ycθd−θc.Taking the differential ofxe:(7)x˙e=X˙d−X˙ccosθc+Y˙d−Y˙csinθc−Xd−Xcθ˙csinθc+Yd−Ycθ˙ccosθc=X˙dcosθc+Y˙dsinθc−X˙ccosθc−Y˙csinθc+yeωc.NoticeX˙ccosθc+Y˙csinθc=x˙c=vc and consider X˙dsinθd+Y˙dcosθd=y˙d≈0; using θd−θe to replace θc, it turns out that(8)x˙e=X˙dcosθd−θe+Y˙dsinθd−θe−vc+yeωc=cosθeX˙dcosθd+Y˙dsinθd+sinθeX˙dsinθd+Y˙dsinθd−vc+yeωc=vdcosθe−vc+yeωc.Similarly, the differential ofPe is obtained as follows:(9)P˙e=x˙ey˙eθ˙e=vdcosθe−vc+yeωc−xeωc+vdsinθeωd−ωc.Based on Equations (6) and (9), a control law updating currant heading velocity vc and steering angular speed ωc is in need to realize Pe⟶0,0,0T within limited steps, which is the path correction in this work. The control law could be obtained by defining switching function and using the backstepping approach:Whenxe⟶0, investigate the Lyapunov function Vy=1/2ye2 by looking at(10)V˙y=yey˙e=ye−xeωc+vdsinθe.IfVy≥0 and V˙y<0, then limn⟶∞ye=0. In practice, the gap between the current and desired heading angle is θe∈−π/2,π/2, which means θe⋅sinθe≥0; therefore, if k0yevd=−θe, where k0 is a positive random coefficient, then V˙y≤0 (V˙y=0 only when θe=0 or ye=0). In other words, if xe⟶0 and θe⟶−k0yevd, then ye⟶0.Design the switch function as(11)S=s1s2=xeθe+k0yevd.Letsi⟶0,i=1,2 obeying exponential reaching law, i.e., s˙i=−kisi in which ki is a positive random coefficient.(12)S˙=s˙1s˙2=−k1s1−k2s2=−k1xe−k2θe+k0yevd=x˙eθ˙e+k0yevd′.Substitute equation (20) into equation (21) and get the controller about vc and ωc as follows:(13)vc=k1xe+vdcosθe+yeωc,1+k0vdxeωc=ωd+k0yev˙d+k0vd2sinθe+k2θe+k0yevd.It could be simplified as the following, by considering that (1)ωc is very small comparing with xe; (2) v˙d≈0 ; and (3) sinθe≈θe.(14)vc=k1xe+vdcosθe,1+k0vdxeωc=ωd+k0vd2+k2sinθe+k0k2yevd.Ifk0 is taken small enough to make k0vdxe≪1, equation (21) is equivalent to(15)vc=k1xe+vdcosθe,ωc=ωd+k0vd2+k2sinθe+k0k2yevd.The control function is finally given by(16)u=vcωc=kxxe+vdcosθeωd+kθsinθe+kyyevd,where kx=k1, ky=k0k2 and kθ=k0vd2+k2; they are random positive coefficients according to the fact that ki,i=1,2 are positive random coefficients, which stand beside the pose errors xe, ye, and θe, respectively.According to the Lyapunov stability criterion, if a suitable Lyapunov functionVxe,ye,θe≥0 is found out and it fulfills V˙xe,ye,θe<0 based on equations (6), (9), and (16), then Pe is limited and limn⟶∞Pe=0.Take the Lyapunov function asV=1/2xe2+1/2ye2+2ksin2θe/2, where k is a random positive number, and consider the differential of V:(17)V˙=xex˙e+yey˙e+2kθ˙esinθe2cosθe2=xevdcosθe−vc+yeωc+ye−xeωc+vdsinθe+kωd−ωcsinθe=−xevc+vdxecosθe+vdyesinθe+kωd−ωcsinθe.Substituting equation (14) into V˙, it is obtained that(18)V˙=−kxxe2+vdyesinθe−kkyyevdsinθe−kkθsin2θe.From equation (18), it is obvious that if kky=1 is taken, then V˙=−kxxe2−kkθsin2θe≤0, where V˙=0 only when xe=0 and θe=0. Therefore, Pe is limited and expected to converge to zero as long as xe⟶0 and θe⟶0 which are driven by the equation (13) and its simplified form, i.e., equation (16).


## 5. Tracking Control Based on State Estimation

Define a state vector for the road-header, such asQ=xe,ye,θe,ωL∗,ωR∗T, where xe,ye,θeT is the calculated pose (position and orientation) error which was introduced in Section 4; ωL∗,ωR∗ given by ωL∗,ωR∗T=ωL1−iL,ωR1−iRT, presenting the approximately real rotating speeds of the driving wheels resulted by the recommended heading velocity and turning angular speed considering the effects of track slips. By updating u=v,ωT according to the observed pose error, the real pose error is then driven to zero regularly.Apparently, the tracking control of the road-header expressed by equations (6), (9), and (16) could be rewritten by the following nonlinear system:(19)Qk+1=Qk+ΔQk+εk,Pe_k+1=hQk+δk+1,where εk is the process noise within the tracking control implementation and δk+1 is the observation error of the deviation between real and planned pose of the road-header. Since the real pose is acquired by measurements, δk+1 is equivalent to measure noise. Hence, the nonlinear system represented by equation (18) can be treated as a joint estimation problem for which the nonlinear filtering method is widely introduced for solution. Figure 5 shows the structure that nonlinear filtering is implied for joint estimation in this paper.Figure 5
The general structure that nonlinear filtering is involved.


### 5.1. Measurement and Process Noises

Fu et al. [15, 16] built up an ultra-wideband (UWB) pose detection system (UPDS) to realize unmanned pose detection based on the principle of local positioning for the working road-header. It was proved by a range of accuracy experiments that the mean error and standard deviation of the measured distances are below 2 cm. The error of the obtained 3D coordinates vary linearly from 4 mm to 3 cm within the narrow and closed tunnel as the length of the tunnel increases from 10∼95 m; the corresponding error of the obtained three attitude angles vary linearly from 0.2° to 1.5°. Based on that, it is assumed in this paper that the measurement noises include two parts: one is the Gaussian noise with zero mean value and 3 cm variance, which is in regard to the measurement error of xe and ye in the state vector Q in equation (19). The other is the Gaussian noise with zero mean value and 1.5° variance, which is in regard to the measurement error over the parameter θe in Q.Since the slipping over the tracks contributes the most on the mismatch between the real and referred values of forwarding velocity and turning angular speed, the process noise here in the linear system expressed by equation (19) is represented by the estimation of the slipping rate which highly depends on the geological conditions and the physical characters of the tracks, which is known as the Bekker theory introduced in [17].Han [17] presented the research on the relationship between the traction force and slipping ratio of the tracks on a deep-sea tracked miner. It proposed that for the track with certain touching length against certain kind of soil, it is possible to produce maximum traction force by controlling the torque of the hydraulic motor of the driving system. In the best scenario, namely the maximum traction force vs. slipping ratio i, the theoretical value of the slipping ratios are believed to reflect the general slipping level under certain situations.Inspired by that, Zhang et al. [14] discussed the approximate slipping ratios of several different soil conditions. These obtained slipping rate scales are used in this paper to conclude an assumption about the distribution of the process noise: the slipping rate varies obeying the uniform distribution with boundary values at [0.1, 0.35].To summarize, the distribution character of measurement and process noises are presented in Table2.Table 2
The process and measurement noises.
Noises
Specification
Process noise
Represented by the slipping rate which is assumed to obey the uniform distribution with boundary values at [0.1, 0.35]
Measurement noise
Includes two parts: one is the measurement position error which is assumed as the Gaussian noise with zero mean value and 3 cm variance; the other is the measurement orientation error which is assumed as the Gaussian noise with zero mean value and 1.5° variance


### 5.2. SVD-Unscented Kalman Filtering

For a brief explanation, equation (19) is rewritten as follows:(20)xk+1=fxk,uk+εk,yk+1=hxk+δk+1,which is a more general form used to discuss about state estimation. The main idea about unscented Kalman filtering (UKF) in state estimation is to approximate the probability distribution of the concerned parameters directly by nonlinear transformation, instead of approximating the linearized model of the system in which the parameters are involved [18].For unscented Kalman filtering (UKF), to realize states estimation without derivative operations, the unscented transform (UT) is commonly used to calculate the statistical properties of random variables that are formulated by nonlinear expressions. To explain the dilemma during applying UKF in this paper, a brief introduction about UT is given as follows.For a random vector, sayingy, which is a nonlinear transformation of an N-dimensional independent variable x by fx, take PXN×N as the covariance matrix of x and x¯ as the mean of x. To obtain the statistics of y, a sigma-point vector matrix χ is required and is formed by using the sigma-point sampling method in usual:(21)χ=x¯i=0x¯+N+λPXii=1,…,Nx¯−N+λPXii=N+1,…,2N,where λ=α2N+κ−N is the scale parameter, in which α decides the diffusion range of the sigma points and is a small positive constant; κ is a subscale parameter taking value at 0 in general state estimation or at 3-N in parameter estimation; N+λPXi means the ith column of the weighted square root matrix N+λPX.In normal cases,PX is a positive definite symmetric matrix, and to figure out N+λPX, take the eigenvalue decomposition of PX: suppose that PX=QΛQ−1 (where Q is a N × N matrix with the eigenvectors of PX as its columns; Λ is the diagonal matrix with the corresponding eigenvalues as its diagonal elements); hence, PX1/2=QΛ1/2Q−1. On the other hand, for a positive definite symmetric matrix, its eigenvalue decomposition is equivalent to its SVD decomposition: suppose that PX=UΛV−1 (where U=V=Q , and QTQ=I is the same as just introduced above); therefore, PX1/2=UΛ1/2VT.Notice that equation (21) stands with a precondition that PX is a positive definite symmetric matrix. However, a morbid covariance matrix is often encountered in the implementation of UKF, i.e., the covariance matrix fails to be positive definite symmetric irregularly during filtering. To solve this dilemma, SVD decomposition is always taken instead of eigenvalue decomposition in the Unscented Transform, which is the main idea about the SVD-unscented Kalman filtering (SVD-UKF).With the SVD-unscented Kalman filtering, an estimated statex^k+1a is taken instead of one xk+1, which stands for the estimated value of Q=xe,ye,θe,ωL∗,ωR∗T in equation (19). It is then put back into equation (16) to revise control variables vc and ωc and finally result in ωL∗,ωR∗T.


## 5.1. Measurement and Process Noises

Fu et al. [15, 16] built up an ultra-wideband (UWB) pose detection system (UPDS) to realize unmanned pose detection based on the principle of local positioning for the working road-header. It was proved by a range of accuracy experiments that the mean error and standard deviation of the measured distances are below 2 cm. The error of the obtained 3D coordinates vary linearly from 4 mm to 3 cm within the narrow and closed tunnel as the length of the tunnel increases from 10∼95 m; the corresponding error of the obtained three attitude angles vary linearly from 0.2° to 1.5°. Based on that, it is assumed in this paper that the measurement noises include two parts: one is the Gaussian noise with zero mean value and 3 cm variance, which is in regard to the measurement error of xe and ye in the state vector Q in equation (19). The other is the Gaussian noise with zero mean value and 1.5° variance, which is in regard to the measurement error over the parameter θe in Q.Since the slipping over the tracks contributes the most on the mismatch between the real and referred values of forwarding velocity and turning angular speed, the process noise here in the linear system expressed by equation (19) is represented by the estimation of the slipping rate which highly depends on the geological conditions and the physical characters of the tracks, which is known as the Bekker theory introduced in [17].Han [17] presented the research on the relationship between the traction force and slipping ratio of the tracks on a deep-sea tracked miner. It proposed that for the track with certain touching length against certain kind of soil, it is possible to produce maximum traction force by controlling the torque of the hydraulic motor of the driving system. In the best scenario, namely the maximum traction force vs. slipping ratio i, the theoretical value of the slipping ratios are believed to reflect the general slipping level under certain situations.Inspired by that, Zhang et al. [14] discussed the approximate slipping ratios of several different soil conditions. These obtained slipping rate scales are used in this paper to conclude an assumption about the distribution of the process noise: the slipping rate varies obeying the uniform distribution with boundary values at [0.1, 0.35].To summarize, the distribution character of measurement and process noises are presented in Table2.Table 2
The process and measurement noises.
Noises
Specification
Process noise
Represented by the slipping rate which is assumed to obey the uniform distribution with boundary values at [0.1, 0.35]
Measurement noise
Includes two parts: one is the measurement position error which is assumed as the Gaussian noise with zero mean value and 3 cm variance; the other is the measurement orientation error which is assumed as the Gaussian noise with zero mean value and 1.5° variance


## 5.2. SVD-Unscented Kalman Filtering

For a brief explanation, equation (19) is rewritten as follows:(20)xk+1=fxk,uk+εk,yk+1=hxk+δk+1,which is a more general form used to discuss about state estimation. The main idea about unscented Kalman filtering (UKF) in state estimation is to approximate the probability distribution of the concerned parameters directly by nonlinear transformation, instead of approximating the linearized model of the system in which the parameters are involved [18].For unscented Kalman filtering (UKF), to realize states estimation without derivative operations, the unscented transform (UT) is commonly used to calculate the statistical properties of random variables that are formulated by nonlinear expressions. To explain the dilemma during applying UKF in this paper, a brief introduction about UT is given as follows.For a random vector, sayingy, which is a nonlinear transformation of an N-dimensional independent variable x by fx, take PXN×N as the covariance matrix of x and x¯ as the mean of x. To obtain the statistics of y, a sigma-point vector matrix χ is required and is formed by using the sigma-point sampling method in usual:(21)χ=x¯i=0x¯+N+λPXii=1,…,Nx¯−N+λPXii=N+1,…,2N,where λ=α2N+κ−N is the scale parameter, in which α decides the diffusion range of the sigma points and is a small positive constant; κ is a subscale parameter taking value at 0 in general state estimation or at 3-N in parameter estimation; N+λPXi means the ith column of the weighted square root matrix N+λPX.In normal cases,PX is a positive definite symmetric matrix, and to figure out N+λPX, take the eigenvalue decomposition of PX: suppose that PX=QΛQ−1 (where Q is a N × N matrix with the eigenvectors of PX as its columns; Λ is the diagonal matrix with the corresponding eigenvalues as its diagonal elements); hence, PX1/2=QΛ1/2Q−1. On the other hand, for a positive definite symmetric matrix, its eigenvalue decomposition is equivalent to its SVD decomposition: suppose that PX=UΛV−1 (where U=V=Q , and QTQ=I is the same as just introduced above); therefore, PX1/2=UΛ1/2VT.Notice that equation (21) stands with a precondition that PX is a positive definite symmetric matrix. However, a morbid covariance matrix is often encountered in the implementation of UKF, i.e., the covariance matrix fails to be positive definite symmetric irregularly during filtering. To solve this dilemma, SVD decomposition is always taken instead of eigenvalue decomposition in the Unscented Transform, which is the main idea about the SVD-unscented Kalman filtering (SVD-UKF).With the SVD-unscented Kalman filtering, an estimated statex^k+1a is taken instead of one xk+1, which stands for the estimated value of Q=xe,ye,θe,ωL∗,ωR∗T in equation (19). It is then put back into equation (16) to revise control variables vc and ωc and finally result in ωL∗,ωR∗T.


## 6. Simulation

In this section, path correction simulation is carried out over a road-header with the initial poses0=0.25,−1.3,π/3,1.27,1.3, as it is shown in Figure 2, being out of the predesigned roadway. In the first case, process and measurement noises are not considered; the simulation is to verify the control procedure described by equations (2), (6), (9), and (16) and using the planned trajectories presented in Table 1 and Figure 3. The other data used for simulation are collected in Table 3. The values of kx, ky, and kθ were initially selected simply by trial and error.Table 3
The used data in simulation.
Items
Value or specification
Radius of the driving wheelr
0.25 m
Distance between the two tracksb
1 m
Width of the trackB
320 mm
Length of the track where it touches the groundl
3 m
Maximum heading velocityvmax
0.8
m
/
Δ
t
Referred velocity
0.6
m
/
Δ
t
Maximum rotating speed of the driving wheel
4
rad
/
s
Angular acceleration rangeβ1,β2
0,0.4
Trajectory 1
kx
0.76
ky
0.67
kθ
0.88
Trajectory 2
kx
0.76
ky
0.48
kθ
0.74
Initial pose of the road-header
0.25
,
−
1.3
,
π
/
3
,
1.27
,
1.3
Aiming pose
Onto the line withx = 2Figure6 illustrates the path correction results using planned trajectory 1 including (1) the offsets between the planned path and real tracking; (2) the recommended angular speeds of the driving wheels on left and right sides, respectively; and (3) the variance of position and orientation errors, respectively, during the procedure.Figure 6
The simulation about path correction using the planned trajectory 1. (a) Planned path and tracking. (b) Angular speed of the driving wheels. (c) Position and orientation errors.
(a)
(b)
(c)It is shown that firstly, the real tracking path sticks to the planned path well, which is also proved by the view that the position and orientation errors reduce to zero within limited adjusting steps (totally 13 steps). Secondly, the variance of the recommended angular speed of the driving wheel from any side is smooth and less during the whole schedule, which is preferred because vivid changing may increase mechanical wear or produce unpredictable slipping of the tracks.More detail is about the suddenly risen position error along theY-coordinate at tracking step No. 8. The possible reason is that when the road-header had just transited onto the target roadway, the turning angular speed ωc could not decrease to zero immediately according to equation (16), while the heading velocity vc is reaching the desired one vd for straight driving. This results in position offset against the ahead planned location. Nevertheless, the road-header drives back onto the right way regularly in the next 3∼5 steps under control.Path correction simulation using planned trajectory 2 is carried out, and the results are shown in Figure7.Figure 7
The simulation about path correction using the planned trajectory 1. (a) Planned path and tracking. (b) Angular speed of the driving wheels. (c) Position and orientation errors.
(a)
(b)
(c)By using different trajectory, it is shown that the recommended angular speeds of the driving wheels as well as the position and orientation errors in the process are quite different from their counterparts in Figure6. Besides, although the tracked path caters to the planned path well in Figure 7(a), the position error ye shown in Figure 7(c) fails to converge to zero within 14 adjustment steps, which means the path correction in this case is unsatisfied compared with the simulation presented by Figure 6.Notice that the values ofkx, ky, and kθ used in this case are the same as that was used in previous simulation. A nature action against the unsatisfied position and orientation errors is to adapt these parameters by trial and error. The renewed values of these parameters are listed in Table 3, and the corresponding path correction simulation results using planned trajectory 2 are shown in Figure 8.Figure 8
The simulation about path correction using the planned trajectory 2 and revised parameters. (a) Planned path and tracking. (b) Angular speed of the driving wheels. (c) Position and orientation errors.
(a)
(b)
(c)Similar simulation results are obtained but with better tracking results, namely, the tracking errors become zero within less adjustment steps. It is also shown that there is obvious difference about the variance of the recommended angular speeds of the driving wheels when using different trajectories, by comparing Figure8 with Figure 6. The variance presented in Figure 8-“Angular speeds of the driving wheels” is much bigger and sharper than that when using planned trajectory 1. Therefore, trajectory 1 is better than trajectory 2 in this case considering possible mechanical wear and unpredictable track slipping.In the second set of path correction simulation, trajectory 1 is used and the process and measurement noises are considered. To evaluate the efficiency of using SVD-unscented Kalman filtering in the path correction, simulation is carried out with and without filtering, respectively. The corresponding simulation results are illustrated separately in Figures9 and 10. The details about noises are explained in Section 5.Figure 9
The path correction simulation using trajectory 1 and considering noises without filtering. (a) Planned path and tracking. (b) Angular speed of the driving wheels. (c) Position and orientation errors.
(a)
(b)
(c)Figure 10
The path correction simulation considering noises and applying SVD-UKF. (a) Planned path and tracking. (b) Angular speed of the driving wheels. (c) Position and orientation errors.
(a)
(b)
(c)Figure9 shows that without appropriate processing against noises in the tracking control, the path correction can hardly be successful.Comparatively, Figure10 shows that the usage of SVD-unscented Kalman filtering improves the tracking accuracy obviously by estimating the pose deviation and rewriting the referred control variables. The planned and real pose schedules shown in Figure 10(a) are very close. Also, the variance of referred angular speeds of the driving wheels is small and smooth, even better than the case shown in Figure 6.The position errorsxe and orientation error θe are reducing to zero after the first 6 steps, but the other position error ye is not. Instead, it varies around 0.13 m. In this situation, simple adaptations over the parameters kx ky, and kθ in the simulation are carried out but with no obvious improvement. Recall that the designed roadway is on the line presented by x = 2, and the first requirement in path correction is to drive the road-header onto the line and make the orientation angle coinciding with the gradient of the line. Therefore, to avoid the situation that xe or θe being too far, the forward velocity is updated restrainedly; consequently, the position error ye as shown in Figure 10(c) is tolerable. However, research on how to reduce the tracking error as in this case is continued in our short future study.


## 7. Conclusion

A whole solution for path correction is proposed in this paper for the working road-header aiming to form an accurate roadway underground. The simulations show the following:(1)
Trajectory planning is necessary because different trajectories result in different scenarios about undesirable excavation space, slipping level, and power consumption(2)
The proposed tracking control algorithm guides the road-header back to the aimed roadway in limited adjusting steps effectively, with smoothly varying rotating speeds and regularly descending position and orientation errors(3)
The proposed SVD-unscented Kalman filtering effectively avoids the highly possible situation that calculation is interrupted as long as the updating covariance matrix becomes morbid(4)
The impacts from process and measurement noises were weakened obviously due to the usage of SVD-unscented Kalman filtering.Because of kinematic constraints, the position errorye fails to converge to zero exactly; however, the real adjustment track sticks to the planned trajectory closely. The whole path correction procedure proposed in this paper for the yawed road-header underground is feasible and potentially practicable.



---

*Source: 5351474-2019-10-23.xml*