Professional Documents
Culture Documents
IET Control Theory Appl - 20用于简化汽车的不变线性二次高斯控制器
IET Control Theory Appl - 20用于简化汽车的不变线性二次高斯控制器
Brief Paper
Abstract: This study proposes a modified non-linear model predictive control (NMPC) to deal with the point stabilisation for the
image-based visual servoing (IBVS) of a wheeled mobile robot by using the iterative linear quadratic Gaussian (iLQG) algorithm.
Firstly, an IBVS non-linear system is modelled by taking the image coordinates and angle into account simultaneously. By
setting a loss function, a model predictive control minimisation problem with an input constraint is formulated. Then, an iLQG-
based NMPC strategy is proposed to obtain the optimal solution where a dual-gradient descent is used to deal with the input
constraint and a specific Lorentzian ρ-function is added into the loss function to reduce the static error. Finally, numerical
simulations and several comparisons are demonstrated to show the effectiveness of the proposed algorithm.
IET Control Theory Appl., 2020, Vol. 14 Iss. 14, pp. 1989-1994 1989
© The Institution of Engineering and Technology 2020
ẋc − xżc velocity of the mobile robot in the x-axis, y-axis, z-axis,
ẋ ṗx zc respectively. Differentiating (3), one has
= = (2)
ẏ ṗ y ẏc − yżc
x˙c xc
żc → →
y˙c = − ω × yc − v (4)
where x and y are the image coordinates of x-axis and y-axis, z˙c zc
respectively. As can be seen in Fig. 1, without loss of generality,
we here assume that the movement of the robot is limited in the that is
X wOwZ w plane. Hence, the velocity vector of the robot in the robot
coordinate can be defined as ξrr = (0, 0, vr, 0, ωr, 0)T and in the ẋc = − vx − ωyzc + ωzyc
camera coordinate can be defined as ξcc = (0, 0, vc, 0, ωc, 0)T. ẏc = − vy − ωz xc + ωxzc (5)
According to the physical relationship between the angular żc = − vz − ωx yc + ωy xc
velocity and the linear velocity shown in Fig. 2, we have
→ →
Then, substituting (5) into (2), a non-linear IBVS model for the
→
v =ω×r (3) mobile robot can be derived as follows:
→ → → 1 x
where v = [vx, vy, vz]T, ω = [ωx, ωy, ωz]T and r denotes the radius − 0 xy − 1 + x2 y
of rotation. vx, vy, vz, ωx, ωy and ωz denote the components of ẋ zc zc
= ξcc (6)
ẏ 1 y 2
0 − 1+y −xy −x
zc zc
Since ξcc = (0, 0, vc, 0, ωc, 0)T, we can rewrite (6) in a more compact
manner:
x
−(1 + x2)
zc
L(x, y, zc) = ,
y
−xy
zc
system input
vc
u= .
ωc
With the aim of driving the robot to a unique position in the world
framework, the angle of the robot should also be considered.
Combining with the fact that θ̇ = ωc, as a result, the model can be
further derived as follows:
x
−(1 + x2)
zc
Lcob = y .
−xy
zc
0 1
By setting (x∗, y∗, θ∗) as the desired image coordinates and angle,
the error vector is defined as follows:
1990 IET Control Theory Appl., 2020, Vol. 14 Iss. 14, pp. 1989-1994
© The Institution of Engineering and Technology 2020
In order to handle the input constraint, the following modified
performance index is further derived by introducing the dual
variables λk and ηk:
t+N−1
min Jt = ∑ lk′(ek, uk, λk, ηk) + lN (eN ) (12)
k=t
where
where lk is the running cost and lN is the terminal cost. In general, and a value function
^ ^
the quadratic items l k = ekTQwek
+ ukTRwuk
and l N = eNT PweN
are V(et) = min Jt (16)
chosen to penalise the system, where Qw and Rw are weight U
lk = ekTW 1ek + ukTRwuk + W 2 log(ekTW 1ek + r) The key to success for the iLQG algorithm lies in the calculation of
the perturbation between tth trajectory and (t + δt)th trajectory.
lN = eNT W 1eN + W N log(eNT W 1eN + r) Hence, the following function is considered:
where the third item of lk (or the second item of lN ) is called the Q δet, δut = lk et + δet, ut + δut − lk et, ut
(18)
Lorentzian ρ-function. W 1 and W 2 are the corresponding positive +V f et + δet, ut + δut − V f et, ut
definite weight matrices which trade off the penalties [between
ekTW 1ek and log(eNT W 1eN + r)], and W N is the terminal weight According to the Taylor series expansion, we expand (18) as the
matrix. The relationship between the error state ek and the values of following second-order form:
this ρ-function is shown in Fig. 3. As we can see in Fig. 2, the
Lorentzian ρ-function is similar to the negative normal distribution, 1 T 0 QeTt QuTt 1
1
and the parameter r is to determine its minimum and its width of Q δet, δut ≃ δet Qet Qetet Qetut δet (19)
distribution. 2
δut Qut Qutet Qu u δut
t t
IET Control Theory Appl., 2020, Vol. 14 Iss. 14, pp. 1989-1994 1991
© The Institution of Engineering and Technology 2020
Substituting (20) into (19), we have
we have
Fig. 5 Comparison with different loss functions
Ot∗+ 1(ek∗) ≤ Ot∗(ek∗) − ∥ et∗ ∥2W1
(25) During the whole simulation, the state is defined as (x, y, θ).
+lN∗ + 1 + ∥ et∗+ N ∥W
2
1
+ ∥ ut∗+ N ∥2Rw The initial state is (0.5, 0.1, 0) and the initial velocity is (0, 0). The
target posture is (0, 0.35, π/4). The camera parameters are given as
By using the similar method in [28], a suitable terminal weigh p0x = 320, p0y = 240, f x = − 542.5, f y = − 542.5 and the camera
matrix for lN∗ + 1 can be designed to guarantee the following height yc = 1 m. The constraint of input is
inequality:
umin = − umax = [ − 1, − 1]T. The prediction step of MPC in each
iLQG step is N = 20 and the parameters in Algorithm 1 are set as
lN∗ + 1 + ∥ et∗+ N ∥W
2
+ ∥ ut∗+ N ∥2Rw ≤ 0 (26)
1 L1 = 2, L2 = 5. The parameters of the proposed loss functions are
given as W 1 = diag{1, 3, 0.5}, Rw = 0.05I, W 2 = 0.01, r = 0.001
As a result, the following inequality
and W N = 2.
As the parameters W 1, W 2 and W N can influence the amplitude
Ot∗+ 1(ek∗) ≤ Ot∗(ek∗) − ∥ et∗ ∥W
2
1
of oscillation and the control accuracy, we further give W 2 = 0.1 to
show the characteristic of the proposed algorithm. Meanwhile, the
holds for all t. Based on the above discussion, the result of
quadratic items with weight matrices Qw = 2I and Pw = I are given
asymptotical stability for the proposed iLQG-based NMPC method
can be stated as the following theorem. and the results are shown in Fig. 5, where blue dash-dot line, red
dotted line and green dashed line represent the results of W 2 = 0.1,
Theorem 1: If there exists the terminal weigh matrix that W 2 = 0.01 and the quadratic items, respectively. It can be seen that
satisfies inequality (26), system (9) along with performance index the trends are totally different at the end of the process. Fig. 5a
(10) will be asymptotically stable and Ot∗(ek∗) will be a Lyapunov shows the trajectories in the image plane. When the quadratic items
are used to perform this task, the control process is smooth.
function.
However, the static error cannot be dealt with under this situation.
The proposed loss functions can tackle the static error well
4 Simulation although the oscillation occurs when close to the desired position.
In this section, some simulations and comparisons are given to In Fig. 5b, the paths of the robot in the world plane are plotted and
substantiate the advantage of the proposed iLQG-based NMPC the quadratic items stay near (1.5, 1.6) which is far from the desired
algorithm. coordinates. When the proposed loss functions are given with
W 2 = 0.1, the robot can to some extent reduce the error in a way of
1992 IET Control Theory Appl., 2020, Vol. 14 Iss. 14, pp. 1989-1994
© The Institution of Engineering and Technology 2020
small prediction steps have short running times but poor control
accuracy. Also, if N becomes larger, the static error will remain
basically unchanged while the running time will increase rapidly.
As illustrated by the above comparisons of control accuracy,
computational burden under different performance indexes and
different prediction steps, the proposed iLQG-based NMPC is
capable of dealing with the non-linear stabilisation with suitable
prediction steps and parameters of the Lorentzian ρ-function.
Although the added Lorentzian ρ-function may cause oscillation
when the robot is close to the desired posture, it can enhance the
performance of the IBVS wheeled mobile robot system in terms of
avoiding the local optimality and reducing the static error
effectively.
5 Conclusion
In this paper, the NMPC controller was designed by using the
iLQG algorithm for the wheeled mobile robot IBVS system to
complete the point stabilisation task. Based on the camera
perspective projection, the error model of the system was
constructed as a non-linear form where the angle was also
considered to guide the robot to the unique posture under the world
Fig. 6 Comparison with different N layer. By defining an MPC performance index, the constrained
MPC minimisation was derived. In order to deal with the local
Table 1 Running times with different prediction steps optimality of the NMPC, an iLQG-based NMPC strategy with the
Prediction steps Running time, s e, mm, mm, deg specific Lorentzian ρ-function was proposed to obtain the optimal
solution. Finally, numerical simulations and comparisons with
0 0.058 (−0.12, − 0.1, − 15)
different prediction steps and different performance indices
10 0.401 (−0.05, 0.02, − 10) illustrated the advantage of the proposed algorithm.
20 0.809 (0.0, 0.0, − 0.302)
30 1.235 (0.0, 0.0, − 0.289) 6 Acknowledgments
The authors thank the anonymous reviewers for their valuable
suggestions to improve the quality of this PAPER. This work was
moving forward and backward. Eventually, red dotted line
supported partly by the National Natural Science Foundation of
(W 2 = 0.01) can reach the desired position. Fig. 5c demonstrates
China under Grants 61973275, the NSFC-Zhejiang Joint
the change of θ. We can see that the value of θ stops changing after
Foundation for the Integration of Industrialization and
approximately 40 steps, which can explain directly why the Informatization under Grant U1709213, the Key R&D Foundation
quadratic items cannot drive the robot to the desired posture. When of Zhejiang under Grant 2020C01109, the Talent Project of
the proposed loss functions are added, the value of θ begins to Zhejiang Association for Science and Technology under Grant
oscillate after approximately 50 steps. Although the blue one also 2018YCGC018.
cannot converge to the desired angle, this pair of loss functions
enhance the ability to reduce the static error. The suitable pair of
loss functions (shown in the Fig. 5c with red dotted line) can 7 References
complete the stabilisation task with the tolerant static error. The [1] Chaumette, F., Hutchinson, S.: ‘Visual servo control. I. basic approaches’,
above analysis proves the advantage of the proposed iLQG-based IEEE Robot. Autom. Mag., 2006, 13, (4), pp. 82–90
NMPC with the specific Lorentzian ρ-function. [2] Shu, T., Gharaaty, S., Xie, W., et al.: ‘Dynamic path tracking of industrial
robots with high accuracy using photogrammetry sensor’, IEEE/ASME Trans.
In order to show the necessity of the combination of the MPC Mechatronics, 2018, 23, (2), pp. 1159–1170
and the strong impact of the prediction step, we choose three [3] Gao, J., Proctor, A., Shi, Y., et al.: ‘Hierarchical model predictive image-
different cases as N = 0, N = 10 and N = 20, and the results are based visual servoing of underwater vehicles with adaptive neural network
shown in Fig. 6. Figs. 6a and b imply that the trend of the pure dynamic control’, IEEE Trans. Cybern., 2016, 26, (10), pp. 2323–2334
[4] Liang, X., Wang, H., Liu, Y., et al.: ‘Image-based position control of mobile
iLQG (i.e. N = 0) is not smooth and unable to reach the desired robots with a completely unknown fixed camera’, IEEE Trans. Autom.
position in 150 steps. Moreover, the angle even degenerates after Control, 2018, 63, (9), pp. 3016–3023
30 steps, falling down from 35° to 30°, which leads to the linear [5] Wu, J., Chen, X., Liu, A., et al.: ‘Predictive control for visual servoing control
of cyber physical systems with packet loss’, Peer-to-Peer Netw. Appl., 2019,
change both in the image layer and in the world layer. The 12, pp. 1774–1784
difference between N = 10 and N = 0 is that the robot can be [6] Liang, X., Wang, H., Chen, W., et al.: ‘Adaptive image-based trajectory
closer to the desired posture when N = 10. When N = 20, the tracking control of wheeled mobile robots with an uncalibrated fixed camera’,
robot approaches the desired posture after 50 steps. Then, it starts IEEE Trans. Control Syst. Technol., 2015, 23, (6), pp. 2266–2282
[7] He, Z., Wu, C., Zhang, S., et al.: ‘Moment-based 2 1/2D visual servoing for
to oscillate in order to reach the desired posture and finally texture-less planar part grasping’, IEEE Trans. Ind. Electron., 2019, 66, (10),
completes the task after 120 steps. Based on these three cases, we pp. 7821–7830
can conclude that it is necessary to combine the MPC into iLQG to [8] Chen, J., Jia, B., Zhang, K.: ‘Trifocal tensor-based adaptive visual trajectory
solve the IBVS stabilisation task for wheeled mobile robot tracking control of mobile robots’, IEEE Trans. Cybern., 2016, 47, (11), pp.
3784–3798
systems. [9] Yi, Y., Chen, D.: ‘Disturbance observer-based backstepping sliding mode
It is well known that MPC is a powerful control scheme which fault-tolerant control for the hydro-turbine governing system with dead-zone
increases the control accuracy by increasing the prediction steps in input’, ISA Trans., 2019, 88, pp. 127–141
a recursive manner, but the computational burden will become [10] Yi, Y., Chen, D., Xie, Q.: ‘Controllability of nonlinear fractional order
integrodifferential system with input delay’, Math. Methods Appl. Sci., 2019,
heavier. We further calculate the running time when 42, (11), pp. 3799–3817
N = 0, 10, 20, 30 in Table 1 by using 50-time Monte Carlo test, [11] Ke, F., Li, Z., Xiao, H., et al.: ‘Visual servoing of constrained mobile robots
which contains average one-step running times and average final based on model predictive control’, IEEE Trans. Syst. Man Cybern. Syst.,
errors after operating 150 loops. We can see that when the 2017, 47, (7), pp. 1428–1438
[12] Liu, A., Zhang, W., Chen, M., et al.: ‘Moving horizon estimation for mobile
prediction step increases from 0 to 20, the error deceases robots with multirate sampling’, IEEE Trans. Ind. Electron., 2017, 64, (2), pp.
immediately. However, when the prediction step increases from 20 1457–1467
to 30, the running time increases from 0.809 to 1.235 but the error
only decreases from −0.302 to −0.289. These data imply that
IET Control Theory Appl., 2020, Vol. 14 Iss. 14, pp. 1989-1994 1993
© The Institution of Engineering and Technology 2020
[13] Lee, S., Myung, H.: ‘Receding horizon particle swarm optimisation-based [21] Park, B., Oh, S., Lee, J.: ‘Stochastic iterative learning model predictive
formation control with collision avoidance for non-holonomic mobile robots’, control based on stochastic approximation’, IFAC-Papers On Line, 2019, 52,
IET Control Theory Applic., 2015, 9, (14), pp. 2075–2083 (1), pp. 604–609
[14] Yi, Y., Zhang, Z., Chen, D, et al.: ‘State feedback predictive control for [22] Qian, Y, Ushio, T.: ‘Receding horizon control with iLQG method considering
nonlinear hydro-turbine governing system’, J. Vib. Control, 2018, 24, (21), computational delay and its application to nonholonomic systems’. IEEE
pp. 4945–4959 Conf. on Control Technology and Applications, Kohala Coast, Hawaii, USA,
[15] Liu, A., Zhang, W., Yu, L., et al.: ‘Formation control of multiple mobile 2017, pp. 644–649
robots incorporating an extended state observer and distributed model [23] Ke, F., Li, Z., Yang, C.: ‘Robust tube-based predictive control for visual
predictive approach’, IEEE Trans. Syst. Man Cybern. Syst., 2018, doi servoing of constrained differential-drive mobile robots’, IEEE Trans. Ind.
10.1109/TSMC.2018.2855444 Electron., 2018, 65, (4), pp. 3437–3446
[16] He, D., Lu, L., Luo, R., et al.: ‘Energy-efficient cooperative predictive control [24] Eberhart, R., Shi, Y.: ‘Particle swarm optimization: developments,
for multi-agent non-linear systems with transmission delay’, IET Control applications and resources’. Proc. of Congress on Evolutionary Computation,
Theory Applic., 2019, 13, (17), pp. 2730–2737 Seoul, South Korea, 2001, pp. 81–86
[17] Shen, C., Shi, Y., Buckham, B.: ‘Trajectory tracking control of an autonomous [25] Pan, Y., Wang, J.: ‘Model predictive control of unknown nonlinear dynamical
underwater vehicle using Lyapunov-based model predictive control’, IEEE systems based on recurrent neural networks’, IEEE Trans. Ind. Electron.,
Trans. Ind. Electron., 2017, 65, (7), pp. 5796–5805 2012, 59, (8), pp. 3089–3101
[18] Yan, Z., Wang, J.: ‘Nonlinear model predictive control based on collective [26] Levine, S., Wagener, N., Abbeel, P.: ‘Learning contact-rich manipulation
neurodynamic optimization’, IEEE Trans. Neural Netw. Learning Syst., 2015, skills with guided policy search’. IEEE Int. Conf. on Robotics and
26, (4), pp. 840–850 Automation, Seattle, Washington, USA, 2015, pp. 156–163
[19] Todorov, E., Li, W.: ‘A generalized iterative LQG method for locally-optimal [27] Boyd, S., Vandenberghe, L.: ‘Convex optimization’ (Cambridge University
feedback control of constrained nonlinear stochastic systems’. Proc. of Press, UK, 2004)
American Control Conf., Portland, OR, USA, 2005, pp. 300–306 [28] Liu, A., Zhang, W., Yu, L.: ‘Robust predictive trajectory tracking control for
[20] Kaczynski, P., Socha, L.: ‘Iterative procedures in application of the LQG mobile robots with intermittent measurement and quantization’, IEEE Trans.
approach to control problems for polynomial stochastic systems’, IEEE Trans. Ind. Electron., 2019, doi: 10.1109/TIE.2019.2962424
Autom. Control, 2010, 55, (8),pp. 1875–1881
1994 IET Control Theory Appl., 2020, Vol. 14 Iss. 14, pp. 1989-1994
© The Institution of Engineering and Technology 2020