Abstract
Precise understanding of the mobility is essential for high performance autonomous tracked vehicles in challenging circumstances, though the complex track/terrain interaction is difficult to model. A slip model based on the instantaneous centers of rotation (ICRs) of treads is presented and identified to predict the motion of the vehicle in a short term. Unlike many research studies estimating current ICRs locations using velocity measurements for feedback controllers, we focus on predicting the forward trajectories by estimating ICRs locations using position measurements. ICRs locations are parameterized over both tracks rolling speeds and the kinematic parameters are estimated in real time using an extended Kalman filter (EKF) without requiring prior knowledge of terrain parameters. Simulation results verify that the proposed algorithm performs better than the traditional method when the pose measuring frequencies are low. Experiments are conducted on a tracked vehicle with a weight of 13.6 tons. Results demonstrate that the predicted position and heading errors are reduced by about 75% and the reduction of pose errors is over 24% in the absence of the real-time kinematic global positioning system (RTK GPS).
1. Introduction
Tracked vehicles are widely used in different areas such as military, agriculture, and planetary exploration due to their high mobility in unstructured environments [1]. The motion direction is changed by adjusting relative velocities of both tracks. However, complex track/ground interactions resulting from significant slippage during steering make it difficult to obtain an accurate motion prediction. The motion prediction is a numerical integration of the equations of motion, in which a slip model is used to determine how the vehicle will respond to the commanded track speed controls [2]. Prediction errors can be significantly reduced with an accurate slip model.
Fully mastering track/terrain interaction of track vehicles requires an in-depth understanding of terramechanics and vehicle dynamics, and much work has been completed by scholars [3]. However, this kind of models focuses on the mechanics of skid steering rather than the navigation of unmanned tracked vehicles [4, 5]. In autonomous vehicle applications, research about modeling of skid steer vehicle can be divided into two areas. The first area studies simplified dynamics of the vehicle-terrain interaction [6–8]. The core hypothesis of this area is that the tractive force can be expressed as a function of slip. The other area utilizes kinematic models and external measuring devices, such as visual odometry, inertial measurement units (IMUs), and global positioning system (GPS), to estimate slip or the instantaneous centers of rotation (ICRs) locations [5, 9–11]. In terms of real time, slip parameters can be identified offline and online. The slip coefficients are obtained as an exponential function of radius of curvature of the path based on experimental analysis in [1]. Reference [4] proposes an effective kinematics approximation and optimizes constant ICR positions for a particular terrain using a genetic algorithm. In [12], sliding parameters of agricultural tracked robot are estimated online based on the unscented Kalman filter. Further work for slip estimation online is done in [5, 13, 14], where the extended Kalman filter is utilized.
In this paper, we propose an online slip model identification method based on the work of Martínez et al. [4] and Seegmiller et al. [15]. Unlike the previously mentioned works, where ICRs locations are modeled as constant values, we parameterize ICRs locations over the lateral acceleration and curvature, which is more in line with reality because of the close relation between the ICRs locations and vehicle running conditions. ICRs provide a mapping between track speeds and vehicle body forward and angle velocity. An EKF is used to learn kinematic parameters online by utilizing the difference between the predicted pose change and actual experienced pose change from time to . This method is superior to the traditional method in that it becomes possible to estimate the slip and improve predicting accuracy only using low-cost and low-frequency sensors because parameters are updated using the measured position rather than the measured velocity. The new updated parameters are then used to predict the future motion of vehicle in a short term.
The remaining sections are organized as follows. Section 2 describes a kinematic model of tracked vehicle based on the ICRs of the tracks. In Section 3, we develop an EKF to estimate kinematic parameters and predict the future vehicle motion in a short term. Simulations are designed to test the proposed algorithm in Section 4. In Section 5, validation experiments with a real tracked vehicle and the results are presented. Finally, we conclude the paper and discuss future works in Section 6.
2. Slip Models of Tracked Vehicles Based on the ICRs
2.1. Kinematics Relationship
As mentioned earlier, due to uncertain soil parameters, the mechanics of tracks/ground interaction are not competent to navigation for autonomous tracked vehicles. Alternatively, we concentrate here on developing an effective kinematic model based on ICRs locations.
Figure 1 shows the geometric and kinematic relationships of a tracked vehicle when turning on a flat ground. The vehicle’s barycenter is assumed to be coincided with its geometric center. We define an inertial frame () and a local vehicle frame (). The origin of vehicle frame is located at the vehicle’s geometric center and its -axis is aligned with the forward motion direction. In the figure, () are forward and lateral velocity of the vehicle mass center and is its angular velocity. and are points in the contact surface of tracks with ground. and are vehicle body’s velocities at points and , also called convected velocity. and are tracks rolling speeds relative to the vehicle body. is the distance between tracks centerlines.
When moving on a plane, the vehicle can be regarded as a rigid body and its motion can be represented by a rotation around its instantaneous centers of rotation. In order to study the influence of slippage on vehicle motion, the tracks’ motion on the contact surface with ground cannot be ignored. Tracks can also be regarded as rigid bodies with one more degree of freedom than vehicle body [4]. This extra degree of freedom is its rolling speed. and denote ICRs of left and right tracks in the local body frame, respectively. The three ICRs locations lie along a line which is parallel to the local -axis.
As the motions of both tracks can be regarded as pure rotations around their instantaneous centers of rotations and , we write the absolute velocity of points and as
On the other hand, the motion of a point in the contact surface of tracks with ground is the composition of the vehicle body’s motion ( and ) and tracks rolling ( and ). So the absolute velocity of points and can also be expressed as
Given the forward velocity of the vehicle barycenter and angular velocity , the vehicle body’s velocities at points and are given by
Combining (1)~(3), the ICRs positions of vehicle body and both tracks are as follows:
Equations (4)~(7) reveal the underlying relationship between ICRs and the tracks rolling speed. Remarkably, the values of can reach infinity if the vehicle runs along a straight line. However, the values of , , and are bounded in any case. This is because of the fact that numerators and denominators in (5)~(7) are infinitesimals of the same order when the angular velocity is close to zero [4].
2.2. Slip Models Based on the ICRs
Without considering slippage, the equations of motion for differential driven vehicles are where is the vector that describes position coordinates and heading of the vehicle in the global inertial frame. denotes the east direction and denotes the north direction. This model is also called nominal kinematic model, where the forward velocity is directly from the tracks rolling speeds and the lateral velocity is equal to zero.
Given a set of tracks velocities sequence at time , the vehicle trajectory can be obtained from the time integration of (8). However, due to assuming no slip, the predicted trajectory is certainly deviated from the actual experienced trajectory. Now let us develop a new model, where slippage is taken into account.
The forward, lateral, and rotation velocity of the vehicle can be expressed as a function of tracks rolling speeds and track ICRs positions by solving (4)~(7). One has
In the global frame, the kinematic differential equations of motion for tracked vehicle are built.
Equation (10) indicates that, given particular tracks rolling speeds sequence , the motion of the vehicle can be accurately predicted by integrating the equation as long as tracks ICRs locations () could be properly estimated in real time.
Although they are bounded to a region, the ICRs of tracks vary over space and time, which depend on the vehicle states and terrain characteristics. Pentzer et al. [5] model the ICRs locations as constants disturbed by random noise. In view of the fact that slippage is closely related to centrifugal force and radius, we assume ICRs locations can be represented as a function of absolute values of the lateral acceleration and curvature as follows:where is the distance between track centerlines; and are left and right tracks’ rolling speeds, respectively. The product of these two rolling speeds denotes vehicle’s lateral acceleration and the division denotes vehicle’s curvature. The column vector is identified in the next section.
Combining (10) and (11), we obtain the complete slip model of tracked vehicles aswhere is the vehicle’s position coordinates and heading in the global inertial frame. is the vector describing tracks ICRs locations, which can be expressed by the unknown parameters as shown in (11).
3. Kinematic Parameters Estimation and Motion Prediction
In this section, an extended Kalman filter (EKF) is used to update the parameters in real time by comparing the predicted pose change and the actual experienced pose change. The new updated parameters are then used to predict the future motion of vehicle in a short term. The flow chart of the system is shown in Figure 2.
Given these parameters and particular tracks velocities sequence , the vehicle motion trajectory is obtained by integrating (12):
The equation is written in discrete-time form aswhere is the sampling period of tracks rolling speeds. We define the parameters as state variables; then the state equation iswhere is the process noise.
The difference between predicted pose change and actual experienced pose change for the last few seconds is given as
The predicted pose change can be calculated by (14). The actual pose change is unknown, but the observed pose change is available from position and heading sensors. Considering the noise of sensors, the measurement equation is written aswhere is vehicle’s position and heading in the global inertial frame. The function is given by (18). is the measurement noise.
The Jacobian matrix for (17) is calculated as
Now, we express the process in a standard form of EKF. The process update equations are and the measurement update equations arewhere is the measurement covariance. The noise of measurements includes the noise of encoders for tracks rolling speeds and the noise of pose sensors.
Given the tracks rolling speeds consequence, the future motion will be predicted using the current estimated parameters. Then, in the next iteration, the residual errors between the measurement and prediction are computed and used to update the parameters as well as the ICRs locations.
It is noted that the vehicle’s pose is not included in the state variables, because we care about predicting the vehicle’s future motion rather than obtaining the current pose. In fact, the pose sensors used in this paper are sufficiently accurate, and the parameters construct a map between tracks rolling speeds and the vehicle’s future motion considering the effects of track/ground interactions.
4. Simulation and Analysis
4.1. Response to Changing Abruptly ICRs Locations
To verify the effectiveness of the proposed algorithm, simulations have been conducted in Matlab. The ICRs locations of tracks are set to change abruptly, which may occur when the vehicle drives from one terrain to another: from 1.23 m to 2.23 m, from −1.23 m to −2.23 m, and from 0 to 0.5 m. The measurement errors of position and heading follow the Gauss distribution with mean and standard deviation , which are similar to the errors of sensor devices we use in experiments. The trajectory is set to be a mix of straight line and arc, and the ICRs locations change occurs at the junction point of straight line and arc. Figure 3 shows the position of the vehicle and tracks velocities.
When tracked vehicles cross terrain boundaries, it is important for the autonomous navigation system to detect rapidly the slippage change and adjust the forward predictive model to adapt the new terrain.
Figure 4 shows this phenomenon, where ICRs values converge to the true values only within a second when they change abruptly at 2 s and 12 s. From 0 s to 2 s, the vehicle is driving in a straight line by commanding a track velocity of 2 m/s, and at 2nd second, the right track velocity is set to be 1 m/s as well as the ICRs locations being changed. The estimated ICRs by EKF change from initial values to the true values in response to the condition.
The coefficient , which is given in (11), over the algorithm running is shown in Figure 5. All of the parameters are initialized to zero and respond immediately to the changed condition at 2nd second. Note that , , and are much smaller than , , and . This means ICRs are more sensitive to the lateral acceleration than curvature.
The aim of estimating ICRs locations and parameters is to predict the vehicle’s future motion in a short term. The algorithm runs continuously during vehicle driving, so the vehicle’s future motion is predicted in real time. Figure 6 shows the predicted position and heading errors with EKF and no-slip prediction at the end of the next two-second path. The position error is reduced from 0.7 m to less than 0.1 m and the heading error is reduced from 0.3 rad to near zero. Significant reduction of prediction errors, in turn, is a good evidence for the estimation correctness.
(a) Predicted position error
(b) Predicted heading error
4.2. Response to Low-Frequency Pose Sensors
In traditional methods [5], the vehicle body’s velocity that is obtained by differentiating the measured position is used to update the state. Therefore, it requires small sample periods to ensure the validity velocity. However, high-frequency measurements are not obtained at all the time and low frequency means low cost. In this section, we investigate the proposed algorithm’s performance using low-frequency pose sensors. The results obtained using this method are compared to those obtained using the traditional method with the same source data. The measurement frequencies of position and heading are set to 1 Hz and the frequency of encoders is 10 Hz.
The results, shown in Figure 7, indicate that estimated ICRs locations converge quickly to the true values using the proposed method. In contrast, the estimated ICRs locations are far from the true values using the traditional method. Due to the more accurate slip model, the position and heading prediction errors for the next two seconds are reduced significantly, as shown in Figure 8.
(a) Comparison of position prediction errors
(b) Comparison of heading prediction errors
5. Experimental Results with a Real Tracked Vehicle
5.1. Tracked Vehicle and Test Scenarios Description
Experiments for this work are conducted on a tracked vehicle with a weight of 13.6 tons as depicted in Figure 9(a). The distance between the centers of the left and right tracks is 2.464 m. The real-time kinematic (RTK) GPS and an IMU are utilized for ground truth position and heading measurements. The left and right track velocities are observed by encoders at 100 Hz. The EKF and motion predictions are implemented once pose measurements are updated at 10 Hz. In this experiment, the tracked vehicle is driven by humans over dirt terrain, as shown in Figure 9(c). The left track is blocked with nonrotating, while the right track velocity is about 2 m/s, as shown in Figure 10, which is the best case for observing slippage.
(a)
(b)
(c)
5.2. Algorithm Performance
The absolute values of prediction residuals with slip and no slip are compared in Figure 11. The mean position error is reduced from 1.116 m to 0.262 m and the heading error is reduced from 0.317 rad to 0.081 rad, reducing by 76.5% and 74.4%, respectively, as shown in Table 1.
(a) Predicted position error
(b) Predicted heading error
ICRs locations and the kinematic parameters are also estimated in Figures 12 and 13.
Figure 14 shows the visualization estimated trajectories for the next 2 s and ultimately experienced trajectories. For the sake of clarity, trajectories are shown every 2 s, despite the fact that the cycle is 100 milliseconds, starting at the 4th s and ending at the 16th s. Green lines and red lines are no-slip estimated and EKF estimated trajectories for the next 2 s, respectively, and blue triangles are the actual experience trajectories. The filled circles with green, red, and blue in the same ellipsoid denote the vehicle’s position at the end of the next 2 s. For example, at the 6th second, the slip parameters have been estimated according to the difference between the past predicted pose change and the experienced pose change. Then the current slip parameters and the linear velocities of both tracks from 6th s to 8th s, which have been recorded, are used as inputs for the predictive model with slip and no slip to predict the vehicle’s future motion from 6th s to 8th s. The red line is represented as the predicted trajectory considering slip, while the green line is represented as the predicted trajectory without slip.
ICRs locations change rapidly at the 12th s in Figure 12, indicating that the vehicle is driving from turning to a straight line, just as what we can see in Figure 14. When the vehicle is turning, the EKF prediction is much better than no-slip; however, during straight sections, there is not obvious difference between the two kinds of methods. This is because when the vehicle moves along a straight line, , , and are convergent to and zero, respectively ( is the width between the left and right tracks).
5.3. Algorithm Performance in the Case of RTK GPS Outages
RTK GPS may black out when the tracked vehicle drives in field environment, for example, under a bridge. The filter was reprocessed again without position measurements. The parameters and the ICRs locations are updated only using heading measurement, which is obtained from the IMU. The prediction residuals are shown in Figure 15. Compared to Figure 11, the position error with slip increases, while the angle error is nearly indistinguishable. The position and heading reduction rate compared with no-slip prediction are 24.6% and 73.8%, respectively, as shown in Table 2.
(a) Predicted position error
(b) Predicted heading error
ICRs locations and the kinematic parameters are shown in Figures 16 and 17. The values of and are similar to the ones in Figure 12, while the values of are much smaller than the ones in Figure 12. The fifth and sixth elements in the matrix are near zero. These changes can be explained by (10), where is not contained within the third subequation. Heading is obtained by integrating the third subequation of (10) and and are only identified by the comparison of the calculated and measured heading.
The position errors distribution in two directions of north and east is investigated with RTK GPS, without RTK GPS, and with no-slip prediction, as shown in Figure 18. The results indicate that although the prediction accuracy is decreased during RTK GPS dropouts, the EKF performance is better than no-slip.
6. Conclusion
In this paper, we have developed and identified a slip model of tracked vehicles based on the instantaneous centers of rotation of the two tracks. ICRs locations were parameterized over the lateral acceleration and curvature rather than being modeled as constant like previous works. An EKF method was applied to estimate the ICRs locations of tracked vehicles in real time using low-cost and low-frequency sensors. Given the estimated kinematic parameters and tracks velocities sequence, a more accurate vehicle motion could be obtained for the next few seconds. Experiments indicated that proposed algorithm is capable of improving predicting accuracy significantly and estimating ICRs locations correctly online.
We note that while the slip model offers a significant improvement to motion prediction, it is only a submodel of the abstract vehicle model to predict the future when commanded vehicle body linear and angular velocity controls are given. The vehicle dynamics model is another part of the abstract vehicle model, which creates a mapping between the left and right tracks’ velocities and the command inputs. Therefore, in order to predict more accurately vehicle’s motion, the vehicle dynamics model will be identified simultaneously in future work.
Competing Interests
The authors declare that there are no competing interests regarding the publication of this paper.