HAL Id: hal-01572366
https://hal.inria.fr/hal-01572366
Submitted on 7 Aug 2017
HAL is a multi-disciplinary open access archive for the deposit and dissemination of sci- entific research documents, whether they are pub- lished or not. The documents may come from teaching and research institutions in France or abroad, or from public or private research centers.
L’archive ouverte pluridisciplinaire HAL, est destinée au dépôt et à la diffusion de documents scientifiques de niveau recherche, publiés ou non, émanant des établissements d’enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.
via a Large Projection Operator
Riccardo Spica, Paolo Robuffo Giordano, François Chaumette
To cite this version:
Riccardo Spica, Paolo Robuffo Giordano, François Chaumette. Coupling Active Depth Estimation and
Visual Servoing via a Large Projection Operator. The International Journal of Robotics Research,
SAGE Publications, 2017, 36 (11), pp.1177-1194. �hal-01572366�
Visual Servoing via a Large Projection Operator
cThe Author(s) 0000 Reprints and permission:
sagepub.co.uk/journalsPermissions.nav DOI: 10.1177/ToBeAssigned www.sagepub.com/
Riccardo Spica
1, Paolo Robuffo Giordano
2, and Franc¸ois Chaumette
3Abstract
The goal of this paper is to propose a coupling between the execution of a Image-Based Visual Servoing (IBVS) task and anactiveStructure from Motion (SfM) strategy. The core idea is to modifyonlinethe camera trajectory in the null-space of the (main) servoing task for rendering the camera motion ‘more informative’ w.r.t. the estimation of the 3-D structure. Consequently, the SfM convergence rate and accuracy is maximized during the servoing transient.
The improved SfM performance also benefits the servoing execution, since a higher accuracy in the 3-D parameters involved in the interaction matrix improves the IBVS convergence by significantly mitigating the negative effects (instability, loss of feature visibility) of a poor knowledge of the scene structure. Active maximization of the SfM performance results, in general, in a deformed camera trajectory w.r.t. what would be obtained with a classical IBVS:
therefore, we also propose an adaptive strategy able toautomatically activate/deactivate the SfM optimization as a function of the current level of accuracy in the estimated 3-D structure. We finally report a thorough experimental validation of the overall approach under different conditions and case studies. The reported experiments support well the theoretical analysis and clearly show the benefits of the proposed coupling between visual control and active perception.
Keywords
Visual Servoing, Motion Control, Adaptive Control
1 Introduction
In many sensor-based robot applications, the state of the robot w.r.t. the environment can only be partially retrieved from its onboard sensors. In these situations, state estimation schemes can be exploited for recovering online the ‘missing information’ then fed to any planner/motion controller in place of the actual unmeasurable states. When considering non-trivial cases, however, state estimation must often cope with the nonlinear sensor mappings from the observed environment to the sensor space. The perspective projection performed by cameras is a classical example in this sense (Ma et al. 2003). Because of these nonlinearities, the estimation convergence and accuracy can be strongly affected by the particular trajectory followed by the robot/sensor which, loosely speaking, must guarantee a sufficient level ofexcitationduring motion (Cristofaro and Martinelli 2010; Achtelik et al. 2013).
In the context of Structure from Motion (SfM), for example, a poor choice of the camera trajectory can make the 3-D scene structure non-observable whatever the employed estimation strategy (Martinelli 2012; Spica and Robuffo Giordano 2013; Eudes et al. 2013; Grabe
et al. 2013). Trajectories with low information content will also result, in practice, in inaccurate (or noisy) state estimation. This, in turn, can degrade the performance of any planner/controller that needs to generate actions as a function of the reconstructed states, possibly even leading to failures/instabilities (De Luca et al. 2008; Malis et al.
2010).
The dependence of the estimation performance on the robot trajectory, and of the control performance on the estimation accuracy, clearly creates a tight coupling between perception and action: perception should be optimized for the sake of improving the action execution performance, and the chosen actions should allow
1University of Rennes1, Irisa and Inria, Rennes, France
2CNRS, Irisa and Inria, Rennes, France
3Inria and Irisa, Rennes, France
Corresponding author:
Paolo Robuffo Giordano, CNRS at Irisa and Inria Rennes Bretagne Atlantique, Campus de Beaulieu, 35042 Rennes Cedex, France.
Email: [email protected]
maximization of the information gathered during motion for facilitating the estimation task (Valente et al. 2012).
In this respect, the goal of this paper is to propose an online coupling between action and perception in the context of robot visual control. We consider, in particular, the class of Image-Based Visual Servoing (IBVS) schemes (Chaumette and Hutchinson 2006) as a representative case study. Indeed, besides being a widespread sensor-based control technique (see e.g., Tahri and Chaumette (2005);
Gans and Hutchinson (2007); Mahony and Stramigioli (2012)), IBVS is also affected by all the aforementioned issues. On the one hand, whatever the chosen set of visual features (e.g., points, lines, planar patches), the associated interaction matrixalways depends on some additional 3-D parameters not directly measurable from the visual input (e.g., the depth of a feature point). These parameters must, then, be approximated or estimated online, via a SfM algorithm, with a sufficient level of accuracy for not degrading the servoing execution or even incurring in instabilities or loss of feature visibility (Malis et al. 2010).
On the other hand, the SfM performance is directly affected by the particular trajectory followed by the camera during the servoing (Martinelli 2012; Spica and Robuffo Giordano 2013; Spica et al. 2014a): the IBVS controller should then be able to realize the main visual task while,at the same time, ensuring a sufficient level of information gain for allowing an accurate state estimation.
In this paper these objectives are met by investigating the online coupling between a recently developed framework for active SfM (Spica et al. 2014a) and the execution of a standard IBVS task. For this purpose, we exploit and extend the preliminary results obtained by Spica et al.
(2014b): in particular, the main idea is to project any optimization of the camera motion (aimed at improving the SfM performance) within the null-space of the considered visual task in order to not affect the servoing execution.
For any reasonable IBVS application, however, a simple null-space projection of a camera trajectory optimization turns out to be ineffective because of a structural lack of redundancy. Therefore, in order to gain the needed freedom, we suitably extend the redundancy framework introduced by Marey and Chaumette (2010) to the case at hand, which requires an action at the camera acceleration level. In addition, an adaptive mechanism is also introduced with the aim of activating/deactivating online the camera trajectory optimization as a function of the accuracy of the estimated 3-D structure for minimizing any ‘distorting’ effect on the camera motion.
The proposed (adaptive) coupling between active perception and visual control constitutes in our opinion an original contribution w.r.t. the existing literature.
Other works have already studied how to fuse visual measurements and different metric cues (e.g. camera
velocity/acceleration, observed target velocities and so on) to estimate the geometry of a scene and/or the camera motion (see e.g. De Luca et al. (2008); Martinelli (2012);
Eudes et al. (2013); Grabe et al. (2013); Chwa et al. (2016)).
Some of these works also identified and discussed the singularities of the problem, but without proposing any active control strategy to avoid them. There obviously exists a vast literature on the topic of trajectory optimizationfor improving the identification/estimation of some unknown parameters/states (see e.g., Achtelik et al. (2013); Wilson et al. (2014); Hollinger and Sukhatme (2014); Miller et al.
(2016)).In the context of SfM, the so-calledNext Best View (NBV) problem has also been addressed before, see Whaite and Ferrie (1997); Chen et al. (2011) for a classical work and a recent survey on this topic. However, many of these strategies are meant for an offlineuse (a whole trajectory is planned, executed, and then possibly re-planned based on the obtained results), and, in any case, do not take into account theonlinerealization of a visual taskconcurrently to the optimization of the estimation. At the other end of the spectrum, several works have already investigated how to plug the online estimation of the 3-D structure into a visual servoing loop, see, e.g., Chesi and Hashimoto (2004);
Fujita et al. (2007); De Luca et al. (2008); Malis et al.
(2009); Petiteville et al. (2010); Corke (2010); Mahony and Stramigioli (2012); Mebarki et al. (2015). In all of these works, however, the SfM scheme is just fed with the camera trajectory generated by the IBVS controller which, on the other hand, has no guarantee of generating a sufficient level of excitation w.r.t. the estimation task.
With respect to this previous literature, our work provides, instead, an online solution to the problem of concurrently optimizing the execution of a IBVS task (visual control) and the performance of the 3-D structure estimation (active perception). We also wish to stress that the proposed machinery is not restricted to the sole class of IBVS problems presented in this paper: indeed, one can easily generalize the reported ideas to other servoing tasks (e.g., exploiting different discrete/dense/geometric visual features than those considered in this work), or apply them to Pose-Based Visual Servoing (PBVS) schemes.
The rest of the paper is organized as follows:
Sect. 2 describes the theoretical setting of the paper and summarizes the active SfM framework presented in Spica and Robuffo Giordano (2013). Then, Sect. 3 details the machinery needed for coupling IBVS execution and optimization of the 3-D structure estimation. The proposed machinery is, then, validated in Sect. 4 via a number of experiments. Subsequently, Sect. 5 introduces an extension of the strategy detailed in Sect. 3 for allowing a smooth activation/deactivation of the camera trajectory optimization as a function of the current estimation accuracy. This extension is experimentally validated in
Sect. 6. Finally, Sect. 7 concludes the paper and proposes some possible future directions.
2 Problem description
2.1 Image-Based Visual Servoing
Consider a moving camera that measures a set of visual features s∈Rm(e.g., thexandy coordinates of a point feature) to be regulated at a desired constant value s∗. As well-known (Chaumette and Hutchinson 2006), the following relationship holds
˙
s=Ls(s,χ)u (1) where Ls∈Rm×6 is the interaction matrix of the considered visual features, χ∈Rp is a vector of unmeasurable 3-D quantities associated tos(e.g., the depth Zfor a point feature), andu= (v,ω)∈R6is the camera linear/angular velocity expressed in the camera frame. By defininge=s−s∗as the visual error vector, one also has
˙
e=Lsu.
If the camera/robot system isredundantw.r.t. the visual task (rank(Ls)<6), a control law that exponentially regulates e(t)→0 can be obtained by solving the following quadratic optimization problem
minu
1
2ku−rk2 s.t.Lsu=−λe
(2)
wherer∈R6represents, in general, the gradient of some suitable scalar cost function representative of secondary objectives. As well-known, the resolution of (2) results in the following control law
u=−λL†se+ (I6−L†sLs)r=−λL†se+P r, λ >0, (3) where L†s denotes the Moore-Penrose pseudoinverse of matrix Ls, and P = (I6−L†sLs)∈R6×6 is used to project the action ofrin the null-space of the main visual task so thatku−rkis minimized while not perturbing the achievement of the main task (Siciliano et al. 2009).
Any implementation of (3) (or variants) must deal with the lack of a direct measurement of vectorχ. A common workaround is to replace the exact interaction matrix Ls(s,χ)with an estimationLˆs=Ls(s,χ)ˆ evaluated on someapproximation χˆ of the unknown true vector χ. In this approximated case, assuming for simplicityr≡0, the closed-loop error dynamics, becomes
˙
e=−λLsLˆ†se, (4) and stability is determined by the eigenvalues of the matrix Ls(s,χ)Ls(s,χ)ˆ †(Malis and Chaumette 2002).
Special approximations, such asχˆ =χ∗=const, where χ∗ is the value of χ at the desired pose, can, at best, only guarantee local stability in a neighborhood ofs∗(see Chaumette and Hutchinson (2006)) and, in any case, require some prior knowledge on the scene (the value ofχ∗must be obtained independently from the execution of the servoing task). Additionally, too rough estimations of the finalχ∗(or other approximation choices for χ) may result in a poor,ˆ or even unstable, closed-loop behavior for the servoing (see Malis et al. (2010) and the illustrative example in Sect. 4.3).
In this context, the use of an incremental estimation scheme, able to generate online a converging χ(t)ˆ → χ(t) from (ideally) any initial approximation χ(tˆ 0), can represent an effective alternative. Indeed, such a scheme can improve the servoing execution by approximating the ideal control law (3) also when far from the desired pose and without needing special assumptions/approximations ofχ since, asχ(t)ˆ →χ(t), one obviously hasLˆs→Ls.
Other factors (e.g., estimation gains) being equal, the convergence rate of a SfM scheme is mainly affected by the particular trajectory followed by the camera w.r.t. the observed scene, with some trajectories being more informative/exciting than other ones. Therefore, the IBVS controller should select (online) the ‘most informative’
camera trajectory, among all the possible ones solving the visual task, for obtaining the fastest possible SfM convergence during the servoing transient. Section 3 will detail how to attain this goal.
2.2 Active Structure from Motion
Excluding degenerate cases (e.g., when a line projects on a single point or a circle projects on a segment, and so on.), the dynamics of any image-based visual feature vector s in (1) can always be expanded linearly w.r.t. the unknown vector χas follows (see Espiau et al. (1992); Chaumette (2004))
s˙ =fm(s,ω) +ΩT(s, v)χ (5) where vector fm(s,ω)∈Rm and matrix Ω(s,v)∈ Rp×mare functions ofknownquantities. As for vectorχ, since its dynamics depends on the particular geometry of the scene, no special structure is assumed apart from a generic smooth dependence on the system states and inputs, i.e.,
˙
χ=fu(s,χ,u). (6) Owing to the linearity of (5) w.r.t. χ, the sensitivity of the feature dynamics w.r.t. the unknown χ is ∂s/∂χ˙ = ΩT(s,v), that is, a function ofonly known quantities(the measured s and the ‘control vector’ v). Therefore, it is possible to act on v in order to increase the conditioning of the ‘sensitivity’ ΩT(s,v) during the camera motion.
This insight has been exploited by Spica et al. (2014a) for proposing an active SfM scheme, built upon the
dynamics (5–6), and yielding an estimation error with an assignableconvergence rate. The machinery of Spica et al.
(2014a) is here briefly summarized.
Let (ˆs,χ)ˆ ∈Rm+p be an estimation of (s,χ), and defineξ=s−ˆsas the ‘prediction error’ andz=χ−χˆ as the 3-D structure estimation error. An estimation scheme for system (5–6), meant to recover the unmeasurableχ(t) from the measureds(t)and knownu(t), can be devised as
s˙ˆ = fm(s,ω) +ΩT(s,v) ˆχ+Hξ
˙ˆ
χ = fu(s,χ,ˆ u) +αΩ(s,v)ξ (7) whereH>0andα >0are suitable gains.
By coupling observer (7) to (5–6), one obtains the following error dynamics
ξ˙ = −Hξ+ΩT(s,v)z
˙
z = −αΩ(s,v)ξ+g(z, t) (8) with g(z, t) =fu(s,χ,u)−fu(s,χ,ˆ u) being a van- ishing perturbation term (g(z, t)→0 as z(t)→0). As discussed in Spica et al. (2014a), the error system (8) can be proven to be semi-globally exponentially stable provided the p×p square matrix ΩΩT remains full rank during motion (therefore, availability ofm≥pindependent mea- surements is needed). Furthermore, the unperturbed version of (8) (i.e., withg=0) enjoys a port-Hamiltonian structure with the associated Hamiltonian (storage function)
H(ξ,z) =1
2ξTξ+ 1
2αzTz. (9) These facts will be important for the developments of Sect. 5.
Following Spica et al. (2014a), the transient response of the SfM estimation errorz(t) =χ(t)−χ(t)ˆ can be exactly characterizedandaffectedby actingonlineon the camera linear velocityv. Indeed, the convergence rate of z(t) is determined by the norm of the square matrix αΩΩT (in particular by its smallest eigenvalueασ21) which plays the role of an observability measure for system (5–6). For a given choice of gain α(a free parameter), the larger σ12 the faster the error convergence with, in particular,σ21= 0 if v= 0 (as well-known, only a translating camera can estimate the scene structure).
SinceΩ=Ω(s,v), one also hasσ12=σ21(s,v)and (σ˙12) =Jσvv˙ +Jσss,˙ (10) where the Jacobian matrices Jσv = ∂σ∂v21 ∈R1×3 and Jσs= ∂σ∂s21 ∈R1×m have a closed form expression function of (s,v) (again, known quantities). It is then possible to exploit relationship (10) for affecting online σ21(t)during motion in order to, e.g., maximize its value
and, as a consequence, increase the convergence rate of the estimation errorz(t).
To conclude, we detail the above machinery for the particular case of point features considered in this paper.
Let s=p= (x, y) = (X/Z, Y /Z) be the perspective projection of a 3-D point (X, Y, Z), and χ= 1/Z with, thus,m= 2andp= 1(note thatm > pas required). From Spica et al. (2014a) we have
σ12=ΩΩT = (xvz−vx)2+ (yvz−vy)2 Jσv= 2
vx−xvz vy−yvz (xvz−vx)x+ (yvz−vy)y Jσs= 2
(xvz−vx)vz (yvz−vy)vz
.
(11)
3 Plugging active sensing in Image-Based Visual Servoing schemes
In the redundant case, the execution of a servoing task can be naturally coupled with the (concurrent) optimization of the estimation of vectorχby exploiting vectorrin (3) for projecting any action aimed at maximizingσ12in the null- space of the visual task. The expression (10) shows that the optimization ofσ12(t)requires an action at thecamera acceleration level. In particular, since
∇uσ12= JTσ
v
0
(12) local maximization ofσ12can be achieved by just following its positive gradient via a camera acceleration vector
˙ uσ=
kσJTσv 0
, kσ>0. (13)
Being e˙ =Lsu and, thus, e¨=Lsu˙ + ˙Lsu, and by formulating an optimization problem analogous to (2) (Siciliano et al. 2009), one can show that the second- order/acceleration levelcounterpart of the classical law (3) for regulating the error vectore(t)to0is simply
˙
u= ˙ue=L†s(−kve˙ −kpe−L˙su) +P r (14) with kp>0 and kv >0. Therefore, by setting r= ˙uσ in (14), one would obtain the desired maximization of σ21 (i.e., of the convergence rate of the 3-D estimation error) concurrently to the execution of the main visual task. This straightforward strategy, although appealing for its simplicity, is unfortunately not viable in most practical situations because of the structural lack of redundancy for implementing action (13) (or any equivalent one) in (14). Indeed, in most visual servoing applications, the feature set s is purposely designed to constrain all the camera DOFs (i.e.,rank(Ls) = 6), and, as a consequence, no optimization of the camera linear velocity v can be performed via the null-space projector operator P. This fundamental limitation motivates the development of the alternative strategy presented in the following section.
3.1 Second-order Visual Servoing using a Large Projection Operator
An alternative control strategy, able to circumvent the redundancy limitations discussed above, can be devised by suitably exploiting the redundancy framework originally proposed by Marey and Chaumette (2010). In this work, it is shown how regulation of the full visual error vectore(a m-dimensional task) can be replaced by the regulation of its normkek(a1-dimensional task). This manipulation results in a null-space of (maximal) dimension6−1 = 5available for additional optimizations. The machinery presented in Marey and Chaumette (2010) (at the first order) can be exploited as follows: lettingν=kek, we have
˙ ν =eTe˙
kek = eTLs
kek u=Lνu, Lν ∈R1×6, and, at second-order,
¨
ν =Lνu˙ + ˙Lνu.
Regulation of ν(t)→0 can then be achieved by the following control law analogous to (14)
u˙ = ˙uν =L†ν(−kvν˙−kpν−L˙νu) +Pνr, (15) with kp>0, kv>0, L†ν = kek
eTLsLTseLTse, and Pν = I6−LTseeTLs
eTLsLTse being the null-space projection operator of the error norm with rank 6−1 = 5 (see Marey and Chaumette (2010)).
By implementing controller (15) in place of (14) one can still obtain regulation of the whole visual task error since, obviously,ν(t) =ke(t)k →0impliese(t)→0. However, contrarily to (14), the new null-space projectorPν allows implementing a broader range of optimization actions including (13) or equivalent ones.
On the other hand, a shortcoming of (15) w.r.t. (14) is that the interaction matrix Lν is singular for kek= 0 and, consequently, the projection matrix Pν, and the pseudoinverse L†ν, are not well-defined when the visual task is close to full convergence. As discussed in Marey and Chaumette (2010), this singularity can be avoided by switching from controller (15) to the classical law (14) when kek becomes sufficiently small. Unfortunately, however, the ‘first-order’ switching strategy proposed by Marey and Chaumette (2010) is not directly transposable to the second-order case. Section 3.3 details, therefore, a suitable ‘second-order’ approach able to guarantee a proper switching from (15) to the classical law (14).
Remark 3.1. Note that (15) also suffers from another sin- gularity occurring when e∈ker(LTs). This corresponds, however, to alocal minimum for the servoing itself, also
0 1 2 3 4 5
-0.2 0 0.2
Figure 1. A representative graph of the cost functionV(v)in (16) for kσ= 1,kd= 0.2,γ= 0.1,kωk= 0, and assumingσ21=kvk2. Note the presence of a finite upper bound forV(v)as desired.
affecting (14): if e∈ker(LTs), no camera motion can instantaneouslyrealize the task. Therefore,any‘local’ con- trol action would be equally affected by this issue, and no simple switching strategy could be employed in this case.
Local minima escaping strategies, such as random walks or global optimizations, are, on the other hand, out of the scope of this paper.
3.2 Optimization of the 3-D Reconstruction
As discussed in Sect. 2.2, the convergence rate of the 3-D estimation errorz(t) =χ(t)−χ(t)ˆ is determined by the eigenvalueσ12. To improve the estimation performance, one could attempt to maximize a cost function of the form V(u) =kσσ21(v). This straightforward solution would result, however, in an unbounded growth of kuk. Indeed, σ21∝ kvk2 (see (11) for the point feature case and Spica et al. (2014a, 2015) for other examples) and, therefore,σ21 can be made arbitrarily large by increasingkvk– the faster the camera motion, the larger value ofσ21.
In order to cope with this issue, it is then necessary to consider a cost function that allows for a finite upper bound w.r.t.kvk. Among the many possible solutions meeting this requirement, we opted for the following cost function
V(u) =kσγlog
γ+σ21(v) γ
−kd
2 kuk2, γ >0, (16) for which a representative graph is depicted in Fig. 1.
This choice is motivated by considering that σ12∝ kvk2 and log(x) =o(g(x)) for any polynomial function g(x).
Therefore, for sufficiently large velocities (kvk → ∞), the damping term k2dkuk2will be dominant w.r.t. the first term in (16), thereby ensuring existence of a finite upper bound w.r.t.kvk.
Maximization of V(u) is, then, obtained as best as possible by plugging in vector r, in (15), the following camera acceleration vector
˙
uV =∇uV = kσγ
γ+σ12∇uσ12−kdu. (17)
3.3 Second-order Switching Strategy
As explained in the previous section, the first-order switching strategy proposed by Marey and Chaumette (2010) does not simply extend to the second-order case and, therefore, we now detail a suitable second-order switching strategy meant to avoid the singularity of controller (15) whenν(t) =ke(t)k →0. We start by noting that controller u˙νin (15) imposes the following second-order dynamics to the error norm
¨
ν+kvν˙ +kpν = 0. (18) Define νkek(t) as the solution of (18) for a given initial condition(ν(t0),ν˙(t0)).
Let now t1> t0 be the time at which the switch from controller (15) to the classical lawu˙ein (14) occurs. For t≥t1, controller u˙e, under the assumption rank(Ls) = m, yields
¨
e+kve˙ +kpe= 0. (19) If rank(Ls)< m, as in the case studies reported in Sect. 4, the ideal behavior (19) can, in general, only be approximately imposed.
Let e∗(t) be the solution of (19) with initial conditions (e(t1),e(t˙ 1)), and letν∗(t) =ke∗(t)k be the corresponding behavior of the error norm. Ideally, one would like to have
ν∗(t)≡νkek(t), ∀t≥t1. (20) In other words, the behavior of the error norm should not be affected by the control switch at time t1, but ν∗(t)(obtained from (19)) should exactly match the ‘ideal’
evolution νkek(t) generated by (18) as if no switch had taken place.
While condition (20) is easily satisfied at the first-order (Marey and Chaumette 2010), this is not necessarily the case at the second-order level. Indeed, the following result holds (see appendix B)
Proposition 3.2. For the second-order error dynam- ics (18–19), condition (20) holds if and only if, at the switching timet1, vectorse(t1)ande(t˙ 1)areparallel.
It is then necessary to introduce an intermediate phase, before the switch, during which any component of e˙ orthogonal toeis made negligible. To this end, let
Pe=
Im−eeT eTe
∈Rm×m
be the null-space projector spanning the (m−1)- dimensional space orthogonal to vectore. Let also
δ=Pee˙ =PeLsu. (21)
The scalar quantityδTδ≥0 provides a measure of the misalignment among the directions of vectors e and e˙ (δTδ= 0 iff e and e˙ are parallel, ∀e6=0,e˙ 6=0). One can then minimize δTδ, compatibly with the main task (regulation of the error norm), by choosing vectorrin (15) as
˙
uδ =−kδ∇u
δTδ 2
!
=−kδLTsPeLsu=−kδJTδ (22) whereJδ=uTLTsPeLs, and the propertiesPe=PTe = PePewere used.
A possible switching strategy, shown in the flowchart in Fig. 2, consists of the following three different control phases:
1. apply the norm controlleru˙ν given in (15) with the null-space vectorrdefined in (17) as long asν(t)≥ νT, withνT >0 being a suitable threshold on the error norm. During this phase, the error norm will be governed by the closed-loop dynamics (18) and the convergence rate in estimatingχˆ will be maximized thanks to (17);
2. when ν(t) =νT, keep applying controller u˙ν, but replace (17) with (22) in vector r. Stay in this phase as long as some terminal condition on the minimization of δTδ is reached. In our case, we opted for a threshold δT on the minimum norm of vectorkPνJTδkas an indication of when no further minimization ofδTδis possible in the null-space of the error norm. Note also that, during this second phase,ν(t)keeps being governed by the closed-loop dynamics (18) sincer acts in the null-space of the error norm (i.e., no distorting effect is produced on the behavior ofν(t)by the change inr);
3. when δTδ has been minimized, switch to the classical controlleru˙egiven in (14) until completion of the task. The minimization of δTδ will ensure a smooth switch as per Prop. 3.2 (and as also demonstrated by the experimental results of Sects. 4 and 6).
Remark 3.3. We stress again that the main benefit of the proposed switching strategy is to guarantee a monotonic decrease of the error norm ν(t) during all phases, in particular when switching from the norm controller (15) to the classical controller (14). Such a monotonic decrease would not be granted, in general, without a specific action (phase 2) in the flowchart). Guaranteeing a monotonic decrease of the error norm in all conditions is particularly relevant for, e.g., ensuring that the features do not leave the camera fov (since their location will keep on converging towards their desired value) and, in general, avoid erratic
phase 1):
use (15) with r= ˙uV in (17) start
ν(t)< νT
phase 2):
use (15) with r= ˙uδ in (22) kPνJTδk< δT
phase 3):
use (14) yes
no
yes
no
Figure 2. Flowchart representation of the switching strategy.
behaviors of the features on the image plane (that can ease the actual tracking/segmentation of the features themselves).
3.4 Final considerations
We remark that the proposed scheme (active SfM (7) coupled to the second-order visual servoing (14–15), null- space terms (17–22) and associated switching strategy of Fig. 2) only requires, as measured quantities, the visual features s and the camera linear/angular velocity u= (v,ω). Indeed from the estimated χ, a (possiblyˆ approximated) evaluation ofallthe other quantities entering the various steps of the second-oder control strategy can be obtained from (s, χ)ˆ and u (the only ‘velocity’
information actually needed). We also note that the level of approximation is clearly a monotonic function ofkχ−χkˆ (i.e., the uncertainty in knowingχ): thus, all quantities will asymptotically match their real values as the estimation error z(t) =χ(t)−χ(t)ˆ converges to zero (the faster the convergence ofz(t), the sooner the ideal closed-loop behaviors (18–19) will be realized).
Assuming kχ−χkˆ is small enough, one can also address the stability of the strategy in Fig. 2 in order to show that no undesired effects may arise due to the switching among the different control laws. In particular, it is easy to show that both quantities ν(t) =ke(t)k and ke(t)k˙ keep bounded during motion and ultimately converge towards zero. First of all, we note that, during all phases, the error normν(t)is governed by the closed- loop dynamics (18) imposing an exponential convergence (with assigned poles). This is obviously the case in phases 1) and 2) (because of the norm controller (15)), and also
holds when switching to phase 3) thanks to the previous optimization action of phase 2) (whose role, as explained, is to enforce condition (20) at the switching). Therefore, the error norm ν(t)will exponentially converge towards zero during all phases.
As for ke(t)k, the norm controller (15) used in phases˙ 1) and 2) guarantees again exponential convergence ofν˙ =
˙
eTe/kek, that is, of the component ofe˙along the direction ofe. The component ofe˙orthogonal toeremains bounded during phase 1) (because of the damping action embedded in (17)), and is afterwards driven to zero during phase 2) by the term (22) (which, indeed, is meant to minimizekδk= kPeek). Finally, during phase 3) the closed-loop error˙ behavior is governed by (19) which, clearly, guarantees an exponential convergence of the whole vectore(t).˙
4 Experimental results
This section reports the results of several experiments meant to illustrate the approach proposed so far for coupling the execution of a visual servoing task with the concurrent optimization of the 3-D structure estimation. All experiments were run by making use of a greyscale camera attached to the end-effector of a6-DOFs Gantry robot. The camera has a resolution of 640×480px and a framerate of 30 fps. The open-source ViSP library (Marchand et al.
2005) was used to implement all the image processing and feature tracking in order to obtain a measurement of the visual features s at the same frequency. To increase numerical accuracy, the SfM estimator (7) and motion controller internal states were updated with a time step of 1 ms. A simple sample-and-hold filter was then used for s(t), which is only updated at30 Hz. Finally, the commands were sent to the robot at100 Hz.
As visual task, we considered the regulation of N = 4 point featurespiwith, thus,s= (p1, . . . ,pN)∈Rm, and Ls= (Ls1, . . . ,LsN)∈Rm×6, m= 8, with Lsi being the standard 2×6 interaction matrix for a point feature (Chaumette and Hutchinson 2006). We then have χ= (χ1, . . . , χN)∈Rp,p= 4, whereχi= 1/Zias explained in Sect. 2.2. The tracked points were black non-coplanar dots belonging to the surface of a white cube. A standard pose estimation algorithm was exploited to obtain the ground truth value of χ(t) from the known object model and the measureds(t).
Because of the high contrast between black dots and white cube surface, the segmentation and tracking of the N points were easily obtained, at video-rate, via the blob tracker available in ViSP. Besides easing the image processing step, this experimental setting also allowed us to reproduce (practically) identical initial experimental conditions across the several trials illustrated in the following sections. The results reported in the next Sect. 6.2 will instead resort to a Lucas-Kanade tracker for
segmenting and tracking a generic set of points lying on a much less structured target object in order to show the viability of our method also in more realistic situations.
As for what concerns the optimization of the 3-D reconstruction, we note that each feature point is characterized by its own (independent) eigenvalue σ1,i2 . Optimization of the estimation of the whole vector χ was then obtained by considering the average of the N eigenvaluesσ2=N1 PN
i=1σ1,i2 as quantity to be optimized.
Being, obviously,
∇uσ2= 1 N
N
X
i=1
JTσ
vi
0
,
the acceleration command (17) was then simply replaced by
˙
uV= kσγ
γ+σ2∇uσ2−kdu (23) during phase 1) of all the following experiments.
We invite the reader to watch the accompanying video in Ext. 1.
4.1 First Set of Experiments
In this first set of experiments, we aim at illustrating the benefits arising from the coupling between the execution of a visual servoing task and the concurrent active optimization of the 3-D structure estimation. To this end, we consider the following four different cases, all starting from the same initial conditions:
case 1) the full strategy (three phases) illustrated in Sect. 3 and Fig. 2 is implemented. The estimator (7) is run in parallel to the servoing task for generating the estimated χ(t)ˆ fed to all the various control terms.
The active optimization of the camera motion (23) takes place for the whole duration of phase 1;
case 2) the classical control law (14) is implemented. The estimator (7) is still run in parallel to the servoing task, but no optimization of the estimation error convergence is performed;
case 3) the classical control law (14) is again implemented, but the estimator (7) is not run. Vector χ(t)ˆ is, instead, taken as χ(t) =ˆ χ∗ =const, as customary in many visual servoing applications;
case 4) the classical control law (14) is again implemented, but by exploiting knowledge of the ground truth value χ(t) =ˆ χ(t) during the whole servoing execution.
This case, then, represents the ‘ideal’ behavior one could obtain if χ(t) were available from direct measurement.
0 10 20 30 40 50 60
0 0.1 0.2 0.3 0.4 0.5
(a)
0 10 20 30 40 50 60
0 1 2 3
(b)
0 10 20 30 40 50 60
0 1 2 3 10-3
(c)
-0.6 -0.4 -0.2 0
00.20.4 0
(d) (e)
Figure 3. First set of experiments. Fig. (a): behavior of the error normν(t)for case 1 (blue), case 2 (red), case 3 (green) and case 4 (dashed black). Fig. (b): behavior of the norm of the approximation error kz(t)k=kχ(t)−χ(t)kˆ with the same color code. Fig. (c): behavior of σ2(t)when actively optimizing the camera motion (case 1 – blue line) or not performing any optimization (case 2 – red line). In the previous plots, the (practically coincident) vertical dashed blue lines represent the switching times between the various control phases used in case 1.
Fig. (d): 3-D camera trajectory during case 1 with arrows representing the camera optical axis and square and circular markers representing the camera initial and final poses respectively. The three phases of Sect. 3.3 are denoted by the following color code: blue – phase 1, red – phase 2, green – phase 3. Fig. (e): trajectory of the four point features in the image plane during case 1 using the same color code, and with crosses indicating the desired feature positions. Superimposed, the initial and final camera images. Finally, solid lines represent the result of implementing phase 2, while dashed lines represent the effects of a direct switch from phase 1 to phase 3.
The following gains and thresholds were used in the experiments: α= 2000in (7),kp= 0.0225andkv= 0.3 in (14–15). Moreover, only for case 1, we used kσ= 20, γ= 0.001andkd= 18in (23),νT = 0.21andδT = 0.004 in the flowchart of Fig. 2 and finally kδ = 100 in (22).
Furthermore, in cases 1 and 2, vector χˆ was initialized asχ(tˆ 0) =χ∗, that is, starting from the (assumed known) value at the desired poseχ∗also exploited in case 3.
Let us first focus on Fig. 3(b), showing the evolution of the estimation error normkz(t)k=kχ(t)−χ(t)kˆ for the four cases. From the plots one can note how the use of observer (7), in cases 1–2 (blue and red lines respectively), makes it possible for the estimation/approximation error kz(t)k to converge faster than in case 3 (green line), where convergence is reached only at the end of the task, whenχ(t)→χˆ =χ∗(as obvious). Furthermore, the convergence time ofkz(t)kis almost three times shorter in case 1 (blue line) than in case 2 (red line). Indeed,kz(t)k becomes smaller than 5% of its initial value after about 3.5 sin case 1 w.r.t. 10.2 sin case 2. This improvement is due to theactiveoptimization of the SfM occurring, during phase 1 of case 1, under the action of (23). Indeed, looking at Fig. 3(c), one can note how the value ofσ2(t)of case 1 (blue line) is approximately4 times larger than in case 2 (red line) during the entire phase 1.
The fast convergence ofkz(t)k →0also translates into a fast accurate evaluation of the interaction matrix Lˆs and any related quantity. Indeed, from Fig. 3(a), one can notice that the behavior ofν(t)for case 1 (blue line) (i) quickly reaches a good match with the ideal behavior of case 4 (dashed black line), and(ii), more importantly, keeps monotonicallydecreasing during all the various phases. On the other hand, due to the larger error in estimating χ(t) (and, hence, evaluatingLˆs), both cases 2 (red line) and 3 (green line) present an initial increase of the error norm ν(t). It is worth noting how this initial divergent phase has, nevertheless, a shorter duration for case 2 w.r.t. case 3 thanks, again, to the use of observer (7).
The camera trajectory, depicted in Fig. 3(d), is also helpful for better understanding the effects of the active optimization of the camera motion during phase 1 of case 1.
Note, indeed, how the camera initially moves along an approximately circular path (blue line) because of the null- space term (23) that generates an ‘exciting’ motion for the estimation of the four point depthsZi. It is also possible to, again, appreciate the benefits of having employed the norm controller (15) during phase 1: indeed, it is only thanks to the large redundancy granted by controller (15) that the camera is made able to follow a quite ‘unusual’
trajectory while, at the same time, ensuring a convergent behavior for the error normν(t). For completeness, the red line in Fig. 3(d) represents (the quite short) phase 2 of the switching strategy (i.e., the alignment among vectorseand
˙
e), while the green line represents phase 3, i.e., the use of the classical controller (14).
As a supplementary evaluation of the theoretical analysis of Sect. 3.3, we now report, for case 1 only, an additional experiment aimed at assessing the importance of having introduced phase 2 in the switching strategy of Sect. 3.3 (i.e., of having enforced the alignment of e ande˙ before switching to the classical controller (14)). To this end,
0 10 20 30 40 50 60
0 0.1 0.2 0.3
4.4 4.45 4.5
0.205 0.21
(a)
0 10 20 30 40 50 60
0 0.01 0.02
4.4 4.45 4.5
2 4 6
10-3
(b)
Figure 4. Regulation of4point features. Behavior of the error norm ν(t) (Fig. (a)) and of kδk, the measure of misalignment between vectorseande˙ (Fig. (b)). In both plots, the blue lines represent the behavior of case 1 (full implementation of the switching strategy of Sect. 3.3), while cyan lines represent the direct switch from phase 1 to phase 3 without the action of vectorrin (22). The small picture-in- picture plots provide a zoomed view of the switching phase.
Fig. 4(a) shows the behavior of the error normν(t)for the previous case 1 (blue line) together with the behavior of ν(t)when notimplementing phase 2 but, instead, directly switching from phase 1 to phase 3 (cyan line). The two (almost coincident) blue vertical lines represent the switch from phase 1 to phase 2 and then phase 3 for the first experiment, and the direct switch from phase 1 to phase 3 for the second experiment. One can note how, in the second experiment, the error normν(t)has a large overshoot when switching to phase 3 because of the misalignment of vectors e ande˙ at the switching time. This overshoot is, instead, clearly not present in the first experiment whereν(t)keeps converging during all phases.
A similar overshoot can be observed in Fig. 3(e), where the point feature trajectories on the image, with phase 2 activated (solid lines) anddeactivated (dashed lines), are reported.
Finally, Fig. 4(b) reports the behavior ofkδkfrom (21), i.e., the measure of misalignment among vectorseande.˙ One can then verify how, in the first experiment, kδk is correctly (and very quickly) minimized, during phase 2, thanks to (22).
4.2 Second Set of Experiments
We now discuss a second set of experiments that involve the same four cases 1–4 introduced in the previous section, but with the camera starting from a different initial pose and with a different desired configurations∗w.r.t. the previous run. The results are reported in Fig. 5.
As compared to Fig. 3, it is worth noting how the sole case 1 (blue line in Fig. 5(a)) results in a successful regulation of the visual task error e(t) thanks, again, to
the fast convergence of the estimation errorkz(t)kduring the active optimization of phase 1 (blue line in Fig. 5(b)).
The servoing fails, instead, in case 2 (red line in Fig. 5(a)), i.e., when coupling the classical controller (14) with observer (7) but without optimizing for the convergence rate ofkz(t)k. In fact, in this case, the very small value of σ(t) during the camera motion (red line in Fig. 3(c)) makes the estimation task ill-conditioned w.r.t. noise and other unmodeled effects (including the disturbanceg(z, t) in (8)), resulting in a divergence of the estimation error kz(t)katt≈9 s(red line in Fig. 5(b)). On the other hand, the active optimization of case 1 is able to increaseσ(t)by approximately a factor of40w.r.t. case 2, thus ensuring a sufficiently high level of excitation for the camera motion and, consequently, a quick convergence of the estimation error kz(t)k. Failure of the servoing is also obtained in case 3, i.e., when exploiting theexact final value χ(t) =ˆ χ∗, because of the large initial error of the visual task that causes a loss of feature visibility (green line in Fig. 5(a)).
Finally, Figs. 5(d) and 5(e) depict the camera and feature trajectories during case 1. One can, again, appreciate, in Fig. 5(d), the initial spiralling motion of the camera that allows the increase of σ(t) during phase 1. It is also worth noting how, in case 1, the error normν(t) keeps a monotonicdecrease during the whole motion (as desired) despite the various switches among the three phases and the
‘unusual’ initial camera trajectory (blue line in Fig. 5(a)).
4.3 Third set of Experiments
In this last section, we report the results of two experiments meant to show how even relatively small inaccuracies in determining the value χ∗ at the desired pose can cause failure of the servoing when setting χ(t) =ˆ χ∗, as classically done in many visual servoing applications.
The two experiments presented here involve the same problem considered in Sects. 4.1 and 4.2 (regulation of 4 point features) and differ from the starting location of the camera w.r.t. the target object: in the first experiment, the camera starts (relatively) far from the desired pose while, in the second experiment, the camera starts at almost the desired pose. In both cases, the classical second order control (14) was employed by takingχˆ =χ∗(1+)with = (−0.0333,0.09,0.0424,−0.0875) (thus, since |i| ≤ 0.09, simulating an uncertainty of up to9%in the accuracy ofχ∗).
Figure 6(a) shows the behavior of the error norm ν(t) for both cases: in the first experiment (blue line), the visual error starts converging from its initial (large) value but then, at aboutt≈8 s, the servoing diverges and the features leave the camera fov. An even more interesting result is obtained in the second experiment (red line): in this case, the error ν(t)starts at a very small value since the camera is already quite close to its desired pose. However, controller (14)
0 10 20 30 40 50
0 0.2 0.4 0.6
(a)
0 10 20 30 40 50
0 2 4
(b)
0 10 20 30 40 50
0 2 4 6 8
10-3
(c)
-1 -0.5
-0.4 0 0.5
00.4 0
(d) (e)
Figure 5. Second set of experiments: regulation of4 point features starting from a different initial camera pose w.r.t. the experiments in Fig. 3. The plot pattern and color codes are the same as in Fig. 3.
is not able to impose a stable closed-loop behavior, and the error norm starts diverging until loss of tracking of the feature points at aboutt≈2.5 s.
These results then provide (for the first time, to the best of our knowledge) an experimental demonstration of the effects discussed in Sect. 2.1 and originally introduced by Malis et al. (2010): a (rather small) error in approximating χ∗ can be sufficient to move part of the eigenvalues of matrix −Ls(s∗,χ∗) ˆLs(s∗,χ)ˆ † to the right-half complex plane, thus resulting in an unstable closed-loop dynamics even when starting arbitrarily close to the desired pose. This demonstrates, once more, the importance of resorting to an online optimized estimation ofχ(t).
5 Adaptive optimization of the 3-D structure estimation
We now propose a further improvement to the strategy detailed in Sect. 3 and experimentally validated in Sect. 4.
The goal is to introduce an automatic mechanism for adaptivelyactivating/deactivatingthe optimization of SfM
0 3 6 9 12 0
0.02 0.04 0.06 0.08
(a) (b)
Figure 6. Third set of experiments: visual servoing of4point features using a constant approximationχ(t) =χ∗where the value ofχ∗is corrupted by a relative error of9%. Fig. (a): behavior of the error norm ν(t)for the first (blue line) and second (red line) experiments. Fig. (b):
image plane trajectory of the4point features during the first experiment with crosses indicating their desired positions. The initial and final (i.e.
until loss of tracking) camera images are superimposed.
as a function of the accuracy in estimating χ(t). This modification is motivated by the following considerations w.r.t. Fig. 2 and the previous experimental results:
• the optimization of σ2 is active during the whole phase 1, i.e., as long as the error norm is larger than some predefined threshold (i.e., ν(t)≥νT).
However, this is obtained at the expense of a possible distortion of the camera trajectory as clear from, e.g., Figs. 3(d) and 5(d) which depict the camera spiralling motion due to action (23) while approaching the final pose. Clearly, a more efficient strategy would implement (23) only when strictly needed, e.g., as long as the estimation errorkz(t)k=kχ(t)−χ(t)kˆ is larger than some threshold;
• similarly, once in phases 2–3, the flowchart of Fig. 2 does not allow any reactivation of the optimization of σ2. On the other hand, a reactivation could be necessary in case of unforeseen events such as, e.g., an unpredictable motion of the target that would make the estimation errorkz(t)kto abruptly increase.
We now detail a modification of the previous strategy of Sect. 3 for addressing these issues. To this end, we first introduce a way to quantify the uncertainty level in the estimation of the unknown vector χ(t). Since the estimation errorz(t)is (obviously) not directly measurable, we consider instead the followingmeasurablequantity
E(t) = 1 T
Z t
t−T
ξT(τ)ξ(τ)dτ, T ≥ >0, (24) where T represents the integration window and ξ=s− ˆs is the feedback term driving observer (7). Indeed, as discussed in appendix C, E(t) plays a role comparable with the unmeasurablez(t): it provides a measure of the uncertainty of the estimatedχˆvs. the actualχ. In particular,
provided the camera trajectory is sufficiently exciting (i.e., σ21(t)>0 during motion), E(t)≡0 iff kz(t)k ≡0 (i.e., the estimation has converged) andE(t)>0otherwise.
One can then leverage knowledge of E(t) for, e.g., (i) automatically switching from phase 1 to phase 2 when the estimation error becomes smaller than a desired threshold, (ii) automatically switching from phase 3 back to phase 1 when the estimation error grows larger than a desired threshold, and (iii) adaptively weighting the first term in action (17) for smoothly activating/deactivating the optimization ofσ12.
Let then 0≤E < E be a fixed minimum/maximum threshold forE(t)and define
kE(E) : [E, E]7→[0,1]. (25) as a monotonically increasing smooth map withkE(E) = 0, and kE(E) = 1. Function kE(E) can be exploited for suitably weighting the optimization of σ12 by simply modifying the cost function (16) as
VE(u, E) =kσkE(E)γlog
γ+σ21(v) γ
−kd 2 kuk2,
(26) resulting in the new optimization action
˙
uVE=∇uVE=kσkE(E)γ
γ+σ12 ∇uσ2−kdu (27) to be plugged in vectorrin (15). This modification clearly grants a smooth modulation of the first term in (27) from a full activation, in case of large estimation inaccuracies (kE(E) = 1 for E≥E), to a full deactivation if the estimation is sufficiently accurate (kE(E) = 0forE≤E).
Exploiting E(t) and the modified optimization action given by (27), we propose the new (adaptive) switching strategy depicted in Fig. 7. This consists of the same three phases of Sect. 3.3, but it now exploits knowledge ofE(t) for implementing an improved switching policy.
We highlight the following features of this new adaptive strategy: first of all, the initial (possible) switch from phase 3 to phase 1 is performed only if E(t)≥E (the estimation error is large enough for justifying an optimization of the camera motion) and ν(t)≥νT
(the visual error norm is large enough for preventing singularities in (15)). As illustration, two scenarios will typically trigger this switch: (i) a camera starting far enough from the desired pose and with a poor enough initial estimation χ(tˆ 0), or(ii) an unpredicted motion of the target object during the servoing task that causes an increase in the error normandin the estimation uncertainty.
The experiments of the next Sect. 6 will indeed address these two practical cases. Furthermore, while in phase 1, the optimization of the SfM will be performed only until either a good enough accuracy has been reached (E(t)<
phase 1):
use (15) with r= ˙uVE in (27)
E < Eor ν(t)< νT
phase 2):
use (15) with r= ˙uδ in (22)
E < Eor ν(t)< νT
kPνLTδk< δT
phase 3):
use (14) E > Eand
ν(t)> νT
yes
no
yes no
yes
no
start
no yes
Figure 7. Flowchart representation of the switching strategy exploiting the measurable error energy for triggering changes of status.
E), or controller (15) is close to become singular (ν(t)<
νT). The new switching conditionE(t)< Ewill then help in minimizing the distortion of the camera trajectory by allowing a quick switch to phase 2 as soon as the estimation accuracy is satisfactory (see again the experiments in Sect. 6).
As a final step, we comment about the choice of the two thresholds E and E exploited for triggering the various switches and for modulating the activation of the optimization of σ12 in (27). Assume the range of possible values of E(t) during the camera motion can be lower/upper bounded as 0≤Emin ≤E(t)≤Emax. It would obviously be meaningful to choose E and E such thatEmin ≤E < E≤Emaxfor properly tuning the adaptive switching strategy.
Concerning the lower bound Emin, being E(t)≥0, a straightforward choice would be Emin= 0. However, presence of measurement noise and other non-idealities can, in practice, preventE(t)to fall below some minimum value even after convergence of the estimation error (up to some residual noise). If needed, this minimum value can be, e.g., experimentally determined by simply averaging,
across a sufficient number of different camera trajectories, the (steady-state) value reached byE(t)once the estimation has converged. This is indeed the solution adopted for the experiments in Sect. 6. As forEmax, any (arbitrarily large) positive value would in principle be a valid choice since, the larger the initial approximation error kz(t0)k=kχ(t0)−
ˆ
χ(t0)k, the wider the possible range of E(t). However, exploiting the properties of observer (7), one can prove (see appendix C) that
E(t)≤kz(t0)k2
α . (28)
Therefore, if an upper boundkz(t0)k ≤zmaxon the initial estimation error can be assumed (as in most practical situations), one can exploit (28) and set
Emax=zmax2
α . (29)
For the interested reader, this result can be given an interesting energetic interpretation (Spica 2015) as a consequence of the port-Hamiltonian structure of (8).
We conclude with the following remarks: sinceE(t)>
0 as long as the estimation error has not converged, the adaptive gain kE(E) in (27) is also guaranteed to never vanish during the estimation transient (by properly placing, if needed, the minimum threshold E). As a consequence, the optimization of the camera motion (i.e., of σ21(t)) will always be active during phase 1. We also note that, in general, no special characterization is possible for the behavior of E(t). Nevertheless, one can show that, if σ21(t)≈const >0during motion, then the error system (8) behaves as a second-order critically-damped linear system, with z(t)playing the role of the ‘position variables’ and ξ(t) that of ‘velocity variables’, see Spica and Robuffo Giordano (2013). In this situation,kξ(t)k2(and, thus,E(t) as well) will approximate a ‘bell-shaped’ profile with a monotonic increase towards a maximum value followed by a monotonic decrease towards zero. Indeed, this is the profile followed byE(t)during the active phases of all the experiments reported in Sect. 6, since maximization of (26) does result (as a byproduct) inσ12(t)≈const.
As for the stability during the switching strategy of Fig. 7, considerations analogous to what discussed in Sect. 3.4 hold in this case too. The main differences are the following: in an ideal condition in which χ(tˆ 0) =χ(t0), one would haveE(t)≡0and, therefore, the system would start and remain in phase 3) during the whole task (by always using the full error controller (14)). If, instead, an initial (large enough) estimation error is present, the quantity E(t) would start increasing, triggering a switch to phase 1). From here on, the same behavior of the previous (non-adaptive) switching strategy is implemented with, thus, a switch to phase 2) followed by phase 3) until
