Optimal Control-Based Algorithm Design and Application for Trajectory Tracking of a Mobile Robot with Four Independently Steered and Four Independently Actuated Wheels

Abstract

This paper deals with the synthesis and implementation of a controller for asymptotic tracking of the desired trajectory of a mobile robot. The mobile robot used for the experimental validation has eight motors with an inner control loop. Four steering actuators are controlled using position controllers and four driving actuators are controlled using velocity controllers. A complex robot kinematic model is converted into a control-oriented linear time-varying system, which is then used to design a time-varying control law that minimizes the quadratic optimality criterion. In contrast to conventional methodologies for solving the corresponding Riccati differential equations, a computational approach that explicitly determines the time-varying controller matrix by employing recurrent matrix computations is proposed. Mobile robot control inputs (linear velocity, steering angles and steering velocities) are forwarded to the steering and driving actuators with properly tuned position and velocity controllers using an inverse kinematic model of the mobile robot. The obtained control law is evaluated on an experimental set-up of a real mobile robot system. The controller is implemented using the Robot Operating System.

1. Introduction

Following the significant advancements in both communication and information technologies, along with actuator and sensor technologies, a multitude of novel mobile robot systems have emerged, garnering substantial interest from both the scientific community and industrial and other applications. This interest has resulted in the publication of numerous review papers, representing a broad range of perspectives on the study and practical application of mobile robot technologies. Examples of this can be found in [1,2,3,4,5] among other places. It is now widely acknowledged that one of the key challenges in the development of efficient mobile robotic systems is the synthesis of algorithms for their trajectory tracking control [6].

Significant research and development has been undertaken in recent years with regard to robots with four wheels that have independent driving and steering. These developments have led to the application of various control strategies for this type of mobile robot system. In [7], the application of nonlinear model predictive control, wherein constraints are represented by the utilization of barrier functions for the guidance of a robot within a human-centric environment, has been proposed. In order to mitigate the latency in the steering angle response, the model predictive control algorithm with dynamic constraints for the tracking of high-speed trajectories was employed in [8]. A hybrid control approach integrating the sliding-mode control law, a near-time-optimal potential function and fuzzy-based adjustment rules were proposed in [9]. A direct yaw moment robust sliding-mode control strategy for a four-wheeled robot’s simultaneous trajectory tracking and disturbance rejection has been reported in [10]. Moreover, the control of four-wheeled mobile robots can be addressed in a comparable manner to that of autonomous vehicles. The principles and methods employed to solve control problems in these systems are analogous. For example, see [11,12,13,14,15,16] and related references.

This paper considers a mobile robotic structure with four independently steered and four independently actuated wheels (4ISW4IAW), part of an experimental set-up designed as a suitable laboratory model for the testing and verification of different control concepts and education. Details on this experimental set-up can be found in [17,18]. While this mobile robot was developed for climbing vertical walls, here we deal with the problem of following a reference trajectory on the ground.

In this paper, a trajectory tracking method for 4ISW4IAW mobile robot is presented. It is based on time-varying linear quadratic (TVLQ) control. Although the theoretical basis of the TVLQ approach is well established and can be considered a standard technique (see, for example, references [19,20] and the literature therein), the synthesis of controllers based on this approach remains a highly active area of research in a variety of applications, as evidenced, for example, by references [21,22,23,24]. Based on the available references, most studies in the area of TVLQ control have used one of two main approaches: the iterative approximate solution of the associated Riccati partial differential matrix equation or the transformation of the problem into a set of linear matrix inequalities (LMIs). It is well established that solving the Riccati equation requires the system to be fully controllable. However, our approach does not necessitate the resolution of the Riccati equation. The optimal control-based algorithm proposed in this paper is founded upon the direct numerical calculation of the time-varying elements of the control law matrix, without recourse to the LMI formalism or the approximation of the solution of the Riccati partial differential matrix equation.

In the approach proposed in this paper, we approximate the nonlinear three-input, two-chain, single-generator chained model of the 4ISW4IAW mobile robot with linear time-varying (LTV) differential error equations. The objective is to stabilize these equations in the vicinity of the desired trajectory. The control problem is defined in terms of determining a time-varying control law matrix that is calculated with respect to the minimization of the quadratic optimality criterion. A conjugate gradient method is employed to achieve a minimum value of the quadratic objective function. In order to determine the gradient, the chain rule for ordered derivatives is employed. The recursive matrix structure of the proposed algorithm is derived from the coupling of the control variables with the state variables, achieved through the use of the Euler method, which discretizes the kinematic equations of the robot.

The previously described method differs from other direct optimization methods in that the differential equations of the system are not treated as constraints that are included in the objective function by applying Lagrange multipliers or penalty functions. In contrast, our proposed approach explicitly leverages the connection between the control and state variables via discretized differential equations, thereby enabling the gradient of the objective function to be calculated with recursive relations. The fundamental concept behind this approach is presented in [25,26]. Here, this concept has been extended to include the synthesis of control laws for linear time-varying systems, which involved the derivation of an expression for the calculation of gradient values at each instant of the time interval.

Two main contributions of this work are as follows:

• The Euler method for discretization, the conjugate gradient method and the line search method with Wolfe’s conditions for minimizing the objective function have been integrated into one numerically efficient algorithm for the direct calculation of the state controller of LTV systems. This algorithm explicitly obtains the desired time-variant control law.
• The most effective version of the algorithm is subjected to both numerical simulations and experimental verification on a laboratory model of a 4ISW4IAW mobile robot system. Based on the authors’ current knowledge, the application and experimental verification of a time-varying controller designed in this way, which additionally includes the transformation into a three-input, two-chain, single-generator chained form, for a robot with four wheels that have independent drive and steer, has not been investigated yet.

The rest of the paper is organized as follows. In Section 2, a description of the experimental set-up of the mobile robot system under consideration is provided. Additionally, the kinematic model and its transformation into a three-input, two-chain, single-generator chained form are presented. The formulation of the trajectory tracking control problem in Section 3 involves approximating a nonlinear differential equation of the mobile robot in chained form with an LTV system. Subsequently, an algorithm for the synthesis of a time-varying state controller is derived. In Section 4, the simulation and experimental responses of the mobile robot system with the proposed control strategy are presented, along with a discussion related to the comparison with methods based on solving the Riccati equation or the LMI formalism. Section 5 concludes the paper.

2. System Description

2.1. Robot Description

The mobile robot used for experimental validation is designed to drive on vertical surfaces and perform nondestructive testing [18]. Figure 1 illustrates the mobile robot and its schematics. The robot is primarily constructed from acrylonitrile styrene acrylate (ASA) material using a 3D printing process, with some components made of carbon fiber [17]. The robot has dimensions of $380\times 300$ mm, a total weight of $3.25$ kg and a payload capacity of $1.5$ kg. The mobile robot is equipped with eight direct current (DC) motors: four for steering and four for driving. Additionally, it features five brushless DC motors, four for thrust and one for the robot’s adhesion system. However, the thrust and adhesion motors are beyond the scope of this research, as the robot will only be operating on horizontal surfaces. The wheels are both driven and steered by Dynamixel XH430-W150 smart servo motors, with rotational positions monitored by AMS AS5601 magnetic rotary sensors.

The mobile robot is also equipped with a Raspberry Pi 4 Model B, running Linux Ubuntu and the Robot Operating System (ROS) as middleware. This set-up allows the execution of high-level control algorithms, logging of all data, and communication with the driving and steering motors. Communication with the Dynamixel motors is achieved using a ROBOTIS U2D2 USB to TTL converter, which relays steering and driving commands from ROS directly to the motors. As previously mentioned, the Dynamixel XH430-W150 smart servo motors have integrated proportional–integral–derivative (PID) control loops in various operation modes, including direct pulse-width modulation (PWM), current, velocity and position control modes. The robot’s driving motors are controlled using the velocity mode, while the steering motors are controlled using the position control mode. Figure 2 illustrates both the PI control loop for velocity and the PID control loop for position control modes.

The parameters for the DC motors were estimated in [27] using the Levenberg–Marquardt method. Both velocity and position controller gains were designed to ensure optimal performance in trajectory tracking for both velocity and position.

Table 1. Steering and driving motor parameters.

${\mathit{R}}_{\mathit{a}}\left[\mathbf{\Omega }\right]$	${\mathit{L}}_{\mathit{a}}\left[\mathbf{H}\right]$	${\mathit{K}}_{\mathit{e}}\left[\frac{\mathbf{Vs}}{\mathbf{rad}}\right]$	${\mathit{K}}_{\mathit{m}}\left[\frac{\mathbf{Nms}}{\mathbf{rad}}\right]$	${\mathit{b}}_{\mathit{d}}\left[\frac{\mathbf{Nms}}{\mathbf{rad}}\right]$	${\mathit{b}}_{\mathit{s}}\left[\frac{\mathbf{Nms}}{\mathbf{rad}}\right]$	${\mathit{J}}_{\mathit{d}}\left[{\mathbf{kgm}}^2\right]$	${\mathit{J}}_{\mathit{s}}\left[{\mathbf{kgm}}^2\right]$
$9.24$	3 × ${10}^{-3}$	$0.0103$	$0.0067$	3.6 × ${10}^{-7}$	5.5 × ${10}^{-7}$	5 × ${10}^{-8}$	6 × ${10}^{-8}$

presents the calculated and estimated electro-mechanical parameters, where $u_a$ represents the armature voltage, $R_a$ and $L_a$ denote the armature resistance and inductance, $K_m$ and $K_e$ are the torque and back electromotive force constants, $b_d$ and $b_s$ are the driving and steering friction coefficients, and $J_d$ and $J_s$ represent the inertia of the wheel and steering system (_s marks steering and _d marks driving system parameters).

Dynamixel DC motors are actuators which comprise a complete system. They feature an integrated PI control loop for the velocity and PID for position. Users are permitted to tune only the gains for those control loops. A PI controller is the preferred option for controlling DC motor velocity due to its simplicity and robustness, and the fact that it is able to meet the control requirements without the additional complications introduced by the derivative term. The inherent noise sensitivity of the derivative term, in conjunction with the existing mechanical filtering properties of the motor system, renders the use of a PID controller less desirable in many practical applications compared to the PI.

In

Table 2. Steering and driving motor parameters.

${\mathit{K}}_{\mathit{vp}}$	${\mathit{K}}_{\mathit{vi}}$	${\mathit{K}}_{\mathit{pp}}$	${\mathit{K}}_{\mathit{pi}}$	${\mathit{K}}_{\mathit{pd}}$	${\mathit{\omega }}_{\mathit{n}}\left[\frac{\mathbf{rad}}{\mathbf{s}}\right]$
1.5	51	14	0	0	32.8

, the controller gains of both the driving and steering systems are presented, where $K_{vp}$ and $K_{vi}$ are the proportional and integral gains of the PI velocity controller, respectively, and $K_{pp}$, $K_{pi}$ and $K_{pd}$ are the proportional, integral and derivative gains of the PID position controller, and ${\omega }_n$ represents the cut-off frequency of the 2^nd order Bessel filter which cannot be changed.

All gains for control systems are chosen according to [27]. The proportional gain of the position control loop is adjusted in a way that prevented overshoot, ensuring a fast and smooth response. The PI control loop is tuned in a similar manner, allowing for a slight overshoot of 2%. During the tuning process, it is observed that, while using strong P gains for both the P position control loop and PI velocity control loop, there is a significant amount of chattering in the DC motor control loop, making it unsuitable for experimental validation. Later in the experimental section, the reference trajectory tracking of the driving velocity and steering angle will be shown using the control system parameters defined here.

Regarding the values presented in Table 2, in the context of position control, it is typically unnecessary to utilize integral and derivative gain (the values of $K_{pi}$ and $K_{pd}$ are zero), given that there are already two integrators following the output torque. The first of these is for velocity control, while the second is for position control. A proportional (P) controller offers a balance between simplicity, quick response and sufficient accuracy for numerous practical position control tasks, making it a preferred choice when these factors are prioritized.

2.2. Mathematical Model

The equations describing the kinematics of a mobile robot with four independently steered and four independently actuated wheels are given in [8,28], so they will not be detailed here. In this paper, we adopt assumptions from our recent work [29] to simplify the model from [8]. These assumptions enable a simpler derivation of the chain model and, consequently, a simpler derivation of the linear time-variant model of the robot. The main reason for the transformation into a chained form is the fact that direct Jacobian linearization of a nonlinear kinematic model would require predetermining the desired steering angle of all wheels, which would be quite complex in the case of some more demanding tracking trajectories.

We assume that the linear velocities of wheels are equal and the front and rear wheels are steered parallel to each other and at the same steering velocity. Hence, we obtain the following set of equations:

$$\begin{array}{cc}\hfill \dot{x}& ={\displaystyle \frac{1}{2}}\left(cos({\delta }_1+\theta )+cos({\delta }_2+\theta )\right)v_1,\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill \dot{y}& ={\displaystyle \frac{1}{2}}\left(sin({\delta }_1+\theta )+sin({\delta }_2+\theta )\right)v_1,\hfill \end{array}$$

(2)

$$\begin{array}{cc}\hfill \dot{\theta }& =\left({\displaystyle \frac{x_{w1}sin{\delta }_1}{2x_{w1}^2+2y_{w1}^2}}+{\displaystyle \frac{x_{w2}sin{\delta }_2}{2x_{w2}^2+2y_{w2}^2}}\right)v_1,\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill \dot{{\delta }_1}& ={\omega }_1,\hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill \dot{{\delta }_2}& ={\omega }_2,\hfill \end{array}$$

(5)

where the reference point of the robot is represented by the coordinates x and y in the Cartesian frame, the orientation angle, denoted by $\theta$, is expressed in relation to the positive x-axis, $v_1$ is the linear velocity, ${\delta }_1$, ${\omega }_1$, ${\delta }_2$ and ${\omega }_2$ are the steering angles and steering velocities of the front and rear wheels, respectively, and $x_{wi}$ and $y_{wi}$, $i=1,2$, are the positions of the wheels predefined as follows: $(x_{w1},y_{w1})=(a,b)$ and $(x_{w2},y_{w2})=(-a,b)$ where a and b are the lengths between the centroid of the robot and each wheel whose numerical values are $a=b=0.1125$ m. Note that in (1)–(5) are five state variables—x, y, $\theta$, ${\delta }_1$ and ${\delta }_2$—and three input variables—$v_1$, ${\omega }_1$ and ${\omega }_2$.

System (1)–(5) can be transformed into the following three-input, two-chain, single-generator chained form

$$\begin{array}{cc}\hfill {\dot{x}}_1& =u_1,\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill {\dot{x}}_2& =u_2,\hfill \end{array}$$

(7)

$$\begin{array}{cc}\hfill {\dot{x}}_3& =x_2{u}_1,\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill {\dot{x}}_4& =u_3,\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill {\dot{x}}_5& =x_4{u}_1.\hfill \end{array}$$

(10)

by changing the coordinates as follows:

$$\begin{array}{cc}\hfill x_1& =x,\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill x_2& ={\displaystyle \frac{D_1+D_2}{c_1+c_2}},\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill x_3& =\theta ,\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill x_4& =tan\left({\displaystyle \frac{{\delta }_1+{\delta }_2}{2}}+\theta \right),\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill x_5& =y,\hfill \end{array}$$

(15)

with the input transformation

$$\begin{array}{ll}{u}_1\hfill & ={\displaystyle \frac{1}{2}}\left(c_1+c_2\right)v_1,\hfill \end{array}$$

(16)

$$\begin{array}{ll}{u}_2\hfill & ={\displaystyle \frac{B_1{\omega }_1+B_2{\omega }_2}{(c_1+c_2)}}+{\displaystyle \frac{(D_1+D_2)(2{\omega }_1{s}_1+2{\omega }_2{s}_2)}{2(c_1+c_2)}}+{\displaystyle \frac{v_1{(D_1+D_2)}^2(s_1+s_2)}{2{(c_1+c_2)}^2}},\hfill \end{array}$$

(17)

$$\begin{array}{ll}{u}_3\hfill & ={\displaystyle \frac{v_1\left(D_1+D_2\right)+{\omega }_1+{\omega }_2}{K}},\hfill \end{array}$$

(18)

where the following notation is introduced for the purpose of simplification:

$$\begin{array}{cc}\hfill c_i& =cos({\delta }_i+\theta ),\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill s_i& =sin({\delta }_i+\theta ),\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill B_i& ={\displaystyle \frac{x_{wi}cos\left({\delta }_i\right)}{{x_{wi}}^2+{y_{wi}}^2}},\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill D_i& ={\displaystyle \frac{x_{wi}sin\left({\delta }_i\right)}{{x_{wi}}^2+{y_{wi}}^2}},\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill K& =1+cos({\delta }_1+{\delta }_2+2\theta ),\hfill \end{array}$$

(23)

for $i=1,2$.

In [29], the previous transformations are carried out in detail. To perform these transformations, it was necessary to apply the procedure from [30,31], where the conditions for the generalization of the chained form to systems with more than two inputs were derived.

3. Algorithm for the Synthesis of a Time-Varying State Controller

3.1. Control Problem Formulation

Based on [32], in this work, we formulate the trajectory tracking control problem utilizing the approach of approximating a nonlinear system of equations of a mobile robot in chained form (6)–(10) by an LTV system.

Let us assume that the desired trajectory in the Cartesian coordinate system is represented by $x_d\left(t\right)$ and $y_d\left(t\right)$, while the desired orientation is ${\theta }_d\left(t\right)=\mathrm{atan}2\left({\dot{y}}_d\left(t\right),{\dot{x}}_d\left(t\right)\right)$. Additionally, let us suppose that the desired trajectory is achievable. This implies the inputs

$${\mathbf{u}}_d\left(t\right)={\left[u_{d1}\left(t\right)u_{d2}\left(t\right)u_{d3}\left(t\right)\right]}^{\mathrm{T}},$$

(24)

and the desired states

$${\mathbf{x}}_d\left(t\right)={\left[x_{d1}\left(t\right)x_{d2}\left(t\right)x_{d3}\left(t\right)x_{d4}\left(t\right)x_{d5}\left(t\right)\right]}^{\mathrm{T}},$$

(25)

i.e., the reference robot can be calculated using (11)–(18). Furthermore, let us write the chained system (6)–(10) in standard vector notation:

$$\dot{\mathbf{x}}\left(t\right)=\mathbf{f}(\mathbf{x}\left(t\right),\mathbf{u}\left(t\right)),$$

(26)

where

$$\mathbf{x}\left(t\right)={\left[x_1{x}_2{x}_3{x}_4{x}_5\right]}^{\mathrm{T}},\mathbf{u}\left(t\right)={\left[u_1{u}_2{u}_3\right]}^{\mathrm{T}},\mathbf{f}={\left[u_1{u}_2{x}_2{u}_1{u}_3{x}_4{u}_1\right]}^{\mathrm{T}}.$$

(27)

The Taylor expansion of system (26) around $({\mathbf{x}}_d\left(t\right),{\mathbf{u}}_d\left(t\right))$, rejecting the terms of higher order, results in

$$\begin{array}{cc}\hfill \dot{\mathbf{x}}\left(t\right)& =\mathbf{f}({\mathbf{x}}_d\left(t\right),{\mathbf{u}}_d\left(t\right))+{\displaystyle \frac{\partial \mathbf{f}({\mathbf{x}}_d\left(t\right),{\mathbf{u}}_d\left(t\right))}{\partial \mathbf{x}}}(\mathbf{x}\left(t\right)-{\mathbf{x}}_d\left(t\right))\hfill \\ \hfill & +{\displaystyle \frac{\partial \mathbf{f}({\mathbf{x}}_d\left(t\right),{\mathbf{u}}_d\left(t\right))}{\partial \mathbf{u}}}(\mathbf{u}\left(t\right)-{\mathbf{u}}_d\left(t\right)).\hfill \end{array}$$

(28)

The state errors and input errors are defined as

$$\tilde{\mathbf{x}}\left(t\right)=\mathbf{x}\left(t\right)-{\mathbf{x}}_d\left(t\right),\tilde{\mathbf{u}}\left(t\right)=\mathbf{u}\left(t\right)-{\mathbf{u}}_d\left(t\right),$$

(29)

respectively, and from (28) the LTV error equations are as follows:

$$\dot{\tilde{\mathbf{x}}}\left(t\right)=\mathbf{A}\left(t\right)\tilde{\mathbf{x}}\left(t\right)+\mathbf{B}\left(t\right)\tilde{\mathbf{u}}\left(t\right),$$

(30)

where

$$\mathbf{A}\left(t\right)={\displaystyle \frac{\partial \mathbf{f}({\mathbf{x}}_d\left(t\right),{\mathbf{u}}_d\left(t\right))}{\partial \mathbf{x}}},\mathbf{B}\left(t\right)={\displaystyle \frac{\partial \mathbf{f}({\mathbf{x}}_d\left(t\right),{\mathbf{u}}_d\left(t\right))}{\partial \mathbf{u}}}.$$

(31)

Then, the control problem of tracking the desired trajectory is formulated as follows:

Problem 1.

Consider the LTV differential error equations (30) in which all state variables are available. The goal is to determine the control law of the form

$$\tilde{\mathbf{u}}\left(t\right)=-\mathbf{K}\left(t\right)\tilde{\mathbf{x}}\left(t\right),$$

(32)

where the matrix $\mathbf{K}\left(t\right)$ is determined according to the quadratic criterion of optimality, and thereby obtain the optimal control law of the form

$${\mathbf{u}}^{*}={\mathbf{u}}_d\left(t\right)-\mathbf{K}\left(t\right)\tilde{\mathbf{x}}\left(t\right),$$

(33)

such that the system in chained form (6)–(10) is stabilized in the vicinity of the desired trajectory.

Problem 1 is known as local LQ trajectory stabilization [20], or the TVLQ control problem [19], and matrix $\mathbf{K}\left(t\right)$ can be calculated by solving the corresponding Riccati differential equation. It is important to note that solving the Riccati equation requires system (30) to be fully controllable. Nevertheless, the methodology presented in this paper avoids the necessity for solving the Riccati equation, instead relying on a direct numerical approach for minimizing the associated objective function.

3.2. Optimal Control-Based Algorithm Derivation

The optimal control-based algorithm for solving Problem 1, as presented in this paper, employs a representation of control vector components as linear functions of state variables with time-varying parameters. The aforementioned parameters are, in fact, elements of the matrix $\mathbf{K}\left(t\right)$, which must be determined.

In order to write the control law (32) in a parametrized form, the vectorization operator (the operator $\mathrm{vec}(·)$ is an operator that stacks the columns of a matrix one underneath the other) can be applied:

$$\mathrm{vec}\left(\tilde{\mathbf{u}}\left(t\right)\right)=-\mathrm{vec}\left(\mathbf{K}\left(t\right)\tilde{\mathbf{x}}\left(t\right)\right)=-\left({\tilde{\mathbf{x}}}^{\mathrm{T}}\left(t\right)\otimes \mathbf{I}\right)\mathrm{vec}\left(\mathbf{K}\left(t\right)\right).$$

(34)

In this context, the symbol ⊗ denotes the Kronecker product, while the identity matrix $\mathbf{I}$ has the same number of rows as the matrix $\mathbf{K}\left(t\right)$. In order to facilitate further analysis, the control law is introduced in the following notation.

$$\tilde{\mathbf{u}}\left(t\right)=\tilde{\mathbf{X}}\left(t\right)\mathbf{k}\left(t\right),$$

(35)

where $\tilde{\mathbf{X}}\left(t\right)={\tilde{\mathbf{x}}}^{\mathrm{T}}\left(t\right)\otimes \mathbf{I}$ and $\mathbf{k}\left(t\right)=-\mathrm{vec}\left(\mathbf{K}\left(t\right)\right)$.

The problem of finding $\mathbf{k}\left(t\right)$ in a control law (35) is based on the minimization of the objective function

$$J={\int }_0^{t_f}\left({\tilde{\mathbf{x}}}^{\mathrm{T}}\left(t\right)\mathbf{Q}\tilde{\mathbf{x}}\left(t\right)+{\tilde{\mathbf{u}}}^{\mathrm{T}}\left(t\right)\mathbf{R}\tilde{\mathbf{u}}\left(t\right)\right)dt,$$

(36)

for all $t_f\ge 0$ with $\mathbf{Q}={\mathbf{Q}}^{\mathrm{T}}\ge \mathbf{0},\mathbf{R}={\mathbf{R}}^{\mathrm{T}}>\mathbf{0}$ subject to the error equations (30).

Remark 1.

Note that the objective function (36) implicitly depends on elements of $\mathbf{k}\left(t\right)$ from (35), so the matrix $\mathbf{K}\left(t\right)$ in (32) can be determined after appropriate reshaping of the vector $\mathbf{k}\left(t\right)$.

Our approach can be classified as a direct optimization method, whereby a continuous-time system is transformed into a discrete-time system. It can therefore be assumed that the time interval $\left[0,t_f\right]$ is divided into $N-1$ sub-intervals of equal length. The resulting time grid is then composed of points $t_i=i\tau$, where $i=0,1,2,\dots ,N-1$ and $\tau$ is defined as the time step length, with a value of $t_f/N$. Subsequently, the Euler approximation of the continuous-time state Equation (30) is

$$\tilde{\mathbf{x}}(i+1)=\tilde{\mathbf{x}}\left(i\right)+\tau \left(\mathbf{A}\left(i\right)\tilde{\mathbf{x}}\left(i\right)+\mathbf{B}\left(i\right)\tilde{\mathbf{u}}\left(i\right)\right),\tilde{\mathbf{x}}\left(0\right)={\tilde{\mathbf{x}}}_0.$$

(37)

The discretized control law (35) can be expressed as

$$\tilde{\mathbf{u}}\left(i\right)=\tilde{\mathbf{X}}\left(i\right)\mathbf{k}\left(i\right).$$

(38)

The discrete-time form of the objective function (36) with (38) results in

$$J=\tau \sum _{i=0}^{N-1}\left({\tilde{\mathbf{x}}}^{\mathrm{T}}\left(i\right)\mathbf{Q}\tilde{\mathbf{x}}\left(i\right)+{\mathbf{k}}^{\mathrm{T}}\left(i\right){\tilde{\mathbf{X}}}^{\mathrm{T}}\left(i\right)\mathbf{R}\tilde{\mathbf{X}}\left(i\right)\mathbf{k}\left(i\right)\right).$$

(39)

Considering everything mentioned above, the optimization problem can be summed up as follows:

$$\begin{array}{cc}\hfill & \underset{\mathbf{k}\left(i\right)}{min}\left\{\tau \sum _{i=0}^{N-1}\left({\tilde{\mathbf{x}}}^{\mathrm{T}}\left(i\right)\mathbf{Q}\tilde{\mathbf{x}}\left(i\right)+{\mathbf{k}}^{\mathrm{T}}\left(i\right){\tilde{\mathbf{X}}}^{\mathrm{T}}\left(i\right)\mathbf{R}\tilde{\mathbf{X}}\left(i\right)\mathbf{k}\left(i\right)\right)\right\},\hfill \\ \hfill & \mathrm{subject}\mathrm{to}:\hfill \end{array}$$

(40)

$$\begin{array}{cc}\hfill & \tilde{\mathbf{x}}(i+1)=\tilde{\mathbf{x}}\left(i\right)+\tau \left(\mathbf{A}\left(i\right)\tilde{\mathbf{x}}\left(i\right)+\mathbf{B}\left(i\right)\tilde{\mathbf{X}}\left(i\right)\mathbf{k}\left(i\right)\right),\tilde{\mathbf{x}}\left(0\right)={\tilde{\mathbf{x}}}_0.\hfill \end{array}$$

(41)

The main concept of the proposed approach to solving the optimization problem (40) and (41) is based on the integration of a conjugate gradient method and line search method with Wolf conditions into one algorithm which uses recursive matrix relations to compute the first-order derivatives and provide a direct numerical calculation of the time-varying elements of vector $\mathbf{k}$. The minimization in (40) is performed by a conjugate gradient descent algorithm in the following form

$$\begin{array}{cc}\hfill {\mathbf{k}}^{(l+1)}\left(j\right)& ={\mathbf{k}}^{\left(l\right)}\left(j\right)+\eta {\mathbf{s}}^{\left(l\right)}\left(j\right),\hfill \end{array}$$

(42)

$$\begin{array}{cc}\hfill {\mathbf{s}}^{(l+1)}\left(j\right)& =-{\left({\displaystyle \frac{\partial J}{\partial \mathbf{k}\left(j\right)}}\right)}^{(l+1)}+\beta {\mathbf{s}}^{\left(l\right)}\left(j\right),\hfill \end{array}$$

(43)

for $j=0,1,2,\dots ,N-1$ and $l=0,1,2,\dots ,M$. N is the number of time instants, M is the number of gradient algorithm iterations and $\mathbf{s}$ is the search direction vector. Here we note that all vectors in (42) and (43) are column vectors including the gradient of the objective function. This should be kept in mind in later expressions so that the dimensions of matrices and vectors are not misunderstood.

The standard method for computing $\beta$ is

$$\beta ={\displaystyle \frac{\mu {\left(\frac{\partial J}{\partial {\mathbf{k}}^{\mathrm{T}}\left(j\right)}\right)}^{(l+1)}{\left(\frac{\partial J}{\partial \mathbf{k}\left(j\right)}\right)}^{(l+1)}+[1-\mu ]{\left(\frac{\partial J}{\partial {\mathbf{k}}^{\mathrm{T}}\left(j\right)}\right)}^{(l+1)}\left[{\left(\frac{\partial J}{\partial \mathbf{k}\left(j\right)}\right)}^{(l+1)}-{\left(\frac{\partial J}{\partial \mathbf{k}\left(j\right)}\right)}^{\left(l\right)}\right]}{\nu {\left(\frac{\partial J}{\partial {\mathbf{k}}^{\mathrm{T}}\left(j\right)}\right)}^{\left(l\right)}{\left(\frac{\partial J}{\partial \mathbf{k}\left(j\right)}\right)}^{\left(l\right)}+[1-\nu ]{\mathbf{s}}^{\mathrm{T}\left(l\right)}\left[{\left(\frac{\partial J}{\partial \mathbf{k}\left(j\right)}\right)}^{(l+1)}-{\left(\frac{\partial J}{\partial \mathbf{k}\left(j\right)}\right)}^{\left(l\right)}\right]}}.$$

(44)

It should be noted that in (44) there are four distinct possible combinations for the parameters $\mu$ and $\nu$ that can be utilized in accordance with references [33,34,35,36].

In order to find the convergence step $\eta >0$, we employ a line search strategy with the Wolfe conditions. The method of line search with Wolf’s conditions is considered quite standard and well-known, and there is no need to present it in detail here. Further details about Wolfe conditions and the line search technique are available in [37].

In the l-th iteration of (42) and (43) the gradient of the objective function with respect to $\mathbf{k}$ is given by

$$\begin{array}{cc}\hfill {\displaystyle \frac{\partial J}{\partial \mathbf{k}\left(j\right)}}& =\tau \sum _{i=0}^{N-1}{\displaystyle \frac{\partial \left[{\tilde{\mathbf{x}}}^{\mathrm{T}}\left(i\right)\mathbf{Q}\tilde{\mathbf{x}}\left(i\right)+{\mathbf{k}}^{\mathrm{T}}\left(i\right){\tilde{\mathbf{X}}}^{\mathrm{T}}\left(i\right)\mathbf{R}\tilde{\mathbf{X}}\left(i\right)\mathbf{k}\left(i\right)\right]}{\partial \mathbf{k}\left(j\right)}}=\hfill \\ \hfill & =\tau \left({\displaystyle \frac{\partial \left[{\tilde{\mathbf{x}}}^{\mathrm{T}}\left(j\right)\mathbf{Q}\tilde{\mathbf{x}}\left(j\right)+{\mathbf{k}}^{\mathrm{T}}\left(j\right){\tilde{\mathbf{X}}}^{\mathrm{T}}\left(j\right)\mathbf{R}\tilde{\mathbf{X}}\left(j\right)\mathbf{k}\left(j\right)\right]}{\partial \mathbf{k}\left(j\right)}}\right.\hfill \\ \hfill & \left.+\sum _{i=j+1}^{N-1}{\displaystyle \frac{\partial \left[{\tilde{\mathbf{x}}}^{\mathrm{T}}\left(i\right)\mathbf{Q}\tilde{\mathbf{x}}\left(i\right)+{\mathbf{k}}^{\mathrm{T}}\left(i\right){\tilde{\mathbf{X}}}^{\mathrm{T}}\left(i\right)\mathbf{R}\tilde{\mathbf{X}}\left(i\right)\mathbf{k}\left(i\right)\right]}{\partial \mathbf{k}\left(j\right)}}\right),\hfill \end{array}$$

(45)

for $j=0,1,2,\dots ,N-1$, where the terms with $i<j$ are equal to zero.

Obviously, in the previous expression, the first term in parentheses is

$${\displaystyle \frac{\partial \left[{\tilde{\mathbf{x}}}^{\mathrm{T}}\left(j\right)\mathbf{Q}\tilde{\mathbf{x}}\left(j\right)+{\mathbf{k}}^{\mathrm{T}}\left(j\right){\tilde{\mathbf{X}}}^{\mathrm{T}}\left(j\right)\mathbf{R}\tilde{\mathbf{X}}\left(j\right)\mathbf{k}\left(j\right)\right]}{\partial \mathbf{k}\left(j\right)}}=2{\tilde{\mathbf{X}}}^{\mathrm{T}}\left(j\right)\mathbf{R}\tilde{\mathbf{X}}\left(j\right)\mathbf{k}\left(j\right).$$

(46)

Since the second term in the parentheses in (45) depends on $\mathbf{k}\left(j\right)$ implicitly through $\tilde{\mathbf{x}}\left(i\right)$ for $i>j$, in what follows, we will derive recursive relations for its computation. Using the basic chain rule arithmetic, the term in the sum of (45) is

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial \left[{\tilde{\mathbf{x}}}^{\mathrm{T}}\left(i\right)\mathbf{Q}\tilde{\mathbf{x}}\left(i\right)+{\mathbf{k}}^{\mathrm{T}}\left(i\right){\tilde{\mathbf{X}}}^{\mathrm{T}}\left(i\right)\mathbf{R}\tilde{\mathbf{X}}\left(i\right)\mathbf{k}\left(i\right)\right]}{\partial \mathbf{k}\left(j\right)}}=\hfill \\ \hfill & ={\displaystyle \frac{\partial \tilde{\mathbf{x}}\left(i\right)}{\partial \mathbf{k}\left(j\right)}}{\displaystyle \frac{\partial \left[{\tilde{\mathbf{x}}}^{\mathrm{T}}\left(i\right)\mathbf{Q}\tilde{\mathbf{x}}\left(i\right)+{\mathbf{k}}^{\mathrm{T}}\left(i\right){\tilde{\mathbf{X}}}^{\mathrm{T}}\left(i\right)\mathbf{R}\tilde{\mathbf{X}}\left(i\right)\mathbf{k}\left(i\right)\right]}{\partial \tilde{\mathbf{x}}\left(i\right)}}=\hfill \\ \hfill & =2{\displaystyle \frac{\partial \tilde{\mathbf{x}}\left(i\right)}{\partial \mathbf{k}\left(j\right)}}\mathbf{Q}\tilde{\mathbf{x}}\left(i\right)+{\displaystyle \frac{\partial \tilde{\mathbf{x}}\left(i\right)}{\partial \mathbf{k}\left(j\right)}}{\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}\left(i\right){\tilde{\mathbf{X}}}^{\mathrm{T}}\left(i\right)\mathbf{R}\tilde{\mathbf{X}}\left(i\right)\mathbf{k}\left(i\right)\right]}{\partial \tilde{\mathbf{x}}\left(i\right)}}.\hfill \end{array}$$

(47)

Note that $\tilde{\mathbf{x}}\in {\mathbb{R}}^5$, $\mathbf{k}\in {\mathbb{R}}^{15}$ and ${\displaystyle \frac{\partial \tilde{\mathbf{x}}\left(i\right)}{\partial \mathbf{k}\left(j\right)}}\in {\mathbb{R}}^{15\times 5}$; thus, the above expression is dimensionally well defined.

From (41) it follows that

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial \tilde{\mathbf{x}}(j+1)}{\partial \mathbf{k}\left(j\right)}}=\tau {\tilde{\mathbf{X}}}^{\mathrm{T}}\left(j\right){\mathbf{B}}^{\mathrm{T}}\left(j\right),\hfill \\ \hfill & {\displaystyle \frac{\partial \tilde{\mathbf{x}}\left(i\right)}{\partial \mathbf{k}\left(j\right)}}={\displaystyle \frac{\partial \tilde{\mathbf{x}}(i-1)}{\partial \mathbf{k}\left(j\right)}}\left(\mathbf{I}+\tau \mathbf{A}(i-1)\right),\hfill \end{array}$$

(48)

for $i=j+2,\dots ,N-1$.

Next, let us denote the sum from Equation (45) by

$${\mathsf{\Sigma }}_j=\sum _{i=j+1}^{N-1}{\displaystyle \frac{\partial \left[{\tilde{\mathbf{x}}}^{\mathrm{T}}\left(i\right)\mathbf{Q}\tilde{\mathbf{x}}\left(i\right)+{\mathbf{k}}^{\mathrm{T}}\left(i\right){\tilde{\mathbf{X}}}^{\mathrm{T}}\left(i\right)\mathbf{R}\tilde{\mathbf{X}}\left(i\right)\mathbf{k}\left(i\right)\right]}{\partial \mathbf{k}\left(j\right)}}.$$

(49)

Based on (47) for $j=N-2$, $i=N-1$, it follows

$$\begin{array}{cc}\hfill {\mathsf{\Sigma }}_{N-2}& =2{\displaystyle \frac{\partial \tilde{\mathbf{x}}(N-1)}{\partial \mathbf{k}(N-2)}}\mathbf{Q}\tilde{\mathbf{x}}(N-1)\hfill \\ \hfill & +{\displaystyle \frac{\partial \tilde{\mathbf{x}}(N-1)}{\partial \mathbf{k}(N-2)}}{\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}(N-1){\tilde{\mathbf{X}}}^{\mathrm{T}}(N-1)\mathbf{R}\tilde{\mathbf{X}}(N-1)\mathbf{k}(N-1)\right]}{\partial \tilde{\mathbf{x}}(N-1)}}.\hfill \end{array}$$

(50)

Substituting (48) in (50), we obtain

$$\begin{array}{cc}\hfill {\mathsf{\Sigma }}_{N-2}& =2\tau {\tilde{\mathbf{X}}}^{\mathrm{T}}(N-2){\mathbf{B}}^{\mathrm{T}}(N-2)\mathbf{Q}\tilde{\mathbf{x}}(N-1)\hfill \\ \hfill & +\tau {\tilde{\mathbf{X}}}^{\mathrm{T}}(N-2){\mathbf{B}}^{\mathrm{T}}(N-2){\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}(N-1){\tilde{\mathbf{X}}}^{\mathrm{T}}(N-1)\mathbf{R}\tilde{\mathbf{X}}(N-1)\mathbf{k}(N-1)\right]}{\partial \tilde{\mathbf{x}}(N-1)}}.\hfill \end{array}$$

(51)

Furthermore, for $j=N-3$, $i=N-1,N-2$ from (47), it follows

$$\begin{array}{cc}\hfill {\mathsf{\Sigma }}_{N-3}& =2{\displaystyle \frac{\partial \tilde{\mathbf{x}}(N-2)}{\partial \mathbf{k}(N-3)}}\mathbf{Q}\tilde{\mathbf{x}}(N-2)\hfill \\ \hfill & +{\displaystyle \frac{\partial \tilde{\mathbf{x}}(N-2)}{\partial \mathbf{k}(N-3)}}{\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}(N-2){\tilde{\mathbf{X}}}^{\mathrm{T}}(N-2)\mathbf{R}\tilde{\mathbf{X}}(N-2)\mathbf{k}(N-2)\right]}{\partial \tilde{\mathbf{x}}(N-2)}}\hfill \\ \hfill & +2{\displaystyle \frac{\partial \tilde{\mathbf{x}}(N-1)}{\partial \mathbf{k}(N-3)}}\mathbf{Q}\tilde{\mathbf{x}}(N-1)\hfill \\ \hfill & +{\displaystyle \frac{\partial \tilde{\mathbf{x}}(N-1)}{\partial \mathbf{k}(N-3)}}{\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}(N-1){\tilde{\mathbf{X}}}^{\mathrm{T}}(N-1)\mathbf{R}\tilde{\mathbf{X}}(N-1)\mathbf{k}(N-1)\right]}{\partial \tilde{\mathbf{x}}(N-1)}}.\hfill \end{array}$$

(52)

Substituting (48) in (52), we obtain

$$\begin{array}{cc}\hfill {\mathsf{\Sigma }}_{N-3}& =2\tau {\tilde{\mathbf{X}}}^{\mathrm{T}}(N-3){\mathbf{B}}^{\mathrm{T}}(N-3)\mathbf{Q}\tilde{\mathbf{x}}(N-2)\hfill \\ \hfill & +\tau {\tilde{\mathbf{X}}}^{\mathrm{T}}(N-3){\mathbf{B}}^{\mathrm{T}}(N-3){\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}(N-2){\tilde{\mathbf{X}}}^{\mathrm{T}}(N-2)\mathbf{R}\tilde{\mathbf{X}}(N-2)\mathbf{k}(N-2)\right]}{\partial \tilde{\mathbf{x}}(N-2)}}\hfill \\ \hfill & +2\tau {\tilde{\mathbf{X}}}^{\mathrm{T}}(N-3){\mathbf{B}}^{\mathrm{T}}(N-3)\left(\mathbf{I}+\tau \mathbf{A}(N-2)\right)\mathbf{Q}\tilde{\mathbf{x}}(N-1)\hfill \\ \hfill & +\tau {\tilde{\mathbf{X}}}^{\mathrm{T}}(N-3){\mathbf{B}}^{\mathrm{T}}(N-3)\left(\mathbf{I}+\tau \mathbf{A}(N-2)\right)\hfill \\ \hfill & ·{\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}(N-1){\tilde{\mathbf{X}}}^{\mathrm{T}}(N-1)\mathbf{R}\tilde{\mathbf{X}}(N-1)\mathbf{k}(N-1)\right]}{\partial \tilde{\mathbf{x}}(N-1)}},\hfill \\ \hfill {\mathsf{\Sigma }}_{N-3}& =\tau {\tilde{\mathbf{X}}}^{\mathrm{T}}(N-3){\mathbf{B}}^{\mathrm{T}}(N-3)\hfill \\ \hfill & ·\{2\mathbf{Q}\tilde{\mathbf{x}}(N-2)+{\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}(N-2){\tilde{\mathbf{X}}}^{\mathrm{T}}(N-2)\mathbf{R}\tilde{\mathbf{X}}(N-2)\mathbf{k}(N-2)\right]}{\partial \tilde{\mathbf{x}}(N-2)}}\hfill \\ \hfill & +\left(\mathbf{I}+\tau \mathbf{A}(N-2)\right)\hfill \\ \hfill & ·\left[2\mathbf{Q}\tilde{\mathbf{x}}(N-1)+{\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}(N-1){\tilde{\mathbf{X}}}^{\mathrm{T}}(N-1)\mathbf{R}\tilde{\mathbf{X}}(N-1)\mathbf{k}(N-1)\right]}{\partial \tilde{\mathbf{x}}(N-1)}}\right]\}.\hfill \end{array}$$

(53)

The preceding procedure can be carried out in the following manner:

$$\begin{array}{cc}\hfill {\mathsf{\Sigma }}_{N-4}& =\tau {\tilde{\mathbf{X}}}^{\mathrm{T}}(N-4){\mathbf{B}}^{\mathrm{T}}(N-4)\hfill \\ \hfill & ·\{2\mathbf{Q}\tilde{\mathbf{x}}(N-3)+{\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}(N-3){\tilde{\mathbf{X}}}^{\mathrm{T}}(N-3)\mathbf{R}\tilde{\mathbf{X}}(N-3)\mathbf{k}(N-3)\right]}{\partial \tilde{\mathbf{x}}(N-3)}}\hfill \\ \hfill & +\left(\mathbf{I}+\tau \mathbf{A}(N-3)\right)\hfill \\ \hfill & ·\left[2\mathbf{Q}\tilde{\mathbf{x}}(N-2)+{\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}(N-2){\tilde{\mathbf{X}}}^{\mathrm{T}}(N-2)\mathbf{R}\tilde{\mathbf{X}}(N-2)\mathbf{k}(N-2)\right]}{\partial \tilde{\mathbf{x}}(N-2)}}\right]\hfill \\ \hfill & +\left(\mathbf{I}+\tau \mathbf{A}(N-2)\right)\hfill \\ \hfill & ·\left[2\mathbf{Q}\tilde{\mathbf{x}}(N-1)+{\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}(N-1){\tilde{\mathbf{X}}}^{\mathrm{T}}(N-1)\mathbf{R}\tilde{\mathbf{X}}(N-1)\mathbf{k}(N-1)\right]}{\partial \tilde{\mathbf{x}}(N-1)}}\right]\}.\hfill \end{array}$$

(54)

Finally, based on the previous derivations from (45) to (54), the recursive relations for calculating the gradient in (43) are

$$\begin{array}{cc}\hfill {\mathbf{\kappa }}_{N-1}& =\mathbf{0},\hfill \end{array}$$

(55)

$$\begin{array}{cc}\hfill {\mathbf{\kappa }}_j& =2\mathbf{Q}\tilde{\mathbf{x}}(j+1)+{\displaystyle \frac{\partial \left[{\mathbf{k}}^{\mathrm{T}}(j+1){\tilde{\mathbf{X}}}^{\mathrm{T}}(j+1)\mathbf{R}\tilde{\mathbf{X}}(j+1)\mathbf{k}(j+1)\right]}{\partial \tilde{\mathbf{x}}(j+1)}}\hfill \end{array}$$

(56)

$$\begin{array}{cc}\hfill & +\left(\mathbf{I}+\tau \mathbf{A}(N-2)\right){\mathbf{\kappa }}_{j+1},\hfill \end{array}$$

(57)

$$\begin{array}{cc}\hfill {\mathsf{\Sigma }}_j& =\tau {\tilde{\mathbf{X}}}^{\mathrm{T}}\left(j\right){\mathbf{B}}^{\mathrm{T}}\left(j\right){\mathbf{\kappa }}_j,\hfill \end{array}$$

(58)

$$\begin{array}{cc}\hfill {\displaystyle \frac{\partial J}{\partial \mathbf{k}\left(j\right)}}& =\tau \left(2{\tilde{\mathbf{X}}}^{\mathrm{T}}\left(j\right)\mathbf{R}\tilde{\mathbf{X}}\left(j\right)\mathbf{k}\left(j\right)+{\mathsf{\Sigma }}_j\right),\hfill \end{array}$$

(59)

for $j=N-2,N-3,\dots ,0$.

The algorithm described above is summarized in the following steps:

• For the Euler approximation (37); set the initial state vector ${\mathbf{x}}_0$; set final time $t_f$; set number of time intervals N; calculate time step length $\tau$.
• Choose the weight matrices $\mathbf{Q}$, $\mathbf{R}$ and an arbitrary initial values of $\mathbf{k}$ for the objective function (40).
• For the conjugate gradient descent algorithm (42) and (43), set number of iterations M; choose initial convergence rates $\beta$ and $\eta$.
• For everything previously initialized, first calculate the state vector using the Euler method (41) and then calculate the initial search direction vector according to (59) as $\mathbf{s}=-{\displaystyle \frac{\partial J}{\partial \mathbf{k}}}$.
• Calculate the new $\mathbf{k}$ using (42).
• Calculate the gradient ${\displaystyle \frac{\partial J}{\partial \mathbf{k}}}$ using recursive matrix expressions from (55) to (59).
• Calculate the new convergence rate $\beta$ from (44) and new search direction vector $\mathbf{s}$ using (43). In order to determine new step-size $\eta$ in (42), employ the line search strategy satisfying the Wolfe conditions as outlined in reference [37].
• Calculate the state vector using the Euler method (41).
• Shift $l=l+1$ in (42) and (43) and go back to step 5.

4. Simulation and Experimental Validation

4.1. Simulation Results

This subsection presents the results of a simulation conducted to assess the effectiveness of a controller synthesis procedure, previously described in detail in preceding sections of this work.

The matrices $\mathbf{A}\left(t\right)$ and $\mathbf{B}\left(t\right)$ from (31) are defined as follows:

$$\mathbf{A}\left(t\right)=\left[\begin{array}{ccccc}0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& u_{d1}\left(t\right)& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& u_{d1}\left(t\right)& 0\end{array}\right],\mathbf{B}\left(t\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ x_{d2}\left(t\right)& 0& 0\\ 0& 0& 1\\ x_{d4}\left(t\right)& 0& 0\end{array}\right],$$

(60)

and thus the LTV system (30) is determined, for which the synthesis of control law (32) is performed. The algorithmic procedure, described in Section 3.2 for calculating the time-varying elements of the vector $\mathbf{k}\left(t\right)$ at each discrete point of the time interval, by solving the optimization problem (40) and (41), which is then reshaped into the controller matrix $\mathbf{K}\left(t\right)$, has been implemented in the C programming language.

The tracking of reference trajectories is considered in three cases: straight line, circle and curvilinear. In all three cases, the time step length for discretization by the Euler method is set to $\tau =0.016$ s. This is consistent with the sampling frequency of the experimental set-up, which is $62.5$ Hz. Moreover, in order to calculate the convergence rate $\beta$ from Equation (44), the values of $\mu$ and $\nu$ are set to 1 and 0, respectively. This involved the application of the Dai-Yuan method [36]. All calculations are performed on a standard portable computer.

Figure 3 shows a block diagram of the entire control strategy for trajectory tracking.

4.1.1. Straight Line Trajectory Tracking

The desired straight line trajectory is defined as follows:

$$x_d\left(t\right)={\displaystyle \frac{1}{20}}t,y_d\left(t\right)={\displaystyle \frac{1}{20}}t,{\theta }_d\left(t\right)=\mathrm{atan}2\left({\dot{y}}_d\left(t\right),{\dot{x}}_d\left(t\right)\right)={\displaystyle \frac{\pi }{4}}.$$

(61)

In order to obtain the reference of the robot, (61) together with (11)–(15) is included in (6)–(10), from which the desired robot states are calculated as follows:

$$x_{d1}={\displaystyle \frac{1}{20}}t,x_{d2}=0,x_{d3}={\displaystyle \frac{\pi }{4}},x_{d4}=1,x_{d5}={\displaystyle \frac{1}{20}}t,$$

(62)

and the desired inputs are calculated as follows:

$$u_{d1}={\displaystyle \frac{1}{20}},u_{d2}=0,u_{d3}=0.$$

(63)

The simulation results of tracking a straight line are shown in Figure 4 and Figure 5. The initial time is $t_0=0$ s, the final time is $t_f=40$ s and the number of optimization time intervals is $N=2500$. The initial positions and orientation angle of the robot are set to $\left[\begin{array}{ccc}x\left(t_0\right)& y\left(t_0\right)& \theta \left(t_0\right)\end{array}\right]=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]$. The weight matrices $\mathbf{Q}$ and $\mathbf{R}$ of the objective function are chosen as $\mathbf{Q}=\mathrm{diag}({10}^3,1,{10}^2,1,{10}^3)$ and $\mathbf{R}=\mathrm{diag}({10}^4,{10}^2,{10}^2)$.

In Figure 4a, it can be seen that after the transition from the initial conditions the robot follows the desired trajectory very well. Time responses of positions x and y, orientation angle $\theta$, and steering angles ${\delta }_1$ and ${\delta }_2$ are shown in Figure 4b and Figure 4c, respectively. Furthermore, the time responses of control inputs, i.e., linear velocity $v_1$ and steering velocities ${\omega }_1$ and ${\omega }_2$, are shown in Figure 5a. From their responses and maximum values, it can be seen that these velocities are achievable on the actuators of the experimental set-up of the 4ISW4IAW mobile robot. On the experimental mobile robot, due to certain mechanical limitations, but also due to applications for other purposes, the maximum value of $v_1$ is set to $0.13$ m/s. The time dependencies of the elements of the state controller matrix calculated by the proposed algorithm are shown in Figure 5b.

4.1.2. Circular Trajectory Tracking

The desired circular trajectory is defined as follows:

$$x_d\left(t\right)=sin\left(\omega t\right),y_d\left(t\right)=1-cos\left(\omega t\right),{\theta }_d\left(t\right)=\mathrm{atan}2\left({\dot{y}}_d\left(t\right),{\dot{x}}_d\left(t\right)\right),$$

(64)

where $\omega ={\displaystyle \frac{2\pi }{t_f}}$ and $t_f$ is the final time. In order to obtain the reference of the robot, (64) together with (11)–(15) is included in (6)–(10), from which the desired robot states are calculated as follows:

$$\begin{array}{cc}\hfill x_{d1}& =sin\left(\omega t\right),\hfill \\ \hfill x_{d2}& ={\displaystyle \frac{1}{cos\left(\omega t\right)}},\hfill \\ \hfill x_{d3}& =arctan\left(tan\left(\omega t\right)\right),\hfill \\ \hfill x_{d4}& =tan\left(\omega t\right),\hfill \\ \hfill x_{d5}& =1-cos\left(\omega t\right),\hfill \end{array}$$

(65)

and the desired inputs are calculated as follows:

$$\begin{array}{cc}\hfill u_{d1}& =\omega cos\left(\omega t\right),\hfill \\ \hfill u_{d2}& =-{\displaystyle \frac{\omega sin\left(\omega t\right)}{sin{\left(\omega t\right)}^2-1}},\hfill \\ \hfill u_{d3}& =-{\displaystyle \frac{\omega }{sin{\left(\omega t\right)}^2-1}}.\hfill \end{array}$$

(66)

Remark 2.

In order to eliminate the discontinuities caused by the tan (·), arctan$(·)$ and atan2$(·)$ functions, it is necessary to add multiples of $\pm 2\pi$ when the phase difference between consecutive angles of $\left({\dot{y}}_d\left(t\right),{\dot{x}}_d\left(t\right)\right)$ exceeds π radians.

The simulation results of tracking a circular trajectory are shown in Figure 6 and Figure 7. The initial time is $t_0=0$ s, the final time is $t_f=56$ s and the number of optimization time intervals is $N=3500$. The initial positions and orientation angle of the robot are set to $\left[\begin{array}{ccc}x\left(t_0\right)& y\left(t_0\right)& \theta \left(t_0\right)\end{array}\right]=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]$, while the initial steering angles are set according to Ackermann steering geometry as $\left[\begin{array}{cc}{\delta }_1\left(t_0\right)& {\delta }_2\left(t_0\right)\end{array}\right]=\left[\begin{array}{cc}arctan\left({\displaystyle \frac{2a}{R}}\right)& -arctan\left({\displaystyle \frac{2a}{R}}\right)\end{array}\right]$, where a is the distance between the wheels (see Figure 1b) and R is the radius of the desired circle. The weight matrices $\mathbf{Q}$ and $\mathbf{R}$ of the objective function are chosen as $\mathbf{Q}=\mathrm{diag}({10}^4,1,{10}^3,1,{10}^4)$ and $\mathbf{R}=\mathrm{diag}({10}^2,{10}^{-6},{10}^{-6})$.

In Figure 6a, it can be seen that, due to quite precisely chosen initial conditions based on Ackermann’s formula, the robot instantly starts following the desired trajectory. The time responses of positions x and y, orientation angle $\theta$, and steering angles ${\delta }_1$ and ${\delta }_2$ are shown in Figure 6b and Figure 6c, respectively. Furthermore, the time responses of control inputs, i.e., linear velocity $v_1$ and steering velocities ${\omega }_1$ and ${\omega }_2$, are shown in Figure 7a. From their responses and maximum values, it can be seen that these velocities, as it was in the previous case of a straight line, are achievable on the actuators of the experimental set-up of the 4ISW4IAW mobile robot. The time dependencies of the elements of the state controller matrix calculated by the proposed algorithm in the case of a circular trajectory are shown in Figure 7b.

4.1.3. Curvilinear Trajectory Tracking

The desired curvilinear trajectory is defined as follows:

$$\begin{array}{cc}\hfill x_d\left(t\right)& ={\displaystyle \frac{cos\left(\omega t\right)}{2}}+{\displaystyle \frac{sin\left(2\omega t\right)}{2}}-{\displaystyle \frac{1}{2}},\hfill \\ \hfill y_d\left(t\right)& ={\displaystyle \frac{cos\left(2\omega t\right)}{2}}+{\displaystyle \frac{sin\left(\omega t\right)}{2}}-{\displaystyle \frac{3}{5}},\hfill \\ \hfill {\theta }_d\left(t\right)& =\mathrm{atan}2\left({\dot{y}}_d\left(t\right),{\dot{x}}_d\left(t\right)\right),\hfill \end{array}$$

(67)

where $\omega ={\displaystyle \frac{2\pi }{t_f}}$ and $t_f$ is the final time. In order to obtain the reference of the robot, (67) together with (11)–(15) is included in (6)–(10), from which the desired robot states are calculated as follows:

$$\begin{array}{cc}\hfill x_{d1}& ={\displaystyle \frac{cos\left(\omega t\right)}{2}}+{\displaystyle \frac{sin\left(2\omega t\right)}{2}}-{\displaystyle \frac{1}{2}},\hfill \\ \hfill x_{d2}& =-{\displaystyle \frac{4sin\left(3\omega t\right)-14}{2cos\left(4\omega t\right)-12cos\left(2\omega t\right)+9sin\left(\omega t\right)+4sin\left(5\omega t\right)}},\hfill \\ \hfill x_{d3}& =arctan\left({\displaystyle \frac{cos\left(\omega t\right)-2sin\left(2\omega t\right)}{2cos\left(2\omega t\right)-sin\left(\omega t\right)}}\right),\hfill \\ \hfill x_{d4}& =-{\displaystyle \frac{cos\left(\omega t\right)-4cos\left(\omega t\right)sin\left(\omega t\right)}{-4{cos\left(\omega t\right)}^2+sin\left(\omega t\right)+2}},\hfill \\ \hfill x_{d5}& ={\displaystyle \frac{cos\left(2\omega t\right)}{2}}+{\displaystyle \frac{sin\left(\omega t\right)}{2}}-{\displaystyle \frac{3}{5}},\hfill \end{array}$$

(68)

and the desired inputs are calculated as follows:

$$\begin{array}{cc}\hfill u_{d1}& =-{\displaystyle \frac{\omega \left(4{sin\left(\omega t\right)}^2+sin\left(\omega t\right)-2\right)}{2}},\hfill \\ \hfill u_{d2}& =-{\displaystyle \frac{2\omega \left(17cos\left(\omega t\right)+128cos\left(5\omega t\right)-2cos\left(7\omega t\right)\right)}{{\left(2cos\left(4\omega t\right)-12cos\left(2\omega t\right)+9sin\left(\omega t\right)+4sin\left(5\omega t\right)\right)}^2}}\hfill \\ \hfill & -{\displaystyle \frac{2\omega \left(164sin\left(2\omega t\right)-38sin\left(4\omega t\right)-8sin\left(8\omega t\right)\right)}{{\left(2cos\left(4\omega t\right)-12cos\left(2\omega t\right)+9sin\left(\omega t\right)+4sin\left(5\omega t\right)\right)}^2}},\hfill \\ \hfill u_{d3}& ={\displaystyle \frac{\omega \left(2sin\left(3\omega t\right)-7\right)}{{\left(4{sin\left(\omega t\right)}^2+sin\left(\omega t\right)-2\right)}^2}}.\hfill \end{array}$$

(69)

In Figure 8 and Figure 9, the simulation results of tracking a curvilinear trajectory are presented. The initial time is $t_0=0$ s, the final time is $t_f=140$ s and the number of optimization time intervals is $N=8750$. The initial positions and orientation angle of the robot are set to $\left[\begin{array}{ccc}x\left(t_0\right)& y\left(t_0\right)& \theta \left(t_0\right)\end{array}\right]=\left[\begin{array}{ccc}0& 0& 0\end{array}\right]$. In this case, the initial steering angles are chosen as $\left[\begin{array}{cc}{\delta }_1\left(t_0\right)& {\delta }_2\left(t_0\right)\end{array}\right]=\left[\begin{array}{cc}-0.46& 0.46\end{array}\right]$. The weight matrices $\mathbf{Q}$ and $\mathbf{R}$ of the objective function are chosen as $\mathbf{Q}=\mathrm{diag}({10}^2,1,10,1,{10}^2)$ and $\mathbf{R}=\mathrm{diag}(1,1,1)$.

From Figure 8a, it can be observed that, after the transition from the initial conditions, the desired tracking is also achieved in this case of a curvilinear trajectory. In Figure 8b,c, the time responses of positions x and y, orientation angle $\theta$, and steering angles ${\delta }_1$ and ${\delta }_2$ are shown. The time responses of control inputs, i.e., linear velocity $v_1$ and steering velocities ${\omega }_1$ and ${\omega }_2$, can be seen in Figure 9a. It should be noted that the transition time from the initial conditions, the maximum values of control variables and the tracking accuracy can be further improved by appropriate adjustment of the weights in the $\mathbf{Q}$ and $\mathbf{R}$ matrices. This should be carried out in a manner that respects the limitations of the experimental robot system, i.e., it should involve seeking a balance with the desired response performance. The time dependencies of the elements of the state controller matrix calculated by the proposed algorithm are shown in Figure 9b.

4.2. Experimental Results

The objective of this subsection is to assess the efficacy of the previously developed control strategy within the context of the experimental set-up, which is detailed in Section 2.1. As mentioned earlier, the experimental set-up shown in Figure 1 was used to validate the control algorithm design.

The experiment is conducted in a way that the gains of the $\mathbf{K}\left(t\right)$ matrix are calculated in discrete time with a discretization time of $0.016$ s, and the values are stored in a .dat file. A frequency of $62.5$ Hz is chosen because the motor sampling time on the robot is set to this value, i.e., the motor states are sampled at this frequency. The robot control algorithm is executed on a standalone computer, and the velocity and steering angle values are sent via WiFi using ROS. The computer containing ROS and the robot with a single-board computer containing ROS are connected to a common WiFi network. The values are sent to the /trajectory topic, which contains a vector record of four velocities and four steering angles in the form $[v_1,v_1,v_1,v_1,{\delta }_1,{\delta }_2,{\delta }_2,{\delta }_1]$; due to previously introduced conditions for switching to a chained form where the assumptions are made that all wheel velocities are equal $\left(v_1\right)$, the front two wheels have the same steering angle $\left({\delta }_1\right)$ and the rear two wheels have the same steering angle $\left({\delta }_2\right)$. Figure 10 shows a schematic of the laptop and the robot.

From Figure 10, it can be observed that the robot’s single-board computer collects data on the motor states (position, velocity and current) and stores them in the message type JointState and sending them to the laptop via the /joint_states topic. The sent motor states data is saved on the standalone computer using built-in rosbag modules and, upon completion of the experiment, the data is transferred to a .csv file for analysis and plotting. In the next section, the collected data from the experiment is shown together with a comparison to the reference and simulation data. More information regarding the previously mentioned ROS commands and functions can be found in the standard reference [38].

The results of the experimental validation of the proposed control strategy obtained from the data collected from the measuring system of the robot are shown in Figure 11, Figure 12, Figure 13, Figure 14, Figure 15 and Figure 16. The controller parameters and desired trajectory utilized in the experiment are the same as those selected and computed in the simulations.

Based on the presented experimental results, it can be concluded that, on the real system, by applying the proposed control strategy, a very good tracking accuracy of the desired trajectories is also achieved, as well as a fairly good match with the simulation results. Certain differences between the experimental and simulation results are caused by the eventual discrepancies in the values of the geometrical parameters of the robot that are used in the simulation from those that are real.

A closer examination of Figure 14a reveals the presence of oscillations in the steering angle responses. In order to achieve an optimal performance for trajectory tracking, it was necessary to implement high gains for the P controllers. However, if the gains were too high, oscillations were worse but tracking was better. The presented results demonstrate an optimal balance between good trajectory tracking for steering angles and smaller possible oscillations. Moreover, the implementation does not utilize a real-time kernel in Linux, resulting in a frequency of 62.5 Hz with an associated oscillation of approximately 1–2%. This oscillations may also be influenced by imperfections in the discretization time.

Furthermore, it should be mentioned that the presented results are obtained from the robot’s internal measurement system and, for a better insight into the robot’s positions and orientations in the global coordinate system, an external measurement system such as OptiTrack should be used. The application of this system will be included in future research.

4.3. Comparison with Other Methods

The main advantage of the approach proposed in this paper in comparison to other methods known to the authors for the design of a time-varying state controller for LTV systems is that it does not necessitate the iterative solution of the associated Riccati partial differential equation. This fact can be most effectively demonstrated in the context of tracking a circular trajectory. Upon examination of matrices $\mathbf{A}\left(t\right)$ and $\mathbf{B}\left(t\right)$ from Equation (60), it becomes evident that for the system to be fully controllable—which is the fundamental condition for solving the Riccati equation—it is necessary for $u_{d1}\left(t\right)$ to remain non-null throughout the time interval under consideration. From Equation (66), it is evident that $u_{d1}\left(t\right)$ is equal to zero for $t={\displaystyle \frac{t_f}{4}}$, indicating that the system becomes uncontrollable. Consequently, the Riccati equation is unable to be solved or, at least, depending on the chosen discretization step, numerical issues may arise. It is evident from the presented simulations and experimental results that these problems do not arise in our approach.

Additionally, our approach avoids the computational complexity that can arise from the dimension of the LTV system. This is due to the structure of the procedure proposed for gradient calculation, which has a recursive time structure. In contrast, the Galerkin method of weighted residuals for solving Riccati differential equations has a cost that increases exponentially with the dimension of the LTV system. Furthermore, in comparison with the conventional numerical optimal control-based algorithms that employ penalty function methods or methods of Lagrange multipliers to obtain unconstrained formulations, the proposed algorithm is not constrained by high dimensions.

Compared to methods that use LMIs to solve the Riccati equation iteratively at each discrete time step, our approach requires fewer variables to be optimized. In this specific case of the control of a mobile robot, in our approach, due to the direct approach to the solution of the optimization problem, it is necessary to determine a total of 15 elements of the matrix $\mathbf{K}\left(t\right)$. In the case of the application of LMIs, due to the bilinear dependence on the matrix $\mathbf{K}\left(t\right)$, it would be necessary to introduce an appropriate substitution, which would extend the problem to the determination of a total of 29 optimization variables (it would be necessary to determine a symmetric positive definite matrix of dimension $5\times 5$ and a matrix of dimension $3\times 5$).

In comparison with the aforementioned methods, our approach has the limitation of lacking comprehensive recommendations for the initialization of specific algorithm parameters. This can been seen in the summarized steps of the algorithm presented in Section 3.2, where no particular guidance is provided on this matter. The initialization of these parameters directly affects the overall number of iterations required for convergence. LMI solvers are capable of converging with fewer iterations.

It should be noted that the algorithmic procedure presented in Section 3.2 is not intended for online calculation. The objective is to calculate the control law matrix as a function of time in an offline manner. It should be noted, however, that the overall control strategy depicted in Figure 3 is implemented in an online sense and also comprises the nonlinear equations of the chain model of the robot. Furthermore, the proposed strategy is easily programmable, as outlined in Section 4.2. While this paper has focused on the particular problem of mobile robot trajectory tracking, the proposed algorithmic optimization procedure has a number of applications in the context of the control of LTV systems. These include the assessment of system performance limits with different actuator configurations, the setting of realistic performance targets for online control systems and the tuning of parameters of online control systems.

5. Conclusions

In this paper, an approach to design a control system for a 4ISW4IAW mobile robot in a closed loop with a time-varying state controller, which has the goal of asymptotically following the desired trajectory, has been developed. In order to synthesize a controller, the three-input, two-chain, single generator chain model has been linearized in the vicinity of the desired trajectory into a system of control-oriented time-varying error equations.

An approach for the direct calculation of the time-varying state controller of LTV systems has been developed by combining several well-known techniques from the literature. This approach is an efficient algorithm that incorporates the conjugate gradient descent method, the line search method with Wolfe’s conditions, the Euler method for discretization and the chain rule for ordered derivatives, which were selected for their proven effectiveness.

In comparison with previous existing methods, the methodology presented in this paper offers two main advantages: firstly, it does not require the iterative solution of the corresponding Riccati partial differential equation; secondly, it has fewer optimization variables, which makes it more efficient. To the best of the authors’ knowledge, the application of the proposed approach for the synthesis of a 4ISW4IAW mobile robot control law has not yet been investigated.

A simulation and an experimental analysis of the proposed control strategy were performed on a real robot system. The results indicated that the applied control system was effective in following the desired trajectory with an acceptable degree of accuracy.

Future research could focus on the problem of position estimation. This can be accomplished by calibrating the kinematic parameters of the mobile robot, integrating an inertial measurement unit and tracking camera with an Extended Kalman Filter to obtain more accurate estimation.

In addition, future studies could be directed towards controlling both the robot’s dynamics and kinematics, whilst accounting for external disturbances and internal uncertainties inherent to the robot model. From the perspective of differential games, this class of problem can be regarded as one in which the control vector is the “player” that minimizes the optimality criterion, whereas the uncertainty/disturbance vector is the “player” that maximizes the optimality criterion—in other words, minimization and maximization must be carried out with respect to the same objective function. It should be noted here that the maximization of the performance criterion with respect to uncertainty/disturbance can be achieved simply by the change in sign in front of the gradient in the conjugate gradient method. Therefore, it is believed that the same principle of recursive calculation derived in this paper can be applied to the min–max control of LTV systems, thereby enabling the tracking of the desired trajectory of the 4ISW4IAW mobile robot. Furthermore, rather than assuming the initial positions of the 4ISW4IAW mobile robot are known a priori, it is possible to consider the case where the initial positions are treated as unknown uncertainty and the min–max multi-objective optimization problem should be solved.