Visual Motion Observer-based Stabilizing Receding Horizon Control
via Image Space Navigation Function
Toshiyuki Murao, Hiroyuki Kawai and Masayuki Fujita
Abstract— This paper investigates stabilizing receding horizon control via an image space navigation function for athree-dimensional (3-D) visual feedback system. Firstly, a briefsummary of a visual motion observer is given. Next, a visualmotion error system is reconstructed in order to apply totime-varying desired motion. Then, visual motion observerbased stabilizing receding horizon control for the 3-D visualfeedback system, highly nonlinear and relatively fast system, is
proposed. Moreover, a path planner to be appropriate for thevisual motion error system is designed through the image space
navigation function to keep all features into a camera field ofview. Finally, simulation results are shown in order to confirm
the proposed method.

I. I NTRODUCTION
Visual feedback control is now a very flexible and useful
method in robot control [1]. Recently, Gans and Hutchinson [2] proposed hybrid switched-system control which
utilizes image-based and position-based visual feedback control. In [3], an occlusion problem for a hybrid eye-inhand/eye-to-hand multicamera system was tackled by using
an extended Kalman filter and a multiarm robotic cell.
Especially, there has been an increase of interest in problems that are all feature points remain within a camera
field of view since the work of Chaumette [4], while many
researchers have tackled various problems for visual feedback control. Cowan et al. [5] proposed a visual feedback
controller to bring a robot to rest at a desired configuration
for the field of view problem by using navigation functions,similar to artificial potential functions. Chen et al. [6]developed an off-line path planner based on an image space
navigation function with an adaptive 2 1/2-D visual servoing
controller. However, the desired control performance cannot
be guaranteed explicitly, since these control methods [5], [6]are not based on optimization.
On the other hand, receding horizon control, also recognized as model predictive control is a well-known control
strategy in which the current control action is computed bysolving a finite horizon optimal control problem on-line [7].
Sauvée et al. [8] proposed an image-based visual servoing
scheme based on nonlinear model predictive control for robot
systems. Allibert et al. [9] has been tested it on a planar
manipulator arm equipped with an omnidirectional camerain simulation. Although good receding horizon control approaches for the visual feedback system considering themechanical and visibility constraints are reported in thoseThis work was not supported by any organization
T. Murao is with Master Program of Innovation for Design and Engineering Advanced Institute of Industrial Technology, Tokyo 140-0011, Japanmurao-toshiyuki@aiit.ac.jp
H. Kawai is with Department of Robotics, Kanazawa Institute of Technology, Ishikawa 921-8501, Japan
M. Fujita is with Department of Mechanical and Control Engineering,Tokyo Institute of Technology, Tokyo 152-8550, Japan

Camera5c
gcoTargetObjectFig. 1.

gwc

5o

gwo

5w
World Frame

Eye-in-hand visual feedback systems.

papers, stability does not addressed. In [10], the authors
proposed stabilizing receding horizon control for an eyein/to-hand visual feedback system which includes both aneye-in-hand system and an eye-to-hand one as the special
cases. However, this position-based visual feedback control
method through nonlinear receding horizon approach canallow feature points to leave the field of view.
In this paper, stabilizing receding horizon control via
an image space navigation function is applied to a threedimensional (3-D) visual feedback system with an eye-inhand configuration as shown in Fig. 1, highly nonlinear
and relatively fast system. Firstly, an brief summary of avisual motion observer is given. Secondly, a visual motion
error system is reconstructed in order to handle time-varying
desired motion. Next, stabilizing receding horizon control
for the visual motion error system using a control Lyapunov
function is proposed. Then, a path planner to be appropriate
for the visual motion error system is designed throughan image space navigation function in order to keep allfeatures into a camera field of view. The path planning
based on an image space could be of significant benefit
when used in conjunction with the proposed position-based
receding horizon control by using an error defined on a
Cartesian space. Finally, control performance with visibility
maintenance of the proposed control scheme and the previous
one [10] is evaluated through simulation results.
II. V ISUAL M OTION O BSERVER
This section mainly reviews our previous work [11] via
passivity-based visual feedback control with the eye-in-handconfiguration.
A. Body Velocity of Relative Rigid Body Motion
Visual feedback systems with the eye-in-hand configuration use three coordinate frames which consist of a world
frame Σw , a camera frame Σc , and an object frame Σo as
in Fig. 1. Let pco ∈ R3 and eξ̂θco ∈ SO(3) be a position
vector and a rotation matrix from the camera frame Σc to theobject frame Σo . Then, the relative rigid body motion fromΣc to Σo can be represented by gco = (pco , eξ̂θco ) ∈ SE(3).
Similarly, gwc = (pwc , eξ̂θwc ) and gwo = (pwo , eξ̂θwo ) denoterigid body motions from the world frame Σw to the camera

frame Σc and from the world frame Σw to the object frame
Σo , respectively.
Position-based visual feedback control needs, in general,
to derive the relative rigid body motion from image data. The
relative rigid body motion from Σc to Σo can be led by using
the composition rule for rigid body transformations [12] as−1follows:
gwo .
(1)
gco = gwc
The relative rigid body motion involves the velocity of eachrigid body. To this aid, let us consider the velocity of a rigid
body. We define the body velocity of the camera relativeb
T
T T
to the world frame Σw as Vwc= [vwcωwc] , where vwc
and ωwc represent the velocity of the origin and the angularvelocity from Σw to Σc , respectively [12].
Differentiating Eq. (1) with respect to time, the body
velocity of the relative rigid body motion gco can be written
as follows (See [11]):
b
b
b
−1 V
= −Ad(gco(2)
Vco) wc + Vwo ,bis the body velocity of the target object relative
where Vwo
to Σw .

B. Image Features of Pinhole Camera ModelIn this visual feedback system, we use the pinhole camera
model with a perspective projection. Here, we consider m(≥4) feature points on the rigid target object in this paper.
Let λ be a focal length, poi ∈ R3 and pci ∈ R3
be the position vectors of the target object’s i-th featurepoint relative to Σo and Σc , respectively. We assume thatmultiple point features on a known object poi are given. The
perspective projection of the i-th feature point onto the image
plane gives us the image planecoordinate
fi := [fxi fyi ]T ∈

R2 as
λ
xci
fi =
,
(3)
zci yci
where pci = [xci yci zci ]T . It is straightforward to extend thismodel to m image points by simply stacking the vectors ofT T] ∈the image plane coordinate, i.e., f (gco ) := [f1T · · · fm
R2m . Hereafter, fab means f (gab ) for simplicity.
C. Estimation Error System and Visual Motion Observer
In this subsection, we consider a nonlinear observer inorder to estimate the relative rigid body motion gco from theimage feature fco . Firstly, using Eq. (2), we choose estimatesbof the relative rigid body motion and velocity,ḡco and V̄corespectively as
b
b
−1 V
V̄co
= −Ad(ḡco(4)) wc + ue .
The new input ue is to be determined in order to drive thebto their actual values.
estimated values ḡco and V̄co
In order to establish the estimation error system, we define
the estimation error between the estimated value ḡco and theactual relative rigid body motion gco as
−1
gco .
(5)
gee = ḡco
We next define the error vector of the rotation matrix eξ̂θab aseR (eξ̂θab ) := sk(eξ̂θab )∨ where sk(eξ̂θab ) denotes 12 (eξ̂θab −e−ξ̂θab ). Using this notation, the vector of the estimation
error is given by ee := [pTee eTR (eξ̂θee )]T .
Suppose the attitude estimation error θee is small enoughthat we can let eξ̂θee  I +sk(eξ̂θee ). Therefore, using a firstorder Taylor expansion approximation, the estimation error
vector ee can be obtained from the image feature fco andthe estimated value of the relative rigid body motion ḡco as
follows:(6)
ee = Je† (ḡco )(fco − f¯co ),where f¯co is an estimated value of the image feature andJe (ḡco ) is an image Jacobian-like matrix [11]. In the same
way as Eq. (2), the estimation error system can be represented bybb−1 ue + V
= −Ad(gee
(7)
Vee
wo .
)
This estimation error system is passive from the input ue tothe output −ee . Based on the passivity property, we consider
the stabilizing control law as follows:ue = −Ke (−ee ), Ke > 0.
(8)
By the proposed visual motion observer constructed by Eqs.
(4) and (8), the unmeasurable motion gco will be exploitedas the part of control law.
III. V ISUAL M OTION O BSERVER - BASED S IMPLE P OSE
C ONTROL
Although the time-varying desired motion is needed to thepath planner, the desired motion in our previous works [10],[11] is assumed to be constant. Thus, we have to reconstruct
the visual motion error system in order to handle the timevarying desired motion.
A. Pose Control Error System
Let us consider the pose control error system in orderto achieve the control objective. First, we define the posecontrol error as follows:−1
gec = gcd
gco ,(9)which represents the error between the relative rigid body
motion gco and the reference one gcd . It should be remarked
that gco can be calculated by using the estimated relative
rigid body motion ḡco and the estimation error vector ee =[pTee eTR (eξ̂θee ) ] equivalently as follows:(10)
gco = ḡco geeξθee =

sin−1 eR (eξ̂θee )

eR (eξ̂θee ),

(11)
eR
although gco cannot be measured directly [10]. Using thenotation eR (eξ̂θ ), the vector of the pose control error isdefined as ec := [pTec eTR (eξ̂θec )]T .
Differentiating Eq. (9) with respect to time, the posecontrol error system canbe represented as (eξ̂θee )

b
b
b
b
−1
Vec
= −Ad(gec) Ad(g−1 ) Vwc + Vcd + Vwo ,
cd

(12)

b
where Vcd
is the body velocity of the reference of the relative
rigid body motion gcd .

B. Visual Motion Error System and Stabilizing Control Law
Combining the estimation error system (7) and the posecontrol one (12), we construct the visual motion error system
as follows: b   
−1
−Ad(gec0
VecI)b, (13)
=
u
+
Vwo
b
−1
I
0
−Ad(gee
Vee
)
b
b
+ Vcd
. Let uswhere u := [uTc uTe ]T and uc := Ad(g−1 ) Vwc
cddefine the error vector of the visual motion error system as
x := [eTc eTe ]T .
The passivity between the input u and the output −x ofthe visual motion error system (13) is derived by using thefollowing energy function
11
V (x) = pec 2 + φ(eξ̂θec ) + pee 2 + φ(eξ̂θee ),(14)22where φ(eξ̂θ ) := 12 tr(I − eξ̂θ ) is the error function of therotation matrix. Here, we consider the following control input
u = −K(−x)ν, K > 0.
(15)
By differentiating V (x) along the trajectory of the system
and using the control input u, the next equation is obtainedb
if Vwo= 0 is satisfied.
V̇ = uT (−x) = −xT Kx.
(16)
Therefore, if w = 0, the equilibrium point x = 0 for theclosed-loop system (13) and (15) is asymptotic stable, i.e.,
u is a stabilizing control law (15) for the system.
However, the stabilizing control law (15) is not basedon optimization, the desired control performance cannot
be guaranteed explicitly. In the next section, a stabilizing
receding horizon control based on optimal control theory is
proposed.
IV. V ISUAL M OTION O BSERVER - BASED S TABILIZINGR ECEDING H ORIZON C ONTROLThe objective of this section is to propose visual feedback
control based on optimal control theory. A camera canprovide more information than the current derivation froma nominal position at the sample instant. This property
can be exploited to predict the target’s future position andimprove the control performance. In this section, we proposea stabilizing receding horizon control based on optimization.
A. Control Lyapunov Function
In this section, the finite horizon optimal control problem(FHOCP) for the visual motion error system (13) is considered. The FHOCP for the system (13) at time t consists ofthe minimization with respect to the input u(τ, x(τ )), τ ∈[t, t + T ], of the following cost function
 t+T
J(x0 , u, T ) =l(x(τ ), u(τ ))dτ + F (x(t + T )) (17)tl(x(t), u(t)) = Eqc (gec (t)) + Eqe (gee (t)) + uT (t)R(t)u(t)(18)
F (x) = ρV (x), qpi (t)≥0, qRi (t)≥0, ρ > 0, (19)where R(t) is a positive diagonal matrix, and Eqi (gei (t)) :=qpi (t)pei (t)2 + qRi (t)φ(eξ̂θei (t) ) (i ∈ c, e), with the statex(t) = x0 . The speciality of the cost function (17)–(19) isthat the terminal cost is derived from the energy functionof the visual motion error system. Furthermore, the rotation
error related part of the stage cost is derived from the error
function φ(eξ̂θab ) instead of the commonly used quadratic
form eR (eξ̂θab )2 . For a given initial condition x0 , wedenote this solution of the FHOCP as u∗ (τ, x(τ )), τ ∈[t, t + T ]. In receding horizon control, at each sampling timeδ, the resulting feedback control at state x0 is obtained bysolving the FHOCP and settinguRH := u∗ (δ, x0 ).
(20)
A control Lyapunov function closely related to stability isdefined as follows:
Definition 1 ([7]): A control Lyapunov function S(x) isgiven by(21)
inf Ṡ(x) + l(x, u) ≤ 0,u

where l(x, u) is a positive definite function.
The following lemma concerning the control Lyapunov
function is important to prove a stabilizing receding horizon
control.

b= 0, θec ≤ π2 , θee ≤ π2Lemma 1: Suppose that Vwo
and the design parameter ρ satisfies
ρ2 I≥4QR,(22)
where Q := diag{qpc I3 , qRc I3 , qpe I3 , qRe I3 }. Then, theenergy function ρV (x) of the visual motion error system(13) can be regarded as a control Lyapunov function.
Proof: Using Eq. (16), which is the time derivativeof V along the trajectory of the system (13), the positive
definite function l(x(t), u(t)) (18) and the stabilizing control
law uk (15) with K = ρ2 R−1 for the system, Eq. (21) canbe transformed into

inf [Ṡ(x) + l(x, u)]uhi= inf ρV̇ + Eqc (gec ) + Eqe (gee ) + uT Ru
u »
“
”T “
” ρ2
ρ
ρ
= inf u − R−1 x R u − R−1 x − xT R−1 xu224+Eqc (gec ) + Eqe (gee )]ρ2= − xT R−1 x + qpc pec 2 + qRc φ(eξ̂θec )4+qpe pee 2 + qRe φ(eξ̂θee )ρ2≤ − xT R−1 x + qpc pec 2 + qRc eR (eξ̂θec )2
4
2
ξ̂θee
+qpe p)2«R (e„ ee2 + qRe e
ρ −1
R − Q x,
= −xT
(23)
4

where we have used the fact that φ(eξ̂θab )≤eTR (eξ̂θab ) forall θab ≤ π2 . Therefore, the condition inf u [Ṡ(x)+l(x, u)]≤0
will be satisfied, if the assumption ρ2 I≥4QR.
Lemma 1 shows the energy function ρV (x) of the visualmotion error system (13) can be regarded as a control
Lyapunov function in the case of ρ2 I≥4QR.
B. Visual Motion Stabilizing Receding Horizon Control
Suppose that the terminal cost is the control Lyapunov
function ρV (x), the following theorem concerning the stability of the receding horizon control holds.
Theorem 1: Consider the cost function (17)–(19) for theb= 0,visual motion error system (13). Suppose that Vwo
θec ≤ π2 , θee ≤ π2 , and ρ2 I≥4QR, then the receding horizon control for the visual motion error system is asymptotically stabilizing.
Proof: Our goal is to prove that J(x∗ (t), uRH , T ),which is the cost-to-go applying the receding optimal control
uRH , will qualify as a Lyapunov function for the closed loop
system. Construct the following suboptimal control strategy
for the time interval [t + δ, t + T + δ]u∗ (τ )uk (τ ) = ρ2 R−1 xũ =τ ∈ [t + δ, t + T ](24)τ ∈ [t + T, t + T + δ]

where uk is the stabilizing control law (15) with K = ρ2 R−1
for the visual motion error system. The associated cost isJ(x∗ (t + δ), ũ, T )∗= J(x(t),u∗ , T ) + ρ[V (x(t +Z T + δ)) − V (x (t + T ))]
Z
t+δ

−

l(x∗ (τ ), u∗ )dτ +tt+T +δ

l(x∗ (τ + T ), uk )dτ, (25)t+T

where x∗ is the optimal state trajectory. This cost, which isan upper bound for J(x∗ (t + δ), u∗ , T ), satisfies
J(x∗ (t + δ), u∗ , T ) − J(x∗ (t), u∗ , T )∗≤ ρ[VZ (x(t + T + δ)) − V (xZ(t + T ))]
t+δ

−
t

l(x∗ (τ ), u∗ )dτ +t+T +δ

t+Tl(x∗ (τ + T ), uk )dτ. (26)

b
Vcd

f

b
Vwc

gcd

Visual Motion
Pose
b
gco
ObserverController
Vwc
e
u
e
u
e
e
e
c
c
RH
RH
...
ue ... e ... uc ...

b
Vwo

Vision f
CameraVisual Motion Observer-based
Stabilizing Receding Horizon Pose Controller

Fig. 2. Block diagram of visual motion stabilizing receding horizon control.

Using the positive definite function l(x(t), u(t)) (18) and thestabilizing control law uk (15) for the system, and dividing
both sides by δ and taking the limit as δ → 0, Eq. (26) canbe transformed intoJ(x∗ (t + δ), u∗ , T ) − J(x∗ (t), u∗ , T )
δ→0
δ
ρ2 ∗T
−1 ∗
∗
(t + T ))≤ − x (t + T )R x (t + T ) + Eqc (gec
4
∗
∗T
∗
∗T
(t)
−
u
Ru∗
+Eqe (gee (t +„T )) − x (t)Qx«2
ρ −1≤ −x∗T (t + T )R − Q x∗ (t + T )4−x∗T (t)Qx∗ (t) − u∗T Ru∗ .
(27)
lim

Considering that the control input during first δ is
uRH = u∗ , by the assumption ρ2 I≥4QR, the derivativeof J(x∗ (t), uRH , T ) is negative definite. Therefore, we haveshown that J(x∗ (t), uRH , T ) qualifies as a Lyapunov function and asymptotic stability is guaranteed.
Theorem 1 guarantees the stability of the receding horizon
control using the control Lyapunov function for the 3D
visual motion error system (13) which is a highly nonlinear
and relatively fast system. The block diagram of the visualmotion stabilizing receding horizon control is shown in
Fig. 2. Since the stabilizing receding horizon control designis based on optimal control theory, the control performance
should be improved compared to the passivity-based simple
pose control uk (15), under the condition of adequate gain
assignment in the cost function. It should be noted that theerror function φ(eξ̂θab ) of the rotation matrix can be directly
used in the stage cost (18).
Remark 1: One of the contributions in this paper is thatthe proposed control law can be applied to the time-varying
desired relative rigid body motion gcd (t), similar to in thecase of the constant one.
V. I MAGE S PACE NAVIGATION F UNCTION - BASED PATHP LANNING
In [4], the inherent problem of the position-based visualfeedback control by using only the error defined on the
Cartesian space, is that the target object cannot always
remain in the camera field of view during the servoing.
Because the proposed stabilizing receding horizon control
in the previous section is the position-based method, it
may leave the camera field of view. In this section, a path
planner to be appropriate for the visual motion error system
is designed through an image space navigation function toguarantee to keep all features into the camera field of view.
The control objective in this paper is stated as follows:
Control Objective: The vision camera follows the target
object, i.e., the relative rigid body motion gco (t) is coincidedwith the time-varying desired one gcd (t), which is generated
to keep all features into the camera field of view andconverges the final desired one gcdf .

From the proposed stabilizing receding horizon control
law for the visual motion error system, the input to vision
camera is designed as follows:
b
b
,
(28)
= Ad(gcd ) uRH
− Vcd
Vwc
c
RH
RH T
RH T T
where u= [(uc ) (ue ) ] . Hence, the vision cambof theera input is only needed the body velocity Vcd
1
reference of the relative rigid body motion gcd .
Here, we introduce the navigation function-based method
as a technique for constructing artificial potential fields inbwhich can achieve the control objective.
order to design Vcd
Firstly, we define the desired image feature vector and thefinal one as fcd := f (gcd ) and fcdf := f (gcdf ), respectively.
The navigation functions used in this paper are defined asfollows:
Definition 2 ([6],[13]): Let D be a space where all feature points of the target remain visible, and let fcdf be in theinterior of D. A map ϕ : D → [0, 1] is a navigation function
if it is1) smooth on D (at least a C (2) function);
2) a unique minimum exists at fcdf ;3) admissible on D, i.e., uniformly maximal on theboundary of D;4) a Morse function.
A. Path Planning of Desired Body Velocityb
To develop the desired body velocity Vcd, we derive a
relationship between fcd defined on the image space andbdefined on the Cartesian space. Differentiating Eq. (3),
Vcd
we have the following relation between the desired featureb:
point pcdi and the body velocity Vcdξ̂θcdb
I −p̂oi Vcdṗcdi = e.
(29)Moreover, the relation between the desired image feature fcdi
and the desired feature
be expressedas λ point pcdi can

λxcdi
0
−
2
zcdi
zcdi
f˙cdi =
(30)
ṗcdi .
λycdi
λ
0
−
2
zcdi
zcdi
Hence, the desired image feature vector and the desired body
velocity can be related asbf˙cd = JL (gcd )Vcd
,
(31)
2m×6
where JL (gcd ) : SE(3) → R
is defined asT
T
TT(gcd ) JL2(gcd ) · · · JLm(gcd ) (32)
JL (gcd ) := JL1
 λ

cdi
0
− λx
2
zcdi
zcdi
JLi (gcd ) :=eξ̂θcd I −p̂oi ,
λ
cdi
0
− λy
2
zcdi
zcdi
(i = 1, · · · , m).
(33)
Inspired by Eq. (31) and the definition of the navigationb
is designed as folfunction, the desired body velocity Vcd
†
b
lows:
(34)
Vcd = −kcd JL (gcd )∇ϕ,T
∂ϕ(fcd )where ∇ϕ(fcd ) :=denotes the gradient vector∂fcdof ϕ(fcd ) and kcd ∈ R is the positive gain. The design ofthe image navigation function ϕ(fcd ) is considered in thenext subsection. Substituting (34) into (31), the velocity ofthe desired image feature vector yields
f˙cd = −kcd JL (gcd )JL† (gcd )∇ϕ.
(35)
Here, it is assumed that ∇ϕ(fcd ) ∈ N S(JLT (fcd )) where
N S(·) denotes the null space operator, similar to [6]. Since
fcd is chosen a priori via the off-line path planning routinein (35), this assumption can be satisfied.
1 The relative rigid body motion g
cd can be obtained solving ġcd =b .
gcd Vcd

B. Image Space Navigation Function
In this subsection, we develop the image space navigation
function ϕ(fcd ) [5], [6]. Here, the image space navigation
function is designed that all image features keep into visible
set.
Firstly, we define two auxiliary functions η(fcd ) : R2m →[−1, 1]2m and s(η) : [−1, 1]2m → R2m as follows:
j
ﬀ
2
2
2
η(fcd ) = diag
,
,···
fcd
fyM − fy–m
» fxM − fxm fyM − fymTfxM + fxm fyM + fym
fyM + fym
−
···
(36)
fxM − fxm fyM − fym
fyM − fym"#T
η2m − ηf 2m
η1 − ηf 1
·
·
·
,
(37)
s(η) =
112κ 2κ
(1 − η12κ ) 2κ
(1 − η2m)

where η(fcd ) = [η1 (fcd ) η2 (fcd ) · · · η2m (fcd ) ]T and κ >0 ∈ R is an additional parameter to change the potential
field. η(fcdf ) = [ηf 1 (fcd ) ηf 2 (fcd ) · · · ηf 2m (fcd ) ]T :R2m → [−1, 1]2m is defined as same as (36). fxM , fxm ,fyM and fym ∈ R denote the maximum and minimum pixel
values along the x- and y-axis, respectively. η and s are
the functions in order to normalize the current pixel value
for the maximum and minimum pixel values, and to define
the error between the current image feature and the final
one, respectively. Then, the model space navigation functionϕ̃(η) ∈ R2m → [0, 1] is defined as
ϕ̄
.
(38)
ϕ̃(η) :=
1 + ϕ̄
Moreover, the objective function ϕ̄(η) ∈ R2m → R is
defined as
1(39)ϕ̄(η) := sT (η)Ks s(η),
22m×2m
where Ks ∈ R
is a positive definite symmetricmatrix.
The image space navigation function denoted by ϕ(fcd ) ∈D → R, can be developed as follows:ϕ(fcd ) := ϕ̃ ◦ ϕ̄ ◦ s ◦ η(fcd ),(40)
where ◦ denotes the composition operator. The gradientvector ∇ϕ(fcd ) can be represented as
«T
∂ϕ
∂fcd(
)
2κ−1
1 − η12κ−1 ηf 1
1 − η2m
ηf 2m
sT Ks
=
×diag
,
·
·
·
,
2κ+1
2κ+1
2κ
(1 + ϕ̄)2(1 − η12κ ) 2κ
(1 − η2m
) ﬀ2κ
j
2
2
2
. (41)
×diag,,··· ,
fxM − fxm fyM − fym
fyM − fym„∇ϕ(fcd ) :=It should be noted that fcd → fcdf from (36)–(41) when∇ϕ(fcd ) → 0.
C. Convergence Analysis of Path PlannerSuppose that ∇ϕ(fcd ) is not a member of the null space
JLT (fcd ), the following theorem concerning the convergenceof the path planner holds.
Theorem 2: Suppose that ∇ϕ(fcd ) ∈ N S(JLT (fcd )) andthe initial desired image feature vector fcd (0) satisfies
fcd (0) ∈ D. Then, the desired image feature vector (35)ensures that fcd (t) ∈ D and has the asymptotically stableequilibrium point fcdf .
Proof: Consider the following positive definite function:

gcdf
Image Space
Navigation
Function-based
Path Planner

gcd
b
Vcd
f

Visual Motionb
Observer-based Vwc
Stabilizing
Receding Horizon
Pose Controllerb
Vwo

Vision f
CameraFig. 3. Block diagram of visual motion stabilizing receding horizon control
with image space navigation function-based path planner.

Vn (fcd (t)) = ϕ(fcd (t)).
(42)
Evaluating the time derivative of Vn (fcd ) along the trajectories of Eq. (35) gives us
V̇n (fcd (t)) = (∇ϕ)T f˙cd
= −kcd (∇ϕ)T JL (gcd )JL† (gcd )∇ϕ= −kcd (JLT ∇ϕ)T (JLT JL )−1 JLT ∇ϕ≤ −kJLT ∇ϕ2 ,
(43)
2
T
T
−1
where we use the property ka ≤kcd a (JL JL ) a, ∀a ∈R6 and k denotes a positive constant. It is clear from Eq.
(43) that Vn (fcd ) is a non-increasing function in the sensethat
Vn (fcd (t)) ≤ Vn (fcd (0)).
(44)
From Eqs. (42) and (44), the condition fcd (t) ∈ D, ∀t > 0
is satisfied for any initial condition fcd (0) ∈ D. By following
LaSall’s Theorem [14], it can be proved that the only invariant set that satisfies JLT (fcd )∇ϕ(fcd ) = 0 is the origin.
Considering the assumption ∇ϕ(fcd ) ∈ N S(JLT (fcd )), we
have shown that ∇ϕ(fcd ) = 0. Therefore, it can beconcluded that fcd → fcdf from Sec V-B.
Theorem 2 guarantees the convergence of the time-varying
desired image feature vector fcd (t) to the final one fcdf . The
path planner can be designed to keep all features into thecamera field of view based on the image space navigation
function. The block diagram of the visual motion stabilizing
receding horizon control with the image space navigation
function-based path planner is shown in Fig. 3.
Although position-based control can allow feature points
to leave the field of view, the principle advantage of itis that it is possible to describe tasks in terms Cartesianpose as is common in many applications, e.g., robotics [1],and that it does not need a desired image a priori. Thus,
the proposed method which is connected the position-based
receding horizon control and the image-based path planner
allows us to extend technological application area. The main
contribution of this paper is to show that the path planner
which always remains in the camera field of view during theservoing is designed for the position-based visual feedback
receding horizon control based on optimal control theory.
Remark 2: It is also interesting to note that the Jacobian
JL (·) between the image feature vector and the body velocity
is exactly the same form as the Jacobian Je (·) for the estimation error which is derived using a first Taylor expansion
approximation.
Remark 3: Our previous work has proposed the stabilizing receding horizon control for visual feedback systems
with the manipulator dynamics [10]. In a similar way, ourproposed approach can be applied to robot systems whichhave to be controlled with a small sampling period.

200

f4(6)

f3(6)

f2(6)

f1(6)

0.8
perx [m]

300

0
0
0

100

2

4

6

2

4

6

pery [m]

0

-0.4

-100

f2(0)f4(0)

-200

-0.8
0
0.5

f1(0)f3(0)perz [m]fy [pixel]

0.4

Camera Field of View

-300
-300

-200

-100
100
0
fx [pixel]

200

VII. C ONCLUSIONS

0

-0.5

300

ones with x-axis and y-axis are monotonically decreasing.
This means that the vision camera moves away once in orderto keep the target object. This validates one of the expectedadvantages of the stabilizing receding horizon control withthe path planner for the visual feedback system.

-1
0

2

time [s]46Fig. 4. Trajectory of image fea- Fig. 5. Pose control translational
ture points fco : Solid; with proposed error per .
method: Dashed; with previous one.

VI. S IMULATION R ESULTS
In this section, we present simulation results for thevisual feedback control with the path planner via the image
space navigation function, compared with the simple constantdesired motion proposed in [10]. The control objective isthat the vision camera tracks the static target object to keep
all target feature points (four points) inside the camera fieldof view. In other words, it is bring the actual relative rigid
body motion gco (t) to a given reference one gcdf using atime-varying reference one gcd (t), and it can be achieved tomake both the estimation and the pose control errors zero.
The simulation is carried out with the initial conditionpco = [0.2 0.2 − 1.35]T m, ξθco = [0 0 0]T rad. The final
desired relative rigid body motion is pcdf = [−0.23 − 0.3 −1.35]T m, ξθcdf = [0 0 π/2]T rad. This condition meansthat the vision camera moves from end to end of the fieldof view diagonally with optical axis rotation. To solve thereal time optimization problem, the software C/GMRES [15]is utilized. The control input with the receding horizon
control is updated every 2 ms. It must be calculated by thereceding horizon controller within that period. The horizon
was selected as T = 0.02 s.
The simulation results are presented in Figs. 4 and 5. Fig. 4
shows the trajectory of image feature points fco . In Fig. 4, thesolid lines denote the trajectory applying the proposed stabilizing receding horizon control with the path planner, andthe dashed lines denote those for the simple constant desired
value, respectively. fco (0) := [f1 (0) f2 (0) f3 (0) f4 (0)] andfco (6) := [f1 (6) f2 (6) f3 (6) f4 (6)] shows the values of theimage feature vector in the case of the initial condition andthose of in the case of t = 6 s, respectively. The control
method have to be designed that the feature points do not
get out of the camera field of view which is denoted as
the rectangle in Fig. 4. From Fig. 4, it is concluded that the
proposed method can make the vision camera keep all featurepoints in the field of view. Although the convergence to thedesired values is also achieved in the case of the previous
method [10] in the simulation, it corresponds to fail in theactual experiment as the vision camera miss the target object.
Fig. 5 shows the actual translational control error per ,which is the position error vector between the current relative
rigid body motion gco (t) and the final desired one gcdf ,instead of the time-varying desired one gcd (t). It should be
noted that the position error with z-axis increases, while theThis paper proposes a stabilizing receding horizon control
via an image space navigation function for a 3-D eye-inhand visual feedback system, which are highly nonlinear andrelatively fast system. The main contribution of this paper
is to show that the path planner which always remains in
the camera field of view during the servoing is designed forthe position-based visual feedback receding horizon control
based on optimal control theory. Especially, our proposed
method does not need a desired image a priori. Simulation
results are presented to verify the control performance withvisibility maintenance of the proposed control scheme andthe previous one [10].
R EFERENCES[1] F. Chaumette and S. A. Hutchinson, “Visual Servoing and Visual
Tracking,” In:B. Siciliano and O. Khatib (Eds), Springer Handbookof Robotics, Springer–Verlag, pp. 563–583, 2008.
[2] N. R. Gans and S. A. Hutchinson, “Stable Visual Servoing throughHybrid Switched-System Control,” IEEE Trans. on Robotics, Vol. 23,
No. 3, pp. 530–540, 2007.
[3] V. Lippiello, B. Siciliano and L. Villani, “Position-based Visual
Servoing in Industrial Multirobot Cells using a Hybrid Camera Configuration,” IEEE Trans. on Robotics, Vol. 23, No. 1, pp. 73–86, 2007.
[4] F. Chaumette, “Potential Problems of Stability and Convergence inImage-based and Position-based Visual Servoing,” D. J. Kriegman,
G. D. Hager and A. S. Morse (Eds.), The Confluence of Vision andControl, Lecture Notes in Control and Information Sciences 237,
Springer–Verlag, pp. 66–78, 1998.
[5] N. J. Cowan, J. D. Weingarten and D. E. Koditschek, “Visual Servoing
via Navigation Functions,” IEEE Trans. Robotics and Automation,
Vol. 18, No. 4, pp. 521–533, 2002.
[6] J. Chen, D. M. Dawson, W. E. Dixon and V. K. Chitrakaran, “Navigation Function-based Visual Servo Control,” Automatica, Vol. 43,
No. 7, pp. 1165–1177, 2007.
[7] D. Q. Mayne, J. B. Rawlings, C. V. Rao and P. O. M. Scokaert,“Constrained Model Predictive Control: Stability and Optimality,”
Automatica, Vol. 36, No. 6, pp. 789–814, 2000.
[8] M. Sauvée, P. Poignet, E. Dombre and E. Courtial, “Image based
Visual Servoing through Nonlinear Model Predictive Control,” Proc.
45th IEEE Conference on Decision and Control, pp. 1776–1781, 2006.
[9] G. Allibert, E. Courtial and Y. Touré, “Visual Predictive Control for
Manipulators with Catadioptric Camera,” Proc. 2008 IEEE International Conference on Robotics and Automation, pp. 510–515, 2008.
[10] T. Murao, H. Kawai and M. Fujita “Predictive Visual Feedback Control
with Eye-in/to-Hand Configuration via Stabilizing Receding Horizon
Approach,” Proc. of the 17th IFAC World Congress on AutomaticControl, pp. 5341–5346, 2008.
[11] M. Fujita, H. Kawai and M. W. Spong, “Passivity-based Dynamic
Visual Feedback Control for Three Dimensional Target Tracking:Stability and L2 -gain Performance Analysis,” IEEE Trans. on Control
Systems Technology, Vol. 15, No. 1, pp. 40–52, 2007.
[12] R. Murray, Z. Li and S. S. Sastry, A Mathematical Introduction to
Robotic Manipulation, CRC Press, 1994.
[13] E. Rimon and D. E. Koditschek, “Exact Robot Navigation using
Artificial Potential Functions,” IEEE Trans. Robotics and Automation,
Vol. 8, No. 5, pp. 501–518, 1992.
[14] H. K. Khalil, Nonlinear Systems (3rd ed.), Prentice Hall, 2002.
[15] T. Ohtsuka, “A Continuation/GMRES Method for Fast Computationof Nonlinear Receding Horizon Control,” Automatica, Vol. 40, No. 4,
pp. 563–574, 2004.