An Experimental Study of Stabilizing Receding
Horizon Control of Visual Feedback Systemwith Planar Manipulators
Masayuki Fujita, Toshiyuki Murao, Yasunori Kawai and Yujiro Nakaso
Department of Mechanical and Control Engineering
Tokyo Institute of Technology
2-12-1 S5-26 O-okayama Meguro-ku, Tokyo 152-8552, Japanfujita@ctrl.titech.ac.jp

This paper investigates vision based robot control based on a receding horizon control strategy. The stability of the receding horizon control scheme isguaranteed by using the terminal cost derived from an energy function of
the visual feedback system. By applying the proposed control scheme to a
two-link direct drive manipulator with a CCD camera, it is shown that thestabilizing receding horizon control nicely works for a planar visual feedback
system. Furthermore, actual nonlinear experimental results are assessed withrespect to the stability and the performance.

1 Introduction
Robotics and intelligent machines need sensory information to behave autonomously in dynamical environments. Visual information is particularly
suited to recognize unknown surroundings. In this sense, vision is one of thehighest sensing modalities that currently exist. Vision based control of robotic
systems involves the fusion of robot kinematics, dynamics, and computer vision to control the motion of the robot in an eﬃcient manner. The combinationof mechanical control with visual information, so-called visual feedback control or visual servoing, is important when we consider a mechanical system
working in dynamical environments [1].
In previous works, Kelly [2] considered the set-point problem with a static
target for a dynamic visual feedback system that includes the manipulator
dynamics which is not be negligible for high speed tasks. The authors discussed passivity based control of the eye-in-hand system [3, 4]. However, thecontrol law proposed in [3] is not based on optimization, the desired control
performance cannot be guaranteed explicitly.
Receding horizon control, also recognized as model predictive control is awell-known control strategy in which the current control action is computed

2

Masayuki Fujita, Toshiyuki Murao, Yasunori Kawai and Yujiro Nakaso
Yw

CameraXw

Ywr
Manipulator 2

Sw

Manipulator

l
Zc

q2

r1

TargetObjectXc

Camera
Yc
Sc

Zw

q1

l 2 Yil1

Image Plane
Xi
Si

Xw

f
TargetObjectFig. 1. Planar visual feedback system

Zw

Fig. 2. Schematic diagram

by solving, a ﬁnite horizon optimal control problem on-line [5]. A large number of industrial applications using model predictive control can be found inchemical industries where the processes have relatively slow dynamics. On
the contrary, for nonlinear and relatively fast systems such as in robotics, few
implementations of the receding horizon control have been reported. For thereceding horizon control, many researchers have tackled the problem of stability guarantees. An approach proposed by Parisini et al. [6] is based on using aquadratic endpoint penalty of the form axT (t + T )P x(t + T ) for some a > 0,some positive deﬁnite matrix P and a terminal state x(t+ T ). Jadbabaie et al.
[7] showed that closed-loop stability is ensured through the use of a terminal
cost consisting of a control Lyapunov function. Moreover, these results wereapplied to the Caltech Ducted Fan to perform aggressive maneuvers [8, 9].
Visual feedback, however, is not considered here. Predictive control could beof signiﬁcant beneﬁt when used in conjunction with visual servoing. With theincorporation of visual information, the system could anticipate the target’s
future position and be waiting there to intercept it [10].
In this paper, stabilizing receding horizon control is applied to the planar
visual feedback system in [3], a highly nonlinear and relatively fast system.
This represents a ﬁrst step towards high performance visual servoing targeting
more aggressive maneuvers. The main idea is the use of the terminal costderived from an energy function of the visual feedback system. By applying
the proposed control scheme to a two-link direct drive manipulator with a
CCD camera, it is shown that the stabilizing receding horizon control nicely
works for the planar visual feedback system. Furthermore, the experimental
results are assessed with respect to performance.
First, passivity-based control of a planar visual feedback system is reviewed. Next, a stabilizing receding horizon control for a planar visual feedback system using a control Lyapunov function is proposed. Then, the control
performance of the stabilizing receding horizon control scheme is evaluatedAn Stabilizing Receding Horizon Control of Visual Feedback System

3

through experiments with a two-link direct drive manipulator with a camera
as shown in Fig. 1.

2 Visual Feedback System with Planar Manipulator
The dynamics of n-link rigid robot manipulators can be written as
M (q)q̈ + C(q, q̇)q̇ + g(q) = τ(1)where q, q̇ and q̈ are the joint angle, velocity and acceleration, respectively, τ
is the vector of the input torque [11]. We assume that the robot evolves in aplane of n = 2, referring to Figs. 1 and 2.
The objective of visual feedback control is to bring the camera which ismounted on the end-eﬀector of the manipulator to the position of the target
object, i.e., to bring a image feature parameter vector f = [fx fy ]T to theorigin. The image feature parameter vector f is obtained from a perspective
transformation. Although details are omitted for lack of space, the planar
visual feedback system is given as follows [3, 12].

 
  
−M (q)−1 C(q, q̇)ξ + wf M (q)−1 JpT Rwc f
0
M (q)−1
ξ̇
u (2)
+
=
T
T
T
0 − zsλRwc
JpṘwc fRwc
Jp ξ − Rwc
− zsλ
f˙
wo
wo
where u := [uTξ uTd ]T is the control input, ξ := q̇ − ud is the error vector withrespect to the joint velocity, the scalar wf > 0 is a weight for the input torque,Rwc is a rotation matrix and Jp is the manipulator Jacobian. A scalar s > 0
is the scale factor in pixel/m, λ is the focal length of the camera and zwo is aconstant depth parameter. We deﬁne the state of the visual feedback system
as x := [ξ T f T ]T . The purpose of this paper is to control this planar visualfeedback system (2) by using stabilizing receding horizon control.
In previous work [3], the passivity of the visual feedback system (2) isderived by using the following energy function V (x)V (x) =

wf zwo T
1 T
ξ M (q)ξ +
f f.
2
2sλ

(3)

Here, we consider the following control input




I
0
Kξ 0
u = −Kν := uk , K :=, ν := N x :=x, (4)
0 −wf JpT Rwc
0 Kd
where Kξ := diag{kξ1 , kξ2 } ∈ R2×2 and Kd := diag{kd1 , kd2 } ∈ R2×2 arepositive gain matrices. Diﬀerentiating V (x) along the trajectory of the system
and using the control input uk , the next equation is derived.
V̇ = ν T u = −xT N T KN x.

(5)

Therefore, the equilibrium point x = 0 for the closed-loop system (2) and (4)is asymptotic stable, i.e., uk is a stabilizing control law for the system.

4

Masayuki Fujita, Toshiyuki Murao, Yasunori Kawai and Yujiro Nakaso

3 Stabilizing Receding Horizon Control
In this section, the ﬁnite horizon optimal control problem for the visual feedback system (2) is considered. Receding horizon schemes are often based on
the following cost function.
 t+T
l(x(τ ), u(τ ))dτ + F (x(t + T )),J(u, t) =tTl(x(t), u(t)) = x (t)Q(t)x(t) + uT (t)R(t)u(t),F (x(t + T ))≥0 (6)Q(t)≥0, R(t) > 0.

(7)

The resulting open loop optimal control input u∗ is implemented until a newstate update occurs, usually at pre-speciﬁed sampling intervals. Repeatingthese calculations yields a feedback control law.
The following lemma concerning a control Lyapunov function is importantto prove a stabilizing receding horizon control. The deﬁnition for a control
Lyapunov function M (x) is given by(8)
inf Ṁ (x) + l(x, u) ≤ 0,u

where l(x, u) is a positive deﬁnite function [7].
Lemma 1. Suppose that the following matrix P is positive semi definite.
P := ρN T KN − Q − N T K T RKN,

ρ > 0.

(9)Then, the energy function ρV (x) of the visual feedback system (2) can be
regarded as a control Lyapunov function.
The proof is straightforward using a positive deﬁnite function l(x(t), u(t))(7) and the stabilizing control law uk (4) for the system. Suppose that theterminal cost is the control Lyapunov function ρV (x), the following theorem
concerning the stability of the receding horizon control holds.
Theorem 1. Consider the following cost function for the visual feedback system (2).
 t+T
J(u, t) =l(x(τ ), u(τ ))dτ + F (x(t + T ))tTl(x(t), u(t)) = x (t)Q(t)x(t) + uT (t)R(t)u(t),F (x) = ρV (x),

ρ > 0.

(10)

Q(t)≥0, R(t) > 0 (11)(12)

Suppose that P (9) is positive semi definite, then the receding horizon control
for the visual feedback system is asymptotically stabilizing.
This theorem is proven by using a similar method as in [7], details are omitted due to lack of space. Theorem 1 guarantees the stability of the receding

An Stabilizing Receding Horizon Control of Visual Feedback System5horizon control using a control Lyapunov function for the planar visual feedback system (2) which is a highly nonlinear and relatively fast system. Since
the stabilizing receding horizon control design is based on optimal control
theory, the control performance should be improved compared to the simple
passivity-based control [3], under the condition of adequate gain assignment
in the cost function. In this paper, as a ﬁrst step, we propose unconstrained
stabilizing receding horizon control schemes. In the near future, we will consider constraints which represent one of the advantages of receding horizon
control, and develop it using level set, see [7].
Moreover, focused on the inverse optimality approach [12], the following
corollary is derived.
Corollary 1. Consider the following weights of the cost function (10)-(12).
Q(t) = qN T (t)KN (t),−1

R(t) = rK ,√
ρ = 2 qr.

r>0

q≥0

(13)
(14)
(15)

Then, the receding horizon control for the visual feedback system is asymptotically stabilizing, the receding horizon control law isqu∗ = −
KN x(16)
r
and the cost-to-go is given byJ ∗ = ρV (x).

(17)

If the weights of the terminal cost function are set to (13)-(15), then thecontroller that satisﬁes inf u [Ṁ (x) + l(x, u)] = 0 is analytically derived.
In the next section, the stabilizing receding horizon control is applied to aplanar visual feedback system. It is expected that the control performance isimproved using the receding horizon control.

4 Experimental Results
In this section, the proposed stabilizing receding horizon control is tested onan actual planar visual feedback system which is an image based direct visual
servo system. The manipulator used in the experiments (see Fig. 1), is controlled by a digital signal processor (DSP) from dSPACE Inc., which utilizes
a powerPC 750 running at 480 MHz. Control programs are written in MATLAB and SIMULINK, and implemented on the DSP using the Real-TimeWorkshop and dSPACE Software which includes ControlDesk and Real-TimeInterface. A XC-HR57 camera is attached to the tip of the manipulator. The
video signals are acquired by a frame graver board PicPort-Stereo-H4D and6

Masayuki Fujita, Toshiyuki Murao, Yasunori Kawai and Yujiro Nakaso

the image processing software HALCON. The sampling time of the controller
and the frame rate provided by the camera are 16.7 [ms] and 60 [fps], respectively. To solve the real time optimization problem, the software C/GMRES[13] is utilized. The target object is projected on the liquid crystal monitor.
The control objective is to bring the image feature parameter vector f to theorigin. The experiment is carried out with the initial condition q1 (0) = π/6[rad], q2 (0) = −π/6 [rad], q̇1 (0) = q̇2 (0) = 0 [rad/s], wf = 0.0001, zwo = 0.9[m], sλ = 1230 [pixel], f (0) = [−120 − 160]T [pixel] (1 [pixel] = 0.74 [mm]).
In this experiment, we compare the performance of the receding horizon control law proposed in Theorem 1 and the passivity based control
law uk (4). The weights of the cost function (10) were selected as Q =diag{65, 1.5, 10, 100} × 10−9 , R = diag{0.04, 1.7, 0.005, 0.00045} and ρ = 1
satisfy P ≥0. The controller parameters for the passivity based control law uk(4) were empirically selected as Kξ = diag{6.5, 0.15} and Kd = diag{50, 550}.
The control input with the receding horizon control is updated within every
16.7 [ms]. It must be calculated by the receding horizon controller within that
period. The horizon was selected as T = 0.02 [s].
The experimental results are presented in Fig. 3, showing the velocity
error ξ2 , the image feature parameter fy and the control inputs uξ2 and ud2 ,respectively. The rise time applying the receding horizon control is shorter
than that for the passivity based control. The controller predicts the movementof the target object using the visual information, as a result the manipulator
moves more aggressively. This validates one of the expected advantages ofthe stabilizing receding horizon control for the visual feedback system. From
Fig. 3, the asymptotic stability can be also conﬁrmed experimentally. The
steady state performance is also better than for the passivity based control.
Still, a non-vanishing steady state error is observed most probably due to theinﬂuence of the unmodeled manipulator dynamics(e.g. friction). This problem
will be investigated in the near future. We assume that an integrator in thecontrol will improve the steady state performance [14].
The performance for other parameter values T and ρ is compared in termsof the integral cost in Table 1. Since the cost of the stabilizing receding horizon
method is smaller than the passivity based control method under conditionsof the adequate cost function, it can be easily veriﬁed that the control performance is improved. With increasing weight of the terminal cost from ρ = 1
to ρ = 1.1 the cost increases, too. With higher terminal cost the state value
is reduced more strictly, using a large control input. In this experiment, since
the weights of the control input are larger than those of the state, the cost
increased consequently. As the horizon length increases from T = 0.02 to
T = 0.1, the cost is reduced. In the case of T = 0.5, the calculation can not
be completed within one sampling interval, due to limited computing power.

An Stabilizing Receding Horizon Control of Visual Feedback System

75 Conclusions
This paper proposes a stabilizing receding horizon control for a planar visualfeedback system, which is a highly nonlinear and relatively fast system. It isshown that the stability of the receding horizon control scheme is guaranteedby using the terminal cost derived from an energy function of the visual feedback system. Furthermore, it is veriﬁed that the stabilizing receding horizon
control nicely works for the planar visual feedback system through experiments with a nonlinear experimental system. In the experimental results, thecontrol performance of the stabilizing receding horizon control is improved
compared to that of the simple passivity based control. In this paper, thestabilizing receding controller was implemented for a low level inner loop, inthe near future, we would like to tackle the implementation on a high levelouter loop.

Acknowledgement
The authors would like to thank Mr. S. Mimoto, Mr. H. Matsuda and Mr. T.
Yamada, Tokyo Institute of Technology for their time and invaluable help.
Table 1. Values of the Integral Cost
Control Scheme
Passivity based Control
Receding Horizon Control (T = 0.02 [s], ρ = 1)
Receding Horizon Control (T = 0.02 [s], ρ = 1.05)
Receding Horizon Control (T = 0.02 [s], ρ = 1.1)
Receding Horizon Control (T = 0.05 [s], ρ = 1)
Receding Horizon Control (T = 0.1 [s], ρ = 1)

cost
106.1
61.8
108.9
209.2
56.3
55.1

References
1. S. Hutchinson, G.D. Hager and P.I. Corke (1996). A tutorial on visual servo
control. IEEE Trans. Robotics and Automation. 12(5):651–670.
2. R. Kelly (1996). Robust asymptotically stable visual servoing of planar robots.
IEEE Trans. Robotics and Automation. 12(5):759–766.
3. A. Maruyama and M. Fujita (1998). Robust control for planar manipulatorswith image feature parameter potential. Advanced Robotics. 12(1):67–80.
4. H. Kawai and M. Fujita (2004). Passivity-based dynamic visual feedback control for three dimensional target tracking: stability and L2 -gain performance
analysis. Proc. 2004 American Control Conference. 1522–1527.
5. D.Q. Mayne, J.B. Rawlings, C.V. Rao and P.O.M. Scokaert (2000). Constrainedmodel predictive control: stability and optimality. Automatica. 36(6):789–814.
6. T. Parisini and R. Zoppoli (1995). A receding-horizon regulator for nonlinear
systems and a neural approximation. Automatica. 31(10):1443–1451.
7. A. Jadbabaie, J. Yu and J. Hauser (2001). Unconstrained receding-horizon control of nonlinear systems. IEEE Trans. Automatic Control. 46(5):776–783.

Masayuki Fujita, Toshiyuki Murao, Yasunori Kawai and Yujiro Nakaso

ξ2 [rad/s]

8

6
4
2
0
−2
−4
0

0.5

1

1.5

2

2.5

3

0.5

1

1.5

2

2.5

3

0.5

1

1.5

2

2.5

3

0.5

1

1.5
time [s]

2

2.5

3

fy [pixel]

100
0

-100
-200

uξ2 [Nm]

0
1
0

−1

ud2 [m/s]

−2
0
5

0
−5
0

Fig. 3. Experimental comparison with diﬀerent control schemes(solid: receding horizon control (T = 0.02 [s], ρ = 1), dashed: passivity based control)

8. J. Yu, A. Jadbabaie, J. Primbs and Y. Huang (2001). Comparison of nonlinear
control design techniques on a model of the caltech ducted fan. Automatica.
37(12):1971–1978.
9. A. Jadbabaie and J. Hauser (2002). Control of a thrust-vectored ﬂying wing:a receding horizon – LPV approach. International Journal of Robust and Nonlinear Control. 12(9):869–896.
10. A.E. Hunt and A.C. Sanderson (1982). Vision-based predictive robotic tracking
of a moving target. Technical Report. Carnegie Mellon University.
11. M.W. Spong, S. Hutchinson and M. Vidyasagar (2006). Robot modeling andcontrol. John Wiley & Sons.
12. M. Fujita, A. Maruyama, M. Watanabe and H. Kawai (2000). Inverse optimal H∞ disturbance attenuation for planar manipulators with the eye-in-handsystem. Proc. 39th IEEE Conference on Decision and Control. 3945–3950.
13. T. Ohtsuka (2004). A continuation/GMRES method for fast computation of
nonlinear receding horizon control. Automatica. 40(4):563–574.
14. J.B. Rawlings (2000). Tutorial overview of model predictive control. IEEE Control Systems Magazine. 20(3):38–52.