Robust servo linear quadratic regulator controller based on state compensation and velocity feedforward of the spherical robot: Theory and experimental verification

Abstract

There are few studies on the lateral motion of spherical robots. In this article, a new algorithm is proposed to solve the problem of low control accuracy of the lateral motion. A single lateral motion model is established, and the optimal solution of the linear quadratic regulator in infinite time domain is obtained. Aiming at the problems of longitudinal velocity and lateral angle coupling and low control precision, state compensation and velocity feedforward are carried out, and an improved robust servo linear quadratic regulator control algorithm is proposed. Experiments show that the proposed lateral control algorithm has strong adaptability and robustness to changing speeds and lateral angles, and the control effect is stable and reliable.

Introduction

In recent years, with the development of the field of robotics, all kinds of robots play an important role in industrial, agricultural, medical applications, and other fields. ¹ Spherical robot is a new type of mobile robot. Different from wheeled or footed robots, the closed spherical shell makes the robot not have the risk of overturning, and it is also more friendly to the sensors on board. ² With the deepening of its research, spherical robots are gradually applied in the fields of security, patrol, detection, and inspection operations. According to the different propulsion mechanisms, spherical mobile robots (SMRs) are mainly divided into wheel-based SMR, ³ angular momentum-based SMR, ^4,5 pendulum-based SMR, ⁶ centroid shift-based SMR, ⁷ wind-based SMR, ⁸ and deformable SMR. ⁹ The pendulum-based spherical robot studied in this article is a typical nonholonomic, nonlinear, and underactuated system, which also brings great challenges to the control of SMR.

Most studies on SMRs focus on trajectory following, and less on its motion control. ^{10 –13} For the motion control of SMRs, it is often decoupled into longitudinal control and lateral control. As the most widely used controller in the industry, the proportional–integral–derivative (PID) controller is also used in the control of SMRs and has achieved certain results. Although the PID controller does not require model-based operation, there are problems such as excessive overshoot or long adjustment time in the control effect. ^14,15 Liu et al. achieved acceptable results for a range of speeds in longitudinal control using linear quadratic regulator (LQR). ¹⁶ The sliding mode control is also used in the control of SMRs this year, but its free-chattering problem has great harm to the actuator. ¹⁷ Ayati et al. proposed a hierarchical sliding mode controller, ^18,19 but it requires a trade-off between chattering and robustness. ²⁰ Because fuzzy control does not need the model of the object, it is often combined with other controllers to form a control algorithm with better robustness, but it has the problem of low control accuracy. ^21,22 Most of the above control algorithms focus on the discussion of longitudinal motion control of spherical robots. Due to the complexity of the model, the lateral motion control and the longitudinal velocity are coupled with each other, and most researchers still focus on the simulation discussion. In practice, the lateral control algorithm is still dominated by PID.

LQR is a common method for designing optimal full-state feedback controllers for linear or linearized systems. LQR has been widely used in self-balancing unicycle robots, ^23,24 multi-stage inverted pendulums, ²⁵ drones, ²⁶ legged robots, ²⁷ and other fields. ^{28 –30} This article proposes a robust servo lateral motion controller for SMRs based on LQR. Firstly, a decoupled spherical robot lateral motion dynamic model is established and linearized, and the LQR method is used to solve the optimal control solution in the infinite time domain. Aiming at the problem of low control accuracy caused by linearization, state compensation is carried out, and velocity feedforward is used to solve the coupling problem of lateral motion and longitudinal motion. Finally, an integral term is added to improve the robustness of the controller. In order to verify the robustness and adaptability of the algorithm, a series of physical experiments rather than simulation experiments were designed and implemented.

The article is organized as follows: the second section establishes an independent spherical robot lateral model. The third section linearizes the established model and proposes a robust servo LQR (RSLQR) controller based on state compensation and velocity feedforward for lateral control of the spherical robot. The fourth section illustrates the experimental background and configuration. In the fifth section, practical experiments are carried out, the results are presented and compared with other controller. Finally, conclusions are given in the last section.

System description and modeling

The spherical rolling robot shown in Figure 1 studied in this article is based on a pendulum. The frame is equipped with the communication modules, microcontrollers, and other necessary modules. A pendulum with sufficient mass can swing freely in a space of two degrees of freedom. There is a motor on each of the longitudinal and lateral axes to drive the pendulum to swing. The output of the longitudinal axis motor makes the frame and the pendulum rotate around the longitudinal axis, which in turn causes the spherical robot to generate speed and start rolling. The output of the lateral axis motor makes the pendulum rotate around the lateral axis, causing the sphere to tilt, thereby changing the direction of movement of the robot. Figure 2 shows the motion process of the spherical robot. It is worth noting that the tilt of the sphere does not directly cause the robot to move laterally. The lateral motion of the robot is realized by the yaw angle changed by the tilt of the sphere during rolling, that is, this kind of spherical robot cannot turn in place. For the convenience of description, the symbols in Table 1 are used to represent the corresponding physical quantities. The spherical robot moves in a way similar to a car, the turning radius can be calculated by the following formula:

Figure 1.

Physical and simplified diagrams of the spherical robot.

Figure 2.

Simplified diagram of spherical robot motion: (a) Side view. (b) Front view.

Table 1.

List of nomenclature.

Symbol	Description (value)
$\tau$	Output torque of the lateral axis motor
α	The angle of the spherical shell from the initial position
$\theta$	The swing angle of the frame
$\beta$	The swing angle of the pendulum from the vertical position in the horizontal axis direction
qr	The tilt angle of the longitudinal axis, that is, the tilt angle of the sphere
$\psi$	The yaw angle of the robot
mp	The mass of the pendulum (15.9 $\text{kg}$ )
ms	The mass of the shell (13.5 $\text{kg}$ )
mf	The mass of the frame (10.8 $\text{kg}$ )
m	The total mass of the spherical robot, and $m=m_p+m_s+m_f$
$I_{mr}$	The moment of the pendulum (0.062 $\text{kg}\cdot {\text{m}}^2$ )
$I_{sr}$	The moment of the shell (0.888 $\text{kg}\cdot {\text{m}}^2$ )
$I_{fr}$	The moment of the frame (0.056 $\text{kg}\cdot {\text{m}}^2$ )
l	The distance between the gravity center of the pendulum and the center of the spherical shell (0.215 m)
R	The radius of the spherical shell (0.3 m)

$r=\frac{R}{\text{tan\hspace{0.17em}}{q}_r}$

1

where r and q_r represent the turning radius and the tilted angle of the spherical robot, respectively.

Therefore, the attitude control of the spherical robot is actually the control of its speed and turning radius which is achieved by changing the tilted angle q_r of the robot.

Dynamic analysis and modeling

Dynamics of the proposed spherical robot is abstracted into two coupled differential equations in this article related to generalized coordinates ( $\beta$ and q_r ). The system of equations is derived by the Lagrangian approach. Relevant Lagrangian function is shown as equation (2)

$L=T-V$

2

where T and V represent the kinetic energy and gravitational potential energy of the system, respectively.

The spherical robot system is approximated into three parts (spherical shell, the frame, and the pendulum) shown as Figure 1, and their kinetic energy and gravitational potential energy are obtained respectively.

Spherical shell

$L_s=T_s-V_s=\frac{1}{2}{m}_s{\dot{q}}_r^2{R}^2+\frac{1}{2}{I}_{sr}{\dot{q}}_r^2-m_{s}gR$

3

The frame

$L_f=T_f-V_f=\frac{1}{2}{m}_f{\dot{q}}_r^2{R}^2+\frac{1}{2}{I}_{fr}{\dot{q}}_r^2-m_{f}gR$

4

The pendulum

$\begin{array}{l}{L}_p=T_p-V_p=\frac{1}{2}(I_{mr}+m{l}^2){\dot{\beta }}^2\hfill \\ +\frac{1}{2}{m}_p{\dot{q}}_r^2{R}^2-m_{p}g(R-l\text{cos}\beta )\hfill \end{array}$

5

The Lagrangian function of the overall system can be obtained from the above formulas

$L=L_s+L_f+L_m=h_1{\dot{q}}_r^2+h_2{\dot{\beta }}^2+h_3$

6

where

$\begin{array}{ll}{h}_1\hfill & =\frac{1}{2}(I_{sr}+I_{fr})+\frac{1}{2}(m_s+m_f+m_p)R\hfill \\ h_2\hfill & =\frac{1}{2}(I_{mr}+m_p{l}^2)\hfill \\ h_3\hfill & =(\dot{\beta }{\dot{q}}_{r}R+g)m_{p}lcos\beta -(m_s+m_f+m_p)gR\hfill \end{array}$

The generalized forces of the system are $Q_{\beta }=\tau$ and $Q_r=\tau$ . The Lagrangian equations for the generalized coordinates q_r and $\beta$ are respectively established as follows

$\{\begin{array}{l}\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\beta }}\right)-\frac{\partial L}{\partial \beta }=Q_{\beta }\hfill \\ \frac{d}{dt}\left(\frac{\partial L}{\partial {\dot{q}}_r}\right)-\frac{\partial L}{\partial q_r}=Q_r\hfill \end{array}$

7

Bringing equations (3) to (6) into equation (7), the following system dynamics model can be obtained

$\{\begin{array}{l}{\chi }_1\ddot{\beta }+{\chi }_2{\ddot{q}}_r+m_{p}gl\text{sin}\beta =\tau \hfill \\ {\chi }_3{\ddot{q}}_r+{\chi }_2\ddot{\beta }-m_{p}Rl\text{sin}\beta {\dot{\beta }}^2=\tau \hfill \end{array}$

8

where

$\begin{array}{ll}{\chi }_1\hfill & =I_{mr}+m_p{l}^2\hfill \\ {\chi }_2\hfill & =m_{p}Rl\text{cos}\beta \hfill \\ {\chi }_3\hfill & ={\chi }_3=I_{sr}+I_{fr}+(m_s+m_f+m_p)R^2\hfill \end{array}$

Linearization and state-space representation

The state-space form of a continuous linear time-invariant system is generally expressed as follows

$\{\begin{array}{l}\dot{\mathit{x}}(t)=\mathit{A}\mathit{x}(t)+\mathit{B}\mathit{u}(t)\hfill \\ \mathit{y}=\mathit{C}\mathit{x}(t)\hfill \end{array}$

9

where A is the state matrix, B is the input matrix, C is the output matrix, $\mathit{u}(t)$ is the input vector, $\mathit{x}(t)$ is the state vector, and $\mathit{y}(t)$ is the output vector.

Equation (8) can be expressed in the following form

$\mathit{M}(\mathit{q})\ddot{\mathit{q}}+\mathit{N}(\mathit{q},\dot{\mathit{q}})=\mathit{\tau }$

10

where

$\mathit{M}(\mathit{q})=\left[\begin{array}{cc}{I}_{mr}+m_p{l}^2& m_{p}Rl\text{cos}\beta \\ m_{p}Rl\text{cos}\beta & I_{sr}+I_{fr}+(m_s+m_f+m_p)R^2\end{array}\right]$

$\mathit{N}(\mathit{q},\dot{\mathit{q}})=\left[\begin{array}{c}{m}_{p}gl\text{sin}\beta \\ -m_{p}Rl{\dot{\beta }}^2\text{sin}\beta \end{array}\right],\mathit{q}=\left[\begin{array}{c}\beta \\ q_r\end{array}\right],\mathit{\tau }=\left[\begin{array}{c}\tau \\ \tau \end{array}\right]$

In order to use a LQR, the system needs to be linearized first. Ignore the effect of higher order term $N_2=-m_{p}Rl{\dot{\beta }}^2\text{sin}\beta$ . In fact, the angle $\beta$ is always less than ${15}^{\circ }$ (0.262 rad), so the values of $\text{cos}\beta$ and $\text{sin}\beta$ can be approximated to 1 and $\beta$ , respectively. Therefore, equation (8) can be approximately linearized as follows

${\mathit{M}}_L(\mathit{q})\ddot{\mathit{q}}+{\mathit{N}}_L(\mathit{q})\mathit{q}=\mathit{\tau }$

11

where

${\mathit{M}}_L(\mathit{q})=\left[\begin{array}{cc}{I}_{mr}+m_p{l}^2& m_{p}Rl\\ m_{p}Rl& I_{sr}+I_{fr}+m{R}^2\end{array}\right]$

${\mathit{N}}_L(\mathit{q})=\left[\begin{array}{c}{m}_{p}gl\\ 0\end{array}\right]$

Let

${\mathit{M}}_L^{-1}(\mathit{q})=\left[\begin{array}{cc}{M}_1& M_2\\ M_2& M_4\end{array}\right]$

Convert equation (11) to the standard state-space expression shown in equation (9), where $x=[\beta \dot{\beta }{q}_r{\dot{q}}_r{]}^T$ is the state vector, $y=q_r$ is the output

$\begin{array}{ll}\mathit{A}\hfill & =\left[\begin{array}{cccc}0& 1& 0& 0\\ -c{M}_1& 0& 0& 0\\ 0& 0& 0& 1\\ -c{M}_2& 0& 0& 0\end{array}\right]\hfill \\ \mathit{B}\hfill & =[0M_1+M_{2}0M_2+M_4{]}^T\hfill \\ \mathit{C}\hfill & =[0010]\hfill \end{array}$

12

Now that the state-space representation of a SMR has been formulated, LQR can be employed to control the robot states and evaluate its performance.

Lateral motion controller of SMR

LQR controller

In practical applications, it is always desirable to control the system in this way: to make the system output as close to a desired output as possible. Let r denote the desired output, then

$\mathit{e}=\mathit{r}-\mathit{y}$

13

is called the error vector.

In robot lateral motion control, in addition to ensuring the optimal response of the system to the initial state in a limited time, the system also requires the ability to maintain a stable state. Such problems requiring both optimality and stability should be solved using infinite time regulator. Given the linear and time-invariant system with state-space dynamics in equation (9), the LQR formulation minimizes the following infinite quadratic cost function under dynamic constraints ⁹

$\underset{u}{{\displaystyle \text{min}}}=J(u)$

14

where

$\begin{array}{ll}J(u)\hfill & =\frac{1}{2}{\mathit{x}}^T\mathit{F}\mathit{x}+\frac{1}{2}{\displaystyle {\int }_0^{\infty }}({\mathit{x}}^T\mathit{Q}\mathit{x}+{\mathit{u}}^T\mathit{R}\mathit{u})\hfill \\ \hfill & =\frac{1}{2}{\mathit{e}}^T\mathit{F}\mathit{e}+\frac{1}{2}{\displaystyle {\int }_0^{\infty }}({\mathit{e}}^T\mathit{Q}\mathit{e}+{\mathit{u}}^T\mathit{R}\mathit{u})\hfill \end{array}$

15

where the matrices F , Q , and R are called the terminal weight matrix, the error weight matrix, and the control weight matrix, respectively. Here, Q , R are regarded as design parameters to achieve the desirable closed-loop goals, and the two matrices are usually designed as symmetric positive-definite matrices. ³¹

For infinite-time state regulator, the endpoint index is usually not considered, and the weight matrix $\mathit{F}=0$ is taken. This is because when $t\to \infty$ , it is hoped that $\mathit{e}=0$ , that is, the steady-state error is expected to be zero, so it is not necessary to add the terminal term; at the same time, the engineering implementation requires the response to occur in a limited time, so the terminal term when $\mathit{F}=0$ has no engineering significance.

if $\mathit{r}\ne 0$ , the linear quadratic problem boils down to: when the desired output r is expected to act on the system, the system is required to generate an input, so that the cost function 15 is extremely small, that is, the actual output y of the system always follows the change of r .

For spherical robot, ^9,12 the system is fully controllable because of the full rank of the array pair $\{\mathit{A},\mathit{B}\}$ , i.e.

$\text{rank}\left(\right[\mathit{B}\mathit{A}\mathit{B}{\mathit{A}}^2\mathit{B}{\mathit{A}}^3\mathit{B}\left]\right)=4$

16

Then there is a unique optimal control law for the system ³¹

$\begin{array}{ll}u\hfill & =-\mathit{K}\mathit{x}\hfill \\ \mathit{K}\hfill & ={\mathit{R}}^{-1}{\mathit{B}}^T\mathit{P}\hfill \end{array}$

17

where P is a symmetric, non-negative matrix and is found by solving the continuous time algebraic Riccati equation

$\mathit{P}\mathit{A}+{\mathit{A}}^T\mathit{P}-\mathit{P}\mathit{B}{\mathit{R}}^{-1}{\mathit{B}}^T\mathit{P}+\mathit{Q}=0$

18

For the actual spherical robot system, the control objective is that the error of tilt angle of the sphere is zero, so the control law can be expressed as follows

$u_L=-\mathit{K}\mathit{x}=-k_1\beta -k_2\dot{\beta }-k_3(q_{rd}-q_r)-k_4{\dot{q}}_r$

19

where $q_{rd}$ represents the desired tilt angle trajectory.

Robust servo LQR

The RSLQR optimal control method is developed on the basis of the traditional LQR control method. The idea of RSLQR is to introduce the integral link into the forward loop of the control law and define the state deviation as a new state vector. In this way, the adaptability of the controller to disturbances can be enhanced, so that the system has the ability to accurately track the input control commands. ³²

Define a new state variable as $\mathit{z}=[\mathit{e}\dot{\mathit{x}}{]}^T$ , then the system state equation is

$\dot{\mathit{z}}=\overline{\mathit{A}}\mathit{z}+\overline{\mathit{B}}\mathit{\mu }$

20

where $\mathit{\mu }=\dot{\mathit{u}}$ and

$\overline{\mathit{A}}=\left[\begin{array}{cc}0& \mathit{C}\\ 0& \mathit{A}\end{array}\right]$

$\overline{\mathit{B}}=\left[\begin{array}{c}0\\ \mathit{B}\end{array}\right]$

Then the performance index function of the new system of equation (20) is

$J(\mathit{\mu })=\frac{1}{2}{\displaystyle {\int }_0^{\infty }}(z^T\mathit{Q}\mathit{z}+{\mathit{\mu }}^T\mathit{R}\mathit{\mu })$

21

The Riccati equation for the new system is

$\mathit{P}\overline{\mathit{A}}+{\overline{\mathit{A}}}^T\mathit{P}-\mathit{P}\overline{\mathit{B}}{\mathit{R}}^{-1}{\overline{\mathit{B}}}^T\mathit{P}+\mathit{Q}=0$

22

Get the control gain matrix shown below

${\mathit{K}}_{\mathit{s}}={\mathit{R}}^{-1}{\overline{\mathit{B}}}^T\mathit{P}=[{\mathit{K}}_{\mathit{i}}\mathit{K}]$

23

Since $\mathit{\mu }=\dot{\mathit{u}}$ , $\mathit{\mu }=-{\mathit{K}}_{\mathit{s}}\mathit{z}$ , then

$\begin{array}{ll}{\mathit{u}}_{\mathit{s}}\hfill & ={\displaystyle {\int }_0^{\infty }}\mu =-{\mathit{K}}_{\mathit{s}}{\displaystyle {\int }_0^{\infty }}\mathit{z}\hfill \\ \hfill & =-\mathit{K}\mathit{x}-{\mathit{K}}_{\mathit{i}}{\displaystyle {\int }_0^{\infty }}\mathit{e}\hfill \end{array}$

24

State compensation and velocity feedforward

In the second section, the effect of higher order term in the system is ignored for the convenience of linearization. The error model of the system can be expressed as

$\dot{\mathit{e}}=\mathit{A}\mathit{e}+\mathit{B}\mathit{u}+\mathit{\zeta }$

25

where $\mathit{\zeta }$ is the ignored higher order term.

LQR will make $\dot{\mathit{e}}$ close to $0$ , but it cannot ensure that e is $0$ in steady state. Bring the control law $\mathit{u}=-\mathit{K}\mathit{x}$ into the above formula, in the final steady state ( $\dot{\mathit{e}}=0$ )

$\mathit{e}=-{(\mathit{A}-\mathit{B}\mathit{K})}^{-1}\mathit{\zeta }$

26

In order to make the error of the system approach $0$ in steady state, a state compensation term $\mathit{\delta }$ is introduced. Now the control law of the system is $\mathit{u}=-\mathit{K}\mathit{x}+\mathit{\delta }$ . Rewrite the error model of the system to get

$\dot{\mathit{e}}=\mathit{A}\mathit{e}+\mathit{B}\mathit{u}+\mathit{\zeta }=\mathit{A}\mathit{e}+\mathit{B}(-\mathit{K}\mathit{e}+\mathit{\delta })+\mathit{\zeta }$

27

then

$\mathit{e}=-{(\mathit{A}-\mathit{B}\mathit{K})}^{-1}(\mathit{B}\mathit{\delta }+\mathit{\zeta })$

28

The solution of the problem can be equivalently expressed as follows: how to choose the value of $\mathit{\delta }$ so that e can be $0$ in finite time.

Let

$\mathit{B}\mathit{\delta }+\mathit{\zeta }=0$

29

then

$\mathit{\delta }=-{\mathit{B}}^{-1}\mathit{\zeta }$

30

In the spherical robot system of this article, the state compensation term takes the following value:

$\delta =f(\mathit{x},\dot{\mathit{x}})=\frac{m_{p}Rl{\dot{\beta }}^2\text{sin}\beta }{M_2+M_4}$

31

In addition, for the convenience of calculation and solution, the motion of the longitudinal axis and the lateral axis are considered to be completely decoupled when modeling. In fact, the tilted angle control of sphere is affected by the velocity. When the speed is higher, a larger centripetal force is required to achieve the same turning radius. Therefore, it is necessary to perform feedforward compensation according to the speed and the set angle. Experiments show that this compensation is a quadratic function of speed. Take the value of the following formula for velocity feedforward

$\rho =g(v,q_r)=\text{sgn}(q_r)*({\alpha }_1{v}^2+{\alpha }_{2}v)$

32

where $\text{sgn}$ is the sign function, ${\alpha }_1$ and ${\alpha }_2$ are both feedforward coefficients.

IRSLQR controller

So far, the design of the overall control algorithm of the spherical robot has been completed. The schematic diagram of the improved robust servo LQR (IRSLQR) controller is shown in Figure 3. The control output of the algorithm consists of the following parts: the calculated torque of RSLQR, state compensation, and speed feedforward compensation, as shown in the following formula,

Figure 3.

Schematic diagram of the control system.

$u=\delta +\rho +u_s$

33

Stability analysis

For the stability research of the control system, Lyapunov stability theory is often used for analysis.

Define the Lyapunov function as follows

$V(\mathit{z})={\mathit{z}}^{\mathit{T}}\mathit{P}\mathit{z},\mathit{P}={\mathit{P}}^{\mathit{T}}\text{\hspace{0.17em}and }\mathit{P}>0$

34

where P is a positive definite matrix. Only when $\mathit{z}=0$ , there is $V(\mathit{z})=0$ ; for non-zero z , since $\mathit{P}>0$ , there must be $V(\mathit{z})>0$ . Obviously

$\underset{\parallel z\parallel \to \infty }{{\displaystyle \text{lim}}}V(\mathit{z})=\underset{\parallel z\parallel \to \infty }{{\displaystyle \text{lim}}}{\mathit{z}}^{\mathit{T}}\mathit{P}\mathit{z}=\infty$

35

Then, take the derivative of z with respect to $V(\mathit{z})$

$\dot{V}(\mathit{z})={\dot{\mathit{z}}}^{\mathit{T}}\mathit{P}\mathit{z}+{\mathit{z}}^{\mathit{T}}\mathit{P}\dot{\mathit{z}}$

36

According to the system equation and control law, the following formula is obtained

$\dot{V}(\mathit{z})={\mathit{z}}^{\mathit{T}}[{\overline{\mathit{A}}}^{\mathit{T}}\mathit{P}-{\mathit{K}}^{\mathit{T}}{\overline{\mathit{B}}}^{\mathit{T}}\mathit{P}+\mathit{P}\overline{\mathit{A}}-\mathit{P}\overline{\mathit{B}}{\mathit{K}}_{\mathit{s}}]\mathit{z}$

37

Bring the feedback matrix calculated by RSLQR into the calculation

$\dot{V}(\mathit{z})={\mathit{z}}^{\mathit{T}}[{\overline{\mathit{A}}}^{\mathit{T}}\mathit{P}+\mathit{P}\overline{\mathit{A}}-2\mathit{P}\overline{\mathit{B}}{\mathit{R}}^{-1}{\overline{\mathit{B}}}^{\mathit{T}}\mathit{P}]\mathit{z}$

38

From the aforementioned Riccati equation, it can be known that

$\mathit{Q}=\mathit{P}\overline{\mathit{B}}{\mathit{R}}^{-1}{\overline{\mathit{B}}}^{\mathit{T}}\mathit{P}-\mathit{P}\overline{\mathit{A}}-{\overline{\mathit{A}}}^{\mathit{T}}\mathit{P}$

39

Substituting equation (6) into equation (5)

$\dot{V}(\mathit{z})=-{\mathit{z}}^{\mathit{T}}[\mathit{Q}+\mathit{P}\overline{\mathit{B}}{\mathit{R}}^{-1}{\overline{\mathit{B}}}^{\mathit{T}}\mathit{P}]\mathit{z}$

40

In the above formula, $\mathit{Q}>0$ , $\mathit{R}>0$ , and $\mathit{P}>0$ , so $\dot{V}(\mathit{z})\le 0$ ( $\dot{V}(\mathit{z})=0$ if and only if $\mathit{z}=0$ ). According to the Lyapunov stability theory, the system is asymptotically stable.

Experimental setup

All the following experiments are based on the actual spherical robot with a diameter of 60 cm shown in Figure 1. The longitudinal and lateral motor torques are used to control the robot’s speed and lateral angle, respectively, which are the core principles of the robot’s operation. The measurement system consists of three sensors: two motor encoders for feedback of parameters such as torques and positions of the motors; a three-axis inertial measurement unit (IMU) for obtaining the real-time states of the robot (including acceleration, Euler angle, etc.) with Kalman filter and data fusion. Drapikowski et al. demonstrated that such a measurement system is sufficient for robotics. ³³ The low-level control algorithm of the robot is implemented in a high-performance embedded system with C language, the sampling frequency is 100 Hz, and the control frequency is 50 Hz. Data transmission, Data recording, mapping, and navigation algorithm are implemented in the ROS middleware of the host computer.

Experimental results

This section provides experimental results of direct application of LQR to the spherical robot. The section also includes the implementation details and quantitative results of the IRSLQR controller and fuzzy PID (FPID) controller, and compares the performance of the two controllers.

LQR controller

In the previous control theory analysis, the system was linearized around the origin far away from the control boundary, and the LQR gain obtained by the linear system analysis. Note that the dynamic analysis of the system assumes that the motion speed and lateral motion of the robot are completely decoupled, which is not possible in real physical systems. In fact, the moving speed of the spherical robot is higher, the coupling between the speed and the tilted angle of the spherical robot is larger.

Figure 4 shows the results of controlling the robot using LQR alone. The shaded part in pink in the figure represents the difference between the actual tilted angle and the expected tilted angle of the robot. The red line represents the expected tilted angle of the sphere. Since the oscillation period of the spherical robot is uncertain, in order to better calculate the stabilized spherical robot tilted angle and verify the effectiveness of the controller, a filter with a constantly increasing time window is employed to smooth the tilted angle data and the results were represented by the blue line. Figure 4(a) shows the performance of the LQR controller when the robot was running at 0.5 m/s and the expected tilted angle was 0.105 rad. The actual tilted angle of the robot fluctuated around the expected angle because the point at which the system is linearized is the equilibrium origin (0 rad). At the same time, it is assumed that the motions in the longitudinal axis and lateral axis directions are decoupled during modeling, that is, the velocity and the tilted angle do not affect each other. Therefore, when the expected titled angle is larger and the speed is faster, the performance of the LQR controller is worse. Figure 4 supports this view.

Figure 4.

The robot ran at 0.5 m/s for (a) and (b), 1.0 m/s for (c) and (d). (a) Expected angle: 0.105 rad. (b) Expected angle: 0.192 rad. (c) Expected angle: 0.105 rad. (d) Expected angle: 0.192 rad.

In order to qualitatively describe the experimental results, the maximum offset and the root mean square error (RMSE) are used. Among them, the maximum positive/minimum negative offset represents the maximum positive/minimum negative value of the actual tilted angle deviating from the expected angle within the time from the first time the spherical robot reaches the desired angle to the end of the experiment. RMSE measures the deviation between the observed value and the expected value. When the robot moved at a low speed of 0.5 m/s and the desired angle was 0.105 rad, RMSE is 0.0610 rad, the maximum positive offset is 0.096 rad, and the minimum negative offset is −0.134 rad. When only increasing the desired angle (0.192 rad), RMSE increases to 0.1006 rad, the maximum positive offset is 0.028 rad, and the minimum negative offset is −0.196. When only increasing the speed (1.0 m/s), RMSE increases to 0.0743 rad, the maximum positive offset is 0.013 rad, and the minimum negative offset is −0.136 rad. Increasing the speed to 1.0 m/s, while increasing the desired angle to 0.192 rad, results in an increase in RMSE to 0.1348 rad with a maximum positive offset of 0 rad and a minimum negative offset of −0.237 rad. It can be seen that the control performance of pure LQR drops sharply when the speed and desired angle are far from the linearized equilibrium origin.

IRSLQR controller

Aiming at the problems of poor robustness and large steady-state error of LQR controller, an IRSLQR controller is proposed in this article, as shown in Figure 3. To verify the effectiveness and robustness of the proposed controller, different tests are designed to conduct experiments, and its performance is compared with the FPID controller proposed in the previous paper. ³⁴ For a strict comparison between the model-based controller (IRSLQR) and the error-based controller (FPID), experiments were conducted on the spherical robot repetitively and analyzed with representative values.

Single-angle tracking experiment at a single speed

The first test involved of the experiments is the tracking performance of the two controllers at different speeds for angles. Due to the limitation of the mechanical structure of the spherical robot and the centrifugal force, when the speed of the robot is higher than 1.3 m/s, the tilted angle can no longer reach 0.192 rad. Therefore, in the test, three speeds of 0.5 m/s, 1.0 m/s, 1.3 m/s and two desired tilted angles of 0.105 rad and 0.192 rad were set to verify the effectiveness of the proposed controller. Under the two controllers, the robot independently tracked different desired angles based on different speeds. Figure 5 shows the experimental results. The pink shaded part and the yellow shaded part in the figures represent the difference between the actual tilted angle and the expected tilted angle of the spherical robot controlled by IRSLQR and FPID controller, respectively. The red line represents the expected tilted angle, and the blue and green lines are the actual tilted angle data obtained by smooth filtering of the IRSLQR and FPID controller, respectively. In this section, in addition to the RMSE and maximum offset mentioned above, the minimum steady-state error (MSSE) is also used to characterize the control effect. MSSE is defined as the minimum error after the system enters the steady state during the entire control process, which is reflected in the minimum deviation between the blue line/green line and the red line in Figure 5. Table 2 shows all the performance indicators of the two controllers. It can be seen from the experimental results that the RMSE of IRSLQR is about half smaller than that of the FPID controller, and the MSSE is an order of magnitude smaller, regardless of high speed or low speed, large angle or small angle. The maximum offset distribution of IRSLQR is relatively uniform, and the absolute value is less than 0.0785 rad. With the increase of speed and angle, the maximum positive offset of the FPID controller gradually decreases, and the minimum negative error gradually increases. It may not even reach the expected angle. The control accuracy of the proposed controller is higher than that of FPID, and the robustness and adaptability are also better.

Figure 5.

The robot ran at 0.5 m/s for (a) and (b), 1.0 m/s for (c) and (d), 1.3 m/s for (e) and (f). (a) Expected angle: 0.105 rad. (b) Expected angle: 0.192 rad. (c) Expected angle: 0.105 rad. (d) Expected angle: 0.192 rad. (e) Expected angle: 0.105 rad. (f) Expected angle: 0.192 rad.

Table 2.

The experimental results comparison of the two controllers.

Controller	Velocity (m/s)	Expected angle (rad)	Indicators
			RMSE (rad)	MSSE (rad)	Maximum positive offset (rad)	Minimum negative error (rad)
Fuzzy PID	0.5	0.105	0.0531	0.0260	0.0711	−0.1136
		0.192	0.0640	0.0400	0.0612	−0.1393
	1	0.105	0.0553	0.0410	0.0509	−0.1223
		0.192	0.0602	0.0475	0.0032	−0.1310
	1.3	0.105	0.0467	0.0341	0.0259	−0.1067
		0.192	0.0949	0.0754	0.0710	−0.1697
IRSLQR	0.5	0.105	0.0245	0.0030	0.0684	−0.0503
		0.192	0.0375	0.0020	0.0542	−0.0755
	1	0.105	0.0245	0.0000	0.0545	−0.0483
		0.192	0.0362	0.0000	0.0523	−0.0592
	1.3	0.105	0.0232	0.0040	0.0472	−0.0445
		0.192	0.0367	0.0044	0.0610	−0.0784

PID: proportional–integral–derivative; IRSLQR: improved robust servo linear quadratic regulator.

Single-angle tracking experiment with variable speed

To further verify the robustness of the proposed controller with respect to the spherical robot velocity, the desired velocity profile was designed in stages while keeping the expected angle constant. The experimental result is shown in Figure 6. The black line corresponds to the ordinate on the right, representing the variation law of the velocity in the whole process. The shades of blue and green represent the difference between the expected tilted angle (red line) and the actual tilted angle of the spherical robot, respectively. IRSLQR has a RMSE of 0.0344 rad, a maximum positive offset of 0.0558 rad, and a minimum negative offset of −0.0839 rad, while FPID has a RMSE of 0.0594 rad, a maximum positive offset of 0.1105 rad, and a minimum negative offset of −0.1546 rad. The velocity change will not have much influence on the control effect of IRSLQR, and its robustness to speed is better than that of FPID.

Figure 6.

Controllers response at continuously changing speed.

Variable-angle tracking experiment with single speed

In practical application scenarios, the expected tilted angles are usually varied. The experiment continuously changed the desired tilted angle within 30s to test the response performance of the two controllers. Throughout the experiment, RMSE of IRSLQR is 0.0325 rad, the maximum positive offset is 0.0610 rad, the minimum negative offset is −0.0917 rad, and RMSE of FPID is 0.0505 rad, the maximum positive offset is 0.1040 rad, and the minimum negative offset is −0.1080 rad. The performance of both controllers degraded for a period of time after the desired angle change. Compared to FPID, IRSLQR could adjust faster and stabilize the angle around the desired angle again. It can be seen from the experimental results that the robustness and adaptability of the proposed controller to the angle is better than that of FPID.

Figure 7.

Controllers response at continuously changing expected angles.

Conclusion

In this study, we focus on lateral motion control of the spherical robot. To this end, a spherical robot lateral dynamics equation decoupled from the velocity is proposed, and linearized at the equilibrium point, and the corresponding optimal controller (LQR controller) is designed according to the linearized equation. However, only using the LQR controller designed after linearization will have steady-state errors and its control accuracy is not enough. Therefore, we propose an IRSLQR controller based on state feedback and speed feedforward, which has high robustness and strong adaptability.

To verify the performance of the proposed controller, we designed and conducted various experiments and compared the experimental results with the FPID controller. The experimental results show that the IRSLQR controller has better performance both in control accuracy and speed/angle robustness. This indicates that the proposed controller has broader application prospects in practical scenarios.

However, the lateral motion of spherical robots still has many challenges. For example, in unknown terrain environments, the established model can no longer be applied, so new algorithms are required for the stable control of the angle. In addition, stable control on a sloped road is also a challenging problem. We will then focus on related topics for research.