Spatial based path following MPC of SSMR with static obstacle avoidance is developed in MATLAB environment with ACADO optimization toolkit using spatial kinematic model of SSMR. Inspired by [31] and [16], this work proposes a data-driven MPC of SSMR with a kinematic model represented by a deep neural network.
Kinematic modelling of SSMR
Previous research has reported a method to obtain an optimized kinematic model for sliding tracked vehicles by experimentally determining the limitations of the instantaneous centers of rotation (ICR) of the tread on the plane of motion. The result is a kinematic model for skid steer vehicles that can include the influence of vehicle dynamics by considering only two points in local coordinates, which are 𝐼𝐶𝑅𝑙 and 𝐼𝐶𝑅𝑟.
Model Predictive Control
Hence, in the new trajectory tracking error based model, the inputs are the gas pedal position and steering angles. The covariance associated with the trajectory indicates the level of confidence the path planner has in the effectiveness of his calculated trajectory.
Path following Control
A conventional approach to dealing with motion planning and control for AGVs involves solving two separate problems. Such a design can perform local motion planning in unstructured environments with static and moving obstacles, including people.
Learning-based Model Predictive Control
Spatial SSMR kinematics
The kinematic model of the skid steer robot is established based on the diagram in Figure 4-1. The robot is modeled as a solid rectangular body driven by four wheels that stay in place. The robot is assumed to follow a designated path, denoted by 𝑃, while simultaneously varying the virtual instantaneous centers of rotation for the center of mass (𝐼𝐶𝑅), the left wheel (𝐼𝐶𝑅𝑙) and the right wheel (𝐼𝐶𝑅𝑟), in accordance with the previous works [25 ].
The symbols 𝑋, 𝑌, 𝜃, 𝑣𝑥, and 𝜔 represent two position and one orientation coordinates, as well as the longitudinal and angular velocities of the robot's center of mass, respectively. The fundamental principle of path following control is to guide the robot along a specified path while maintaining a predetermined forward speed profile. In path-following control, the target reference frame is a moving frame represented by 𝑃, as illustrated in Figure 4-1.
The path curvature 𝑐(𝑠), defined as a function of the arc length 𝑠 of the path, is an extrinsic parameter in the represented state space model following the path. According to [20], the value of 𝑐(𝑠) can be calculated using the definition of path coordinates as a function of arc lengths 𝑥𝑓(𝑠), 𝑦𝑓(𝑠) as well as theirs. As stated in [25], the longitudinal and angular velocities (𝑣𝑥 and 𝜔) of the SSMR can be translated into the velocities of its left and right wheels (𝑣𝑟 and 𝑣𝑙), taking into account the correction factors 𝛼𝑟 and 𝛼𝑙 to account for the condition of the wheels and/or the characteristics of the terrain surface :.
![Figure 4-1: 4-wheeled skid-steer mobile robot spatial kinematics based on Fig. 1 in [15].](https://thumb-ap.123doks.com/thumbv2/azdokorg/10950275.0/38.918.141.775.439.942/figure-wheeled-steer-mobile-robot-spatial-kinematics-based.webp)
MPC Configuration
The term is defined as follows: 4.4) The original path by kinematics, as formulated in [34], used an ideal unicycle robot model. The movement of the moving reference frame along the path is determined by the path state variable 𝑠. The velocity of the moving reference frame along the path (𝑣𝑠) is calculated as 𝑣𝑠 =𝑣𝑥cos𝜃𝑒+𝑥𝑖𝑐𝑟sin𝜃𝑒𝜔.
The error coordinates are defined as the deviation of the robot's position, orientation and velocity from the desired trajectory. This term represents the speed of the moving frame of reference along the path, which is approximated as the SSMR velocity along its 𝑥𝑒 axis. In terms of limitations, the speeds and accelerations of the left and right wheels are limited so as not to exceed the SSMR cars' limits.
The value of 𝑠 for each time step is calculated using the odometry of the SSMR and the MPC prediction of the future movement of the SSMR. This allows the MPC to determine the path curvature for each point along the forecast horizon, which is necessary for the SSMR to follow the desired path. First, it is necessary to estimate the robot's path coordinate 𝑠 using the coordinates 𝑥 and 𝑦 along with the functions 𝑥𝑓(𝑠) and 𝑦𝑓(𝑠).
Simulation
Furthermore, the track dimensions consisted only of the track width, which was determined to be 5.5 m and was used in terms of boundary avoidance. These values were set so that the kinematic model does not assume wheel slip and assumes that the instantaneous centers of rotation for the left and right wheels are located in the middle of the respective sides of the robot. The weights were manually adjusted to ensure that the robot could successfully complete a lap around the track without any collisions or boundary violations while traveling at speeds up to 3 m/s.
The performance evaluation of the MPC control system was done under different parameters for the target speed and the forecast horizon. The initial evaluation of the controller included an assessment of the robot's ability to follow a path at various target velocities. The charts reveal that SSMR was able to maintain target speed for most of the lap with only minor deviations around obstacles.
To assess the accuracy of obstacle-free tracking, the average error was calculated from the second half of the track where there are no obstacles. Longer prediction horizons allow the MPC to generate control signals that minimize deviation from the target trajectory around obstacles, as shown in the figure. The MPC algorithm was able to generate control signals that allowed the SSMR to reach speeds of up to 3 m/s while following the desired path and avoiding obstacles.
Time-domain MPC in ACADOS-Python
Time-Domain Kinematic Model of SSMR
It can be observed that, after the first time step, the computation time in the worst case does not exceed the 50ms sampling interval and is only 6.10ms on average for a 3m/s target, as shown in Table 4.1. Tests performed on the MPC approach have shown that the design objectives were successfully achieved while maintaining a reasonable computational cost.
MPC Configuration
This MPC is made to reach a point in 2-dimensional space denoted by 𝑥𝑟𝑒𝑓, 𝑦𝑟𝑒𝑓 in the reference vector ℎ𝑟𝑒𝑓 so as to minimize the difference between the current state and the reference.
Simulation
Time-Domain Data-Driven MPC in ACADOS-Python
- Data Generation
- Architecture and Training
- MPC Configuration
- Simulation
In WeBot's real-time simulation, the MPC controller was successfully implemented and was able to repeatedly reach the desired target point as shown in Figure 4-9. Neural network architecture is a simple multilayer perceptron with 3 hidden layers which are 32, 128 and 128 units wide, ReLU activation, dropout layer and a linear output head. Training was performed for up to 500 epochs using Adam optimizer [19] with early stop schedule and learning rate.
As with the time domain MPC discussed earlier, the goal here is to reach a goal point in the environment depicted in 3-1 under the same assumptions. As before, MPC uses the learned state space model to solve the optimal control problem (OCP) and applies the resulting control signals to the SSMR. In the learned state space model, derivatives of system states 𝑥, 𝑦, 𝜃, 𝑣𝑙, 𝑣𝑟 are predicted by the data-driven model based on the current system state and control signal.
The optimal control problem, MPC cost, references, constraints, system parameters, and feedback data were unchanged from the time domain MPC of Section 4.2. The offline simulation demonstrated in Figure 4-13 shows that the data-driven MPC successfully computes a path to a goal with the corresponding control inputs, albeit with some inherent discontinuities for the neural network. In the real-time simulation, since the SSMR motion is no longer estimated by the learned model itself, the data-driven MPC was able to reliably reach the target despite the disruption seen in the offline tests (figure 4-14).
Spatial-Based MPC in ACADOS-Python
MPC configuration
Similar to the MATLAB-ACADO version, the control objective is to drive the SSMR along the reference trajectory in real time while avoiding the boundaries without avoiding static obstacles. The SSMR kinematic model and state space model remain as the MATLAB-ACADO implementation of Section 4.1. However, since ACADOS does not allow custom terms in its default cost function and requires the programming of a special CASADI cost function for this functionality, the OCP has been modified to target the progression of the 𝑠 path for simplicity.
The above OCP aims to minimize the error coordinates representing the difference between the reference trajectory and the SSMR trajectory. The feedback data collection and calculation approach was unchanged from the MATLAB-ACADO version in principle; the methods were re-implemented in Python using the scipy library for cubic splines and numpy.
Simulation
Spatial-Based Data-Driven MPC in ACADOS-Python
Learned Model Architecture and Training
MPC Configuration
Compared to the state-space model used in MATLAB and Python implementations of path by MPC discussed in Sections 4.1 and 4.4. Data-driven state-space model used here is a hybrid of nominal model proposed in [16] and learned model developed in Section 4.3. Given system state vector𝑥= [𝑥𝑒,𝑦𝑒,𝜃𝑒,𝑣𝑙,𝑣𝑟, 𝑠]and parameter𝐾 that defines the ratio of nominal and learned model contributions to the state-space model.
Simulation
The developed SSMR control strategies include spatially-based and obstacle-avoiding path-following control in the MATLAB-ACADO framework, time-domain control in the Python-ACADOS framework, data-driven time-domain control in the Python-ACADOS framework, spatially-based path-following control in the Python-ACADOS framework, data-driven, spatially based path-following control in the Python-ACADOS framework. Time domain control in the Python-ACADOS framework was developed with the aim of navigating SSMR to a target point using the time domain variant of the SSMR kinematic model proposed in [21]. Data-driven time domain control was created by extending the time domain control in the Python-ACADOS framework via the PyTorch-Casadi framework presented in [31].
This allowed the creation of an MPC strategy that incorporated data-driven components into its state space model. The Python-ACADOS implementation of MPC spatial path tracking was made as a re-implementation of the MATLAB-ACADO version in a new framework. Similar to data-based time-domain control, data-based trajectory tracking control was developed as an extension of space-based trajectory tracking control in the Python-ACADOS framework.
A data-driven dynamic model from a data-driven time-domain control implementation was combined with nominal space-based path kinematics following control to create a hybrid SSMR state-space model. The hybrid model was incorporated into the optimal control problem to create a new data-driven approach. Controlling wheeled mobile robots with sliding control along a path at higher speeds on different types of terrain.
