Investigation of Internal Model for Unmanned Vehicle Control in Case of Its Aggressive Motion along a Spatial Trajectory ^†

Abstract

The work is devoted to the study of methods that are used to control the movement of an object along a given trajectory. A control method involving an accurate internal model is proposed. This internal model was built on the basis of the object’s mathematical model and real object, performed by artificial neural networks. For a limited period of time the model is able to determine the object state without surveillance system usage. The dynamic model of an unmanned vehicle was obtained by method developed at the Robotics Center of the FRC CSC RAS. This method acquires experimental data and performs model identification by means of a neural network. The trajectory is a set of spatial points generated by the developed real unmanned vehicle simulator. The control was carried out on the basis of PID-controller and model predictive control method. The comparison of control methods for a real and virtual unmanned vehicles was conducted in the simulator developed. The results of field experiments, during which control by internal model was applied, are presented.

1. Introduction

Unmanned vehicles control from the initial state to the final one should meet the desired quality requirements. At the same time, it is necessary to synthesize such control laws that ensure the achievement of the control goal in the wide class of the uncertainty of vehicle’s dynamics. To synthesize the required control laws, the unmanned vehicles control system simulation should be performed. A mathematical model of a real system can be obtained with the help of well-known identification methods [1,2]. Herein the following system identification methods are used, ARMAX (autoregressive moving average models with exogenous inputs) and NARMAX (non-linear ARMAX). These methods allow to obtain linear and non-linear polynomial functions that model the relationship between the perception of the robot sensor and its motor response. These methods are extended to neural network structures [3] and are successfully applied to non-linear unmanned vehicles.

One of the approaches in the field of unmanned mobile systems, such as unmanned vehicles operating in automatic mode, is aimed at the development of non-linear control laws for real time trajectories tracking [4,5,6].

Unmanned vehicles’ real-time trajectory control is usually divided into three sub-tasks, trajectory generation, position determination, and trajectory tracking.

This paper presents the results of model identification of a real unmanned vehicle. Kinematic equations of unmanned vehicle describe mass center position $(x,y)$ and direction of movement $\left(\theta \right)$. These parameters depend on linear velocity $\left(v\right)$ and rotation angle of front wheels $\left(\alpha \right)$. Unmanned vehicle and its kinematic movement control scheme are presented in Figure 1.

Artificial neural networks belong to modern model identification methods used in robotics. Combined with differential equations, ANNs provide satisfying results in real non-linear dynamics systems identification.

The article presents a formal statement of the model identification problem. A universal strategy for its solution that uses ANNs is proposed. The obtained model, or so-called the internal one, is applied in a special simulator developed to automate path generation and the process of obtaining the state of the unmanned vehicle that is conducted in selected control methods in case of some parametrical changes.

2. Unmanned Vehicle Model Identification

The unmanned vehicle (UV) is built on the basis of an automobile chassis on a scale of 1:10, has a weight of 1.5 kg and can reach speed up to 10 m/s. The appearance of the UV is shown in Figure 1a. The front wheels of the UV implement steering, the rear ones monitor the state of the robot. Figure 1b shows the kinematic movement control scheme of the unmanned vehicle, where $x,y$ refer to the coordinates; $\alpha$ is the angle of front wheels rotation; $\theta$ is the UV orientation relative to the x axis; L is the distance between the front and rear axes of the robot; and R is the base point located in the center of the robot’s rear axis.

Experimental data on the vehicle movement was obtained according to a special technique developed in the Robotics Center of the FRC CSC RAS.

The vehicle model consists of hybrid finite-difference equations of the following form

$$\mathbf{x}\left(k\right)=\left[\begin{array}{c}x\left(k\right)\\ y\left(k\right)\\ \theta \left(k\right)\\ v\left(k\right)\end{array}\right]=\left[\begin{array}{c}x(k-1)+\Delta tv(k-1)cos\left(\theta \right(k-1\left)\right)\\ y(k-1)+\Delta tv(k-1)sin\left(\theta \right(k-1\left)\right)\\ f^{\theta }(v(k-1),\theta (k-1),u\left(k\right),\omega \left(k\right),\Delta t)\\ f^v(v(k-1),\theta (k-1),u\left(k\right),\omega \left(k\right),\Delta t)\end{array}\right],k=1,\dots ,K,$$

(1)

where x(k) is a state vector,

$$\mathbf{x}\left(k\right)={\left[\begin{array}{cccc}x\left(k\right)& y\left(k\right)& \theta \left(k\right)& v\left(k\right)\end{array}\right]}^T={\left[\begin{array}{ccc}x\left(k\right)& y\left(k\right)& \mathbf{u}\left(k\right)\end{array}\right]}^T,$$

(2)

$v\left(k\right)$ is the velocity of the object, $\theta \left(k\right)$ is the UV orientation relative to the x axis, $\omega \left(k\right)$ is the component of control for orientation $\theta \left(k\right)$, $u\left(k\right)$ is the component of control for linear velocity $v\left(k\right)$, $\Delta t$ is the time span, $f^{\theta }(v(k-1),\theta (k-1),u\left(k\right),\omega \left(k\right),\Delta t)$ and $f^v(v(k-1),\theta (k-1),u\left(k\right),\omega \left(k\right),\Delta t)$ are the outputs of a neural network, K is the number of control steps.

Object movement is determined by orientation $\theta \left(k\right)$ and velocity $v\left(k\right)$ being predicted by neural network. The inputs of the neural network include orientation and speed control signals $\omega \left(k\right)$ and $u\left(k\right)$ along with the previous state of orientation $\theta (k-1)$ and velocity $v(k-1)$. At the output, we obtain the current values of orientation $\theta \left(k\right)$ and velocity $v\left(k\right)$.

The neural network has the following structure

$${\mathbf{z}}^{\left(l\right)}={\mathit{\phi }}^{\left(l\right)}\left({\mathbf{W}}^{\left(l\right)}{\mathbf{z}}^{(l-1)}+{\mathbf{z}}_0^{\left(l\right)}\right),l=1,\dots ,L,$$

(3)

where ${\mathbf{z}}^{\left(l\right)}$ is the output vector of layer l, ${\mathit{\phi }}^{\left(l\right)}\left(\mathbf{s}\right)$ is a vector activation function of layer l, ${\mathbf{W}}^{\left(l\right)}$ is a weight matrix of layer l, ${\mathbf{W}}^{\left(l\right)}=\left[w_{i,j}^{\left(l\right)}\right],i=1,\dots ,n_l,j=1,\dots ,n_{l-1}$, dim${\mathbf{W}}^{\left(l\right)}=n_l\times n_{l-1}$, ${\mathbf{z}}_0^{\left(l\right)}$ is a bias of l, ${\mathbf{z}}_0^{\left(l\right)}={\left[\begin{array}{ccccc}v(k-1)& \theta (k-1)& u\left(k\right)& \omega \left(k\right)& \Delta t\end{array}\right]}^T$, L is the number of layers, ${\mathbf{z}}^{\left(L\right)}={\left[\begin{array}{cc}\theta \left(k\right)& v\left(k\right)\end{array}\right]}^T$, $n_0=5$, $n_L=2$.

Activation function ${\mathit{\phi }}^{\left(l\right)}\left(\mathbf{s}\right)$ is applied to each component of the state vector

$${\mathit{\phi }}^{\left(l\right)}\left(\mathbf{s}\right)={\left[\begin{array}{ccc}{\phi }_1^{\left(l\right)}\left(s_1\right)& \dots & {\phi }_{n_l}^{\left(l\right)}\left(s_{n_l}\right)\end{array}\right]}^T.$$

The sought-for parameters of the neural network include the components of the weight matrix and bias of each layer. The total number of the parameters sought is ${\sum }_{l=1}^L{n}_l(n_{l-1}+1)$.

The quality criterion for neural network parameter optimization is the root-mean-square error of state prediction (2)

$$\sum _{k=1}^K{\parallel {\mathbf{z}}^L\left(k\right)-\mathbf{u}\left(k\right)\parallel }_2\to min.$$

(4)

For the experiments the following neural network was used: $L=4$, $n_0=5,n_1=n_2=n_3=64,n_4=2$. All layers but the last used ReLu as activation function. To find the vector of optimal neural network parameters the stochastic gradient descent method ADAM was used [7].

To collect the required amount of data in the entire robot operating control range, VISLAM system was used. This system is based on RealSense t265 camera, that records data on control and state vectors at frequency of 20 Hz.

Figure 2 shows a trajectory obtained from a training sample (the light dotted line) and a trajectory obtained by internal model (1) with a neural network (the solid line).

Figure 3 depicts the change of state vector components $v\left(t\right)$ and $\theta \left(t\right)$ over time. The light line shows the movement of a real vehicle, the solid one corresponds to the movement according to the model (1), and the dotted line corresponds to control components.

3. Trajectory Tracking Scheme

To implement motion control along the trajectory, it is necessary to solve the control synthesis problem [8] and represent control as function that depends on the coordinates of the state space. In the majority of cases, a trajectory tracking error is calculated and a control signal, that works out this error, is set.

Depending on the task, the desired trajectory is set as a sequence of points on a plane or in three-dimensional space, and instead of calculating the distance to the trajectory, the distance to the points of the desired trajectory is determined. The block diagram of the unmanned vehicle control system, in which either PID-controller or model predictive control is used, is shown in Figure 4.

4. Controllers

4.1. Closed-Loop Control

In the steering control loop of the vehicle a P-controller was used. The control signal had the following form

$$\alpha =arctan\frac{L\omega }{v},$$

(5)

where L is the distance between the axes of the robot, v and $\omega$ is the linear and angular velocities of the robot,

$$\omega =\frac{vk\left(s\right)cos\left({\theta }_e\right)}{1-k\left(s\right)e}-k_{\theta }\mid v\mid {\theta }_e-\left(k_{e}v\frac{sin\left({\theta }_e\right)}{{\theta }_e}\right)e,$$

(6)

where $k\left(s\right)$ is a path curvature, ${\theta }_e$ is an error in motion direction, e is a position error, and $k_{\theta }$ and $k_e$ are controller coefficients.

Figure 5 shows the results of tracking the desired trajectory using P-controller and an internal model. The dashed line shows the desired trajectory, and the solid line shows the trajectory that has been tracked. The dots mark the constraints in the form of gates.

4.2. Model Prediction Control

Predictive control is an optimization strategy that has a variety of advantages, such as accounting constraints imposed on system’s state and control signals, and ability to work with non-linear objects. The optimal trajectory is the solution to the optimization problem. Predictive control includes object simulation under various control actions that ensure current problem solution, for example, bypassing the obstacles or passing through a gate. Among the resulting set of trajectories, one trajectory is selected according to some criterion. As a criterion, the squared error of achieving the goal and the squared control signal with some weighting factors are often used

$$J=\sum _{i=1}^L{w}_x{(r_i-x_i)}^2+\sum _{i=1}^L{w}_u\Delta u_i^2,$$

(7)

where $w_x$ and $w_u$ are weights, $r_i$ is a desired trajectory, $x_i$ is a manipulated variable, $u_i$ is a control signal, L is a control horizon.

The goal of predictive control lies in sequential search for optimal control strategy for a certain time interval that is called control horizon. Each time the first step of the strategy found is the only one applied. This step completed, the prediction horizon is being shifted and new optimization starts.

Online optimization taking place at every step has became possible due to the availability of high-performance computing facilities embedded on the vehicle’s board.

For the control object model (1) and the prediction horizon value equal to 8, the desired and obtained trajectories are shown in Figure 6.

5. Results

To study the approaches proposed, an environment for unmanned vehicles simulation based on ROS1 and ROS2 operating systems, has been developed.

Experimental comparison of control methods that apply internal model for real-time control was conducted, with the problem of unmanned vehicle trajectory tracking served as experimental domain. The control was carried out on the basis of P-controller and model predictive control (MPC). The results of field experiments are shown in

Table 1. Results of field experiments.

Experiment Number	Speed v, m/s	Deviation When Using P-Controller ${\mathit{S}}_1$, m	Deviation When Using MPC ${\mathit{S}}_2$, m
1	1.41	4.4	3.5
2	1.12	2.1	1.9
3	0.88	1.6	1.7

. The difference between the length of the desired trajectory $S_1$ and the length of the path $S_2$ covered under different velocity v was used as a comparison criterion. The desired trajectory length was 67.4 m.

According to the experiments the following conclusions may be performed:

• Control based on P-controller is distinguished by the possibility of using relatively simple controllers;
• With both control methods being used at high vehicle speeds, the deviation from the trajectory increases significantly;
• High speeds can be achieved if controller parameters are fine-tuned or in case of applying an intelligent P-controller that adapts to the speeds and curvature of the trajectory;
• High-performance computers are needed for MPC.