Hybrid Model Predictive Control with Penalty Factor Based on Image-Based Visual Servoing for Constrained Mobile Robots

Abstract

For the constrained mobile robot automatic parking system, the hybrid model predictive control with a penalty factor based on image-based visual servoing (IBVS) is proposed to address the problem of feature point loss and emergency braking in dynamic obstacle scenarios caused by excessive target bias gain when using traditional IBVS control methods. The traditional IBVS control is transformed into an optimization problem with constraints in the finite time domain, by defining the optimization function based on the mobile robot’s positional deviation and image feature point deviation, while using actuator saturation and speed limit as constraints. Based on this, a convex optimization function with penalty factors is defined and combined with incremental model predictive control. This control strategy could ensure the emergency braking performance of the mobile robot when the image feature points are massively obscured by obstacles in dynamic scenes, while improving the accuracy and real-time of its trajectory tracking control. Finally, simulation comparisons are conducted to verify the effectiveness of the proposed control method.

1. Introduction

With the development of science and technology, automatic parking has become a research hotspot, and the trajectory tracking control of mobile robots is a key technology related to the accuracy of automatic parking [1]. In particular, the research of visual servo-based automatic parking and emergency braking for mobile robots constrained by feature points has received increasing attention [2].

Classical visual servo control is mainly divided into position-based visual servoing control (PBVS), image-based visual servoing control (IBVS) and hybrid visual servoing control [3]. The PBVS control needs to define the error signal in the three-dimensional Euclidean space for three-dimensional reconstruction. The direct error control in the three-dimensional space can ensure its convergence [4]. However, this method is vulnerable to system calibration errors and visual measurement noise [5]. The IBVS is controlled by defining the error signal in the two-dimensional image plane [6]. It does not need the 3D pose information of the mobile robot and is robust to the system disturbances [7]. The IBVS has been combined with advanced control algorithms to realize automatic parking for mobile robots. In [8], for the problem of lack of depth information and parameter estimation in the visual servo control of mobile robots, the sliding mode control method based on IBVS three-view pair-pole geometry is proposed, to solve the depth correction problem and ensure the closed-loop stability of the system by expanding the pair-pole geometry to three pictures. In [9], for the limitation that the camera plane must be parallel to the motion plane in the visual servo control of mobile robots, the IBVS-based adaptive controller is proposed, which has a simple structure and high efficiency to achieve good trajectory tracking control performance. Nonetheless, the target deviation gain of the mobile robot visual servo control system is prone to excessive selection, when the camera parameters are not accurately calibrated. Then, the above control methods are difficult to deal with the problem of feature point loss and system instability caused by excessive target deviation gain.

For the problem of losing feature points and system instability due to excessive target deviation gain in the vision servo control system of mobile robots, it is necessary to constrain image feature points to maintain them within the field of view at all times [10]. The existing studies are divided into two main approaches: algorithm design and hardware adjustment. In [11], for the visual servo control of mobile robots, a time-varying continuous hybrid visual servo controller is designed to realize consistent tracking and pose correction for mobile robots, to solve the problem that the target feature points are difficult to maintain within the field of view of the camera, and to ensure the stability of feature point tracking; in [12], for the image feature point constraint problem, a two-stage active visual servo scheme for mobile robots is proposed. By using an external mobile platform camera device, it could keep the feature points around the center of the image plane to achieve mechanical tracking of the image feature points and ensure that the image feature points are always maintained within the field of view. However, none of the above methods are calibrated with camera specific parameters. In [13], for the problem of constrained mobile robot visual servo control when the camera parameters are not accurately calibrated, the specific parameters of the camera are estimated and an adaptive continuous visual servo controller is designed to realize the trajectory tracking control of the mobile robot under the image feature point constraint. The internal constraints such as actuator saturation and speed limitation are not considered in the above methods, and these methods are not universally applicable.

Model predictive control (MPC) is a model-based optimal control strategy [14], which owns superiorities such as rolling optimization, constraint handling, and state prediction [15]. It is suitable for handling the internal constraints of the control system and predicting the system state [16]. The model predictive control algorithm has been introduced to the IBVS-based mobile robot control system. In [17], for the mobile robot automatic parking control system, the constrained IBVS-based model predictive control method is proposed to ensure the accuracy of the mobile robot trajectory tracking control, by considering the internal constraints such as actuator saturation and speed limitation. But the computational burden is large and it is difficult to be applied practically. In [18], for the problem of poor real-time image processing and rolling optimization of the combination of IBVS and MPC, the IBVS-based model predictive control self-triggering switching scheme is designed to reduce the computation time, by eliminating the need for visual measurement and computation of control inputs at two consecutive triggering moments. But it is difficult to meet the high accuracy requirements of mobile robot trajectory tracking control. To solve the problem of difficulty in ensuring both real-time and accuracy of the IBVS-based mobile robot model predictive control system mentioned above, the IBVS-based incremental MPC scheme is proposed in [19]. This method reconstructs incremental control quantities, while considering internal constraints such as actuator saturation and speed limits, to improve the real-time and accuracy of mobile robot trajectory tracking. However, this method only considers the image feature point deviation when defining the optimization function, and does not incorporate the positional deviation of the mobile robot. When the image feature points are intermittently lost due to external disturbances, it is easy to cause a decrease in the accuracy of trajectory tracking control for mobile robots.

In addition, the automatic parking process of mobile robots is prone to emergency braking problems when dynamic obstacles emerge. In [20], for the mobile robot system in narrow environments, the MPC hierarchical obstacle avoidance framework based on scene search is presented. The upper-level scene search scheme is used to plan the obstacle avoidance path, and the lower-level MPC is designed to control the mobile robot to park automatically. Then, the emergency braking control of the mobile robot in dynamic obstacle scenarios is realized. In [21], for the emergency braking problem in the presence of a sudden dynamic obstacle, the predictive obstacle avoidance control scheme is proposed. It is used to avoid dynamic obstacles by using pre-sighting point search methods for local path planning. Nonetheless, all of the above obstacle avoidance algorithms use the search-before-braking scheme, which is easy to lead to emergency braking lag and difficult to ensure the safety and real-time of automatic parking for mobile robots.

In response to the above issues, the main contributions of this paper are reflected in the following aspects:

(1)

For the mobile robot system subject to feature point motion constraints, the IBVS-based incremental model predictive control algorithm is designed, to solve the problem of feature point loss and system instability due to excessive target deviation gain when the traditional IBVS control method is applied to an automatic parking control system. The traditional IBVS control is transformed into an optimization problem with constraints in the finite time domain, by defining the optimization function based on the mobile robot’s positional deviation and image feature point deviation, while using actuator saturation and speed limit as constraints. Then, the accuracy and real-time of the mobile robot tracking control during automatic parking is improved simultaneously.

(2)

For the problem of emergency braking of mobile robot automatic parking in dynamic obstacle scenes, by defining the convex optimization function with penalty factor, the hybrid model predictive control with a penalty factor based on IBVS (IBVS-PF-HMPC) is proposed. Then, it could guarantee the emergency braking performance of the mobile robot automatic parking when the image feature points are massively obstructed by obstacles in dynamic scenes.

This work is organized as follows: Section 2 presents the problem formulation, such as the kinematic model and the IBVS model. The formulation of the mobile robot controller is presented in Section 3 with an IBVS-PF-HMPC scheme. Section 4 shows the comparative results through simulations. The conclusions of this work are presented in Section 5.

2. Problem Formulation

Based on the kinematic model and the IBVS model of the mobile robot, the IBVS-PF-HMPC is designed by considering the position deviation and the image feature point deviation for automatic parking trajectory tracking of the mobile robot. The designed controller still operates based on the position deviation to guarantee the safety and accuracy of the mobile robot system, when the feature points are intermittently lost due to a disturbance at a certain time. For example, the mobile robot will brake urgently when the feature points are obstructed in dynamic environments.

2.1. Model of Mobile Robots

Due to the low-speed characteristics of automated parking, the kinematic characteristics of the mobile robot are much greater than the kinetic characteristics. The mobile robot satisfies the Ackermann steering principle, and the trajectory of its rear axis center point can characterize the lateral motion characteristics, and the front axis as the steering axis can reflect the lateral motion characteristics. The motion model in the world coordinate system is shown in Figure 1, by using the center coordinates of the rear axis and the equivalent deflection angle of the front axis as the objects.

In Figure 1, $\left(X_r,Y_r\right)$ and $\left(X_{\mathit{f}},Y_{\mathit{f}}\right)$ represent the coordinates of the rear and front axle axes of the mobile robot, $\phi$ represents the transverse sway angle, ${\delta }_{\mathit{f}}$ represents the front wheel deflection angle, $v_r$ represents the rear axle center velocity, $v_{\mathit{f}}$ represents the front axle center velocity, $L$ represents the axle distance, $R$ represents the rear wheel steering radius, and $P$ represents the instantaneous center of vehicle rotation, assuming that the lateral eccentricity of the center of mass of the mobile robot remains constant during the steering process, i.e., the instantaneous steering radius is the same as the radius of curvature of the road, and the mobile robot is a rigid body.

At the rear axle center $\left(X_r,Y_r\right)$, the rear axle center velocity is

$$v_r={\dot{X}}_r\cos \phi +{\dot{Y}}_r\sin \phi$$

(1)

The kinematic constraints of the front and rear axles are

$$\left\{\begin{array}{l}{\dot{X}}_{\mathit{f}}\sin \left(\phi +{\delta }_{\mathit{f}}\right)-{\dot{Y}}_{\mathit{f}}\cos \left(\phi +{\delta }_{\mathit{f}}\right)=0\\ {\dot{X}}_r\sin \phi -{\dot{Y}}_r\cos \phi =0\end{array}\right.$$

(2)

Combining (1) and (2) we have

$$\left\{\begin{array}{l}{\dot{X}}_r=v_r\cos \phi \\ {\dot{Y}}_r=v_r\sin \phi \end{array}\right.$$

(3)

According to the geometric relationship between the front and rear wheels, we have

$$\left\{\begin{array}{c}{X}_{\mathit{f}}=X_r+l\cos \phi \\ Y_{\mathit{f}}=Y_r+l\sin \phi \end{array}\right.$$

(4)

Then, the angular velocity can be solved as

$$w=\frac{v_r}{L}\tan {\delta }_{\mathit{f}}$$

(5)

where $w$ is the angular velocity. Then, we can have rear wheel steering radius $R$ and front wheel deflection angle ${\delta }_{\mathit{f}}$:

$$\left\{\begin{array}{l}R=\frac{v_r}{w}\\ {\delta }_{\mathit{f}}=\arctan \left(\frac{L}{R}\right)\end{array}\right.$$

(6)

Then, the kinematic equation of the mobile robot in the geodesic coordinate system is given as

$$\left[\begin{array}{c}{\dot{X}}_r\\ {\dot{Y}}_r\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}\cos \phi \\ \sin \phi \\ \tan {\delta }_{\mathit{f}}/L\end{array}\right]v_r$$

(7)

where ${\dot{X}}_r$ and ${\dot{Y}}_r$ are the velocities in the $X$ axis and $Y$ axis, and $\dot{\phi }$ is the acceleration of the transverse pendulum angle. Due to the large calculation of $\tan {\delta }_{\mathit{f}}/L$, the angular velocity $w$ is introduced, and ${\left[v_r,w\right]}^T$ is the control quantity.

The kinematic model of the mobile robot can be transformed into the following equation:

$$\left[\begin{array}{c}{\dot{X}}_r\\ {\dot{Y}}_r\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}\cos \phi \\ \sin \phi \\ 0\end{array}\right]v_r+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]w$$

(8)

where $x={\left[X,Y,\phi \right]}^T$ is the position of the mobile robot in the world coordinate system, $u={\left[v_r,w\right]}^T$ is the linear and angular velocities.

2.2. Model of IBVS System

It is necessary to establish suitable expressions to change the coordinates of a three-dimensional (3D) space into a two-dimensional (2D) plane. The relationship between image change and mobile robot motion of the calibration-free servo system can be modeled directly from the known image information, i.e., the image Jacobi matrix can be obtained to establish the transformation between the 3D space and 2D plane. This method could reduce the amount of coordinate conversion and system calculations, and does not need to calculate the internal and external parameters of the camera. Thus, it can avoid the impact of errors generated during the calibration process. It is defined as

$$\dot{\mathit{f}}={\mathit{J}}_{\mathit{r}}\left(\mathit{r}\right)\cdot \dot{\mathit{r}}$$

(9)

where $\mathit{f}={\left[{\mathit{f}}_1,{\mathit{f}}_2,\dots ,{\mathit{f}}_m\right]}^T$ is the image feature point coordinates, $\dot{\mathit{f}}$ is the velocity of the image feature point change, $\mathit{r}={\left[r_1,r_2,\dots ,r_n\right]}^T$ is the mobile robot position, $\dot{\mathit{r}}$ is the velocity of the mobile robot position change, and ${\mathit{J}}_{\mathit{r}}\left(\mathit{r}\right)$ is the image Jacobi matrix.

Figure 2 is the camera imaging principle. $O_w-X_w{Y}_w{Z}_w$ is the world coordinate system. $O_c-X_c{Y}_c{Z}_c$ is the camera coordinate system. $O_m-X_m{Y}_m$ is the image coordinate system.

In Figure 2, the coordinate point $P$ of the camera coordinate system is $P_c=\left(x_c,y_c,z_c\right)$, and $P$ of the plane image coordinate system is $P_m\left(x_m,y_m\right)$. According to the camera imaging principle, we have

$$\left\{\begin{array}{c}{x}_m=\frac{\lambda }{d_x}\cdot \frac{x_c}{z_c}+x_m{}_0\\ y_m=\frac{\lambda }{d_y}\cdot \frac{y_c}{z_c}+y_m{}_0\end{array}\right.$$

(10)

where $\lambda$ is the camera focal length, $x_{m0},y_{m0}$ is the image plane origin, and $d_x,d_y$ are the physical sizes of a unit pixel on the horizontal and vertical axes, respectively. Taking the derivative of (10),

$$\left\{\begin{array}{c}{\dot{x}}_m=\frac{\lambda }{d_x}\left(\frac{{\dot{x}}_c}{z_c}-\frac{x_c{\dot{z}}_c}{z_c{}^2}\right)\\ {\dot{y}}_m=\frac{\lambda }{d_y}\left(\frac{{\dot{y}}_c}{z_c}-\frac{y_c{\dot{z}}_c}{z_c{}^2}\right)\end{array}\right.$$

(11)

Let $v={\left[v_x,v_y,v_z\right]}^T$ be the linear velocity of the mobile robot, and $\mathit{w}={\left[w_x,w_y,w_z\right]}^T$ be the angular velocity of the mobile robot. The velocity of a point in space with respect to the camera coordinate system is

$${\dot{P}}_c=-\mathit{w}\times P_c-\mathit{v}$$

(12)

According to (12),

$$\left\{\begin{array}{c}{\dot{x}}_c=-v_x-w_y{z}_c+w_z{y}_c\\ {\dot{y}}_c=-v_y-w_z{x}_c+w_x{z}_c\\ {\dot{z}}_c=-v_x-w_x{y}_c+w_y{x}_c\end{array}\right.$$

(13)

Then we have

$$\left[\begin{array}{c}{\dot{x}}_m\\ {\dot{y}}_m\end{array}\right]=\left[\begin{array}{cccccc}-\frac{\lambda }{z_c}& 0& \frac{x_m}{z_c}& \frac{x_m\cdot y_m}{\lambda }& -\frac{{\lambda }^2+x_m{}^2}{\lambda }& y_m\\ 0& -\frac{\lambda }{z_c}& \frac{y_m}{z_c}& \frac{{\lambda }^2+y_m{}^2}{\lambda }& -\frac{x_m\cdot y_m}{\lambda }& -x_m\end{array}\right]\cdot \left[\begin{array}{c}{v}_x\\ v_y\\ v_z\\ w_x\\ w_y\\ w_z\end{array}\right]$$

(14)

where the velocity of the image feature point change is $\dot{\mathit{f}}={\left[{\dot{x}}_m,{\dot{y}}_m\right]}^T$, the velocity of the mobile robot position change is $\dot{\mathit{r}}={\left[v_x,v_y,v_z,w_x,w_y,w_z\right]}^T$, and the image Jacobi matrix is

$${\mathit{J}}_{\mathit{r}}=\left[\begin{array}{cccccc}-\frac{\lambda }{z_c}& 0& \frac{x_m}{z_c}& \frac{x_m\cdot y_m}{\lambda }& -\frac{{\lambda }^2+x_m{}^2}{\lambda }& y_m\\ 0& -\frac{\lambda }{z_c}& \frac{y_m}{z_c}& \frac{{\lambda }^2+y_m{}^2}{\lambda }& -\frac{x_m\cdot y_m}{\lambda }& -x_m\end{array}\right]$$

(15)

Combining (14) with the kinematic model of the mobile robot, the velocity of the restricted mobile robot’s position change is

$$\dot{\mathbf{r}}={\left[{\dot{X}}_{\mathit{r}},{\dot{Y}}_{\mathit{r}},0,0,0,\dot{\phi }\right]}^T$$

(16)

The relationship between the change speed of 2D image feature points and the mobile robot control quantity can be obtained:

$$\dot{\mathit{f}}={\left[{\dot{x}}_m,{\dot{y}}_m\right]}^T={\mathit{J}}_{\mathit{r}}\cdot \dot{\mathit{r}}={\mathit{J}}_{\mathit{r}}\cdot {\left[\cos \phi \cdot v_r,\sin \phi \cdot v_r,0,0,0,w\right]}^T$$

(17)

where $\dot{\mathit{f}}$ is the velocity of the 2D image feature point change, and $v_r,w$ are the linear and angular velocity control quantities of the mobile robot, respectively.

3. Controller Design

Figure 3 is the control block diagram of the IBVS-PF-HMPC system. $X^{*},Y^{*},{\phi }^{*}$ are the desired positions of the mobile robot, $X,Y,\phi$ are the predicted positions correction of the mobile robot, $x_m{}^{*},y_m{}^{*}$ are the desired coordinate inputs of the image feature points, $x_m,y_m$ are the predicted correction outputs of the image coordinate points, ${\mathit{e}}_r$ is the state deviation input, ${\mathit{e}}_c$ is the predicted deviation, ${\mathit{y}}_s$ is the actual value output, ${\mathit{y}}_c$ is the model prediction output, and $v_r,w$ are the control quantities.

3.1. Design of the Hybrid Model Predictive Control Based on IBVS

Model predictive control (MPC) is an iterative optimization technique. The optimal control volume prediction sequence is obtained by substituting the current state measured or estimated at each sampling time into the optimization function. And the first of these control quantities is taken as the control input at the current moment.

The general form of the nonlinear system can be abstracted as

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

(18)

where $\mathit{x}$ is the state, $\dot{\mathit{x}}$ is the rate of the state change, $\mathit{u}$ is the control input.

Let the current moment be $k$, and the control period be $T$. (18) is discretized by Euler’s method:

$$\begin{array}{c}x\left(k+1|k\right)=\mathit{x}\left(k|k\right)+T\cdot \dot{\mathit{x}}\left(k|k\right)\\ =\mathit{x}\left(k|k\right)+T\cdot f\left(\mathit{x}\left(k|k\right),\mathit{u}\left(k|k\right)\right)\end{array}$$

(19)

in which,

$$\begin{array}{l}\mathit{x}\left(k|k\right),k=1,2,\dots ,N_p\\ \mathit{u}\left(k|k\right),k=1,2,\dots ,N_c\end{array}$$

(20)

where $N_p$ and $N_c$ represent the prediction range $N_p\ge 1$ and the control range $0\le N_c\le N_p$. The state quantities in the prediction time domain are

$$\left\{\begin{array}{c}\mathit{x}\left(k+1|k\right)=\mathit{x}\left(k|k\right)+Tf\left(\mathit{x}\left(k|k\right),\mathit{u}\left(k+1|k\right)\right)\\ ⋮\\ \mathit{x}\left(k+N_c+1|k\right)=\mathit{x}\left(k+N_c|k\right)+Tf\left(\mathit{x}\left(k+N_c|k\right),\mathit{u}\left(k+N_c|k\right)\right)\\ ⋮\\ \mathit{x}\left(k+N_p|k\right)=\mathit{x}\left(k+N_p-1|k\right)+Tf\left(\mathit{x}\left(k+N_p-1|k\right),\mathit{u}\left(k+N_c|k\right)\right)\end{array}\right.$$

(21)

Define the state quantity deviation as $\mathit{e}\left(k|k\right)=\mathit{x}\left(k|k\right)-\mathit{x}{\left(k|k\right)}^{*}$, in which, $\mathit{x}\left(k|k\right)$ is the current state, $\mathit{x}{\left(k|k\right)}^{*}$ is the desired state. Then, the optimization function of MPC is defined as follows:

$$J\left(\mathit{e},\mathit{u}\right)={\displaystyle \sum _{i=1}^{N_p}{‖\mathit{e}\left(k+i|k\right)‖}_{\mathit{Q}}^2}+{\displaystyle \sum _{i=0}^{N_c-1}{‖\mathit{u}\left(k+i|k\right)‖}_{\mathit{R}}^2}$$

(22)

where $\mathit{e}\left(k+i|k\right)$ represents the state deviation of $k+i$, which is predicted at the time of $k$, $\mathit{u}\left(k+i|k\right)$ represents the input deviation of $k+i$, which is predicted at the time of, and $\mathit{Q}$ and $\mathit{R}$ are the weighting matrices of the deviation of the state quantity and the input quantity, respectively.

Rewrite both the state and input quantities as increments:

$$\begin{array}{l}\mathit{E}\left(k+i|k\right)=\mathit{e}\left(k+i|k\right)-\mathit{e}\left(k+i-1|k\right)\\ \mathit{U}\left(k+i|k\right)=\mathit{u}\left(k+i|k\right)-\mathit{u}\left(k+i-1|k\right)\end{array}$$

(23)

Combining (8) and (17), define the mobile robot’s position as ${\mathit{x}}_1={\left[X,Y,\phi \right]}^T$, the image feature point coordinate as ${\mathit{x}}_2={\left[{\mathit{x}}_m,y_m\right]}^T$, and the control quantity of the mobile robot as $\mathit{u}={\left[v_r,w\right]}^T$. The deviation in (22) can be divided into two parts: the deviation of the position and the deviation of the feature points.

$$\begin{array}{l}{\mathit{e}}_1\left(k|k\right)={\mathit{x}}_1\left(k|k\right)-{\mathit{x}}_1{\left(k|k\right)}^{*}\\ {\mathit{e}}_2\left(k|k\right)={\mathit{x}}_2\left(k|k\right)-{\mathit{x}}_2{\left(k|k\right)}^{*}\end{array}$$

(24)

Then, the hybrid model predictive control based on IBVS (IBVS-HMPC) can be obtained:

$$\min J\left({\mathit{E}}_1,{\mathit{E}}_2,\mathit{U}\right)={\displaystyle \sum _{i=1}^{N_p}{‖{\mathit{E}}_1\left(k+i|k\right)‖}_{Q_1}^2}+{\displaystyle \sum _{i=1}^{N_p}{‖{\mathit{E}}_2\left(k+i|k\right)‖}_{Q_2}^2}+{\displaystyle \sum _{i=0}^{N_c-1}{‖\mathit{U}\left(k+i|k\right)‖}_R^2}$$

(25)

$$s.t.\left\{\begin{array}{l}{\mathit{u}}_{\min }\le \mathit{u}\left(k\right)\le {\mathit{u}}_{\max }\\ {\mathit{U}}_{\min }\le \mathit{U}\left(k\right)\le {\mathit{U}}_{\max }\end{array}\right.$$

(26)

where ${\mathit{u}}_{\min },{\mathit{u}}_{\max },{\mathit{U}}_{\min }{\mathit{U}}_{\max }$ are the minimum and maximum values of control quantity and control increment, respectively.

3.2. Design of the IBVS-PF-HMPC

The emergency braking performance of the mobile robot needs to be guaranteed when the image feature points are massively obscured by obstacles in dynamic scenes. So it is necessary to introduce the penalty factor into the above IBVS-HMPC.

The number of feature points in the image is $N$, and the $i_{\mathrm{th}}$ feature points ${\zeta }_i$. The number of obstacle-obscuring feature points is

$$sum={\displaystyle \sum _{i=0}^N{\zeta }_i}$$

(27)

Introducing the penalty factor $C$, the penalty term $C{\displaystyle \sum _{i=0}^N{\zeta }_i}$ of the optimization function with respect to the loss of feature points is obtained from $C=\frac{{\displaystyle \sum _{i=0}^N{\zeta }_i}}{N}$. Then, (25) can be rewritten as

$$\begin{array}{ll}\min J\left({\mathit{E}}_1,{\mathit{E}}_2,\mathit{U}\right)& ={\displaystyle \sum _{i=1}^{N_p}{‖{\mathit{E}}_1\left(k+i|k\right)‖}_{{\mathit{Q}}_{\mathbf{1}}}^2}+{\displaystyle \sum _{i=1}^{N_p}{‖{\mathit{E}}_2\left(k+i|k\right)‖}_{{\mathit{Q}}_2}^2}\\ & +{\displaystyle \sum _{i=0}^{N_c-1}{‖\mathit{U}\left(k+i|k\right)‖}_{\mathit{R}}^2}+C{\displaystyle \sum _{i=0}^N{\zeta }_i}\end{array}$$

(28)

$$s.t.\left\{\begin{array}{l}{\mathit{u}}_{\min }\le \mathit{u}\left(k\right)\le {\mathit{u}}_{\max }\\ {\mathit{U}}_{\min }\le \mathit{U}\left(k\right)\le {\mathit{U}}_{\max }\end{array}\right.$$

(29)

The sequence of control increment predictions for the current moment can be obtained:

$${\mathit{U}}^{*}={\left[\mathit{U}\left(k|k\right),\mathit{U}\left(k+1|k\right),\dots ,\mathit{U}\left(k+N_c|k\right)\right]}^T$$

(30)

The first element is taken as the actual control increment acting on the controlled system at the current moment:

$$\mathit{u}(k|k)=\mathit{u}(k-1|k)+\mathit{U}(k|k)$$

(31)

The predicted state quantity is obtained by modifying the actual state quantity and transmitted to the next control cycle for optimization and a solution. Repeat the above process to complete rolling optimization within the control time domain and the automatic parking trajectory tracking control for mobile robots is realized.

When the feature points are massively lost, $sum={\displaystyle \sum _{i=0}^N{\zeta }_i}$ is large, and the optimization function (28) has no solution. Thus, the control input is 0, and the emergency braking of the mobile robot in dynamic obstacle scenarios could be realized.

The steps of the proposed control methodology of IBVS-PF-HMPC are shown in

Table 1. The steps of the proposed control methodology of IBVS-PF-HMPC.

Steps	Description
1	Defining an automatic parking trajectories based on mobile robot position and parking space coordinate information;
2	Capturing the vehicle position image feature points of the camera and calculating the position deviation and image feature point deviation;
3	Reconstructing the incremental position deviation and image feature point deviation and substituting them into the IBVS-PF-HMPC;
4	Obtaining the control increment prediction sequence at the current moment from IBVS-PF-HMPC;
5	Taking the first element of the control increment predictive sequence as the actual control increment at the current moment, and obtaining the model prediction outputs of the state variables from the mobile robot kinematics predictive model (8) and the IBVS predictive model (17);
6	Modifying model predicted outputs by actual state variables;
7	Substituting the modified model prediction outputs of the state variables into the IBVS-PF-HMPC, and repeating Steps 3–7.

.

4. Simulation Results

In order to verify the effectiveness of the proposed control method, four groups of comparison experiments based on MATLAB are conducted. According to the motion characteristics of the mobile robot, the system limitations during simulation tests are as follows: the line velocity limitation of the mobile robot is $\left[-1 \mathrm{m}/\mathrm{s},1 \mathrm{m}/\mathrm{s}\right]$; the angular velocity limitation of the mobile robot is $\left[-0.2 \mathrm{rad}/\mathrm{s},0.2 \mathrm{rad}/\mathrm{s}\right]$; the linear velocity increment limitation of the mobile robot is $\left[-0.1 \mathrm{m}/\mathrm{s},0.1 \mathrm{m}/\mathrm{s}\right]$; and the angular velocity increment limitation of the mobile robot is $\left[-0.02 \mathrm{rad}/\mathrm{s},0.02 \mathrm{rad}/\mathrm{s}\right]$. The physical parameters of the mobile robot and the parameters of the IBVS-PF-HMPC when it reaches steady-state are given in

Table 2. The parameters of the mobile robot and IBVS-PF-HMPC.

Parameters	Value	Parameters	Value
Wheelbase $L/\mathrm{m}$	1	Feature point error increment penalty weight matrix ${\mathit{Q}}_{\mathbf{2}}$	$\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$
Control period $T/\mathrm{ms}$	50	Control incremental error weight matrix $\mathit{R}$	$\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$
Predicted step-size $N_p$	20	Image feature points $N$	20
Control step-size $N_c$	20	Control incremental constraint $\left[{\mathit{U}}_{\min },{\mathit{U}}_{\max }\right]$	$\left[\begin{array}{cc}-0.1& 0.1\\ -0.02& 0.02\end{array}\right]$
Position error increment penalty weight matrix ${\mathit{Q}}_1$	$\left[\begin{array}{ccc}10& 0& 0\\ 0& 10& 0\\ 0& 0& 50\end{array}\right]$	Control constraint $\left[{\mathit{u}}_{\min },{\mathit{u}}_{\max }\right]$	$\left[\begin{array}{cc}-1& 1\\ -0.2& 0.2\end{array}\right]$

. The control quantity predictive sequence is used as an optimization variable in the optimal control process of the automatic parking for mobile robots. The optimization criterion is minimizing the respective sums of the position deviation increment, the image feature point deviation increment and the control quantity predictive sequence. Then, the controller could take into account the state quantity output and control quantity input to ensure the good trajectory tracking control of the constrained mobile robot in the automatic parking process.

4.1. Parking Trajectory Planning

Both the arc-tangent function curve and the higher polynomial curve fit better with the automatic parking trajectory. Considering that the higher polynomial with more than four times is more complicated in the calculation, therefore, the arc-tangent function is selected design the reference trajectory in Figure 4, and the defined parallel parking trajectory model is as follows:

$$y=a\cdot \arctan \left(b\cdot x+c\right)+d$$

(32)

where $a=-1.024,b=1.143,c=-2.618,d=-1.227$.

4.2. Stability Performance

In the first simulation scenario, the proposed hybrid model predictive control with a penalty factor based on IBVS (IBVS-PF-HMPC) is compared with the traditional IBVS control (T-IBVSC), to verify the tracking performance under the image feature point loss scenario. Figure 5 and Figure 6 exhibit the trajectory tracking curves of the mobile robot under the control of T-IBVSC and IBVS-PF-HMPC, respectively.

As shown in Figure 5 and Figure 6, T-IBVSC and IBVS-PF-HMPC could maintain good trajectory tracking control until 2.5 m in the X axis. But the T-IBVSC fails to complete trajectory tracking control at 2.5 m due to the sudden increase in the control quantity $w$, which causes the image feature points to be lost in the field of view and is out of control in the following time.

4.3. Tracking Accuracy Performance

In the second simulation scenario, the proposed IBVS-PF-HMPC is compared with the model predictive control based on IBVS (IBVS-MPC), to verify the tracking performance under the image feature point loss scenario. Although the IBVS-MPC has a constraint limit on the image feature points, it only considers the image feature deviation without considering the position deviation of the mobile robot at the same time. Then, the image feature points are lost intermittently by adding interference. Figure 7 is the trajectory tracking curves of the mobile robot under the control of IBVS-MPC and IBVS-PF-HMPC. Figure 8 and Figure 9 exhibit the control quantity and the position deviation of the mobile robot under the control of IBVS-MPC and IBVS-PF-HMPC, respectively. And the root means square errors (RMSE) of the deviation of each component of the mobile robot position under the control of IBVS-MPC and IBVS-PF-HMPC are shown in

Table 3. The RMSE of the deviation of each component of the mobile robot position under the control of IBVS-MPC and IBVS-PF-HMPC.

	IBVS-PF-HMPC	IBVS-MPC
$\mathrm{RMSE}\left(\mathit{e}\left(x\right)\right)$	$1.26\times {10}^{-2}$	$2.94\times {10}^{-2}$
$\mathrm{RMSE}\left(\mathit{e}\left(y\right)\right)$	$3.31\times {10}^{-2}$	$9.84\times {10}^{-2}$
$\mathrm{RMSE}\left(\mathit{e}\left(\phi \right)\right)$	$2.47\times {10}^{-2}$	$4.98\times {10}^{-2}$

.

As shown in Figure 7, the maximum tracking error of IBVS-PF-HMPC is $2.3\times {10}^{-2}$ m, and the maximum tracking error of IBVS-MPC is $3.9\times {10}^{-2}$ m. The proposed IBVS-PF-HMPC could achieve higher trajectory tracking accuracy than the IBVS-MPC. This is due to the fact that the proposed controller considers both image feature point deviation and mobile robot position deviation. When the image feature points are intermittently lost due to external perturbations, the position deviation still acts on the mobile robot controller.

As shown in Figure 8 and Figure 9, the average values of each position deviation of the mobile robot are $1.04\times {10}^{-2}$ m, $2.41\times {10}^{-2}$ m and $2.15\times {10}^{-2}$ rad under the control of IBVS-PF-HMPC, and the average values of each position deviation of the mobile robot are $1.62\times {10}^{-2}$ m, $6.85\times {10}^{-2}$ m and $3.15\times {10}^{-2}$ rad under the control of IBVS-MPC. It shows that the position tracking accuracy of IBVS-PF-HMPC is 35.80%, 64.82% and 31.75% higher than that of IBVS-MPC, respectively. This is due to the loss of image feature points, as the control quantity of IBVS-MPC jittered more significantly and reached the limit constraint several times.

As shown in Table 3, the RMSE of the deviation of each component of the mobile robot position under the IBVS-PF-HMPC is smaller than the RMSE of IBVS-MPC. Therefore, the proposed control algorithm owns better trajectory tracking performance in the case of image feature point loss.

4.4. Real-Time Performance

In the third simulation scenario, the proposed IBVS-PF-HMPC is compared with the IBVS-PF-HMPC without increment (NI-IBVS-PF-HMPC), to verify the real-time and tracking accuracy of the mobile robot. Figure 10 is the trajectory tracking curves of the mobile robot under the control of NI-IBVS-PF-HMPC and IBVS-PF-HMPC. Figure 11 and Figure 12 exhibit the control quantity and the position deviation of the mobile robot under the control of NI-IBVS-PF-HMPC and IBVS-PF-HMPC, respectively. And the RMSE of the deviation of each component of the mobile robot position under the control of NI-IBVS-PF-HMPC and IBVS-PF-HMPC are shown in

Table 4. The RMSE of the deviation of each component of the mobile robot position under the control of NI-IBVS-PF-HMPC and IBVS-PF-HMPC.

	IBVS-PF-HMPC	NI-IBVS-PF-HMPC
$\mathrm{RMSE}\left(e\left(x\right)\right)$	$1.26\times {10}^{-2}$	$3.77\times {10}^{-2}$
$\mathrm{RMSE}\left(e\left(y\right)\right)$	$3.31\times {10}^{-2}$	$11.30\times {10}^{-2}$
$\mathrm{RMSE}\left(e\left(\phi \right)\right)$	$2.47\times {10}^{-2}$	$10.56\times {10}^{-2}$

.

As shown in Figure 10, the maximum tracking error of IBVS-PF-HMPC is $2.3\times {10}^{-2}$m, and the maximum tracking error of NI-IBVS-PF-HMPC is $9.5\times {10}^{-2}$m. The simulation results show that the proposed control method owns better tracking accuracy.

As shown in Figure 11 and Figure 12, the average values of each position deviation of the mobile robot are $1.04\times {10}^{-2}$ m, $2.41\times {10}^{-2}$ m and $2.15\times {10}^{-2}$ rad under the control of BVS-PF-HMPC, and the average values of each position deviation of the mobile robot are $2.91\times {10}^{-2}$ m, $9.22\times {10}^{-2}$ m and $8.62\times {10}^{-2}$ rad under the control of IBVS-PF-HMPC. It shows that the position tracking accuracy of IBVS-PF-HMPC is 64.26%, 73.86% and 75.06% higher than that of IBVS-MPC, respectively. Meanwhile, it can be seen that the angular velocity $w$ of the mobile robot has a significant lag, which leads to poor real-time when the state and control quantities are not reconfigured. And the position deviation of NI-IBVS-PF-HMPC is significantly larger than the position deviation of IBVS-PF-HMPC.

As shown in Table 4, the RMSE of the deviation of each component of the mobile robot position under the IBVS-PF-HMPC is smaller than the RMSE of NI-IBVS-PF-HMPC. Therefore, the proposed control algorithm owns better real-time performance and tracking accuracy.

4.5. Emergency Braking Performance

In the fourth simulation scenario, the emergency braking performance of the proposed IBVS-PF-HMPC is verified, by adding more than 10 image feature points that are obscured by dynamic obstacles during the automatic parking of the mobile robot. Figure 13 is the trajectory tracking curve of the mobile robot when the image feature points are massively occluded by obstacles. Figure 14 and Figure 15 are the control quantity and the position deviation when the mobile robot is obscured by the dynamic obstacle, respectively.

As shown in Figure 13, when the image feature points are massively occluded by obstacles, the mobile robot control system no longer tracks the desired trajectory. It can be seen from Figure 14 and Figure 15 that the control quantities tend to 0 when $sum={\displaystyle \sum _{i=0}^N{\zeta }_i}=10,C=\frac{{\displaystyle \sum _{i=0}^N{\zeta }_i}}{N}=0.5$. Consequently, the emergency braking of the mobile robot in the dynamic obstacle scene could be realized in time.

5. Conclusions

For the constrained mobile robot automatic parking system, the IBVS-PF-HMPC is proposed, to address the problem of feature point loss and emergency braking in dynamic obstacle scenarios caused by excessive target bias gain when using traditional IBVS control methods. The main conclusions are as follows:

(1)

The IBVS-based incremental model predictive control algorithm is designed. The traditional IBVS control is transformed into an optimization problem with constraints in the finite time domain, by defining the optimization function based on the mobile robot’s positional deviation and image feature point deviation, while using the actuator saturation and speed limit as constraints. Then, the accuracy and real-time of the mobile robot tracking control during automatic parking is improved simultaneously.

(2)

The convex optimization function with penalty factor is defined. Then, the IBVS-PF-HMPC is proposed, to guarantee the emergency braking performance of the mobile robot automatic parking when the image feature points are massively obstructed by obstacles in dynamic scenes.

(3)

Several simulation comparisons further verify the correctness and effectiveness of the proposed IBVS-PF-HMPC.