At present, the deviation correction strategy of the roadheader is to track the roadway centerline as the target path. Tracking accuracy and speed are the evaluation criteria of tracking algorithms. However, through the analysis of the tunneling technology, it is found that the coal wall will be cut only when the roadheader reaches the end position, while the coal wall will not be cut during the forward movement of the roadheader. Therefore, the forming accuracy of the roadway section has nothing to do with the position accuracy of the roadheader during driving but is related to the pose of the roadheader at the endpoint (i.e., the starting point of the next driving cycle).
In addition, the weight of the roadheader is much larger than that of the traditional mobile robot. Therefore, in the process of correcting the deviation, the control input should be reduced as much as possible to improve the smoothness of the tracking track and the stability of the roadheader body motion. In this paper, the control strategy of point position deviation correction is proposed to ensure the accuracy of deviation correction and improve the stability of roadheader movement in the process of deviation correction. The point deviation correction control strategy includes the three steps: the determination of the starting point and endpoint; path planning from the starting point to the endpoint and tracking of the the planned path.
(1). Determine the starting point and end point;
(2). Path planning from the starting point to the end point;
(3). Track the planned path.
2.2.1. Prediction Method of the Driving Cycle EndPoint
The starting point and endpoint of motion control must be determined before the roadheader body is automatically calibrated. The starting point is the center coordinate of the roadheader body at the initial position of the cycle, which can be measured by the pose detection system. However, the selection of the endpoint needs to consider the requirements of the roadheader motion stability during the deviation correction process, and avoid large changes in the angle speed of the roadheader.
In this paper, we propose a path endpoint prediction method based on the initial state. The basic principle of this method is to predict the end position according to the position and angle of the roadheader at the starting point, and to predict the end position without control input. If the position deviation of the prediction point is less than the initial deviation, the prediction point is set as the endpoint of the cycle. If the deviation of the predicted point is larger than the deviation of the starting point, the endpoint of the cycle is set on the roadway centerline. If the starting point deviation is too large, the roadheader deviation can be corrected in stages within multiple driving cycles. The predicted deviation is less than the initial deviation (|e1| ≤ |e0|).
- (1)
The predicted deviation is less than the initial deviation (|e1| ≤ |e0|)
As shown in
Figure 3, O
0 is the starting point of the roadheader in the ideal state, O
1 is the endpoint of the roadheader in the ideal state, located on the Y axis, and its y coordinate is the distance of the driving footage of each cycle. P
0 is the actual starting point, that is, the current position of the roadheader; α
0 is the yaw angle of the roadheader at the starting point and e
0 is the deviation of the body position of the roadheader at the starting point. Furthermore, P
1 is the endpoint predicted according to the pose of the roadheader at the starting point;
e1 is the deviation of the prediction point and
Lc is the driving cycle footage.
The position deviation of the roadheader is indicated to tend to decrease by|e1| ≤ |e0|. In order to stabilize the movement of the roadheader, the control input should be reduced as much as possible. The initial speed and direction of the roadheader remain unchanged and the forward travel distance Lc reaches the point P1. If this point is set as the endpoint, the coordinates of the endpoint are (e0-Lctan α0, Lc).
- (2)
The predicted deviation is greater than the initial deviation, and the initial deviation is less than the maximum deviation correction amount (|e1| > |e0| and |e0| ≤ [e]).
When the lateral (X-axis) deviation at the starting point is large, if you try to eliminate the lateral deviation within a driving cycle, it may affect the flatness of the roadway. Defining [
e] as the maximum adjustment amount allowed for a single drive cycle, signifies [
e] as the X-axis difference between the start and end of the cycle. Since |
e0| ≤ [
e] indicates that the starting point deviation of the roadheader at the starting point is small, the roadheader position deviation can be adjusted to zero within one cycle. In fact, |
e1| > |
e0| indicates that the position deviation of the roadheader continues to increase. At this time, the target point
P1 is set on the Y-axis with coordinates (0,
Lc), as shown in
Figure 4.
- (3)
The predicted deviation is greater than the initial deviation, and the initial deviation is greater than the maximum correction amount (|e1| > |e0| and |e0| > [e]).
While
e1 >
e0 indicates that the position deviation of the roadheader continues to increase, |
e0| > [
e] indicates that the deviation of the anchor point of the windlass at the anchor point is large, which is not suitable for eliminating the lateral deviation of the roadheader in a driving cycle. If the maximum allowable adjustment amount in a single cycle is [
e], then the coordinate of the trajectory endpoint
P1 is (
e1 ± [
e],
Lc). When
e1 is greater than zero, the negative sign is taken and when
e1 is less than zero, the positive sign is taken, as shown in
Figure 5.
2.2.2. Path Planning Research
The main research aim is to generate a path from the starting point
P0 to the endpoint
P1 of the driving cycle, which satisfies the kinematic constraints, as a reference for driving the body motion of the roadheader. To ensure that the path is feasible, the following constraints should be met [
28]:
The first derivative of the path curve function is continuous.
The tangent of the starting point on the path is consistent with the initial speed direction of the roadheader, and the tangent direction of the endpoint on the path is consistent with the roadway centerline (i.e., Y axis).
The curvature of each point on the path should be continuous and bounded.
The trajectories satisfying the above conditions include plane curves, such as polynomial fitting curves and Bezier curves.
Polynomial Fitting Path
If the above constraint conditions are to be met, at least cubic polynomial curve shall be used. The general formula of a polynomial is:
The classification and discussion according to different cycle endpoints are as follows:
At this time, the target point
P1 is set on an extension line in the initial speed direction. Clearly, the planned path is a straight line, starting from
P0 and ending at
P1. The curve equation is:
- (2)
|e1| > |e0| and |e0| ≤ [e]
As shown in
Figure 6, the target path is a cubic polynomial curve connecting the start point
P0 and the endpoint
P1.
According to the constraint conditions, the polynomial boundary conditions are:
The first two constraints represent the position constraints of the roadheader at the starting point and the endpoint, respectively. The last two constraints represent the orientation constraints of the roadheader at the start and endpoints. In order to avoid the attitude sudden change of the roadheader when stared, the tangent vector of the start point is consistent with the direction of the initial speed. In order to correct the heading of the roadheader, the tangent vector at the endpoint is consistent with the Y axis.
By substituting the boundary condition (5) into the general Formula (4), the coefficient can be obtained
- (3)
|e1| > |e0| and |e0| > [e]
As shown in
Figure 7, the solution of the curve equation is similar to the second case, and the boundary conditions are:
By substituting the boundary condition (7) into Equation (4), the coefficients can be obtained
Figure 7.
The cubic polynomial path when |e1| > |e0| and |e0| > [e].
Figure 7.
The cubic polynomial path when |e1| > |e0| and |e0| > [e].
The Bezier Curve Path
The Bezier curve is a two-dimensional curve obtained by a recursive method according to the positions of starting point, endpoint and control point. The shape of the Bezier curve can be changed by adjusting the position of the control points. The n control point corresponds to the Bessel curve of order n − 1. The third-order Bessel curve has the following important characteristics: the derivative of the curve at t = 0 is the same as the slope (derivative) of P0C1, and the derivative at t = 1 is the same as the slope (derivative) of C2P1. Therefore, the third-order Bessel curve meets the requirements of path planning.
The third-order Bezier curve needs two control points, C1 and C2. Adjusting the distances from the control points (C1 and C2) to the starting point and the endpoint changes the shape of the curve accordingly. The distance from the control point to the starting point (endpoint) is defined as KsLc, where Lc is the driving cycle footage and Ks is the shape coefficient of Bessel curve, which is taken from 0 to 1. Different shape coefficients correspond to different curve shapes. The classification and discussion according to different cycle endpoints are as follows:
The yaw angle of the roadheader at the start point on the target path is the same as that at the endpoint.The Bezier curve degenerates into a straight line, and the target path is a straight line between the start point P0 and the endpoint P1.
- (2)
|e1| > |e0| and |e0| ≤ [e]
In order to meet the requirements of the automatic deviation correction trajectory constraint of the roadheader, the control point
C1 is selected on the tangent of the curve starting point, that is, the extension line of the initial speed direction. The control point
C2 is selected on the tangent line at the end of the curve, that is, on the
Y-axis, as shown in
Figure 8.
The start and end coordinates are P0(e0,0) and P1(0,Lc), respectively. The distance between the control point C1 and the starting point P0, and the distance from control point C2 to endpoint O1 are KsLc. The coordinate of the control point is (C1 (K = Lc,e0 + KsLctanα0), C2 (Lc − KsLc,0)).
- (3)
|e1| > |e0|and|e0| > [e]
Similarly to the second case, the control point
C1 is selected on the tangent of the curve starting point, that is, the extension line of the initial speed direction. The control point
C2 is selected on the parallel line between point
P1 and the Y axis, as shown in
Figure 9.
The coordinate of the control point C2 is (Lc − KsLc, e0 ± [e]), and the rest is the same as the second case.
2.2.3. Research on the Trajectory Tracking Algorithm
Trajectory tracking control methods include the PID tracking model, the pure tracking model and the neural network. The PID tracking control algorithm is relatively mature, but the optimal control parameters are complex, and it is difficult to obtain the optimal parameters under complex conditions. The neural network control algorithm needs to collect a large number of samples for training and verification, which is difficult to realize [
29]. The pure tracking algorithm adopts the preview following principle, and the tracking process is very smooth, which meets the requirements of the windlass deviation correction for stability. To sum up, this paper uses the pure tracking algorithm to track the planned path.
The pure tracking algorithm works via the curvature of the computer body moving from the current position to the target point, as shown in
Figure 10. Firstly, a target point is selected, which is located at a certain distance in front of the roadheader. The target trajectory is discretized to obtain the tracking target point sequence G
k (
xgk′,
ygk′)
k = 1, 2, 3 ……. X
0O
0Y
0.
Figure 10 represents the roadway coordinate system, and XBOBYB represents the roadheader coordinate system. G is an arbitrary target point on the planning path, and (
xg,
yg) is the coordinate of the target point in the roadheader coordinate system. The yaw angle is
α,
φ is the angle at which the roadheader rotates from the current point O
B to the target point G along the arc and L
fc is the preview distance.
Assuming that the roadheader moves on a plane, the motion curvature of the roadheader body can be defined as the reciprocal of the distance from the center of the body to the instantaneous rotation center, and the sign is opposite to the sign of the motion radius. This entails taking the negative sign in the clockwise direction, and taking the positive sign in the reverse direction. The curvature also represents the instantaneous change of the relative movement distance in the roadheader direction, as shown in Equation (9) [
30].
It must be established that the coordinate system XBOBYB is attached to the roadheader, and the YB axis is consistent with the movement direction. The instantaneous center is always located on the XB axis in the roadheader coordinate system. According to the pure tracking control strategy, the curvature of the roadheader at the current position is equal to the radius of the fitting arc from the current point to the target point, and the distance from the target point to the current point becomes the preview distance [
31]. The arc is a tangent to the YB axis, so the coordinates of the target point in the roadheader body coordinate system can be calculated by the following formula:
where
φ is the angle that the roadheader body turns from the current point to the target point along the arc and
ρsp is the curvature of the current point.
The preview distance can be calculated by the following formula:
In accordance with (11) and (12), the roadheader body angle, is:
It should be noted that
xg and
yg are local coordinates under the roadheader body coordinate system, and the target point sequence is the global coordinates under the roadway coordinate system. Therefore, it is necessary to convert the global coordinates into local coordinates, that is
where
α is the yaw angle and (
x,
y) is the current position coordinate of the windlass body.
The state update equation of motion is:
where
ak is the acceleration, which can be calculated by the following formula
where
vg is the target speed,
vk−1 is the current speed and
K is the speed gain.