Visual Motion Observer-based Stabilizing Receding Horizon Control
via Obstacle Avoidance Navigation Function
Toshiyuki Murao, Hiroyuki Kawai and Masayuki Fujita
Abstract— This paper investigates stabilizing receding horizon control via an obstacle avoidance navigation function forthree-dimensional (3-D) eye-to-hand visual feedback systems.
Firstly, a visual motion observer for a controlled mobile robot
is presented. Then, visual motion observer-based stabilizing
receding horizon control for 3-D visual feedback systems, highly
nonlinear and relatively fast systems, is proposed. Moreover, apath planner to be appropriate for the visual motion error
system is designed through an obstacle avoidance navigation
function to keep collision-free during servoing. Finally, the effectiveness of the proposed method is verified through computer
simulations.

I. I NTRODUCTION
Visual feedback control, which can be regarded as a crosspoint between control theory and computer vision, is now
a very flexible and useful method in robot control [1].
Recently, Lippiello et al. [2] presented position-based visual
servoing for hybrid eye-in-hand/eye-to-hand multicamera
systems by using an extended Kalman filter and a multiarm robotic cell. Gans and Hutchinson [3] proposed hybrid
switched-system control which incorporates both an imagebased visual servoing controller and a position-based one.
Swensen et al. [4] presented featureless kernel-based visual
servoing and discussed a domain of attraction of the method
through experiments. Allibert et al. [5] reported comparison
results between two image prediction models for an imagebased visual servoing scheme based on nonlinear model
predictive control. Gans et al. [6] proposed a method which
regulates functions of the current image features without theneed of a goal image or a goal feature trajectory. The authors
have been proposed passivity-based visual feedback control
for three-dimensional (3-D) target tracking in a series of
papers [7]–[9].
On the other hand, obstacle avoidance coupled with accurate path following control which is to move a vehicle
towards a target location free of collisions with the obstacles
has attracted much attention of a great amount of robotics
researchers [10]. One of the representative works for obstacle
avoidance problems is a method using a navigation functionthat is globally convergent potential function proposed by
Rimon and Koditschek [11]. The feature of this efficientThis 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

Fixed Camera

5c
5w

5h
Controlled
Mobile Robot
Fig. 1.

Target Object

5o

5b1

Obstacle 1

5bn

Obstacle n

Eye-to-hand visual feedback system with obstacles.

approach is that automatically merge path finding and trajectory generation in a closed-loop fashion. Some navigation
function approaches for visual feedback systems are reported
in [12]–[14]. Cowan et al. [12] proposed a visual feedback
controller to bring a robot to rest at a desired configuration
keeping all feature points within a camera field of viewby using navigation functions. Chen et al. [13] and theauthors [14] developed an off-line path planner based on animage space navigation function with an adaptive 2 1/2-D
visual servoing controller and a stabilizing receding horizon
one, respectively. However, all these approaches [12]–[14]take advantage of navigation functions to solve the camera
field of view problems but not to deal with the obstacle
avoidance problems.
Among the different proposals from the navigation function approach, a number of navigation strategies for obstacle
avoidance in conjunction with visual information has beenreported. Huang et al. [15] presented a local navigation
method with a single camera for obstacle avoidance using
headings to obstacles and their angular widths. Cherubini and
Chaumette [16] proposed appearance-based visual navigation
with obstacle avoidance for real outdoor environments. Thisappearance-based visual navigation, which is one of the few
methods for obstacle avoidance through the camera model,
avoids new obstacles by using a range scanner. Althoughgood vision-based navigation approaches for the obstacle
avoidance problems are presented in [15] and [16], they are
restricted to a ground vehicle. Moreover, the desired control
performance cannot be guaranteed explicitly, since thesecontrol methods are not based on optimal control theory.
In this paper, stabilizing receding horizon control via anavigation function for obstacle avoidance is applied to 3-Dvisual feedback systems with an eye-to-hand configuration
as shown in Fig. 1, highly nonlinear and relatively fastsystems. Firstly, a visual motion observer for a controlledmobile robot is constructed. Secondly, stabilizing receding
horizon control for a visual motion error system using acontrol Lyapunov function is proposed. Then, a path planner
to be appropriate for the visual motion error system isdesigned through an obstacle avoidance navigation functionin order to move the robot towards a desired pose and awayfrom obstacles without additional sensors such as a range
scanner. Convergence analysis of the proposed path planner
is provided. Finally, control performance of the proposed
control scheme with obstacle avoidance and the previous
one [9] is evaluated through simulation results.
II. V ISUAL M OTION O BSERVER FOR C ONTROLLED
M OBILE ROBOTThe section II-A and II-B mainly reviews our previous
work [7] via the passivity-based visual feedback control.

where pcai = [xcai ycai zcai ]T . It is straightforward toextend this model to m image points by simply stacking
the vectors of the image plane coordinate, i.e., f (gca ) :=TT
· · · fcam
]T ∈ R2m . Hereafter, fca means f (gca ) for[fca1
simplicity. The image features fch and fco only depend on
the relative poses gch and gco , respectively.
B. Estimation Error System for Target Object

A. Vision Camera Model
Eye-to-hand visual feedback systems with obstacles use
four and n coordinate frames which consist of a world frameΣw , a fixed camera frame Σc , a mobile robot frame Σh , atarget object frame Σo and n obstacle frames Σb1 , · · · , Σbn
as depicted in Fig. 1. Let pho ∈ R3 and eξ̂θho ∈ SO(3) be aposition vector and a rotation matrix from the mobile robot
frame Σh to the target object frame Σo . Then, a relative posefrom Σh to Σo can be represented by gho = (pho , eξ̂θho ) ∈SE(3).
The objective of position-based visual feedback control is,in general, to bring the relative pose gho to a final reference
one ghodf . The relative pose gho can be represented by using
the composition rule for rigid body transformations [17] asfollows:
−1
gco .
(1)
gho = gch
The relative poses gch and gco can be led using poses under
the world frame Σw as−1−1
gch = gwc
gwh , gco = gwc
gwo .
(2)
The relative pose involves a velocity of each rigid body. We
define a body velocity of the mobile robot relative to thebbbb= [(vwh)T (ωwh)T ]T , where vwh
world frame Σw as Vwhband ωwh represent a velocity of an origin and an angularvelocity from Σw to Σh , respectively [17]. Differentiating
Eq. (2) with respect to time, the body velocities of the relative
poses gch and gco can be written as follows (See [8]):
b
b
b
b
Vch
= Vwh
, Vco
= Vwo
,

pai should be regarded, with a slight abuse of notation,
as [pTcai 1]T and [pTai 1]T . We assume that the position ofmultiple points on the mobile robot phi and that on the target
object poi are given. The perspective projection of the i-thfeature point onto the image plane gives us the image plane
coordinate fcai := [fcaix fcaiy ]T ∈ R2 as


λ
xcai
fcai =,(4)
zcai ycai(3)bis the body velocity of the target object relative towhere VwoΣw . Here, we exploit the property of the eye-to-hand visualfeedback system whose camera is fixed in the environment.
In this visual feedback system, we use a pinhole camera
model with a perspective projection. Here, we consider m(≥4) feature points on the rigid mobile robot and target object
in this paper. Let λ be a focal length, pai ∈ R3 and pci ∈ R3
be the position vectors of the i-th feature point relative toΣo and Σc , respectively. Here, we represent each observed
frame (i.e., Σh or Σo ) as Σa . Using a transformation of
the coordinates, we have pcai = gca pai , where pcai andIn this subsection, we consider a nonlinear observer (wecall a visual motion observer) for the target object in orderto estimate the relative pose gco from the image feature fco .
Using the body velocity of the relative pose gco (3), weˆbchoose estimates ḡco = (p̄co , eξ̄θ̄co ) and V̄coof the relativepose and velocity, respectively asbV̄co= uo .
(5)
The new input uo is to be determined in order to drive thebestimated values ḡco and V̄coto their actual values.
In order to establish an estimation error system, we
define an estimation error for the target object between theestimated value ḡco and the actual relative pose gco as
−1
gco .
(6)
geo = ḡco
We next define an error vector of the rotation matrix eξ̂θei as
rei := sk(eξ̂θei )∨ where sk(eξ̂θei ) denotes 12 (eξ̂θei − e−ξ̂θei ).
Using this notation, the vector of the estimation error for
T T] . Suppose an
the target object is given by eo := [pTeo reo
attitude estimation error θeo is small enough that we can leteξ̂θeo  I + sk(eξ̂θeo ). Therefore, using a first-order Taylor
expansion approximation, the estimation error vector eo canbe obtained from the image feature fco and the estimatedvalue of the relative pose ḡco (i.e.,the measurement and theestimate) as follows:
eo = J † (ḡco )(fco − f¯co ),(7)
where f¯co is an estimated value of the image feature andJ(ḡco ) is an image Jacobian-like matrix [8]. In the same
way as Eq. (3), the estimation error system for the target
object can be represented bybb−1 uo + V
Veo
= −Ad(geo
(8)
wo .
)
C. Estimation Error System for Controlled Mobile Robot
In our previous work [7], we constructed the visual motion
observer for the mobile robot, which estimates the relativepose gch from the image feature fch , using the same dynamical model as the visual motion observer for the targetb= ueh . However, the estimation input ueh
object, i.e., V̄chhas directly affected not only estimation but also control;

as a result, this leads to degraded estimation performancefrom the practical viewpoint. In order to remove the above
negative effect in this paper, we design a novel visual motion
observer to embed the input of the controlled mobile robot.
Firstly, we define the estimation error for the controlled
mobile robot between the relative pose gch and the estimatedvalue ḡch as−1
geh = ḡch
gch .
(9)
The vector of the estimation error for the controlled mobile
T T] . It should be
robot is also represented as eh := [pTeh rehnoted that this estimation error vector eh can be calculatedaseh = J † (ḡch )(fch − f¯ch ),(10)
where f¯ch is an estimated value of the image feature fch .
Next, we consider the following dynamical model in orderto estimate the controlled mobile robot:bbV̄ch= Ad(geh ) Vwh+ uh .

(11)

Here, the estimation error matrix geh is extracted using
the estimation error vector eh in Sec III. Also, the new
input uh is designed in the next section. The visual motion
observer for the target object cannot embed the body velocitybbecause the movement of the target object is unknown
Vwo
information. On the other hand, the input of the controlledb
can be utilized to the visual motion
mobile robot Vwh
observer as Eq. (11).
Differentiating Eq. (9) with respect to time, the estimation
error system for the controlled mobile robot can be represented byb= −Ad(g−1 ) uh .
(12)
Veh
eh

III. V ISUAL M OTION O BSERVER - BASED S TABILIZINGR ECEDING H ORIZON C ONTROL
A. Pose Control Error System
Let us consider the dual of the estimation error system inorder to achieve the control objective. We define the posecontrol error as follows:−1
gec = ghod
gho ,
(13)
which represents the error between the relative pose gho andthe reference one ghod . It should be remarked that the relativepose gho can be calculated by using the estimated relative
poses ḡch and ḡco and the estimation error vectors eh =T T
T T
] and eo = [pTeo reo] equivalently as follows:[pTeh reh−1 −1
gho = gehḡch ḡco geo
(14)
−1
−1
sin reh 
sin reo 
reh , ξθeo =
reo , (15)ξθeh =
reh 
reo 

although gho cannot be measured directly (see [9] for moredetails). It should be noted that the estimation matrix geh
is calculated in this process. Similar to the estimation error
vectors, the vector of the pose control error is defined as
T T
ec := [pTec rec] .
Differentiating Eq. (13) with respect to time, the posecontrol error system can be represented as 

b
b
b
b
−1
= −Ad(gecVec) Ad(g−1 ) Vwh + Vhod + Vwo , (16)hodbis the body velocity of the reference of thewhere Vhod
relative pose ghod .

B. Passivity of Visual Motion Error System
Combining the estimation error systems (8) (12) andthe pose control one (16), we construct the visual motion
observer-based pose control error system (we call the visualmotion error system) as follows:⎤ ⎡ ⎤⎡ b ⎤ ⎡−100−Ad(gecI
Vec)⎥ ⎣ ⎦ bb ⎦ ⎢⎣ Veh
0 −Ad(g−1 )00 Vwo , (17)
=⎣
u+
⎦
eh
b
I
Veo
0
0 −Ad(g−1 )
eo

where
u :=

uTc

uTh

uTo

T

b
b
, uc := Ad(g−1 ) Vwh+ Vhod.(18)
hod

Let us define the error vector of the visual motion error
system as x := [eTc eTh eTo ]T which consists of the posecontrol error vector ec and the estimation error vectors eh
and eo . It should be noted that if the vectors of the posecontrol error and the estimation ones are equal to zero, thenthe actual relative pose gho tends to the reference one ghod
when x → 0.
The visual motion error system (17) has important relation
between the input and the output.
b
Lemma 1: If Vwo= 0, then the visual motion error system(17) satisfies  T
uT (−x)dt ≥ −β, ∀T > 0(19)0

where β is a positive scalar.
The Lemma 1 can be proved by using the following
positive definite function:
V = E(gec ) + E(geh ) + E(geo ),(20)
where E(gei ) := 12 pei 2 +φ(eξ̂θei ) and φ(eξ̂θei ) := 12 tr(I −eξ̂θei ) is an error function of the rotation matrix.
C. Visual Motion Observer-based Stabilizing Receding Horizon Control
In this subsection, the finite horizon optimal control problem (FHOCP) for the visual motion error system (17) is
considered. The FHOCP for the system (17) at time t consistsof the minimization with respect to the input u(τ, x(τ )), τ ∈[t, t + T ], of the following cost function
 t+T
l(x(τ ), u(τ ))dτ + F (x(t + T )) (21)J(x0 , u, T ) =tl(x(t), u(t)) = Eqc (gec (t)) + Eqh (geh (t)) + Eqo (geo (t))+uT (t)R(t)u(t)(22)
F (x) = ρV (x)(23)
qpi (t)≥0, qRi (t)≥0, ρ > 0,
where R(t) is a positive diagonal matrix, and Eqi (gei (t)) :=qpi (t)pei (t)2 +qRi (t)φ(eξ̂θei (t) ) (i ∈ c, h, o), with the statex(t) = x0 . The speciality of the cost function (21)–(23) 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 rab 2 . For a given initial condition x0 , we denote
this solution of the FHOCP as u∗ (τ, x(τ )), τ ∈ [t, t + T ].
In receding horizon control, at each sampling time δ, theresulting feedback control at state x0 is obtained by solving
the FHOCP and settingu = u∗ (δ, x0 ).

(24)

Suppose that the target object is static, the following theorem concerning the convergence of the stabilizing receding
horizon control for the visual motion error system.
Theorem 1: Consider the cost function (21)–(23) for theb= 0,visual motion error system (17). Suppose that Vwoπππ2θec ≤ 2 , θeh ≤ 2 , θeo ≤ 2 , and ρ I≥4QR, then thereceding horizon control for the visual motion error system
is asymptotically stabilizing.
Theorem 1 concerning stability of the receding horizon
control can be proved by using the control Lyapunov function
for the 3-D visual motion error system (17) similar to [9].
Since the stabilizing receding horizon control design is basedon optimal control theory, the control performance should be
improved under the condition of adequate gain assignment
in the cost function.
Remark 1: One of the contributions in this paper is thatthe proposed control law can be applied to the 3-D eye-tohand visual feedback systems with the mobile robot, because
the visual motion error system is combined with the visualmotion observer for the controlled mobile robot unlike [9].
IV. O BSTACLE AVOIDANCE NAVIGATIONF UNCTION - BASED PATH P LANNING
Vision-based navigation with obstacle avoidance should
offer great perspectives in many applications, such as surveillance, patrolling, search and rescue or high risk missions.
However, few results have been applied to the full 3-D visualfeedback system that includes the camera model with theperspective projection. In this section, as a first step for avision-based navigation, we design a path planer through an
obstacle avoidance navigation function for 3-D eye-to-handvisual feedback systems. The control objective in this paper
is stated as follows:
Control Objective: The mobile robot follows the target
object, i.e., the relative pose gho (t) is coincided with thetime-varying desired one ghod (t) which is generated to avoid
unexpected obstacles, and which converges the final desired
one ghodf .
From the proposed stabilizing receding horizon control
law for the visual motion error system, the input to themobile robot is designed as follows:


b
b
.
(25)
Vwh
= Ad(ghod ) uc − Vhod
Hence, the mobile robot input is needed the body velocitybbb= [(vhod)T (ωhod)T ]T of the reference of the relative
Vhod
1pose ghod .
1 The relative pose gbhod can be obtained solving ġhod = ghod V̂hod .

A. Rotation Error for Path Planning
In rotation control, we consider only convergence to thefinal desired rotation. We define the rotation error between
the time-varying relative desired rotation eξ̂θhod and the final
one eξ̂θhodf as follows:eξ̂θed = e−ξ̂θhodf eξ̂θhod .
(26)
The vector form is defined as red := sk(eξ̂θed ). Differentiating Eq. (26) with respect to time, the rotation error systemfor path planning can be written asbbωed= ωhod.
(27)
B. Visual Motion Observer for Path Planning
Obstacle avoidance navigation function-based path planning needs the relative position of the obstacles from thecamera frame pcbi . In this section, we design the visualmotion observer in order to estimate the relative position pcbi
using the image feature fcbi := f (gcbi ). Now, we assume thatthe camera can observe the multiple points on the obstacles
and the position of them pbii is known, similar to the case of
the target object and the controlled mobile robot. The body
velocity of the relative pose gcbi can be written as follows:bbVcbb i = −Ad(g−1 ) Vwc
+ Vwb
,
i
cbi

(28)

b
is the i-th obstacle body velocity. Using the bodywhere Vwbivelocity Vcbb i (28), we establish a following estimate modelfor the obstacles:bV̄cbb i = −Ad(ḡ−1 ) Vwc+ ubi ,
(29)cbi

where ḡcbi and V̄cbb i are the estimated values of gcbi and Vcbb i ,respectively. We define the estimation error for the obstacles
gebi between the actual relative pose gcbi and the estimatedone ḡcbi as follows:−1
gebi = ḡcb
gcbi .
(30)i

The vector of the estimation error for the obstacles ebi =T T] can be obtained by exploiting the image feature[pTebi reb
ifcbi and the estimate values ḡcbi and f¯cbi as(31)
eb = J † (ḡcb )(fcb − f¯cb ).
i

e

i

i

i

Hence, the relative position pcbi can be calculated as follows:ˆ

pcbi = eξ̄θ̄cbi pebi + p̄cbi(32)

through gcbi = ḡcbi gebi . The estimation error system for the
obstacles can be obtained asb.
Vebb i = −Ad(g−1 ) ubi + Vwb
i
ebi

(33)

C. Obstacle Avoidance Navigation Function
In this subsection, we develop the obstacle avoidancenavigation function ϕ(pchd ) [11], [18]. In this paper, we
assume that the workspace and the obstacles are spherical.
This assumption does not constrain the generality of thiswork since it has been proven that navigation properties
are invariant under diffeomorphisms in [18]. Here, we select pchd , instead of phod , as the variable of the obstacle
avoidance navigation function2.
2 There is a relationship g−1
hod = gchd gco between pchd and phod .

Firstly, we define a space D where the mobile robot canmove avoiding obstacles as follows:
D=F −

M


Bi ,

(34)

i=1

where F := {pchd : pchd − pc0 2 ≤ ρ20 } is the mobile robot
movable space which represents a Euclidean 3-dimensional
disk with the center pc0 and the radius ρ0 > 0, and Bi :={pchd : pchd − pcbi 2 < ρ2i }, i = 1 . . . M denotes a M th obstacle space in F with the center pcbi and the radius
ρi > 0. We impose the additional constraint that all obstacles
closures are contained in the interior of the workspace;i.e. pcbi − pc0  + ρi < ρ0 , 1 ≤ i ≤ M , and that none ofthem intersect; i.e. pcbi − pcbj  > ρi + ρj , 1 ≤ i, j ≤ M .
If pchd ∈ D, the mobile robot keeps collision-free.
Here, we introduce the navigation function-based method
as a technique for constructing artificial potential fields inborder to design Vhod
which can achieve the control objective.
The navigation functions used in this paper are defined asfollows:
Definition 1 ([11],[18]): A smooth Morse function
ϕ(pchd ) : D → [0, 1] is a navigation function if it is1) a unique minimum exists at pchdf 3 ;2) uniformly maximal on the boundary of D.
A smooth vector field on any sphere world with a unique
attractor, must have at least as many saddles as obstacles. The
property of a Morse function whose Hessian at all criticalpoints is non-degenerate establishes that the initial conditions
that bring the system to saddle points are sets of measurezero. In view of this property, all initial conditions away fromsets of measure zero are brought to the unique minimum [18].
In order to design the obstacle avoidance navigation function, we utilize the following function which represents the
distance to the final desired relative position pchdf :s(pchd ) = pchd − pchdf 2κ ,(35)
where κ > 0 ∈ R is an additional parameter to change thepotential field. Next, let define the obstacle function which
includes the function η0 for keeping in the space F asMη(pchd ) =ηi (pchd )
(36)


i=0

ηi (pchd ) =ρ20 − pchd − pc0 2for i = 0
pchd − pcbi 2 − ρ2i . for i = 1 · · · M

Then, the model space navigation function ϕ̃(x) ∈R2m → [0, 1] and a κ-th root function are defined asxϕ̃(x) =
(37)
μ+x
and
1
ρ(x) = x κ ,(38)respectively, where μ > 0 ∈ R is a parameter. The
function (38) is important in order to change pchdf to a nondegenerate critical point. From Eqs. (35)–(38), the obstacle−1
3 The position p
chdf can be calculated as gchdf = gco ghodf . The
relative pose gco can be obtained by using the visual motion observer forthe target object in Sec II-B.

avoidance navigation function denoted by ϕ(pchd ) ∈ D →R, can be developed as follows:sϕ(pchd ) = ρ ◦ ϕ̃ ◦ (pchd )η κ1s(pchd )=(39)μη(pchd ) + s(pchd )where ◦ denotes the composition operator. Using a similar
way in [18], it can be verified that the function (39) is thenavigation function if the parameter κ is selected adequately.
D. Path Planning of Desired Body Velocityb
We design the desired body velocity Vhod
and the input
for the estimation ubi as follows:


b
b
b
−e−ξ̂θhod (vchd− p̂hod ωchd
) + vcob
Vhod =(40)−Kdr e−ξ̂θed red(41)
ubi = Keb ebi ,

where



b
Vchd
=


b
=
Vco

b
vchd
b
ωchd
b
vco
b
ωco




=


−e−ξ̂θchd Kdp ∇ϕ(pchd )bb− ωhod)eξ̂θhod (ωco

−1ġco )∨ .
= (gco

(42)
(43)

Kdp
:=
diag{kdp1 , kdp2 , kdp3 },Kdr:=diag{kdr1 , kdr2 , kdr3 } and Kb := diag{kb1 , · · · , kb6 }are the positive gain matrices for the translation and therotation of the desired body velocity and for the estimationT∂ϕ(pchd )input, respectively. ∇ϕ(pchd ) :=
which
∂pchd
denotes the gradient vector of ϕ(pchd ) can be calculated as
Eq. (44) at the bottom of the next page. The relative posegco can be obtained by using the visual motion observer forthe target object in Sec II-B.
We are now in a position to state the main result of thispaper concerning the convergence of the path planner.
bbTheorem 2: Suppose that Vwo= 0 and Vwb= 0, and theiinitial desired relative position pchd (0) satisfies pchd (0) ∈D. Then, the desired relative position pchd (t) ensures thatpchd (t) ∈ D and ghod (t) has the asymptotically stableequilibrium point ghodf .
Proof: Consider the following positive definite funcMtion:Vn = ϕ(pchd ) + φ(eξ̂θed ) +E(gebi ).
(45)
i=1

Evaluating the time derivative of Vn along the trajectories ofEqs. (27), (33), (40)–(42) gives us
T ξ̂θed beωed +V̇n = (∇ϕ)T ṗchd + red

MXeTbi Ad
i=1

ξ̂θeb
i)

(e

b
Veb
i

b
T ξ̂θed b= (∇ϕ)T eξ̂θchd vchd
+ red
e
ωhod
M
X T
+
ebi Ad ξ̂θeb (−Ad−1(geb ) ubi )
i=1

(e

i)

T ξ̂θchd −ξ̂θchd= −(∇ϕ) ee
M “”X−eTbi Kb ebi
+
i=1

i

T ξ̂θedKdp ∇ϕ − rede
Kdr e−ξ̂θed red= −(∇ϕ)T Kdp ∇ϕ − (e−ξ̂θed red )T Kdr e−ξ̂θed red

-1-2

MobileRobot

2perz [m]−eTbi Kb ebi .

(46)

From Eqs. (45) and (47), the condition pchd (t) ∈ D, ∀t > 0
is satisfied for any initial condition pchd (0) ∈ D. Since theestimation error vector ebi → 0, the position pcbi which
is utilized in the obstacle functions converges actual value.
Thanks to the property of navigation functions [18], it canbe shown pchd (t) → pchdf through ∇ϕ(pchd ) → 0. On theother hand, the rotation eξ̂θcd (t) → eξ̂θedf can be verified
because of e−ξ̂θed red → 0. Using the relationship ghod =−1
gco , it can be concluded that ghod (t) → ghodf .
gchd
Theorem 2 guarantees the convergence of the time-varying
desired pose ghod (t) to the final one ghodf . The path planner
can be designed to keep collision-free based on the obstacle
avoidance navigation function. The block diagram of thevisual motion stabilizing receding horizon control with the
obstacle avoidance navigation function-based path planner isshown in Fig. 2.
Our proposed approach, which deals with the estimation
problem from the visual features explicitly, can be applied to
3-D visual feedback systems without restriction to the groundvehicle. Thus, the proposed method which is connected thevisual motion observer-based stabilizing receding horizon
control and the obstacle avoidance navigation function-based
path planner without additional sensors allows us to extend
technological application area. The main contribution of thispaper is to provide that the path planner which can achieve
obstacle avoidance during the servoing is designed for thevisual feedback receding horizon control based on optimal
control theory.
Remark 2: In our passivity-based visual feedback control approach, the image Jacobian-like matrix J(·) of thepinhole camera model is exactly the same form as that ofthe panoramic camera model [19]. Therefore, the proposed
approach can be applied to 3-D eye-to-hand visual feedback
systems with a panoramic camera using the image Jacobianlike matrix J(·) in [19].

∇ϕ(pchd ) =1+κκ(μη + s) κ

4

6

8

0.4
0.2
0

0

2

4

6

8

0

2

4

6

8

0

2

4
6
time [s]

8

0

-0.2

0

2

4

6

8

1
0
2

4
6
time [s]Fig. 3.

-0.4

8

1
0.5
0

Pose control error er .

Remark 3: Our previous work has proposed the stabilizing receding horizon control for visual feedback systems withthe manipulator dynamics [9]. In a similar way, our proposed
approach can be applied to robot systems which have to becontrolled with a small sampling period.
V. S IMULATION R ESULTS
In this section, we present simulation results for the visualfeedback control with the path planner via the obstacle
avoidance navigation function, compared with the constantdesired relative pose proposed in [9].
Firstly, we present some results for stability analysis withthe static target object and the two static obstacles. The
simulation is carried out with the initial condition pho =[−1.697 0.859 1.214]T m, ξθho = [π/12 − π/12 π/3]Trad. The final desired relative pose is phodf = [0 0.3 0]Tm, ξθhodf = [0 0 0]T rad. The other conditions are set
as pco = [0.3 2 − 2]T m, ξθco = [0 0 0]T rad, pcb1 =[−0.2 0.9 − 2.75]T m, ξθcb1 = [0 0 0]T rad, ρ1 = 0.5m, pcb2 = [0.75 1.4 − 2.25]T m, ξθcb2 = [0 0 0]T rad,
ρ2 = 0.5 m. To solve the real time optimization problem,the software C/GMRES [20] is utilized. The control input
with the receding horizon control is updated every 10 ms. It
must be calculated by the receding horizon controller withinthat period. The horizon was selected as T = 0.01 s.
The simulation results for the static obstacles are presented
in Figs. 3 and 4 and Table I. Fig. 3 shows the actual
pose control error er , which is the error vector between thecurrent relative pose gho (t) and the final desired one ghodf .
The asymptotic stability can be confirmed by steady state
performance in Fig. 3.
Fig. 4 presents the trajectory of the position of the mobile
robot pwh . In Fig. 4, the spheres and the cube show thestatic obstacles and target object, respectively. The solid (red)lines denote the trajectory applying the proposed stabilizing
receding horizon control with the path planner, and thedashed (blue) lines denote those with the constant desired

0
2μ

2

0

0

i=1

It is clear from Eq. (46) that Vn is a non-increasing functionin the sense thatVn ≤ Vn (0).
(47)

0

1

-1

Fig. 2. Block diagram of visual motion stabilizing receding horizon control
with obstacle avoidance navigation function-based path planner.
M
X

NG erx [rad]VisionCameraNG ery [rad]Visual Motionb
Vhod
Observer-basedfchStabilizingb
fco Receding Horizon Vwh
Pose Controller

0

NG erz [rad]Obstacle
Avoidance
Navigation
Function-based
Path Planner

1

b Vb
Vwo
wbi

perx [m]

fcbi

ghod

pery [m]

ghodf

0
2@

@κη(pchd − pchdf ) + pchd − pchdf 

M
Y

i=1

ηi (pchd − pc0 ) −M
MXY
j=1 i=0,i=j11
ηi (pchd − pcbj )AA (44)

to-hand visual feedback systems, which are highly nonlinear
and relatively fast systems. The main contribution of this paper is to show that the path planner which can avoid obstacles
during servoing is designed for the visual feedback receding
horizon control based on optimal control theory. Simulation
results are presented to verify the control performance withobstacle avoidance of the proposed control scheme.

zw [m]-2zw [m]

-2

-3

-3

1

2

m]
w [

0
xw [m
]

y

1

]
y w [m

-1

2

1

0

R EFERENCES
0

Fig. 4.
Trajectory of
position pwh with static
obstacles:
Solid(Red);
applying proposed method:
Dashed (Blue); applying
previous one [9].

-10xw [m]

1

Fig. 5. Trajectory of position
pwh with moving obstacle
TABLE IVALUES OF THE COST FUNCTION .
Control Scheme
Feedback Control without Optimization
Receding Horizon Control (T = 0.01 s)
Receding Horizon Control (T = 0.1 s)
Receding Horizon Control (T = 1 s)

cost
0.557
0.409
0.180
0.057

value. The trajectory should be designed that the mobile
robot moves toward the cube avoiding the spheres. From
Fig. 4, it is concluded that the proposed method can make themobile robot avoid all obstacles. Although the convergenceto the desired values is also achieved in the case of theprevious method [9] in the simulation, it corresponds tofail in the actual experiment since the mobile robot hits the
obstacles.
Subsequently, the performance for the stabilizing receding
horizon control is compared in terms of the integral cost in
Table I. The cost is calculated by the following function:
 8l(x(τ ), u(τ ))dτ.
(48)
Cost =
0

Since the cost of the stabilizing receding horizon method
is smaller than the previous method without optimal control
theory [8] under conditions of the adequate cost function,
it can be easily verified that the control performance isimproved. In addition, the cost is reduced as the horizon
length increases from T = 0.01 to T = 1.
Next, we perform the simulation with the moving obstacle.
b
Although one of the assumptions of Theorem 2, i.e., Vwb=i0, is not satisfied in the case of the moving obstacle, we
also validate the effectiveness of the proposed method froma practical point of view. The condition of the obstacle 2changes to pcb2 = [0.75 1.4 − 0.05t − 2.25]T m from theaforementioned one. The simulation results for the moving
obstacle are presented in Fig. 5. From Fig. 5, the path planner
generates the reference trajectory avoiding the obstacle andthe mobile robot follows the generated trajectory. This result
leads to the consideration that the proposed control scheme
would be useful in the case of the moving obstacle.
VI. C ONCLUSIONSThis paper proposes stabilizing receding horizon control
via an obstacle avoidance navigation function for 3-D eye-[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] 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.
[3] 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.
[4] J. P. Swensen, V. Kallem and N. J. Cowan, “Empirical Characterization of Convergence Properties for Kernel-based Visual Servoing,”
In:G. Chesi and K. Hashimoto (Eds.), Visual Servoing via Advanced
Numerical Methods, Springer–Verlag, pp. 22–38, 2010.
[5] G. Allibert, E. Courtial and F. Chaumette, “Predictive Control for
Constrained Image-based Visual Servoing,” IEEE Trans. on Robotics,
Vol. 26, No. 5, pp. 933–939, 2010.
[6] N. R. Gans, G. Hu, K. Nagarajan and W. E. Dixon, “Keeping Multiple
Moving Targets in the Field of View of a Mobile Camera,” IEEE Trans.
Robotics, Vol. 27, No. 4, pp. 822–828, 2011.
[7] H. Kawai, T. Murao and M. Fujita, “Passivity-based Dynamic VisualFeedback Control with Uncertainty of Camera Coordinate Frame,”
Proc. of the 2005 American Control Conference pp. 3701–3706, 2005.
[8] 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.
[9] 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.
[10] J. Minguez, F. Lamiraux and J.-P. Laumond, “Motion Planning andObstacle Avoidance,” In:B. Siciliano and O. Khatib (Eds), Springer
Handbook of Robotics, Springer–Verlag, pp. 827–852, 2008.
[11] 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.
[12] 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.
[13] 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.
[14] T. Murao H. Kawai and M. Fujita, “Visual Motion Observer-based
Stabilizing Receding Horizon Control via Image Space Navigation
Function,” Proc. of the 2010 IEEE Multi-conference on Systems andControl, pp. 1648–1653, 2010.
[15] W. H. Huang, B. R. Fajen, J. R. Fink and W. H. Warren, “Visual Navigation and Obstacle Avoidance using a Steering Potential Function,”
Robotics and Autonomous Systems, Vol. 54, No. 4, pp. 288–299, 2006.
[16] A. Cherubini and F. Chaumette, “Visual Navigation with Obstacle
Avoidance,” Proc. 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1593–1598, 2011.
[17] R. Murray, Z. Li and S. S. Sastry, A Mathematical Introduction to
Robotic Manipulation, CRC Press, 1994.
[18] D. E. Koditschek and E. Rimon, “Robot Navigation Functions on
Manifolds with Boundary,” Advances in Applied Mathematics, Vol. 11,
No. 4, pp. 412–442, 1990.
[19] H. Kawai, T. Murao and M. Fujita, “Passivity-based Visual Motion
Observer with Panoramic Camera for Pose Control,” Journal ofIntelligent and Robotic Systems, Vol. 64, No. 3–4, pp. 561–583, 2011.
[20] T. Ohtsuka, “A Continuation/GMRES Method for Fast Computationof Nonlinear Receding Horizon Control,” Automatica, Vol. 40, No. 4,
pp. 563–574, 2004.