*Article* **Smart Vehicle Path Planning Based on Modified PRM Algorithm**

**Qiongqiong Li, Yiqi Xu, Shengqiang Bu and Jiafu Yang \***

College of Mechanical and Electronic Engineering, Nanjing Forestry University, Nanjing 210037, China

**\*** Correspondence: jfyang@njfu.edu.cn; Tel.: +86-13951004006

**Abstract:** Path planning is a very important step for mobile smart vehicles in complex environments. Sampling based planners such as the Probabilistic Roadmap Method (PRM) have been widely used for smart vehicle applications. However, there exist some shortcomings, such as low efficiency, low reuse rate of the roadmap, and a lack of guidance in the selection of sampling points. To solve the above problems, we designed a pseudo-random sampling strategy with the main spatial axis as the reference axis. We optimized the generation of sampling points, removed redundant sampling points, set the distance threshold between road points, adopted a two-way incremental method for collision detections, and optimized the number of collision detection calls to improve the construction efficiency of the roadmap. The key road points of the planned path were extracted as discrete control points of the Bessel curve, and the paths were smoothed to make the generated paths more consistent with the driving conditions of vehicles. The correctness of the modified PRM was verified and analyzed using MATLAB and ROS to build a test platform. Compared with the basic PRM algorithm, the modified PRM algorithm has advantages related to speed in constructing the roadmap, path planning, and path length.

**Keywords:** smart vehicle; probabilistic roadmap algorithm; pseudo-random sampling; collision detection; path smoothing

#### **1. Introduction**

In recent years, smart vehicles have received more attention with the development of emerging technologies such as cloud computing, big data, and the full-scale launch of 5G construction [1,2]. Smart vehicles have significant effects in relieving driving pressure, avoiding traffic jams, and reducing environmental pollution [3] Path planning and motion control are significant and complex navigation tasks in smart vehicles. Path planning technology is the basis of smart vehicles to make motion decisions and navigate positioning [4,5]. To achieve successful path planning and motion control to be able to reach a target safely, smart vehicles must be provided with the ability to perceive and detect obstacles to be avoided [6]. Many sensors are installed on the body of smart vehicles, which ensure that they can perceive and interpret information gathered from the environment to determine position, direction to the target, position of obstacles, and navigation in both structured or unstructured environments [7].A smart vehicle is expected to perform these tasks with the safest and shortest path, reaching the target in the shortest time, and ultimately performing the specified task without the intervention of humans. Path planning in smart vehicles refers to determining how the smart vehicle reaches its target point safely to ensure obstacle avoidance. Smart vehicle path planning is described as a multi-objective optimization problem as it requires the generation of appropriate trajectories as well as obstacle avoidance in the environment [8].

The methods of smart vehicle path planning can be classified in different ways. Ayawli et al. [7] categorized them into nature-inspired computation methods, traditional methods, and hybrid methods. Methods and strategies that imitate natural phenomena

**Citation:** Li, Q.; Xu, Y.; Bu, S.; Yang, J. Smart Vehicle Path Planning Based on Modified PRM Algorithm. *Sensors* **2022**, *22*, 6581. https://doi.org/ 10.3390/s22176581

Academic Editors: Luige Vladareanu, Hongnian Yu, Hongbo Wang and Yongfei Feng

Received: 11 July 2022 Accepted: 29 August 2022 Published: 31 August 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

are described as nature-inspired computation methods. Meanwhile those that have nothing to do with imitating nature phenomena are described as the conventional method. Approaches that combine two or more strategies are described as hybrid methods. Natureinspired computing consists of a metaheuristic algorithm that simulates, based on nature phenomena given by natural science [9]. A number of researchers have attempted to solve the problem of mobile robotics path planning by using nature-inspired algorithms including genetic algorithms (GA) [10,11], artificial neural networks (ANN) [12,13], simulated annealing (SA) [14], ant colony optimization (ACO) [15], particle swarm optimization (PSO) [16], and artificial bee colonies (ABC) [17].In order to take advantage of the strengths of some methods while reducing the effects of their disadvantages, some researchers combine two or more methods to provide an efficient hybrid path planning method for controlling smart vehicles. These approaches include APF combined with GA [18], APF combined with PSO [19], and fuzzy logic combined with Kalman filtering [20,21]. Conventional path planning methods have been used for many years. These methods mainly rely on distance information from the object to the smart vehicles, repulsive force and attractive force clustering, or graphical map calculations to determine the path planning of smart vehicles. Even though conventional methods of path planning are computationally expensive, they are easy to implement. Conventional methods mainly consist of the rapidly-exploring random tree (RRT) algorithm [22], probabilistic roadmap algorithm (PRM) [23], artificial potential field (APF) [24,25], sliding mode control (SMC) [11,26], A \* algorithm [27], D \* algorithm [28,29], and simultaneous localization and mapping (SLAM) [30].

PRM is one of the most popular sampling based planners. PRM is a space planner that uses multiple-query planning. The key idea in PRM is to distribute the nodes across the space and then connect these nodes using simple local planning and straight lines to form a graph roadmap. By connecting the available space, the PRM succeeds in exploring a faster path by reducing the search to a graph [31]. However, PRM has shortcomings, including lack of orientation in the selection of sampling points, low reuse rate of the roadmap, and low search efficiency. Moreover, due to the random sampling of nodes in PRM, there exists a narrow passage problem that generates an unconnected graph. To enhance the efficiency of sampling-based algorithms, Kantaros et al. [32] introduced bias into the sampling process. Vasile et al. [33] maintained sparsity of generated samples. Sparseness was also explored by Dobson and Berkis for PRM using different techniques [34]. Amato et al. [35] proposed parallelizing strategies; the PRM method has massive inherent parallelism, which can be easily and best exploited. Berkis et al. [36] used the probabilistic roadmap method (PRM) with bidirectional rapidly exploring random trees (BI-RRT) as the local planner to solve multiple queries for motion planning problems with single query planners. Kurniawait et al. [37] designed an improved PRM algorithm, which was based on obstacle boundary sampling and evaluated the optimal feasible region to optimize the dispersion of random sampling of the PRM algorithm. Esposito et al. [38] proposed a processing algorithm for optimizing probabilistic roadmaps. Dealing with the format of convex cells in free space with a number of nodes that requires a lot of computation, this algorithm could simplify the computation required for this step by sparse decomposition. Gao Junli et al. [39] proposed to combine the deep reinforcement learning twin-delayed deep deterministic policy gradient algorithm with the traditional PRM algorithm as a new path planner, and the experimental results showed that this incremental training mode could significantly improve search efficiency. Moreover, this new path planner effectively improved the generalization of the model. Chen Gang et al. [40] proposed an improved PRM method. Based on a virtual force field, a new sampling strategy of PRM was proposed to generate a configuration that is more appropriate for practical application in free space.

RAVANKAR et al. [41] proposed a method for global planning using a hierarchical hybrid PRM and the APF method, using a decomposition method of node distribution that used map segmentation to generate regions of high and low potential, and proposed a method to reduce the dispersion of sample sets during roadmap building. Xu Zhenfan et al. [42] changed the sampling strategy so that nodes were incrementally

added and evenly distributed in the exploration region to produce the best viewpoints and PRM enabled the planner to quickly search for alternative paths and avoid dynamic obstacles for safe exploration.

Aiming to improve the shortcomings of the PRM algorithm, the main innovation of this paper is that we propose a pseudo-random sampling strategy with the main spatial axis as the reference axis, set the distance threshold between road points, and adopt a two-way incremental method for collision detections. We aim to find the shortest path between the start point and target point and shorten the time of the planning path. The key road points of the path are extracted as discrete control points of the Bessel curve. We use Bezier curve to make the path smoother, whereas the path is more like the actual driving condition of the smart vehicle.

#### **2. Modified PRM Algorithm**

*2.1. PRM Algorithm*

The PRM algorithm includes sampling and query phases.

Sampling phase: the PRM algorithm randomly samples in the planning space and judges the reasonableness of the sampling points by the local planner. By repeating the sampling times *n* to generate a collection of valid waypoints *V* traversing the *V*, the algorithm connects all the feasible paths between the waypoints to expand to the whole planning space and forms the waypoint graph. *V* = {*v*1, .*v*2,..., *vn*} denotes the set of waypoints; *E* = + *vi*, *vj vi*, *vj* ∈ *V* , denotes the set of edges between waypoints.

Query phase: the start point *qinit* and target point *qgoal* are put into the wayfinding graph *G*(*V*, *E*), and the algorithm enters the path search phase. We use the graph search algorithm in the wayfinding graph *G*(*V*, *E*) to find a collision-free path connecting the start point *qinit* and target point *qgoal*.

#### *2.2. Pseudo-Random Sampling*

In the PRM algorithm, the number of sampling points generated by the random sampling strategy increases with an increase in planning space. It is difficult to achieve a global uniform distribution and easy to create redundancy in sampling points. There is a considerable probability that the shortest path occurs in the area where the starting point and target point connects. This region is regarded as a focused sampling region, referred to as the spatial principal axis region.

To construct the spatial principal axis information, we set the coordinates of the starting point to be *S*(*xs*, *ys*) and the coordinates of the target point to be *G xg*, *yg* . Length *L* and declination of the spatial principal axis *θ* was denoted by:

$$L = \|G - \mathcal{S}\|\_2 \tag{1}$$

$$\theta = \frac{\pi}{2} - \arctan \frac{|y\_{\mathcal{S}} - y\_s|}{|x\_{\mathcal{S}} - x\_s|} \tag{2}$$

We designed the spatial principal axes with the length *L*, and number of sampling points *n*, then obtained the longitudinal sampling spacing *Nd*, as:

$$N\_d = \frac{L}{n} \tag{3}$$

Referring to the random sampling method, the sampling points were symmetrically distributed in the sector area near the main axis of space, and sampling points *Pi*,*j*(*x*, *y*) were calculated as follows:

$$\mathbf{x} = \mathbf{x}\_s + r\_d \times \cos(\theta + \phi\_j) \tag{4}$$

$$y = y\_s + r\_d \times \sin(\theta + \phi\_{\bar{\jmath}}) \tag{5}$$

$$r\_d = \mathbf{i} \times \mathbf{N}\_d, \mathbf{i} = [1, 2, \dots, n] \tag{6}$$

where (*xs*, *ys*) indicates the starting point of the intelligent vehicle; *rd* indicates the sampling radius; sampling radius is centered on the starting point; *φ<sup>j</sup>* ∈ [−*φm*, *φm*] indicates the angle of deflection of the sampling point and; *φ<sup>m</sup>* indicates the maximum deflection angle. It is used to control the angle of the sector sampling area, that is, the range of lateral sampling.

According to Figure 1a,b, the sampling points are symmetrically distributed on both sides of the main spatial axis, and sampling range is controlled by the maximum deflection angle *φm*. With the increase of *φm*, the sampling points spread in all directions along the main spatial axis. To make the sampling point distribution more uniform, the lateral sampling range is adjusted along the main axis of space, and sampling range is adjusted in increments using Δ*φ* = *φ*m/*n*. The distribution of sampling points after adjustment is shown in Figure 1c,d.

**Figure 1.** Sampling method based on spatial principal axis: (**a**) *φ<sup>m</sup>* = 10, (**b**) *φ<sup>m</sup>* = 20, (**c**) *φ<sup>m</sup>* = 10, and (**d**) *φ<sup>m</sup>* = 20.

Integrating the characteristics of uniform sampling, we counted the number of sampling points *p* in free space and the effective sampling rate of the horizontal sampling layer is defined as *R*:

$$R = \frac{p}{N} \tag{7}$$

where *N* indicates the total number of samples in the current sampling layer and the size of the effective sampling rate *R* reflects the connectivity of the current sampling layer. The larger *R* is, the better the connectivity of the sampling layer. If *R* is too small, this means that most of the sampling points in the sampling layer have fallen into the obstacle space. If the sampling layer edge subsequently has the same sampling interval, the chance of sampling points falling into the obstacle space will increase.

In order to improve the ability of the sampling points in avoiding obstacles, we introduced random increments Δ*r* to adjust the sampling interval of sampling points. Based on Figure 1d, we adjust the size of the random increment Δ*r* to get Figure 2. As the value of the random increment Δ*r* increases, the sampling points tend to approach random distribution. With a decreasing value of Δ*r*, the sampling points tend to approach uniform distribution.

Referring to Figure 3, hollow dots indicate the sampling points before adjusting the sampling spacing, solid dots indicate the adjusted sampling points, red markers represent the sampling points falling into the obstacle space, and black markers represent the sampling points in the free space. The effective sampling rate of the front sampling layer is low (*R* = 0.3), the radius fluctuation rate (*R* = 0.8) of the subsequent sampling layer is adjusted, and the sampling points avoid the obstacles by using the pseudo-random sampling strategy, which improves the quality of sampling point generation.

**Figure 2.** Pseudo-random-based sampling method: (**a**) Δ*r* = *r*, (**b**) Δ*r* = 0.5*r*, and (**c**) Δ*r* = 0.25*r*.

The sampling radius after adding random increments Δ*r* is shown in Equation (8):

$$
\tau\_d' = \tau\_d + \Delta r \tag{8}
$$

**Figure 3.** Schematic of sampling point adjustment.

#### *2.3. Bidirectional Incremental Collision Detection*

Collision detection is used to determine whether the connected line segments between the sample points intersect with the obstacle space, and the sample points are connected to each other by collision detection to form a roadmap *G*(*V*, *E*). The traditional PRM algorithm usually takes an incremental detection strategy. According to a fixed step size, the planner selects discrete points and detects whether the point falls into the obstacle space. To improve the efficiency of collision detection execution, we combined this incremental detection method with the dichotomous method, proposing a two-way incremental detection strategy.

First, the two-way incremental detection method judges the reasonableness of the first and last connected sample points (Figure 4a). Then, we end the detection if the sample points belong to the obstacle space. If the sample points belong to the self-use space, we select the test point in both directions gradually along the first and last connected sample points and judge the reasonableness of the test point. If the selected test point belongs to the obstacle space, the detection is stopped to discard the path, as shown in Figure 4b. The sample points are connected to each other by collision detection, and finally form a roadmap *G*(*V*, *E*).

#### *2.4. Neighbouring Layer Connection Strategy*

In the roadmap *G*(*V*, *E*), the threshold distance between road points is an important factor affecting the efficiency of roadmap construction. The path formed by connecting road points in the same sampling layer is not conducive to shorten the global path length. Taking the distribution characteristics of the longitudinal sampling layer into account, we set the connection threshold of the longitudinal sampling spacing *LTH* to screen the paths that met the threshold conditions and make the connection between road points from the full connection to adjacent sampling layer connection, improving roadmap construction efficiency.

**Figure 4.** Schematic diagram of two-way incremental detection strategy: (**a**) reasonable path and (**b**) illegal path.

The sampling points generated based on the pseudo-random sampling strategy (*N* = 20) were selected to obtain the roadmap constructed under the drive of two connection strategies, as shown in Figure 5. Figure 5a shows the wayfinding graph generated by the full connectivity strategy, with the red solid line representing the filtered paths. Figure 5b indicates the wayfinding graph generated by the neighbouring layer connectivity strategy. In terms of time consumption, the composition time using these different connection strategies was 0.906 s and 0.437 s, respectively, and the latter optimized composition efficiency by 48.2%.

**Figure 5.** Comparison of road signs: (**a**) full connection and (**b**) neighbouring layer connection.

#### **3. Path Smoothing**

In this paper, Bessel curves were chosen to smooth the paths planned by the modified PRM algorithm.

The *n* order Bessel curve expressions were defined as:

$$B(t) = \sum\_{i=0}^{n} P\_i b\_{i,n}(t), \ (t \in [0, 1]) \tag{9}$$

where *Pi* represents the *n* + 1 control point of the Bessel curve and *bi*,*n*(*t*) represents the Bernstein basis function. The value of this function is shown in Equation (10):

$$b\_{i,n}(t) = \mathbb{C}\_n^i t^i (1-t)^{n-i} = \frac{n \text{ } ! \text{ } }{(n-i) \text{ } i \text{ } !} t^i (1-t)^{n-i} \text{ } i = 0, 1, 2, \dots, n \tag{10}$$

In this paper, a 4th order Searle curve was chosen, and the formula is as follows:

$$B(t) = (1-t)^4 P\_0 + 4P\_1(1-t)^3 t + 6P\_2(1-t)^2 t^2 + 4P\_3(1-t)t^3 + P\_4 t^4, \; t \in [0, 1] \tag{11}$$

The curvature of the Bessel curve at any point *κ*(*t*) is:

$$\kappa(t) = \frac{|B'(t) \times B''(t)|}{|B'(t)|^2} \tag{12}$$

Assuming that the planning path *path* = {*Pn*} consists of a series of discrete points (*n* ≥ 5), the discrete points are used as the control points *Pi* of the Bessel curve, and the curvature of the Bessel curve *κ*(*P*) can be obtained according to Equation (12):

$$\kappa(P) = \frac{P\_{\mathbf{x}}' P\_{\mathbf{y}}'' - P\_{\mathbf{y}}' P\_{\mathbf{x}}''}{\left(P\_{\mathbf{x}}'^2 + P\_{\mathbf{y}}'^2\right)^{3/2}} \tag{13}$$

The curvature of the Bessel curve at the starting point is *κ*(0):

$$\kappa(0) = \frac{3|\left(P\_1 - P\_0\right) \times \left(P\_2 - P\_1\right)|}{4\left(P\_1 - P\_0\right)^3} \tag{14}$$

In this specific implementation, the key waypoints of the path searched by the modified PRM algorithm were extracted, discrete control points of the Bessel curve *Pi* were obtained by discretizing the line between key waypoints, and the discrete points were interpolated and fitted by Equation (9) to realize the smoothing of the path.

#### **4. Simulation Test and Analysis**

To verify the composition and path planning efficiency of the modified PRM algorithm, MATLAB (MATLAB2018b, MathWorks. Inc., Natick, MA, USA) was used to build a simulation experiment platform and a ROS (ROS1.0, Willow Garage. Inc., Menlo Park, CA, USA) experimental platform was used to verify the correctness of the modified PRM algorithm. Our computer configurations included: a Windows 10 operating system, 512 GB hard disk, and 8 GB RAM.

#### *4.1. Comparison of Algorithm Composition Efficiency*

The planning space of the known map is shown in Figures 6 and 7. The two algorithms kept the same total number *N* = *m* × *n* of sampling points in the sampling phase, where *m* and *n* represent the number of horizontal and vertical sampling points of the algorithm, respectively. We focused on the planning path length and roadmap construction time and repeated the test several times (recorded 10 times). The results are shown in Table 1 in mean values.

**Figure 6.** Planning results of the basic PRM algorithm (*N* = 60): (**a**) roadmap and (**b**) planned path.

**Figure 7.** Planning results of the modified PRM algorithm (*N* = 60): (**a**) roadmap and (**b**) planned path.


**Table 1.** The results of algorithm comparison.

Taking sampling points *N* = 60 as an example, we analyzed the results of the roadmap construction (Figures 6a and 7a). The sampling points were widely distributed in the PRM algorithm and there were many redundant sampling points. On the other hand, for the roadmap constructed by the modified PRM algorithm (Figure 7a), the location selection of the sampling points had a certain orientation, mainly distributed along the main axis of space, and there were fewer redundant sampling points.

In Figures 6 and 7 and Table 1, it is shown that when the number of sampling points *N* is 30, the length of the planned path increases by 1.9% and composition time is reduced by 57.8%. When the number of sampling points *N* is 60, the length of the planned path is reduced by 1.9% and composition time is reduced by 37.1%. When the number of sampling points increase to 90, the length of the planned path is reduced by 5.9% and composition time is reduced 50%. It shows that the changes in path length according to different number *N* are not consistent. Compared with the PRM algorithm, there is no great advantage in path length for the modified PRM algorithm. However, the modified PRM algorithm showed great advantages in decreasing the construction time of the roadmap; the efficiency of constructing maps was significantly improved.

In Figure 8, keeping all other conditions equal, when the number of fold points of the path increased, path smoothness gradually improved as the number of sampling points increased. The overall trend of the path remains unchanged, indicating that the quality of the path solution solved by the modified PRM algorithm is stable.

To obtain Figure 9, we used the Bessel curve to deal with Figure 8b, the solid blue line indicating the modified PRM algorithm planning path and the black hollow circle logo representing the key road points, used as the Bessel curve control points. The path obtained after the smoothing process (shown by the red line) was more consistent with intelligent vehicle driving road conditions.

#### *4.2. Comparison of Path Planning Efficiency*

To verify the path planning efficiency of the modified PRM algorithm, the basic PRM algorithm was used as the comparison algorithm for the case test, where Case A is a square maze and Case B is a narrow channel. The success rate was measured by a ratio of the

number of successful path searches to total search number. The results of the case test are shown in Figures 10 and 11 and Table 2.

**Figure 8.** Comparison of planning results of modified PRM algorithm: (**a**) *N* = 30, (**b**) *N* = 60, and (**c**) *N* = 90.

**Figure 9.** Path smoothing diagram.

(**a**) PRM algorithm (**b**) Modified PRM algorithm

Case A: Square maze

**Figure 10.** Comparison of algorithm planning results.

**Figure 11.** Algorithm success rate comparison.

**Table 2.** Comparison results of algorithm efficiency.


Referring to Figure 10, in the experiment of Case A, the number of sampling points falling into the obstacle space was comparable in both algorithms, but the sampling points in the self-use space were widely distributed in the PRM algorithm, which caused redundancy. In the modified PRM algorithm, the sampling points were concentrated on both sides of the main axis of the space, which improved the utilization of sampling points. In the experiment of Case B, most of the sampling points in the PRM algorithm fell into the obstacle space, and there were very few sampling points in the self-use space, which affected the quality of the path solution. In the modified PRM algorithm, the sampling points were distributed along the main axis of the space, and the larger number of sampling points in the self-use space provided the possibility of seeking a better path solution.

In Table 2 and Figure 11, for Case A, the modified PRM algorithm could not successfully plan the path when the number of sampling points was low (*N* = 30). When the number of sampling points increased to 60 (*N* = 60), the differences between the two algorithms in path length, running time, and success rate were not obvious. When the number of sampling points increased to 90 (*N* = 90), the modified PRM algorithm was better than the basic PRM algorithm in path length and running time. For Case B, when the number of sampling points was low (*N* = 30), both algorithms could not successfully plan the path, and as the number of sampling points increased, the modified PRM algorithm had a higher success rate in path planning and the quality of the path solution was more reliable.

#### *4.3. ROS Simulation Test*

In order to further verify the implementability of the modified PRM algorithm, simulation tests were designed, based on the ROS experimental platform. The composition of the ROS trolley is shown in Figure 12.

**Figure 12.** ROS car composition.

We mainly addressed the path planning problem of smart vehicles in a two-dimensional environment, using the function package provided by the ROS experimental platform to implement the LIDAR map building function. The test site is shown in Figure 13, and the SLAM map building effect is shown in Figure 14. Based on this environmental map, we defined the localization result of ROS itself as the starting point and specified the target point. The modified PRM algorithm was executed and the path planning results are shown in Figure 15.

**Figure 13.** Field map.

**Figure 14.** SLAM map.

**Figure 15.** Path planning: (**a**) wayfinding map and (**b**) planning path.

From the simulation results, a road map was established in the SLAM map by the modified PRM algorithm. Meanwhile, the modified PRM algorithm planned a path successfully connecting the starting and target point, verifying the feasibility of the modified PRM algorithm.

#### **5. Conclusions**

In order to improve the overall quality of the PRM algorithm in path planning, a pseudo-random sampling method based on uniform sampling was designed to optimize the quality of sampling point generation. Random increments were introduced to adjust the fluctuation range of sampling points to effectively avoid the obstacle space. Due to the disadvantage of a low rate of roadmap construction, a two-way incremental collision detection strategy was used to set the connection threshold between road points to reduce the number of collision detection calls. Finally, the correctness of the modified PRM algorithm was verified and analyzed using MATLAB and ROS test platforms. The test results showed that the modified PRM algorithm has obvious advantages in enhancing the stability of the roadmap, shortening the length of the planned path, and improving the search rate of the algorithm. However, the majority of current algorithms, including the modified PRM algorithm, are model-driven, and face many limitations. These algorithms need to be further researched. Data-driven and cloud-network fusion technologies could be added to these algorithms to achieve better path planning and obstacle avoidance in smart vehicles.

**Author Contributions:** Conceptualization, S.B. and J.Y.; methodology, Q.L.; software, Q.L. and S.B.; validation, Q.L. and. S.B.; formal analysis, Q.L.; investigation, Y.X.; resources, Q.L.; data curation, Q.L.; writing—original draft preparation, Q.L. and. S.B.; writing—review and editing, Q.L.; visualization, Q.L.; supervision, J.Y.; project administration, J.Y.; funding acquisition, Q.L. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was funded by Graduate Research Innovation Program Project of Jiangsu Province, China, grant number "KYCX22\_1059".

**Institutional Review Board Statement:** Not applicable.

**Informed Consent Statement:** Not applicable.

**Data Availability Statement:** The study did not report any data.

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

#### **References**


**Jun He \*, Yanlong Sun, Limin Yang and Feng Gao**

State Key Laboratory of Mechanical System and Vibration, School of Mechanical Engineering, Shanghai Jiao Tong University, Shanghai 200240, China; sunyanlong@sjtu.edu.cn (Y.S.); ylm20159@sjtu.edu.cn (L.Y.); fengg@sjtu.edu.cn (F.G.)

**\*** Correspondence: jhe@sjtu.edu.cn

**Abstract:** Amid increasing demands for planetary exploration, wide-range autonomous exploration is still a great challenge for existing planetary rovers, which calls for new planetary rovers with novel locomotive mechanisms and corresponding control strategies. This paper proposes a novel wheeled– legged mechanism for the design of planetary rovers. The leg suspension utilizes a rigid–flexible coupling mechanism with a hybrid serial–parallel topology. First, the kinematic model is derived. Then, a control strategy for the wheeled–legged rover that includes a trajectory tracking module based on the model predictive control, the steering strategy, and the wheel speed allocation algorithm is proposed. After that, three groups of cosimulations with different trajectories and speeds, and experiments are carried out. Results of both the simulations and experiments validate the proposed control method.

**Keywords:** mobile robot; advanced intelligent control; wheeled–legged; trajectory tracking; model predictive control

#### **1. Introduction**

The planetary rovers that were deployed for the exploration of the moon and Mars, such as Curiosity and Perseverance, are purely wheeled robotic systems [1,2]. They adopt the passive rocker–bogie suspension configuration. There are two identical linkage mechanisms on each side of the rover, which consist of a rocker and a bogie. A differential mechanism is adopted to connect the two linkage mechanisms. One wheel is fixed at one end of the rocker, while the bogie has two wheels that are mounted on the other end of the rocker. Recently, China's Zhurong Mars rover adopted an active rocker–bogie suspension. There was a novel angle-adjusting mechanism between the two rockers to generate a wheel-step motion that could help the rover avoid wheel slip sinkage [3]. Although many remarkable achievements have been made in the field of planetary exploration, the capability of wide-range autonomous exploration is still a great challenge for planetary rovers.

The hybrid leg–wheel mechanism can be used in the design of planetary rovers. Legged–wheeled robots have the merits of being both wheeled and legged robots. They can robustly deal with uncertainties or disturbances caused by the unstructured discontinuous terrain encountered during planetary exploration. Moreover, they have a relatively high locomotion efficiency. There are three categories of leg–wheel robotic systems that differ according to leg morphology [4]. The first one is the serial leg configuration. For example, the Jet Propulsion Laboratory (JPL) developed an articulated–wheeled lunar robot called ATHLETE [5]. Each leg was a 6R (rotational joint) serial mechanism with six degrees of freedom (DOFs). It could roll over flat smooth terrain on rotating wheels and could also use the wheels as feet to walk over irregular and steep terrain. Grand et al. [6] addressed a wheeled–legged robot called Hylos, which had 16 actively actuated DOFs, with each leg combining a two-DOF leg and the steering and rotation DOFs in the wheel. Smith et al. [7] presented the PAW, a four-legged vehicle with a T-shaped body and compliant

**Citation:** He, J.; Sun, Y.; Yang, L.; Gao, F. Model Predictive Control of a Novel Wheeled–Legged Planetary Rover for Trajectory Tracking. *Sensors* **2022**, *22*, 4164. https://doi.org/ 10.3390/s22114164

Academic Editors: Luige Vladareanu, Hongnian Yu, Hongbo Wang and Yongfei Feng

Received: 19 April 2022 Accepted: 28 May 2022 Published: 30 May 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

legs. Each leg had two DOFs, including a passive prismatic joint. The second one was a leg with a parallel topology. Xu et al. [8] proposed a parallel legged–wheeled robotic system called BIT-NAZA, which had four parallel platforms with six DOFs and a 6-UPU (universal–prismatic–universal joints) configuration. There were four active wheels that were mounted on the feet of the parallel legs. Compared to their serial counterparts, parallel legged–wheeled systems usually have more payload and stiffness [9]. In addition, the actuators of the parallel leg system can be installed on the body, and the inertia of the moving part of the leg can be reduced. The electric devices comprising the actuator, such as the encoder and the torque sensor, can be easily protected [10]. Finally, there is the third category of hybrid wheeled–legged robots, namely transformable wheeled–legged vehicles, such as the Whegs series [11,12], Quattroped [13], Wheel Transformer [14], TurboQuad [15], and STEP [16]. For these robots, the wheel and leg morphology can be switched via the active joints. Transformable leg–wheel robots often adopt simple mechanical structures to simplify the control strategy. Thus, the stability and maneuverability of hybrid robots are inevitably sacrificed [17].

In spite of the excellent kinematic characteristics of the leg mechanisms discussed above, they cannot be directly applied to extraterrestrial exploration rovers because of the existence of special requirements such as maneuverability and security. For example, planetary rovers such as Curiosity [18] have additional wheels and legs to maintain a high level of security to overcome the tough terrains of the outer planet. Exploration rovers must have a fault tolerance feature to ensure the safety of the vehicle. These rovers can continue to move and carry out exploration missions even if one or multiple actuators are not working. For instance, the wheeled–legged rover ShearpTT [19] adopted self-locking gears in the actuator design for the suspension. There are two benefits to this: The first one is that the rover would not fall down when the actuator in the knee joint is invalid. The other is that the rover can support its own weight through the self-locking mechanism without the need for additional electrical energy due to the motor brake.

Legged suspension can change the center of gravity, the body posture, the distribution of contact forces, and even raise wheels to negotiate obstacles. Hence, controlling wheeled– legged robots is more complex than traditional exploration rovers with rocker–bogie suspension, especially in terms of trajectory tracking. Lamon et al. [20] proposed a control method for three-dimensional trajectory tracking. Furthermore, feedback control based on stereo vision efficiently improved the accuracy of trajectory tracking [21,22]. For rovers with independent front and rear steering and four wheels that are driven independently, path tracking becomes more complicated. Krid et al. [23] developed a dynamics-based tracking controller on a horizontal plane using a linear quadratic regulator (LQR). LQR controllers are able to track the line trajectory with quite a good accuracy. However, there is an obvious decrease in the accuracy with regard to steering. In contrast, model predictive control (MPC) can handle complex trajectories [24]. The control algorithm based on MPC can be derived in a recursive form, which is computationally more efficient than the other methods. The computing efficiency is a key evaluation index for planetary rovers because there are very limited computing resources in space. Though the MPC method has been applied to wheeled robots [25,26], there are some differences for wheeled-legged robots. First, wheeled-legged robots possess terrain-adaptive capabilities [27]. The leg length can be adjusted by changing the knee joint angle even if the wheel is always under the hip joint. The attitude angles of the robot can be controlled through changing the leg lengths. Thus, the terrain-adaptive capability needs to be involved in the trajectory tracking when the robot runs across irregular terrains. Second, having wheels that drive independently can lead to an uneven speed distribution, resulting in the occurrence of wheel slip [28]. For wheeled–legged rovers, the pose of the body, the wheel–soil contact force, and the height of the gravitational center can all be adjusted by coordinating the motion of the hip and knee joints when the wheeled–legged robot moves over rough terrain [29]. Both the motion of the hip and knee joints affect the motion characteristics of the wheel, resulting in slippage. Therefore, it is necessary to provide a suitable speed for each wheel that is based on the motion characteristics of the whole robot on rough terrain.

In this paper, a novel wheeled–legged mechanism called TAWL is proposed for the design of planetary rovers. The leg suspension utilizes a rigid–flexible coupling mechanism with a hybrid serial–parallel topology. A kinematic model is derived first. Then, a control strategy for a wheeled–legged rover is proposed that includes a trajectory-tracking module based on MPC, the steering module, and the wheel speed allocation module. After that, a cosimulation model is established in both NX/Motion and Simulink software to verify the control strategy. Finally, experiments are also carried out to validate the proposed control method.

The remainder of the paper is organized as follows: Section 2 reports the hardware design and the kinematics of the rover; Section 3 details the control strategy; Section 4 presents the simulations, experiments, and the discussion of the results; and finally, Section 5 offers the conclusions.

#### **2. Hardware and Kinematics of the Rover**

#### *2.1. Mechanical Structure*

There are two aspects that need to be considered for leg design: First, the leg inertia must be as low as possible. Each leg has four DOFs, as illustrated in Figure 1, namely the hip abduction/adduction (HAA) joint, the hip flexion/extension (HFE) joint, the hip endo/exorotation (HEE), and the knee flexion/extension (KFE) joint. The HEE joint can also be used to steer the wheels. To reduce the rotational inertia of the robot's legs, the actuators of the HFE and KFE joints are coaxially located at the hip. The KFE joint is actuated by a pantograph mechanism. In addition, to increase the driving torques of the HFE and KFE joints, a gear reducer stage was adopted at each of their output shafts. Second, compliant mechanisms are necessary for legged robotic systems to handle uncertainties or disturbances such as ground contact collisions. A telescopic structure with a passive damped spring was used for the leg design. In addition, there are two spring ball plungers that trigger the spring–damper mechanism. When the impact force from the ground exceeds the threshold value of the spring ball plunger, the spring–damper mechanism works to dissipate the impact energy. After that, the lower leg returns to its original length with the restoring force of the spring.

**Figure 1.** The leg–wheel structure.

The TAWL robot has four identical legs with wheels. The four legs are mounted to the torso in an axially symmetric distribution, as depicted in Figure 2. Its four hip joints are located on a circle with a diameter of 1.2 m. From a biological view, to obtain highly dynamic characteristics in the longitudinal direction, the ratio of the length to the width of the torso should be more than 1. However, the wheeled mode is the primary motion mode for the TAWL robot. An axially symmetrical arrangement was adopted for the robot design so that the robot would have all-directional locomotion capability in both the wheeled and legged modes (walking or trotting). Furthermore, this arrangement also increases the number of legged locomotion modes. There are at least three leg configurations for legged locomotion, i.e., the M-configuration, O-configuration, and X-configuration.

**Figure 2.** The TAWL robot.

#### *2.2. Perception and Control System*

There are two types of sensors: proprioceptive and exteroceptive sensors. Proprioceptive sensors contain the joint encoder, the joint torque sensor, and the inertial measurement unit (IMU), as seen in Figure 3. All of the joint angles are precisely measured by absolute encoders. Since the angle measures of each motor are absolute, the robot does not have to be homed at startup. The IMU sensor is mounted on the body and is responsible for the poses of the robot's torso. Exteroceptive sensors include visual and nonvisual sensors, which are employed to measure environmental information such as the geometrical parameters of the terrain and ground contact forces. Here, a stereo vision system was attached to the front part of the main body. Furthermore, an independent computer was implemented to deal with the vision algorithms. To improve the reliability, we did not assemble force sensors for ground contact force measurements to the end of each leg. We established a distribution measurement model and then evaluated the ground contact forces using the measurement data from the joint torque sensors.

An onboard main controller was used to run the entire control program. The main controller communicates with 20 servo drives and 16 joint torque sensors in real time via the EtherCAT industrial network protocol (Bechoff, Verl, Germany). The measurement data from the IMU sensor are transferred into the main controller according to the RS-485 serial data standard. The main controller communicates with the visual controller by means of the ADS (automation device specifications) protocol. The TAWL robot's control software was developed using the TwinCAT software platform (Bechoff, Verl, Germany), a real-time PC-based control system. In addition, there are two on-board lithium batteries that the robot can use to run for about 1.5 h.

**Figure 3.** The perception and control system.

#### *2.3. Kinemactics of the Rover*

The body frame {OB—XBYBZB} of the whole robot is located at the center of the plane and is composed of the centers of four hip joints. We established the D-H coordinate systems in Figure 4 for each leg. Because each leg has the same kinematic structure, the D-H parameters of the four legs are also the same. {O0—x0y0z0} is the base frame of each leg (i.e., the leg frame), which is located at the center of the hip joint. {O4—x4y4z4} is the wheel frame of each leg. The D-H parameters are shown in Table 1. Therefore, the transformation matrix from frame *i* − 1 to *i* for the *i*th limb can be written as

$$\begin{aligned} \mathbf{a}\_i^{i-1} T &= \begin{bmatrix} c\theta\_i & -s\theta\_i c\alpha\_i & s\theta\_i s\alpha\_i & a\_i c\theta\_i\\ s\theta\_i & c\theta\_i c\alpha\_i & -c\theta\_i s\alpha\_i & a\_i s\theta\_i\\ 0 & s\alpha\_i & c\alpha\_i & d\_i\\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned} \tag{1}$$

where *s* and *c* denote the sine and cosine functions.

**Figure 4.** The coordinate systems of the rover.

**Table 1.** The D-H parameters.


For the *i*th leg, the transformation matrix from the wheel frame to the leg frame is written as

$$\begin{aligned} {}^0T^i &= {}^0T^i {}^1T^i {}^2T^i {}^3T^i {}^1T^i \\ &= \begin{pmatrix} \mathbf{R} & \mathbf{P} \\ \mathbf{0} & 1 \end{pmatrix} \\ &= \begin{pmatrix} -s\_{34} & -c\_{34} & 0 & -L\_3s\_{34} - L\_2s\_3 \\ c\_{34}c\_2 & -s\_{34}c\_2 & -s\_2 & c\_2(L\_1 + L\_3c\_{34} + L\_2c\_3) \\ c\_{34}s\_2 & -s\_{34}s\_2 & c\_2 & s\_2(L\_1 + L\_3c\_{34} + L\_2c\_3) \\ 0 & 0 & 0 & 1 \end{pmatrix} \end{aligned} \tag{2}$$

where *s*<sup>34</sup> = sin(*θ*3+*θ*4), *c*<sup>34</sup> = cos(*θ*3+*θ*4), *s*<sup>2</sup> = sin*θ*2, *c*<sup>2</sup> = cos*θ*2, *s*<sup>3</sup> = sin*θ*3, and *c*<sup>3</sup> = cos*θ*3. Here, *θ*<sup>1</sup> = *π*/2 and *P* = (*Px*, *Py Pz*) *<sup>T</sup>*. are the positions of the wheel center with respect to the leg frame.

When *P* is given, the rotational angle of each joint, *θ*2, *θ*3, and *θ*4, can be obtained as

$$\theta\_2 = \operatorname{atan}(P\_z / P\_y)\_\prime \tag{3}$$

$$\theta\_3 = -\text{atan}\left(\frac{\mathbb{C}}{A^2 + B^2 + \mathbb{C}^2}\right) - \text{atan}\left(\frac{B}{A}\right),\tag{4}$$

$$\theta\_4 = -\operatorname{atan}\left(\frac{G}{\sqrt{E^2 + F^2 - G^2}}\right) - \operatorname{atan}\left(\frac{F}{E}\right) - \theta\_3 \tag{5}$$

where

$$A = 2P\_x L\_2$$

$$B = -2(\sqrt{P\_y^2 + P\_z^2} - L\_1)L\_2$$

$$C = (\sqrt{P\_y^2 + P\_z^2} - L\_1)^2 + L\_2^2 + P\_x^2 - L\_3^2$$

$$E = 2P\_x L\_3$$

$$F = -2L\_3 \left(\sqrt{P\_y^2 + P\_z^2} - L\_1\right)$$

$$G = \left(\sqrt{P\_y^2 + P\_z^2} - L\_1\right)^2 + P\_x^2 + L\_3^2 - L\_2^2$$

#### **3. Control Strategy**

In this section, a control architecture for the wheeled–legged rover is proposed, as depicted in Figure 5. The control strategy consists of a planning layer, a controller layer, and a physical layer. First, the planner layer generates the reference trajectory. A planned path is generally composed of discrete points that come from the operator or the planner, which is based on a vision system. Using these points, a Bezier curve was adopted to produce a reference trajectory that included the time information. Thus, the derivation of the reference trajectory yielded the reference velocity. Second, the controller layer includes an MPC module, a steering module, and a wheel speed allocation module. The MPC module calculates the optimal control inputs through the last control inputs and the current state variables. The state variables can be estimated by a data fusion algorithm such as a Kalman filter and a particle filter, which is based on the proprioceptive and exteroceptive

sensors in the robotic system. Considering that the main purpose was to verify the trajectory tracking algorithm, the state variables, including the position and velocity of the robot, were measured by the vision motion capture system directly in the present experimental study. In addition, to eliminate the accumulated errors, a PID control method was added in the loop. Then, the steering module provided the speed and the steering angle of each wheel. After that, the wheel speed allocation algorithm was presented to avoid a wheel slip. Next, the leg joint angles were obtained through the inverse kinematics of the rover. Third, the physical layer received the steering angles, the wheel speeds, and the leg joint angles and sent these orders to servo drives.

**Figure 5.** The rover's control strategy.

#### *3.1. Locomotive Equations*

The rotation matrix from the body frame {B} to the world frame {W} is written as

$${}^{W}\_{B}\mathbb{R}(\mathfrak{a},\mathfrak{b},\mathfrak{v})=\begin{pmatrix}c\gamma c\mathfrak{f} & c\gamma s\mathfrak{f}s\mathfrak{a}-s\gamma c\mathfrak{a} & c\gamma s\mathfrak{f}c\mathfrak{a}+s\gamma s\mathfrak{a} \\ s\gamma c\mathfrak{f} & s\gamma s\mathfrak{f}s\mathfrak{a}+c\gamma c\mathfrak{a} & s\gamma s\mathfrak{f}c\mathfrak{a}-c\gamma s\mathfrak{a} \\ -s\mathfrak{f} & c\mathfrak{f}s\mathfrak{a} & c\mathfrak{f}c\mathfrak{a} \end{pmatrix},\tag{6}$$

where *α*, *β*, and *γ* are the fixed rotational angles with respect to the *x*, *y*, and *z* axes of the world frame, respectively.

Furthermore, the velocity of the centroid of the robot can be denoted by

$$\mathbf{V}\_{cm}^{W} = \begin{pmatrix} \dot{\mathbf{x}} \\ \dot{y} \\ \dot{z} \end{pmatrix} = \begin{pmatrix} c\gamma c\beta & c\gamma s\beta s\alpha - s\gamma c\alpha & c\gamma s\beta c\alpha + s\gamma s\alpha \\ s\gamma c\beta & s\gamma s\beta s\alpha + c\gamma c\alpha & s\gamma s\beta c\alpha - c\gamma s\alpha \\ -s\beta & c\beta s\alpha & c\beta c\alpha \end{pmatrix} \mathbf{V}\_{cm}^{B} \tag{7}$$

where *V<sup>W</sup> cm* and *V<sup>B</sup> cm* are the velocities of the body centroid for frames {W} and {B}, and *VB cm* = *vB <sup>x</sup>* , *v<sup>B</sup> <sup>y</sup>* , *v<sup>B</sup> z <sup>T</sup>* .

The yaw angle of the robot with respect to the body frame, *γB*, can be written as

$$
\tan \gamma = \tan \gamma^B c \alpha / c\beta \tag{8}
$$

where *γ* is the yaw angle in frame {W}.

Thus, we have .

$$
\dot{\gamma} = \omega\_z^\text{B} \text{cn/c}\mathfrak{B}.\tag{9}
$$

Aordingly, the kinematic equation for the trajectory tracking is obtained as

$$\dot{\mathbf{X}} = \begin{pmatrix} \dot{\mathbf{x}} \\ \dot{y} \\ \dot{\gamma} \end{pmatrix} = \begin{pmatrix} c\gamma c\mathcal{B} & 0 \\ s\gamma c\mathcal{B} & 0 \\ 0 & \frac{c\alpha}{c\beta} \end{pmatrix} \begin{pmatrix} v\_x^B \\ \omega\_z^B \end{pmatrix} + \begin{pmatrix} c\gamma s\beta s\alpha - s\gamma c\alpha & c\gamma s\beta c\alpha + s\gamma s\alpha \\ s\gamma s\beta s\alpha + c\gamma c\alpha & s\gamma s\beta c\alpha - c\gamma s\alpha \\ 0 & 0 \end{pmatrix} \begin{pmatrix} v\_y^B \\ v\_z^B \end{pmatrix} \tag{10}$$

where *X* = (*x*, *y*, *γ*) *<sup>T</sup>*, *u* = *vB <sup>x</sup>* , *ω<sup>B</sup> z T* , and . *<sup>X</sup>* <sup>=</sup> . *x*, . *y*, . *γ <sup>T</sup>* = *f*(*X*, *u*, *t*). At a reference point on the trajectory, we have *X<sup>r</sup>* = (*xr*, *yr*, *γr*) *T*, . *<sup>X</sup><sup>r</sup>* <sup>=</sup> *<sup>f</sup>*(*Xr*, *ur*, *<sup>t</sup>*) <sup>=</sup> . *xr*, . *yr*, . *γr T* , and *u<sup>r</sup>* = *vB xr*, *ω<sup>B</sup> zr<sup>T</sup>* .

Therefore, expanding Equation (10) in the Taylor series around the reference point (*Xr*, *ur*) and discarding the high order terms yields

$$\dot{X} = f(X\_{\tau}, u\_{\tau}, t) + \frac{\partial f(X, u, t)}{\partial X}|\_{(X\_{\tau}, u\_{\tau})}(X - X\_{\tau}) + \frac{\partial f(X, u, t)}{\partial u}|\_{(X\_{\tau}, u\_{\tau})}(u - u\_{\tau})\tag{11}$$

where

*∂ f*(*X*, *u*, *t*) *<sup>∂</sup><sup>X</sup>* <sup>|</sup>(*Xr* , *ur*)= ⎛ ⎜⎜⎝ *∂ f*1(*X*,*u*,*t*) *∂x ∂ f*1(*X*,*u*,*t*) *∂y ∂ f*1(*X*,*u*,*t*) *∂γ ∂ f*2(*X*,*u*,*t*) *∂x ∂ f*2(*X*,*u*,*t*) *∂y ∂ f*2(*X*,*u*,*t*) *∂γ ∂ f*3(*X*,*u*,*t*) *∂x ∂ f*3(*X*,*u*,*t*) *∂y ∂ f*3(*X*,*u*,*t*) *∂γ* ⎞ ⎟⎟⎠ = ⎛ ⎝ 0 0 −*sγrcβv<sup>B</sup> xr* + (−*sγrsαs<sup>β</sup>* − *<sup>c</sup>γrcα*)*v<sup>B</sup> <sup>y</sup>* + (−*sγrsβc<sup>α</sup>* + *<sup>c</sup>γrsα*)*v<sup>B</sup> z* 0 0 *cγrcβv<sup>B</sup> xr* + (*cγrsαs<sup>β</sup>* − *<sup>s</sup>γrcα*)*v<sup>B</sup> <sup>y</sup>* + (*cγrsβcα* + *sγrsα*)*v<sup>B</sup> z* 00 0 ⎞ ⎠, *∂ f*(*X*, *u*, *t*) *<sup>∂</sup><sup>u</sup>* <sup>|</sup>(*Xr* , *ur*)= ⎛ ⎜⎜⎝ *∂ f*1(*X*,*u*,*t*) *∂v<sup>B</sup> x ∂ f*1(*X*,*u*,*t*) *∂ω<sup>B</sup> <sup>z</sup> <sup>∂</sup> <sup>f</sup>*2(*X*,*u*,*t*) *∂v<sup>B</sup> x ∂ f*2(*X*,*u*,*t*) *∂ω<sup>B</sup> <sup>z</sup> <sup>∂</sup> <sup>f</sup>*3(*X*,*u*,*t*) *∂v<sup>B</sup> x ∂ f*3(*X*,*u*,*t*) *∂ω<sup>B</sup> z* ⎞ ⎟⎟⎠= ⎛ ⎝ *cβcγ<sup>r</sup>* 0 *cβsγ<sup>r</sup>* 0 0 *cα*/*cβ* ⎞ ⎠.

Accordingly, the state space equation can be denoted by

$$
\dot{\hat{X}} = A(t)\hat{X} + B(t)\hat{u} \tag{12}
$$

where *<sup>X</sup>*<sup>ˆ</sup> <sup>=</sup> (*<sup>x</sup>* <sup>−</sup> *xr*, *<sup>y</sup>* <sup>−</sup> *yr*, *<sup>γ</sup>* <sup>−</sup> *<sup>γ</sup>r*) *T*, . *<sup>X</sup>*<sup>ˆ</sup> <sup>=</sup> . *<sup>x</sup>* <sup>−</sup> . *xr*, . *<sup>y</sup>* <sup>−</sup> . *yr*, . *<sup>γ</sup>* <sup>−</sup> . *γr T* , *u*ˆ = (*u*ˆ*v*, *u*ˆ*ω*) = *<sup>v</sup>* − *<sup>v</sup><sup>B</sup> xr*, *<sup>ω</sup>* − *<sup>ω</sup><sup>B</sup> zr<sup>T</sup>* , *A*(*t*) = *<sup>∂</sup> <sup>f</sup>*(*X*,*u*,*t*) *<sup>∂</sup><sup>X</sup>* <sup>|</sup>(*Xr* , *ur*), and *<sup>B</sup>*(*t*) = *<sup>∂</sup> <sup>f</sup>*(*X*,*u*,*t*) *<sup>∂</sup><sup>u</sup>* <sup>|</sup>(*Xr* , *ur*). *<sup>X</sup>*<sup>ˆ</sup> is the error with respect to the reference trajectory, and *u*ˆ is its associated perturbation control input.

Using forward differences, the approximation of . *X* can be obtained as the following discrete-time form:

$$
\hat{X}(k+1) = G\_k \hat{X}(k) + H\_k \hat{u}(k) \tag{13}
$$

where *Gk* = *TAk* + *I*, and *Hk* = *TBk*. *T* and *k* are the sampling period and the sampling time. *I* is the identity matrix.

#### *3.2. Trajectory Tracking Model Based on MPC*

#### 3.2.1. Objective Function

A controller was designed for the wheeled–legged robot to track the desired trajectory precisely and stably. By changing the current and future inputs of the control system, the optimization problem is the minimization of a predicted performance cost, which is a quadratic function of the states and control inputs as follows:

$$J(t) = \sum\_{i=1}^{N\_P} \hat{X}^T(t+i|t)Q\hat{X}(t+i|t) + \sum\_{j=1}^{N\_\xi - 1} \Delta \hat{u}^T(t+i|t)R\Delta \hat{u}(t+i|t) + \rho \varepsilon^2,\tag{14}$$

where *Np* and *Nc* are the prediction and control horizons, respectively. Here, *Q* and *R* are the weighting matrices; *ρ* is the weight coefficient, and *ε* is the relaxation factor. Let

*ξ*(*k*|*t*) = " *<sup>X</sup>*ˆ(*k*|*t*) *u*ˆ(*k* − 1|*t*) # , (15)

we obtain

$$
\xi(k+1|t) = \hat{A}\_k \xi(k|t) + \hat{B}\_k \Delta \hat{u}(k|t), \tag{16}
$$

$$
\eta(k|t) = \triangle\_k \xi(k|t),
\tag{17}
$$

where *A*ˆ *<sup>k</sup>* = " *G<sup>k</sup> H<sup>k</sup>* **0** *I* # , and *B*ˆ *<sup>k</sup>* = " *Hk I* # .

Furthermore, Equations (16) and (17) can be rewritten as the following matrix form:

$$\mathcal{Y}(t) = \Omega \xi(t) + \Phi \Delta \mathcal{U} \tag{18}$$

where

$$\begin{split} \mathcal{Y}(t) &= \begin{pmatrix} \eta(k+1|t) \\ \eta(k+2|t) \\ \vdots \\ \eta(k+N\_{\mathbb{C}}|t) \\ \vdots \\ \eta(k+N\_{\mathbb{B}}|t) \end{pmatrix}, \boldsymbol{\Omega} = \begin{pmatrix} \mathsf{C}\_{k}A\_{k} \\ \mathsf{C}\_{k}A\_{k}^{2} \\ \cdots \\ \mathsf{C}\_{k}A\_{k}^{N\_{\mathbb{C}}} \\ \cdots \\ \mathsf{C}\_{k}A\_{k}^{N\_{\mathbb{B}}} \end{pmatrix}, \boldsymbol{\Delta}\boldsymbol{\Omega} = \begin{pmatrix} \mathsf{A}u(t|t) \\ \Delta u(t+1|t) \\ \cdots \\ \cdots \\ \Delta u(t+N\_{\mathbb{C}}|t) \end{pmatrix}, \text{ and} \\ \boldsymbol{\Phi} &= \begin{pmatrix} \mathsf{C}\_{k}B\_{k} & 0 & 0 & 0 \\ \mathsf{C}\_{k}A\_{k}B\_{k} & \mathsf{C}\_{k}B\_{k} & 0 & 0 \\ \cdots & \cdots & \cdots & \cdots \\ \cdots & \cdots & \cdots & \cdots \\ \mathsf{C}\_{k}A\_{k}^{N\_{\mathbb{C}}-1}B\_{k} & \mathsf{C}\_{k}A\_{k}^{N\_{\mathbb{C}}-2}B\_{k} & \cdots & \mathsf{C}\_{k}A\_{k} \\ \mathsf{C}\_{k}A\_{k}^{N\_{\mathbb{B}}}B\_{k} & \mathsf{C}\_{k}A\_{k}^{N\_{\mathbb{C}}-1}B\_{k} & \cdots & \mathsf{C}\_{k}A\_{k}B\_{k} \\ \vdots & \vdots & \ddots & \vdots \\ \cdots & \cdots & \cdots & \vdots \\ \cdots & \cdots & \cdots & \mathcal{A}\_{k}A\_{k}^{N\_{\mathbb{B}}-N\_{\mathbb{C}}-1}B\_{k} \end{pmatrix}. \end{split}$$

Equations (14) and (18) yield

$$\begin{array}{l} J(t) = \Delta \boldsymbol{U}^{T}(t) \boldsymbol{R} \boldsymbol{\Delta} \boldsymbol{U}(t) + \boldsymbol{Y}^{T}(t) \boldsymbol{Q} \boldsymbol{Y}(t) + \rho \varepsilon^{2} \\ = \Delta \boldsymbol{U}^{T}(t) \boldsymbol{R} \boldsymbol{\Delta} \boldsymbol{U}(t) + \left( \boldsymbol{\Phi} \boldsymbol{\Delta} \boldsymbol{U}(t) \right)^{T} \boldsymbol{Q} \left( \boldsymbol{\Phi} \boldsymbol{\Delta} \boldsymbol{U}(t) \right) \\ + 2 \left( \boldsymbol{\Omega} \boldsymbol{\xi}(t) \right)^{T} \boldsymbol{Q} \left( \boldsymbol{\Phi} \boldsymbol{\Delta} \boldsymbol{U}(t) \right) + \left( \boldsymbol{\Omega} \boldsymbol{\xi}(t) \right)^{T} \boldsymbol{Q} \left( \boldsymbol{\Omega} \boldsymbol{\xi}(t) \right) + \rho \varepsilon^{2}. \end{array} \tag{19}$$

Here, **Ω***ξ*(*t*) is not affected by the inputs and can thus be discarded. Therefore, the objective function is rewritten as a standard quadratic form:

$$J(\mathbf{t}) = \begin{pmatrix} \Delta \mathbf{U}^T(\mathbf{t}) & \boldsymbol{\varepsilon} \end{pmatrix} H(\mathbf{t}) \begin{pmatrix} \Delta \mathbf{U}^T(\mathbf{t}) & \boldsymbol{\varepsilon} \end{pmatrix}^T + F(\mathbf{t}) \begin{pmatrix} \Delta \mathbf{U}^T(\mathbf{t}) & \boldsymbol{\varepsilon} \end{pmatrix}^T,\tag{20}$$

.

where

$$H(t) = \begin{pmatrix} \Phi^T \mathbf{Q} \Phi + \mathbf{R} & \mathbf{0} \\ \mathbf{0} & \rho \end{pmatrix}, \; F(t) = \begin{pmatrix} \mathbf{2}(\Omega \mathbf{\tilde{x}}(t))^T \mathbf{Q} \Phi & \mathbf{0} \end{pmatrix}.$$

#### 3.2.2. Constraints

There are some constraints when the wheeled–legged robot carries out trajectory tracking tasks. The amplitude of the control input *u* and control input increment Δ*u* satisfy

$$
\mu\_{\min}(\mathfrak{t} + k|\mathfrak{t}) \le \mathfrak{u}(\mathfrak{t} + k|\mathfrak{t}) \le \mathfrak{u}\_{\max}(\mathfrak{t} + k|\mathfrak{t}),\\\ k = 0, 1 \cdots N\_{\mathfrak{t}} - 1,\tag{21}
$$

$$
\Delta \mathfrak{u}\_{\min}(t+k|t) \le \Delta \mathfrak{u}(t+k|t) \le \Delta \mathfrak{u}\_{\max}(t+k|t), \; k = 0, 1 \cdots N\_c - 1,\tag{22}
$$

where *umin* and Δ*umin* are the predefined lower bounds, and *umax* and Δ*umax* are the predefined upper bounds. Furthermore, the variable to be solved in the objective function are the control increment in the control horizon. Therefore, the constraints need be converted into the product form of the control increment and the transformation matrix.

The following relationship exists:

$$
\mu(t+k|t) = \mu(t+k-1|t) + \Delta\mathfrak{u}(t+k|t). \tag{23}
$$

Furthermore, Equation (23) can be reformulated as a matrix form:

$$\mathbf{U}(t) = E\Delta \mathbf{U}(t) + \mathbf{U}(t-1) \tag{24}$$

where

$$\mathbf{U}(t) = \begin{pmatrix} u(t|t) \\ u(t+1|t) \\ \vdots \\ u(t+N\_c - 1|t) \end{pmatrix}, \mathbf{E} = \begin{pmatrix} I & 0 & 0 & 0 \\ I & I & 0 & 0 \\ \vdots & \vdots & \ddots & \vdots \\ I & I & I & I \end{pmatrix},$$

$$\Delta \mathbf{U}(t) = \begin{pmatrix} \Delta u(t|t) \\ \Delta u(t+1|t) \\ \vdots \\ \Delta u(t+N\_c - 1|t) \end{pmatrix}, \mathbf{U}(t-1) = \begin{pmatrix} u(t-1) \\ u(t-1) \\ \vdots \\ u(t-1) \end{pmatrix}.$$

Moreover, from Equations (19) and (23), we obtain

$$\mathbf{U}\_{\min}(t) \le \mathbf{E}\Delta \mathbf{U}(t) + \mathbf{U}(t-1) \le \mathbf{U}\_{\max}(t),\tag{25}$$

where

$$\mathcal{U}\_{\min}(t) = \begin{pmatrix} u\_{\min}(t|t) \\ u\_{\min}(t+1|t) \\ \vdots \\ u\_{\min}(t+N\_{\mathcal{L}}-1|t) \end{pmatrix}, \mathcal{U}\_{\max}(t) = \begin{pmatrix} u\_{\max}(t|t) \\ u\_{\max}(t+1|t) \\ \vdots \\ u\_{\max}(t+N\_{\mathcal{L}}-1|t) \end{pmatrix}.$$

For the control increment, we have

$$
\Delta \mathbf{U}\_{\min}(t) \le \Delta \mathbf{U}(t) \le \Delta \mathbf{U}\_{\max}(t), \tag{26}
$$

where

$$
\Delta \mathbf{U}\_{\min}(t) = \begin{pmatrix} \Delta u\_{\min}(t|t) \\ \Delta u\_{\min}(t+1|t) \\ \vdots \\ \Delta u\_{\min}(t+N\_c-1|t) \end{pmatrix}, \\
\Delta \mathbf{U}\_{\max}(t) = \begin{pmatrix} \Delta u\_{\max}(t|t) \\ \Delta u\_{\max}(t+1|t) \\ \vdots \\ \Delta u\_{\max}(t+N\_c-1|t) \end{pmatrix}.
$$

Accordingly, Equations (20), (25) and (26) yield the following quadratic programming problem

$$J(t) = \begin{pmatrix} \Delta \mathcal{U}^T(t) & \varepsilon \end{pmatrix} H(t) \begin{pmatrix} \Delta \mathcal{U}^T(t) & \varepsilon \end{pmatrix}^T + F(t) \begin{pmatrix} \Delta \mathcal{U}^T(t) & \varepsilon \end{pmatrix}^T,\tag{27}$$
 
$$\text{s.t. } \mathcal{U}\_{\text{min}}(t) \le E \Delta \mathcal{U}(t) + \mathcal{U}(t-1) \le \mathcal{U}\_{\text{max}}(t),\tag{27}$$
 
$$\Delta \mathcal{U}\_{\text{min}}(t) \le \Delta \mathcal{U}(t) \le \Delta \mathcal{U}\_{\text{max}}(t).$$

Sving Equation (27) in each control cycle leads to a series of control increments in the control time domain:

$$
\Delta \mathcal{U}^\*(t) = \begin{pmatrix} \Delta u^\*(t|t) & \Delta u^\*(t+1|t) & \cdots & \Delta u^\*(t+N\_\mathbb{C}-1) \end{pmatrix}, \tag{28}
$$

Furthermore, the first element in the sequence was adopted for the actual control increment

$$
\mu^\*(t|t) = \mu(t-1|t) + \Delta\mu^\*(t|t). \tag{29}
$$

Finally, by repeating the above process in each control cycle, the desired trajectory is tracked.

#### *3.3. Streering Strategy*

Using the aforementioned MPC method, we can obtain the optimal control inputs, *<sup>u</sup>*∗(*t*|*t*) <sup>=</sup> *vB <sup>x</sup>* , *ω<sup>B</sup> z* . Furthermore, the speed and the steering angle of each wheel need to be derived. In the present study, the steering strategy in which all of the wheels make the uniform circular motion was adopted, as seen in Figure 6.

**Figure 6.** The steering strategy.

The point M is the steering center and the steering radius of the robot *R* is

$$R = \frac{\upsilon\_x^B}{\omega\_z^B},\tag{30}$$

where *ω<sup>B</sup> <sup>z</sup>* = 0. Furthermore, the steering radii of the four wheels are written as

$$\begin{cases} R\_{rf} = \sqrt{\left(\frac{l}{2} - \Delta d\right)^2 + \left(R - \frac{l}{2}\right)^2} \\ R\_{lf} = \sqrt{\left(\frac{l}{2} - \Delta d\right)^2 + \left(R + \frac{l}{2}\right)^2} \\ R\_{rr} = \sqrt{\left(\frac{l}{2} + \Delta d\right)^2 + \left(R - \frac{l}{2}\right)^2} \\ R\_{lr} = \sqrt{\left(\frac{l}{2} + \Delta d\right)^2 + \left(R + \frac{l}{2}\right)^2} \end{cases} \tag{31}$$

According to Ackermann's principle, the wheel speeds and the steering angles are obtained as follows:

$$\begin{cases} \omega\_{rf} = \frac{\omega\_z^{\beta} R\_{rf}}{\frac{R\_{w}}{\omega\_z}}\\ \omega\_{lf} = \frac{\omega\_z^{\beta} R\_{lf}}{\frac{R\_{w}}{\omega\_z}}\\ \omega\_{rr} = \frac{\omega\_z^{\beta} R\_{rr}}{\frac{R\_{w}}{\omega\_{ll}}}\\ \omega\_{lr} = \frac{\omega\_z^{\beta} R\_{lr}}{\frac{R\_{w}}{\omega\_w}} \end{cases} \tag{32}$$

$$\begin{cases} \delta\_{rf} = \operatorname{sign}(k) \tan^{-1} \frac{\frac{l}{\varGamma} - \Delta d}{\frac{l}{R} - L/2} \\\delta\_{lf} = \operatorname{sign}(k) \tan^{-1} \frac{\frac{l}{\varGamma} - \Delta d}{\frac{l}{R} + L/2} \\\delta\_{rr} = -\operatorname{sign}(k) \tan^{-1} \frac{\frac{l}{\varGamma} + \Delta d}{\frac{l}{R} - L/2} \\\delta\_{lr} = -\operatorname{sign}(k) \tan^{-1} \frac{\frac{l}{\varGamma} + \Delta d}{\frac{l}{R} + L/2} \end{cases} \tag{33}$$

where *ωr f* , *ωl f* , *ωrr*, and *ωlr* are the wheel speeds, and *δr f* , *δl f* , *δrr*, and *δlr* are the steering angles. Here, *sign*(*k*) is a signum function, and *sign*(*k*) = −1 when the wheel rotates clockwise; *sign*(*k*) = 1 when the wheel rotates anticlockwise. When *ω<sup>B</sup> <sup>z</sup>* = 0 in Equation (30), the four steering angles are all zero, i.e., *δr f* = *δl f* = *δrr* = *δlr* = 0.

#### *3.4. Wheel Speed Allocation (WSA)*

The WSA module calculates the suitable speed for each wheel according to the motion characteristics of the whole robot on rough terrain.

First, according to the kinematic equations, the linear velocity of the wheel center and the angular velocity of the lower leg (i.e., Frame 4) in Figure 4 are written as

$$
\begin{pmatrix} \boldsymbol{\upsilon}\_{iw} \\ \boldsymbol{\omega}\_{iw} \end{pmatrix} = \begin{pmatrix} {}^{4}\_{B}\boldsymbol{R}\_{i} & {}^{4}\_{B}\boldsymbol{R}\_{i}\boldsymbol{\mathcal{S}}\_{iw} \\ 0 & {}^{4}\_{B}\boldsymbol{R}\_{i} \end{pmatrix} \begin{pmatrix} \boldsymbol{\upsilon}\_{i0}^{B} \\ \boldsymbol{\omega}\_{i0}^{B} \end{pmatrix} + {}^{4}\_{0}\boldsymbol{R}\_{i}\boldsymbol{I}\_{i} \begin{pmatrix} \dot{\boldsymbol{\theta}}\_{i1} \\ \dot{\boldsymbol{\theta}}\_{i2} \\ \dot{\boldsymbol{\theta}}\_{i3} \\ \dot{\boldsymbol{\theta}}\_{i4} \end{pmatrix},\tag{34}
$$

where *Siw* is the position vector of the wheel center with respect to the body frame; <sup>4</sup> *<sup>B</sup>Ri* is the rotation transformation matrix from the body frame to frame 4; <sup>4</sup> <sup>0</sup>*Ri* is the rotation transformation matrix from the leg frame to frame 4; *Ji* is the Jacobian matrix with respect to the leg frame; *i* = 1–4 denotes the leg number; and *v<sup>B</sup> <sup>i</sup>*<sup>0</sup> and *<sup>ω</sup><sup>B</sup> <sup>i</sup>*<sup>0</sup> are the linear and angular velocities of the leg frame with respect to the body frame, which are given by

$$
\begin{pmatrix} \boldsymbol{\sigma}\_{i0}^{\mathbb{B}} \\ \boldsymbol{\omega}\_{i0}^{\mathbb{B}} \end{pmatrix} = \begin{pmatrix} \boldsymbol{\omega}^{\mathbb{B}} \times (\boldsymbol{\mathcal{O}}\_{0i} - \boldsymbol{\mathcal{M}}) \\ \boldsymbol{\omega}^{\mathbb{B}} \end{pmatrix}, \tag{35}
$$

where *ω<sup>B</sup>* is the angular velocity of the body.

Second, the ideal (no-slip) linear velocity of the wheel center comes from the driving motor and the rotation of the lower leg, which can be denoted by

$$||v\_{iw}|| = (\omega\_{iwz} + \omega\_{id})R\_{w\prime} \tag{36}$$

where *ωiwz* is the projection of *ωiw* on the direction of the wheel axis.

Accordingly, the rotational speeds of the wheel motors are obtained by

$$
\omega\_{id} = \frac{||\boldsymbol{v}\_{iw}||}{R\_w} - \omega\_{iwz}.\tag{37}
$$

#### **4. Results and Discussion**

*4.1. Simulations*

The numerical program of the control strategy was first developed using MATLAB software. Then, the joint simulation model was established by SIMULINK and UG Motion software. UG motion software provides the joint angles as well as the pitch and roll angles for each control block. In the meantime, the control blocks calculate the joint angles and wheel speeds and provide them to the virtual prototype. There are some system and control parameters that can be grouped into three categories, namely the input parameters, the output parameters, and the control parameters, as seen in Table 2. The input parameters include the points on the reference path, the desired yaw angle of the robot body, and the desired linear and angular velocities of the robot body. The output parameters are the linear and angular velocities of the robot body from the MPC and the steering angles and the wheel speeds. In addition, there are control parameters, including the prediction and control horizons, the weight coefficient and the relaxation factor, and the PID parameters to control the linear and angular velocities of the robot body.


**Table 2.** The system and control parameters.

First, the WSA module was verified. For the simulation, the terrain included two trapezoid and two arc obstacles. The posture as well as the linear and angular velocities of the body, the joint angles and the angular velocities of the joints, and the driving speeds of the wheels were all measured using the virtual model in UG Motion software. Note that the terrain with the obstacles led to the changes of rover attitude angles (*α*, *β*). Here, the terrain-adaptive algorithm in [27] was adopted to control the rover attitude. With this algorithm, the robot attitude was almost kept unchanged in irregular terrains. Then, the practical linear velocity of the wheel centers and the practical angular velocity of the wheels were calculated according to Equations (36) and (37). Therefore, the slip percent could be obtained from

$$\mu = \frac{(\left(\omega\_{id} + \omega\_{iwz}\right)R\_w - v\_i)}{(\omega\_{id} + \omega\_{iwz})R\_w} \times 100\% \tag{38}$$

where *Rw* is the radius of the wheel; *ωid* is the practical angular velocity of the wheel, which can be measured by the wheels' encoders; and *vi* is the practical linear velocity of the wheel center. Figure 7 shows the comparison of the slippage percentages with and without the WSA module. Without the WSA module, the slippage reached up to 0.25, while with the WSA, the maximum of the slippage was less than 0.13. It was found that wheel slip was obviously decreased by the WSA component.

Second, the trajectory tracking based on MPC was verified. An arc trajectory with a radius of 30 m in the plane was selected for the validation simulation. In the simulations, there were three speeds, i.e., 0.1 m/s, 0.2 m/s, and 0.4 m/s. The control parameters for the simulations of the linear trajectory were: *Np* = 6; *Nc* = 3; *ρ* = 10; *ε* = 0 for the lower limit and *ε* = 10 for the upper limit. The PID parameters for the linear velocity were set as: *kp*<sup>1</sup> = 2, *ki*<sup>1</sup> = 1, *kd*<sup>1</sup> = 0. Since there was only a linear velocity in the body frame, the PID module for the control of angular velocities did not work. Figures 8–10 show the trajectory tracking results for three speeds. It was found that the robot could track the corresponding target values within a short period of time under the three different speeds. There was a large increase in trajectory errors at the beginning of the tracking process. The reason for this is that the initial direction of the target speed was the same as the *x* axis in the global coordinate system. There was an obvious delay before the actual speed reached the target value, and the speed error was relatively larger at the beginning. Furthermore, it was found that there were obvious overshoots in the velocity responses from the MPC method in the beginning. These overshoots facilitated trajectory tracking, and thus, the forward velocity of the robot could approximate the desired value quickly. In the meantime, the overshoot

increased as the desired speed increases. The overshoot at 0.4 m/s was the largest one among the three forward speeds. It should also be noted that the final velocity response errors increased as the target speed increased. However, as a whole, the trajectory errors for the three speeds were all relatively small, validating the MPC module and the whole control strategy.

**Figure 7.** Slippage in simulations: (**a**) leg 1; (**b**) leg 2; (**c**) leg 3; (**d**) leg 4.

**Figure 8.** The simulation results (*v* = 0.1 m/s): (**a**) trajectory; (**b**) velocity.

**Figure 9.** The simulation results (*v* = 0.2 m/s): (**a**) trajectory; (**b**) velocity.

**Figure 10.** The simulation results (*v* = 0.4 m/s): (**a**) trajectory; (**b**) velocity.

To further validate the control strategy, a more complicated trajectory, i.e., an S-type trajectory was selected for the tracking simulations. The S-type trajectory consisted of two semicircles with a radius of *R* = 20 m, which can be described as follows:

$$\mathbf{x}\_{cr}^{w} = \begin{cases} \begin{array}{c} R\sin(\omega\_z t), \; 0 \le t < T\_s\\ R\sin[-\omega\_z(t - T\_s)], T\_s \le t \le 2T\_s \end{array} \end{cases} \tag{39}$$

$$\begin{cases} y\_{\sigma}^{w} = \begin{cases} R - R\cos(\omega\_{z}t), \; 0 \le t < T\_{\text{s}} \\ 2R - R\cos(\omega\_{z}T\_{\text{s}}) - R\cos[-\omega\_{z}(t - T\_{\text{s}})], T\_{\text{s}} \le t \le 2T\_{\text{s}} \end{cases} \end{cases} \tag{40}$$

$$\gamma\_{cr}^{w} = \begin{cases} \ \omega\_z t, \ 0 \le t < T\_s\\ \ \omega\_z T\_s - \omega\_z (t - T\_s), T\_s \le t \le 2T\_s \end{cases} \tag{41}$$

where *vx* = 0.4 m/s; *ω<sup>z</sup>* = *vx*/*R*; *Ts* = *π*/*ωz*. During the simulation, the control parameters were set as: *Np* = 6; *Nc* = 3; *ρ* = 10; *ε* = 0 for the lower limit and *ε* = 10 for the upper limit. The PID parameters for the linear velocity were set as: *kp*<sup>1</sup> = 0.7, *ki*<sup>1</sup> = 0, *kd*<sup>1</sup> = 5. Figure 11 gives the comparisons of the theoretical and real trajectories and velocities. It was found that the robot could track well the reference trajectory and the reference velocity when running along the S-type trajectory. Furthermore, the errors in the x and y coordinates were very small, the relative error of which were less than 2% and 1.75%, respectively, as seen in Figure 12.

**Figure 11.** S-type trajectory and velocity: (**a**) trajectories; (**b**) velocities and errors.

**Figure 12.** Coordinates and errors for S-type trajectory: (**a**) *x* coordinate; (**b**) *y* coordinate.

In addition, tracking simulations for high speeds were also carried out. Two speeds, i.e., *vx* = 2 m/s and 4 m/s, were chosen, which were ten times as large as the speeds in the previous simulations. A circle trajectory with a radius of *R* = 35 m was selected for the simulations, which can be described by

$$\begin{cases} \begin{aligned} \chi\_{cr}^w &= R \sin(\omega\_z t) \\ \jmath\_{cr}^w &= R - R \cos(\omega\_z t) \\ \gamma\_r^w &= \omega\_z t \end{aligned} \end{cases} \tag{42}$$

where *ω<sup>z</sup>* = *vx*/*R*. The control parameters for the simulations of the linear trajectory were set as: *Np* = 6; *Nc* = 3; *ρ* = 10; for the lower limit and *ε* = 10 for the upper limit. The PID parameters for the linear velocity were set as: *kp*<sup>1</sup> = 2, *ki*<sup>1</sup> = 1, *kd*<sup>1</sup> = 0. Figure 13 gives the changes of the real velocities. It was found that the robot could still track the reference velocity after a relatively short time. Moreover, with the control, the robot could track the reference trajectories of both the *x* coordinate and the *y* coordinate, depicted in Figures 14 and 15. Compared to the lower speed, the position errors increased. However, the relative errors of the position points were small. The maxima of the relative position errors at 2 m/s and 4 m/s were less than 3% and 8.5%, respectively.

**Figure 13.** Velocity results with higher speeds: (**a**) velocity; (**b**) error of velocity.

**Figure 14.** *x* Coordinates with higher speeds: (**a**) *x* coordinate; (**b**) error of *x* coordinate.

**Figure 15.** *y* Coordinates with higher speeds: (**a**) *y* coordinate; (**b**) error of *y* coordinate.

#### *4.2. Experiments*

To further verify the control strategy, an experimental setup based on the NOKOV vision motion capture system was established that consisted of six cameras, as shown in Figure 16. An L-type tool was used for the benchmark calibration of the vision system. After there were enough cameras placed around the robot, the c vision capture system was calibrated. The L-type tool was mounted on the body, as seen in Figure 17. There are four markers on the L-type tool. The cameras recognize the markers, and thus, the vision

frame (i.e., the global coordinate system) can be established. According to the geometrical relationship between the mounting location of the L-type tool and the robot's body frame, the initial transformation matrix between the vision frame and the body frame could be obtained. Serval markers were bonded to the body of the rover, and thus, the body coordinate system could be established in the world coordinate system. Therefore, the real motion trajectory of the rover could be measured in real time and sent to the rover control system. To clarify the effectiveness of the WSA module, a terrain with a flat surface and two trapezoids was employed in the experiments. Since the terrain included obstacles, the terrain-adaptive algorithm [27] was adopted, similar to the simulations. During the experiments, the control parameters for the MPC module were set as: *Np* = 6; *Nc* = 3; *ρ* = 10; *ε* = 0 for the lower limit and *ε* = 10 for the upper limit. The PID parameters for the control of linear velocities were set as: *kp*<sup>1</sup> = 8, *ki*<sup>1</sup> = 0, *kd*<sup>1</sup> = 0.2. Figure 18 shows the slippage in the experiments. Note that the slippages of Leg 1 and 4 (Leg 2 and 3) are almost the same because they suffer the same terrain condition. It was found that the average slippage of all of the legs demonstrated an obvious decrease after WSA control, up to 20%. Figure 19 shows the experimental results of trajectory tracking. As it can be seen, the rover could strictly track the reference trajectory. The deflection with respect to the reference trajectory was less than 2% F.S., which is a relatively small error. Accordingly, the control strategy was validated by the experiments.

**Figure 16.** The experimental setup.

**Figure 17.** The calibration of the vision frame.

**Figure 18.** Slippage in the experiments: (**a**) Leg 1; (**b**) Leg 2; (**c**) Leg 3; (**d**) Leg 4.

**Figure 19.** Experimental trajectory tracking results: (**a**) *x* axis; (**b**) *y* axis.

#### **5. Conclusions**

In this paper, a novel wheeled–legged planetary rover with four legs was proposed, and each leg had four DOFs with an actuated wheel. The articulated legs utilized a serial–parallel hybrid configuration, and it had the merits of both serial and parallel mechanisms. Moreover, the legs had a rigid–flexible coupling structure that could conform to unstructured terrain using both active and passive compliance. The kinematics equations of the rover were derived. Then, a control scheme including trajectory tracking, the steering

strategy, and the WSA module was proposed. A trajectory tracking model based on MPC that could handle the line and arc trajectory with quite a good accuracy was established. In addition, the WSA was introduced into the control strategy to decrease the slippage. After that, to validate the control method, three groups of cosimulations, i.e., tracking an arc trajectory, tracking a S-type trajectory, and trajectory tracking with high speeds were carried out. Finally, trajectory tracking experiments were conducted through a vision motion capture system. It was found that the average slippage of all of the legs decreased obviously after WSA control, with a slippage up to 20% in our experiments. Moreover, the rover could strictly track the reference trajectory. With respect to the reference trajectory, the deflection was found to be less than 2% F.S., which is a relatively small error. Accordingly, the proposed control strategy was thoroughly verified by the simulations and experiments.

**Author Contributions:** Conceptualization, J.H. and Y.S.; methodology, J.H. and Y.S.; software, Y.S. and L.Y.; investigation, Y.S. and L.Y.; writing—original draft, J.H.; writing—review and editing, J.H.; supervision, F.G.; funding acquisition, J.H. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was funded by National Natural Science Foundation of China, grant no. 52175022, the State Key Laboratory of Mechanical System and Vibration, grant no. MSVZD202106, and Shanghai R&D public service platform project, grant no. 19DZ2291400.

**Institutional Review Board Statement:** Not applicable.

**Informed Consent Statement:** Not applicable.

**Data Availability Statement:** Data will be made available upon reasonable request by the corresponding author.

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

#### **References**

