*5.1. Simulation Setup*

This section evaluates the ATVs' path planning and path tracking algorithms on a simulation platform by MATLAB/Simulink and Recurdyn. Multi-body dynamics software Recurdyn features a high-speed track module. The virtual ATV model in Recurdyn is shown in Figure 4. The parameters of this virtual model are based on reality. This virtual model uses parameters derived from reality. The proposed AMPC controller is evaluated for its tracking performance on this virtual model. The simulation platform's structure is illustrated in Figure, whose parameters are given in Table 1.

We conducted the simulation on the MATLAB 2022a platform to verify the proposed path planning method. The algorithm is implemented in Matlab programming language. We designed two maps with different obstacles. The ATV model was configured with two rectangles, whose sizes are 2.5 × 2 m and 2 × 2 m. The driving speed of the ATVs was set to 2.5 m/s. The parameters of the Hybrid A-star planner and the trajectory optimization are listed in Table 2. The proposed method (Method 2) was compared to the original Hybrid A-star method (Method 1). Method 1 also considers the kinematic characteristics of ATVs and implements the node search by the discretized steer angles. At the same time, the Reed-Shepp

(RS) curve is not adopted by Method 1 in the path search. Moreover, the heuristic function of Method 1 is the Euclidean distance from the current node and the destination.

**Figure 4.** The virtual model of the articulated tracked vehicle constructed on the multi-body dynamics software Recurdyn.

The simulation results of the proposed AMPC algorithm were compared with the fuzzy control and MPC control published in [27,31,32]. The performance of the path-tracking controller could be mainly determined by the lateral position error and the orientation angle error of the front unit with respect to the reference path.

Four conditions were considered in the co-simulation of the path-tracking controllers. In the first condition, the ATV was controlled to track an arc of 25 m radius with a longitudinal velocity of 0.56 m/s. The second condition was to follow a path consisting of three circles with 30 m, 20 m, and 40 m radius, respectively. The longitudinal speed was set as 3 m/s. In the third condition, the reference path is a mixed path with three straight lines and two arcs of 20 m radius. The vehicle was set to move at a speed of 4 m/s. Finally, the fourth condition was to track a path generated by the Hybrid A-star planning method proposed in Section 3. The proposed AMPC controller was compared with the standard—MPC controller in this condition. In the above simulations, the parameters of the proposed controller were fixed and presented in Table 3.


**Table 1.** Kinematic parameters of the articulated tracked vehicle model.

**Table 2.** Parameters of the trajectory planning and optimization.



**Table 3.** Parameters of the trajectory tracking AMPC controller.

#### *5.2. Simulation of Path Planning*

The comparisons of the two simulation results are illustrated in Figure 5a,b. The solid line denotes the results of Method 1, and the dash-dot line indicates the proposed Method 2. As shown in Figure 5a, both path planning methods could generate the path to the goal. A significant improvement can be observed as the path generated by Method 1 contained non-smooth segments, while the planned path of Method 2 is much smoother and contains fewer switches of turning direction. Moreover, in Method 1, the planning path is close to the obstacles when the ATVs try to turn between the obstacles, as shown in Figure 5b. On the contrary, the path by Method 2 could be in the middle of the obstacles to avoid unnecessary turns when crossing the corridor between the obstacles.

**Figure 5.** Original Hybrid A-star (solid line) and the proposed Hybrid A-star (dash-dot line) path planning in the simulation. (**a**) Map A; (**b**) map B.

Table 4 summarizes the comparison between Method 1 and Method 2 regarding the maximum curvature, path length, the number of steering direction changes, and execution time. The higher values indicate higher steering instability, a longer driving path, and a higher computation cost to find the goal. From the comparison, Method 2 generates a path with more minor curvatures than Method 1. Moreover, the path length of Method 2 is longer in map A, but shorter in map B, although the difference between the two methods is not significant (88.46 versus 96.06 and 128.94 versus 116.09, respectively). In both conditions, the proposed method's computation time are both longer than Method 1 because the candidate RS curves must be computed and checked for the collision in Method 2. Whereas the benefit of the RS curve is the much less number of the steering change in Method 2, as the RS curve could simplify the process of the path forward search.


**Table 4.** Comparison of original method and the proposed method.

<sup>1</sup> **Curvature** denotes the maximum curvature of the planned path.2 **Number** denotes the number of the steering direction change.<sup>3</sup> **Length** denotes the overall length of the planned path. <sup>4</sup> **Time** denotes the computation time of the planning method to obtain the planned trajectory.

#### *5.3. Simulation of the Trajectory Tracking*

#### 5.3.1. Simulation Result of Case 1

In Case 1, the ATV is controlled to follow the curved path with a radius of 25 m, and the ATV is assumed to be positioned at the initial point. In the previous research [27], the fuzzy control system was utilized to guide the ATV to follow an arc. The simulation result of the fuzzy control and the proposed AMPC control have been presented in Figures 6–8. The maximum lateral error of the AMPC controller is almost the same as that of the fuzzy-PID controller, as shown in Figure 7. Moreover, the AMPC controller achieves a minor orientation deviation compared to the fuzzy-PID controller, as shown in Figure 8. The maximum orientation error in the AMPC controller has been reduced by 81.84% compared to the fuzzy-PID method. Thus, the trajectory generated by the AMPC controller is closer to the predefined path than the fuzzy-PID method, as shown in Figure 6.

**Figure 6.** The trajectory of the AMPC controller and the fuzzy-PID controller in Case 1.

**Figure 7.** The lateral position error of the AMPC controller and the fuzzy-PID controller in Case 1.

**Figure 8.** The orientation angle error of the AMPC controller and the fuzzy-PID controller in Case 1.

#### 5.3.2. Simulation Result of Case 2

Case 2 consists of three circle paths to test the tracking performance of the ATV for the continuous steering mode. In the previous research [31], the standard-MPC controller was proposed for path tracking of autonomous articulated vehicles. The path-tracking error model of standard-MPC is based on lateral displacement, orientation, and curvature errors. The curvature error is dedicated to the circular path with a constant radius. As our work has not included the curvature error in the path-tracking model, we have compared the lateral position error and orientation error of the AMPC method with that of the standard-MPC.

The reference path and the trajectory produced by the standard-MPC and the AMPC are presented in Figure 9. The trajectory of the AMPC is closer to the defined path than the standard-MPC. The position and orientation errors produced by the standard-MPC and the AMPC have been illustrated in Figures 10 and 11. The AMPC method has achieved better performance of the lateral position error than that of the standard-MPC. The maximum position error of the standard-MPC is 2 m, while the maximum position error of the AMPC is 0.67 m. In addition, the position error response of the AMPC converges to zero at the final, while the standard-MPC retains a significant position error. The standard-MPC produces a more minor orientation angle error than the AMPC controller. The maximum orientation errors of the standard-MPC and the AMPC are 0.002 rad and 0.067 rad, respectively. The reason may be due to the unavoidable skid of the articulated tracked vehicles compared to the articulated vehicle with tires. Nevertheless, the orientation error caused by the AMPC is acceptable for the ATV in practical operation.

**Figure 9.** The trajectory of the AMPC controller and the standard-MPC controller in Case 2.

**Figure 10.** The lateral position error of the AMPC controller and the standard-MPC controller in Case 2.

**Figure 11.** The orientation angle error of the AMPC controller and the standard-MPC controller in Case 2.

#### 5.3.3. Simulation Result of Case 3

Case 3 consists of straight lines and arcs with a radius of 20 m. The previous research in [32] compared the tracking performance between the switch-MPC and the nonlinear MPC (NMPC) on the path tracking of the articulated mining vehicle with tires. Although the articulated vehicle of the research [32] differs from the ATV, the results in [32] have significant value and are worthy of reference.

Figure 12 illustrates the articulation angle response of three controllers. The maximum articulation angle response of the switch-MPC, the NMPC, and the AMPC controller is 0.5589 rad, 0.3940 rad, and 0.272 rad, respectively. Both the articulation angle response of the NMPC and the AMPC controllers exhibit less overshoot and change smoothly compared to the switch-MPC controller. Figure 13 illustrates the lateral position errors of the controllers. The maximum position errors by the switch-MPC, NMPC, and AMPC controller reached 0.7217 m, 0.0874 m, and 0.192 m, respectively. The ultimate position error generated by the AMPC controller was reduced by 73.4 % compared to the switch-MPC controller. Figure 14 presents the orientation errors of all controllers. The maximum orientation errors are 0.1458 rad, 0.0461 rad, and 0.0392 rad for the switch-MPC controller, the NMPC controller, and the AMPC controller, respectively. The maximum orientation error of the AMPC has been reduced by 73.11% compared to the switch-MPC controller and by 14.97 % with respect to the NMPC controller.

According to the research [32], the maximum computation times for the NMPC controller and the switch-MPC controller are 0.014 s, and 0.04 s, respectively. As the AMPC control system is constructed in the Matlab/Simulink, the profile report of the Simulink run-time indicates that the proposed controllers have been invoked 200 times during the

whole simulation time of 40 s. The overall computation time of the controller is 0.399 s. The average computation time of the AMPC controller is approximately 0.002 s at each time step. The average computation time of the AMPC is much less than that of both the switch-MPC controller and the NMPC controller.

**Figure 12.** The articulation angle of the AMPC controller, the NMPC controller, and the switch-MPC controller in Case 3.

**Figure 13.** The lateral position error of the AMPC controller, the NMPC controller, and the switch-MPC controller in Case 3.

**Figure 14.** The orientation angle error of the AMPC controller, the NMPC controller, and the switch-MPC controller in Case 3.

#### 5.3.4. Simulation Result of Case 4

To evaluate the reliability of the AMPC algorithm to track a non-uniform trajectory given by the proposed path planner, we conducted a simulation where the ATV is controlled to follow the non-uniform trajectory with varying curvature.

The simulation results are presented in Figures 15–19. The vehicle is set to move at 3 m/s. As shown in Figure 15, both the standard-MPC and the AMPC controllers could drive the ATV to follow the given path, while the AMPC controller causes less offset from the reference path compared to the standard-MPC controller. Figure 16 illustrates the articulation angle of both controllers. The ultimate articulation angle of the standard-MPC and the AMPC controller reached 0.611 rad and 0.532 rad, respectively. The maximum articulation angle of the standard-MPC is more than the limit on the articulation angle. Moreover, the articulation angle response of the AMPC controller presents less overshoot compared to the standard-MPC controller. Figure 17 illustrates the articulation angle rate of both controllers. The response of the AMPC controller exhibits more drastic changes when compared to the standard-MPC controller. At the same time, the AMPC controller outputs the articulation angle rate in advance to resist the disturbance, which could reduce the tracking error of the ATV. Figure 18 depicts the lateral tracking errors of the AMPC and the standard-MPC controller. The tracking error of the AMPC controller maintains much less than that of the MPC controller through the tracking process. Moreover, the maximum lateral tracking error of the ATV approached 0.832 m and 3.015 m by the AMPC and the standard-MPC controller, respectively. The number of maximum position errors by the AMPC controller was reduced by 72.4% compared to the standard-MPC controller. Figure 19 illustrates the orientation angle error of both controllers. The orientation error of the AMPC controller is also less than the standard-MPC controller along the time. The maximum orientation error of the AMPC controller and the standard-MPC controller is 0.125 rad and 0.269 rad, respectively. The maximum value of the orientation angle error by the AMPC controller was reduced by 53.53% compared to the standard-MPC controller.

**Figure 16.** The articulation angles of the AMPC controller and the standard-MPC controller in Case 4.

**Figure 17.** The articulation angle rates of the AMPC controller and the standard-MPC controller in Case 4.

**Figure 18.** The lateral position errors of the AMPC controller and the standard-MPC controller in Case 4.

**Figure 19.** The orientation angle errors of the AMPC controller and the standard-MPC controller in Case 4.

#### **6. Conclusions**

To enhance the ability of ATVs to drive in a complex environment, we apply the Hybrid A-star method to plan a safe trajectory. Moreover, the planned path was optimized to ensure smoothness and continuity. Comparing the proposed path planner with the original Hybrid A-star method shows that the planner could generate a feasible trajectory with minimum steering direction change. Numerous studies have applied the MPC algorithm and verified its effectiveness for path-tracking control of the articulated vehicle. To achieve the trajectory tracking of the articulated tracked vehicle to follow the planned path, we propose an adaptive model predictive control (AMPC) control method that is based on the time-varying tracking error system. We obtain the following results and conclusions by comparing the AMPC controller with the previously developed fuzzy and MPC controller.

Firstly, the AMPC controller could rapidly track the reference path compared to the fuzzy-PID controller. The AMPC controller also achieves a minor orientation angle error. Secondly, the AMPC controller could achieve more minor tracking errors than the standardMPC method. Thirdly, the tracking accuracy of the AMPC method is still inferior to the NMPC method, while the AMPC method has the advantage of computation efficiency.

From the above analysis, the main contributions of this work could be summarized as follows:


However, the proposed Hybrid A-star planning method has the drawback of extensive computation time, which could be improved by the refined algorithm structure in further research. Moreover, the proposed AMPC method is applied in the kinematic control of the ATV, which could not deal with the high-speed driving condition. In a further study, we will focus on the dynamic control of ATVs and apply the AMPC method in the dynamic control of ATVs.

**Author Contributions:** The contributions of the K.H. incude the conceptualization, methodology, data curation and analysis; writing—original draft; writing—review and editing. The contributions of the K.C. include the supervision, project administration, and funding acquisition. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was funded by the National Key Research and Development Program of China (Grant 2016YFC0802703).

**Data Availability Statement:** The codes for this paper have been uploaded to the GitHub since the data of 24/03/2020. It is accessible for any visitors to this repository in GitHub https://github.com/ HuKangle/Path\_planing-and-Path\_tracking-for-an-articulated-tracked-vehicle.

**Conflicts of Interest:** The authors declare no conflict of interest.

#### **Abbreviations**

The following abbreviations are used in this manuscript:


#### **References**


**Disclaimer/Publisher's Note:** The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.

**Xiao Hu 1, Kai Hu 1, Datian Tao 2, Yi Zhong 2,\* and Yi Han 2,\***


**Abstract:** Maritime transportation is vital to the global economy. With the increased operating and labor costs of maritime transportation, autonomous shipping has attracted much attention in both industry and academia. Autonomous shipping can not only reduce the marine accidents caused by human factors but also save labor costs. Path planning is one of the key technologies to enable the autonomy of ships. However, mainstream ship path planning focuses on searching for the shortest path and controlling the vehicle in order to track it. Such path planning methods may lead to a dynamically infeasible trajectory that fails to avoid obstacles or reduces fuel efficiency. This paper presents a data-driven, efficient, and safe path planning (ESP) method that considers ship dynamics to provide a real-time optimal trajectory generation. The optimization objectives include fuel consumption and trajectory smoothness. Furthermore, ESP is capable of fast replanning when encountering obstacles. ESP consists of three components: (1) A path search method that finds an optimal search path with the minimum number of sharp turns from the geographic data collected by the geographic information system (GIS); (2) a minimum-snap trajectory optimization formulation with dynamic ship constraints to provide a smooth and collision-free trajectory with minimal fuel consumption; (3) a local trajectory replanner based on B-spline to avoid unexpected obstacles in real time. We evaluate the performance of ESP by data-driven simulations. The geographical data have been collected and updated from GIS. The results show that ESP can plan a global trajectory with safety, minimal turning points, and minimal fuel consumption based on the maritime information provided by nautical charts. With the long-range perception of onboard radars, the ship can avoid unexpected obstacles in real time on the planned global course.

**Keywords:** kinematics; improved A\* algorithm; path planning; GIS
