# Adaptive Hybrid Visual Servo Regulation of Mobile Robots Based on Fast Homography Decomposition

Authors: Chunfu Wu; Guodong Li; Qingshun Tang; Fengyu Zhou
Journal: Journal of Control Science and Engineering
 (2015)

---

## Abstract

For the monocular camera-based mobile robot system, an adaptive hybrid visual servo regulation algorithm which is based on a fast homography decomposition method is proposed to drive the mobile robot to its desired position and orientation, even when object’s imaging depth and camera’s position extrinsic parameters are unknown. Firstly, the homography’s particular properties caused by mobile robot’s 2-DOF motion are taken into account to induce a fast homography decomposition method. Secondly, the homography matrix and the extracted orientation error, incorporated with the desired view’s single feature point, are utilized to form an error vector and its open-loop error function. Finally, Lyapunov-based techniques are exploited to construct an adaptive regulation control law, followed by the experimental verification. The experimental results show that the proposed fast homography decomposition method is not only simple and efficient, but also highly precise. Meanwhile, the designed control law can well enable mobile robot position and orientation regulation despite the lack of depth information and camera’s position extrinsic parameters.


---

## Body

## 1. Introduction

Visual sensors have the advantage of sensing rich information at low costs. At the meantime, there have been considerable improvements in image processing and analysis techniques with the popularization of visual sensors. Therefore, using visual sensors on robots as the visual servo system, in order to significantly improve their environmental sensing ability and intelligence level, has become one of the hot topics for research in current robot community.Because robot arms are unconstrained systems, their visual servo regulation problems are relatively simple and have been well studied in previous research work. References [1–3] comprehensively reviewed the visual servo regulation algorithms of robot arms. The key point for robot arm visual servo regulation is the selection of appropriate visual characteristic set and the determination of the interaction matrix that characterizes the nonlinear mapping between the characteristic set and camera movement (also known as the image Jacobian matrix). The major difficulty is how to deal with the image’s information about depth of field missing from the interaction matrix. In order to solve this problem, Malis [4, 5] proposed a 2.5D hybrid visual servo method, where the selected visual characteristic set includes both 3D position information and 2D image information. Homography matrix is also used to process the unknown depth information in the interaction matrix. Benhimane and Malis [6] proposed a homography matrix based visual servo method, which directly uses homography matrix and desired characteristic points to establish visual characteristic sets, in order to derive the interaction matrix that does not include time varying depth information. Piepmeier et al. [7] proposed a typical quasi-Newton method based online estimation for the interaction matrix and successfully applied it to uncalibrated visual servo.Different from robot arms, mobile robot is a typical system with nonholonomic constraint. The existence of nonholonomic constraint makes the visual servo regulation of mobile robots more difficult than that for robot arms. The visual servo of mobile robots includes visual tracking and visual regulation. References [8–10] conducted in-depth research on visual tracking. However, in system stability analysis, the speed and path of the robot need to be constrained. Therefore, these methods can hardly be directly applied in visual regulation.The resolution of mobile robot visual servo regulation problem requires the combination of the visual servo regulation technique and the regulation controller design technique for nonholonomic constrained system. Fang et al. [11] studied the mobile robot visual regulation problem under the situation where the coordinate of the camera completely overlapped that of the robot. They proposed the analytical expression of the homography matrix under this specific situation and decomposed it to obtain the angular difference and imaging depth ratio between the current position and the desired position. Eventually they designed the adaptive regulation control law using the Lyapunov direct method. Based on [11], Zhang et al. [12] further studied the mobile robot visual regulation problem under the situation where only pure translation extrinsic parameter exists between the coordinates of camera and robot. First, angular difference signal between the current posing and the desired posing was extracted using the Faugeras homography decomposition method [13] or other methods [14]; then, it was combined with 2D image error signal to solve the system open-loop error function; finally, an adaptive control law that can compensate the unknown translational displacement and the desired imaging depth was designed using Lyapunov’s direct method.It is obvious that the studies in [11, 12] are both based on the conditions that the optical axis of the camera and the moving orientation of the mobile robot are coincident. They are not applicable when there are posture differences between the coordinates of the camera and the robot. Moreover, [13] and other general homography decomposition methods [15, 16] all exhibit computational complexities and nonunique decomposition results, and extra a priori knowledge is needed to eliminate the wrong decomposition results. Based on the analytical expression of homography matrix proposed in [11], [14] proposed a fast decomposition method for this type of homography matrix. However, it is only applicable to the cases where the coordinates of the camera and the robot are completely overlapped or only with translation transformations. Inspired by [6, 11, 12, 14], this research expands their work and studies the mobile robot visual servo regulation problem when the coordinates of the camera and the robot have both position and orientation transformations. A fast decomposition method for homography matrix which is applicable to a broader scope is also proposed in this work. To be specific, first, the fast homography decomposition method is used to determine the factor of the homography matrix and the angular difference between current position and the expected position; then, combined with the characteristic points of the desired view, a group of error functions are constructed, and consequently the open-loop error function of the system is obtained; finally, using Lyapunov’s direct method, the adaptive regulation control law, which is capable of simultaneously compensating for the translation extrinsic parameter and the desired imaging depth, is designed and experimentally verified.


## 2. Fast Homography Decomposition

### 2.1. Unique Properties of the Euclidean Homography Matrix

Figure1 shows the projective model of the homography matrix.Figure 1
Projective model of the homography matrix.The current and desired coordinates of the camera are represented by{c} and {c∗}, respectively. Im and Im∗ are the corresponding image planes of the two cameras, respectively. The position-orientation relationship between the two coordinates can be described as(1)Tc∗c=Rt0T1,where R∈R3×3 and t∈R3×1 are the relative orientation and translation parameters between {c} and {c∗}, respectively. Let the distances between the origins o and o∗ of {c} and {c∗} and the target plane π be d and d∗, respectively, and let the normal direction of 3D scene plane π in {c} and {c∗} be nc and nd, respectively; then π can be written as πc=[nTc,d]T and πd=nTd,d∗T in {c} and {c∗}, respectively. Let the 3D point P on π in {c} and {c∗} be Pc=[xc,yc,zc]T and Pd=[xd,yd,zd]T, respectively; then mc and md, the normalized projective coordinates of P on the image planes Im, Im∗, are(2)mc=xczcyczc1T=K-1uv1T,md=xdzdydzd1T=K-1u∗v∗1T,where K is the camera intrinsic matrix, u and v are the horizontal and vertical pixel coordinates of point P in camera {c}, and u∗and v∗are the horizontal and vertical pixel coordinates of point P in camera {c∗}. Then, Euclidean homography H satisfying the following equation,(3)σmc=zczdmc=Hmd,can be expressed as (4)H=R-t·nTdd∗=R-t′·nTd,where t′2=t/d∗2=1. According to (4), Euclidean homography matrix H is a rank-1 modified matrix. Its determinant can be written as (5)det⁡H=det⁡I3×3-t·nTcd∗R=1-nTc·td∗=dd∗=r.Property 1 can be derived from (5).Property 1.
The value of the Euclidean homography matrix determinant equals the ratio between the distances from the main points of current camera and expected camera to the scene plane. The ratio must be greater than zero.LetRk,θ be a rotation matrix resulting from rotating θ around unit vector k; the general formula of this rotation transformation is (6)Rk,θ=cos⁡θI3×3+sin⁡θk×+1-cos⁡θkkT. Based on (6), the following equations hold:(7)Rk,θT=R-k,θ=Rk,-θ,(8)Rk,θk=Rk,θTk=k.Figure2 shows the coordinates of the mobile robot {E} and the camera {c}.Figure 2
The mobile robot frame and the camera frame.Let the position-orientation relationship between{E} and {c} be TcE=[RcE∣tcE], where tcE=[tx,ty,0]T. If after movement d=dxe1=[dx,0,0]T, θ=θEe3=[0,0,θE]T, and the robot arrives at position-orientation [Re3,θz∣d] from original position-orientation [I∣0], then the position-orientation relationship of the camera before and after the movement is Tcc~=Rcc~∣tcc~, where(9)Rcc~=RcTERe3,θzTRcE=RRcTEe3,-θz,(10)tcc~=-RcTERe3,θzTRe3,θztcE+d-tcE.Equation (9) is derived from (7) and the Nanson formula At×=A-Tt×A-1.Writing(11)k=RcTEe3, then, with the calibrated orientation extrinsic parameter RcE of the camera, (10) can be used to solve for the a priori solution k^ of the onboard camera rotation axis. Obviously, when the optical axis is perpendicular to the rotation axis of the robot, k^=-e2=0-10T. According to (10) and (11),(12)kTtcc~=0,where equations e3Td=0 and e3TtcE=0 are used. Property 2 can be derived from (9) and (12).Property 2.
The corresponding rotation angleθc of the rotation matrix R decomposed from the onboard homography Hs is of the same size but opposite direction of the rotation angle θE of the robot. In addition, the corresponding rotation axis k of R and the decomposed translational displacement t are mutually orthogonal.According to (4), (8), and (12), the onboard homography Hs satisfies the following equation: (13)HsTk=k.Property 3 can be further derived from (13).Property 3.
The corresponding rotation axisk of the rotation matrix R decomposed from the onboard homography Hs is equal to the corresponding unit characteristic vector of the real eigenvalue HsT at unity.


### 2.2. Fast Decomposition

#### 2.2.1. Calculation of Rotation Axisk

After the calibration of the onboard camera extrinsic parameter, (11) can be first used to solve for the a priori rotation axis k^. Then, with the known normalized projective coordinates of the four groups of coplanar matching points, direct linear transformation (DLT) can be used to solve for the homography H′ that satisfies the constraints H′F=1 and detH′>0. After that, the real eigenvalue βi of H′T and its corresponding unit characteristic vector xi can be obtained:(14)H′Txi=βixi,i=1,2,3.So, the constant axis of the rotation matrix is(15)k=sgn⁡xTk^x,x=argmaxi=1,2,3⁡xiTk^,where sgn(x) is used to take the sign of the term. After k is obtained, the Euclidean homography matrix H is(16)H=1H′Tk2H′.At this point, both the homography matrixH and the rotation axis k of the rotation matrix R are obtained. It is noteworthy to note that k is a constant in most of the cases, which only needs to be solved for once.


#### 2.2.2. Calculation of Rotation Angleθ

As shown in Figure2, when the optical axis of the camera is perpendicular to the rotation axis of the mobile robot and the angle between the image plane vertical axis and the robot rotation axis is zero, with the coordinate {c} of the camera being the reference frame, its rotation axis can be written as -e2. In this case, the homography matrix is in a special form and can be decomposed more easily. On this basis, this section provides a new construct method to solve for rotation angle.First, construct the rotation matrixRk1,θ1, where(17)k1=k×-e2k×-e22,θ1=arccos⁡-kTe2;then equation Rk1,θ1k=-e2 must hold. Left multiply by Rk1,θ1 and right multiply by Rk1,θ1T on both sides of (4) to get(18)Rk1,θ1HRk1,θ1T=Rk1,θ1Rk,θRk1,θ1T-Rk1,θ1t′·Rk1,θ1ndT. Let(19)H~=Rk1,θ1HRk1,θ1T,R~=Rk1,θ1Rk,θRk1,θ1T=R~Rk1,θ1k,θ=R~-e2,θ=R~e2,-θ,t~=Rk1,θ1t′=t~1t~2t~3T,n~=Rk1,θ1nd=n~1n~2n~3T.Because k and t′ are orthogonal, so Rk1,θ1k=-e2 and Rk1,θ1t′ are also orthogonal; that is, the component t~2=0 in t~. Let the element in the ith row and jth column of H~ be written as h~ij; then (18) can be written as(20)cθ-t~1n~1-t~1n~2-sθ-t~1n~3010sθ-t~3n~1-t~3n~2cθ-t~3n~3=h~11h~12h~13h~21h~22h~23h~31h~32h~33. Let the elements on both sides of the equality be equal and rearrange to get(21)t~12+t~32=h~122+h~322+h~11-h~332+h~31+h~132,(22)t~12=cθ-h~112+sθ-h~132+h~122,(23)t~32=cθ-h~332+sθ+h~312+h~322,(24)h~322t~12-h~122t~32=0. According to (21) and (23),(25)psin⁡θ+qcos⁡θ=r,where (26)p=h~31-h~13,q=h~11+h~33,r=1+h~11h~33-h~13h~31. Let(27)ψ=p2+q2;then two groups of possible solutions for θ can be arrived at:(28)θ=atan⁡2rψ,±1-r2ψ2-atan⁡2q,p.Whenh~12 or h~32 in (24) is not zero, substitute (28) into (22)-(23) to get t~12 and t~32 and substitute them back into (24) for verification, so as to determine the unique angle solution. When h~12=h~32=0, it is impossible to determine the unique solution directly from (25). A priori knowledge, such as movement continuity or scene plane normal, needs to be used to eliminate false solutions.When elementsh~12=h~32=0 in the homography matrix H~, it means that the component of n~2=0 in n~. This corresponds to the situation where the scene plane is perpendicular to the floor.


#### 2.2.3. Calculation of Translationt′ and Scene Plane Normal nd

After determining the rotation angleθ of the rotation matrix, substitute it into (20) to get (29)a1T0Ta3T=R~-H~=t~1n~T0t~3n~T. We can obtain (30)n~=±a1a12. Right multiply by n~ on both sides of (29) to get (31)t~=R~-H~n~. Equations (30) and (31) can be used to calculate n~ and t~; then R, t′, and nd in (4) can be solved from (19): (32)R=Rk1,θ1TR~e2,-θRk1,θ1t′=Rk1,θ1Tt~nd=Rk1,θ1Tn~.According to Figure 1, the projection of the expected image plane point md on the scene plane normal nd must be smaller than zero, which introduces the constraint (33)mTd·nd<0;that is, when h~12 or h~32 in (24) is not zero, a unique homography decomposition solution can be obtained. When h~12=h~32=0, two groups of possible homography decomposition solutions can be obtained; in this case, the a priori information of nd needs to be used to eliminate the error solution.According to the homography decomposition process described in this section, this decomposition method can avoid the singular value decomposition calculation of the matrix. At the meantime, it also avoids solving for cubic equation with one unknown when determining the scale factor betweenH′ and H. Therefore, it is a fairly good homography matrix decomposition method that is easy to program and operate.


## 2.1. Unique Properties of the Euclidean Homography Matrix

Figure1 shows the projective model of the homography matrix.Figure 1
Projective model of the homography matrix.The current and desired coordinates of the camera are represented by{c} and {c∗}, respectively. Im and Im∗ are the corresponding image planes of the two cameras, respectively. The position-orientation relationship between the two coordinates can be described as(1)Tc∗c=Rt0T1,where R∈R3×3 and t∈R3×1 are the relative orientation and translation parameters between {c} and {c∗}, respectively. Let the distances between the origins o and o∗ of {c} and {c∗} and the target plane π be d and d∗, respectively, and let the normal direction of 3D scene plane π in {c} and {c∗} be nc and nd, respectively; then π can be written as πc=[nTc,d]T and πd=nTd,d∗T in {c} and {c∗}, respectively. Let the 3D point P on π in {c} and {c∗} be Pc=[xc,yc,zc]T and Pd=[xd,yd,zd]T, respectively; then mc and md, the normalized projective coordinates of P on the image planes Im, Im∗, are(2)mc=xczcyczc1T=K-1uv1T,md=xdzdydzd1T=K-1u∗v∗1T,where K is the camera intrinsic matrix, u and v are the horizontal and vertical pixel coordinates of point P in camera {c}, and u∗and v∗are the horizontal and vertical pixel coordinates of point P in camera {c∗}. Then, Euclidean homography H satisfying the following equation,(3)σmc=zczdmc=Hmd,can be expressed as (4)H=R-t·nTdd∗=R-t′·nTd,where t′2=t/d∗2=1. According to (4), Euclidean homography matrix H is a rank-1 modified matrix. Its determinant can be written as (5)det⁡H=det⁡I3×3-t·nTcd∗R=1-nTc·td∗=dd∗=r.Property 1 can be derived from (5).Property 1.
The value of the Euclidean homography matrix determinant equals the ratio between the distances from the main points of current camera and expected camera to the scene plane. The ratio must be greater than zero.LetRk,θ be a rotation matrix resulting from rotating θ around unit vector k; the general formula of this rotation transformation is (6)Rk,θ=cos⁡θI3×3+sin⁡θk×+1-cos⁡θkkT. Based on (6), the following equations hold:(7)Rk,θT=R-k,θ=Rk,-θ,(8)Rk,θk=Rk,θTk=k.Figure2 shows the coordinates of the mobile robot {E} and the camera {c}.Figure 2
The mobile robot frame and the camera frame.Let the position-orientation relationship between{E} and {c} be TcE=[RcE∣tcE], where tcE=[tx,ty,0]T. If after movement d=dxe1=[dx,0,0]T, θ=θEe3=[0,0,θE]T, and the robot arrives at position-orientation [Re3,θz∣d] from original position-orientation [I∣0], then the position-orientation relationship of the camera before and after the movement is Tcc~=Rcc~∣tcc~, where(9)Rcc~=RcTERe3,θzTRcE=RRcTEe3,-θz,(10)tcc~=-RcTERe3,θzTRe3,θztcE+d-tcE.Equation (9) is derived from (7) and the Nanson formula At×=A-Tt×A-1.Writing(11)k=RcTEe3, then, with the calibrated orientation extrinsic parameter RcE of the camera, (10) can be used to solve for the a priori solution k^ of the onboard camera rotation axis. Obviously, when the optical axis is perpendicular to the rotation axis of the robot, k^=-e2=0-10T. According to (10) and (11),(12)kTtcc~=0,where equations e3Td=0 and e3TtcE=0 are used. Property 2 can be derived from (9) and (12).Property 2.
The corresponding rotation angleθc of the rotation matrix R decomposed from the onboard homography Hs is of the same size but opposite direction of the rotation angle θE of the robot. In addition, the corresponding rotation axis k of R and the decomposed translational displacement t are mutually orthogonal.According to (4), (8), and (12), the onboard homography Hs satisfies the following equation: (13)HsTk=k.Property 3 can be further derived from (13).Property 3.
The corresponding rotation axisk of the rotation matrix R decomposed from the onboard homography Hs is equal to the corresponding unit characteristic vector of the real eigenvalue HsT at unity.


## 2.2. Fast Decomposition

### 2.2.1. Calculation of Rotation Axisk

After the calibration of the onboard camera extrinsic parameter, (11) can be first used to solve for the a priori rotation axis k^. Then, with the known normalized projective coordinates of the four groups of coplanar matching points, direct linear transformation (DLT) can be used to solve for the homography H′ that satisfies the constraints H′F=1 and detH′>0. After that, the real eigenvalue βi of H′T and its corresponding unit characteristic vector xi can be obtained:(14)H′Txi=βixi,i=1,2,3.So, the constant axis of the rotation matrix is(15)k=sgn⁡xTk^x,x=argmaxi=1,2,3⁡xiTk^,where sgn(x) is used to take the sign of the term. After k is obtained, the Euclidean homography matrix H is(16)H=1H′Tk2H′.At this point, both the homography matrixH and the rotation axis k of the rotation matrix R are obtained. It is noteworthy to note that k is a constant in most of the cases, which only needs to be solved for once.


### 2.2.2. Calculation of Rotation Angleθ

As shown in Figure2, when the optical axis of the camera is perpendicular to the rotation axis of the mobile robot and the angle between the image plane vertical axis and the robot rotation axis is zero, with the coordinate {c} of the camera being the reference frame, its rotation axis can be written as -e2. In this case, the homography matrix is in a special form and can be decomposed more easily. On this basis, this section provides a new construct method to solve for rotation angle.First, construct the rotation matrixRk1,θ1, where(17)k1=k×-e2k×-e22,θ1=arccos⁡-kTe2;then equation Rk1,θ1k=-e2 must hold. Left multiply by Rk1,θ1 and right multiply by Rk1,θ1T on both sides of (4) to get(18)Rk1,θ1HRk1,θ1T=Rk1,θ1Rk,θRk1,θ1T-Rk1,θ1t′·Rk1,θ1ndT. Let(19)H~=Rk1,θ1HRk1,θ1T,R~=Rk1,θ1Rk,θRk1,θ1T=R~Rk1,θ1k,θ=R~-e2,θ=R~e2,-θ,t~=Rk1,θ1t′=t~1t~2t~3T,n~=Rk1,θ1nd=n~1n~2n~3T.Because k and t′ are orthogonal, so Rk1,θ1k=-e2 and Rk1,θ1t′ are also orthogonal; that is, the component t~2=0 in t~. Let the element in the ith row and jth column of H~ be written as h~ij; then (18) can be written as(20)cθ-t~1n~1-t~1n~2-sθ-t~1n~3010sθ-t~3n~1-t~3n~2cθ-t~3n~3=h~11h~12h~13h~21h~22h~23h~31h~32h~33. Let the elements on both sides of the equality be equal and rearrange to get(21)t~12+t~32=h~122+h~322+h~11-h~332+h~31+h~132,(22)t~12=cθ-h~112+sθ-h~132+h~122,(23)t~32=cθ-h~332+sθ+h~312+h~322,(24)h~322t~12-h~122t~32=0. According to (21) and (23),(25)psin⁡θ+qcos⁡θ=r,where (26)p=h~31-h~13,q=h~11+h~33,r=1+h~11h~33-h~13h~31. Let(27)ψ=p2+q2;then two groups of possible solutions for θ can be arrived at:(28)θ=atan⁡2rψ,±1-r2ψ2-atan⁡2q,p.Whenh~12 or h~32 in (24) is not zero, substitute (28) into (22)-(23) to get t~12 and t~32 and substitute them back into (24) for verification, so as to determine the unique angle solution. When h~12=h~32=0, it is impossible to determine the unique solution directly from (25). A priori knowledge, such as movement continuity or scene plane normal, needs to be used to eliminate false solutions.When elementsh~12=h~32=0 in the homography matrix H~, it means that the component of n~2=0 in n~. This corresponds to the situation where the scene plane is perpendicular to the floor.


### 2.2.3. Calculation of Translationt′ and Scene Plane Normal nd

After determining the rotation angleθ of the rotation matrix, substitute it into (20) to get (29)a1T0Ta3T=R~-H~=t~1n~T0t~3n~T. We can obtain (30)n~=±a1a12. Right multiply by n~ on both sides of (29) to get (31)t~=R~-H~n~. Equations (30) and (31) can be used to calculate n~ and t~; then R, t′, and nd in (4) can be solved from (19): (32)R=Rk1,θ1TR~e2,-θRk1,θ1t′=Rk1,θ1Tt~nd=Rk1,θ1Tn~.According to Figure 1, the projection of the expected image plane point md on the scene plane normal nd must be smaller than zero, which introduces the constraint (33)mTd·nd<0;that is, when h~12 or h~32 in (24) is not zero, a unique homography decomposition solution can be obtained. When h~12=h~32=0, two groups of possible homography decomposition solutions can be obtained; in this case, the a priori information of nd needs to be used to eliminate the error solution.According to the homography decomposition process described in this section, this decomposition method can avoid the singular value decomposition calculation of the matrix. At the meantime, it also avoids solving for cubic equation with one unknown when determining the scale factor betweenH′ and H. Therefore, it is a fairly good homography matrix decomposition method that is easy to program and operate.


## 2.2.1. Calculation of Rotation Axisk

After the calibration of the onboard camera extrinsic parameter, (11) can be first used to solve for the a priori rotation axis k^. Then, with the known normalized projective coordinates of the four groups of coplanar matching points, direct linear transformation (DLT) can be used to solve for the homography H′ that satisfies the constraints H′F=1 and detH′>0. After that, the real eigenvalue βi of H′T and its corresponding unit characteristic vector xi can be obtained:(14)H′Txi=βixi,i=1,2,3.So, the constant axis of the rotation matrix is(15)k=sgn⁡xTk^x,x=argmaxi=1,2,3⁡xiTk^,where sgn(x) is used to take the sign of the term. After k is obtained, the Euclidean homography matrix H is(16)H=1H′Tk2H′.At this point, both the homography matrixH and the rotation axis k of the rotation matrix R are obtained. It is noteworthy to note that k is a constant in most of the cases, which only needs to be solved for once.


## 2.2.2. Calculation of Rotation Angleθ

As shown in Figure2, when the optical axis of the camera is perpendicular to the rotation axis of the mobile robot and the angle between the image plane vertical axis and the robot rotation axis is zero, with the coordinate {c} of the camera being the reference frame, its rotation axis can be written as -e2. In this case, the homography matrix is in a special form and can be decomposed more easily. On this basis, this section provides a new construct method to solve for rotation angle.First, construct the rotation matrixRk1,θ1, where(17)k1=k×-e2k×-e22,θ1=arccos⁡-kTe2;then equation Rk1,θ1k=-e2 must hold. Left multiply by Rk1,θ1 and right multiply by Rk1,θ1T on both sides of (4) to get(18)Rk1,θ1HRk1,θ1T=Rk1,θ1Rk,θRk1,θ1T-Rk1,θ1t′·Rk1,θ1ndT. Let(19)H~=Rk1,θ1HRk1,θ1T,R~=Rk1,θ1Rk,θRk1,θ1T=R~Rk1,θ1k,θ=R~-e2,θ=R~e2,-θ,t~=Rk1,θ1t′=t~1t~2t~3T,n~=Rk1,θ1nd=n~1n~2n~3T.Because k and t′ are orthogonal, so Rk1,θ1k=-e2 and Rk1,θ1t′ are also orthogonal; that is, the component t~2=0 in t~. Let the element in the ith row and jth column of H~ be written as h~ij; then (18) can be written as(20)cθ-t~1n~1-t~1n~2-sθ-t~1n~3010sθ-t~3n~1-t~3n~2cθ-t~3n~3=h~11h~12h~13h~21h~22h~23h~31h~32h~33. Let the elements on both sides of the equality be equal and rearrange to get(21)t~12+t~32=h~122+h~322+h~11-h~332+h~31+h~132,(22)t~12=cθ-h~112+sθ-h~132+h~122,(23)t~32=cθ-h~332+sθ+h~312+h~322,(24)h~322t~12-h~122t~32=0. According to (21) and (23),(25)psin⁡θ+qcos⁡θ=r,where (26)p=h~31-h~13,q=h~11+h~33,r=1+h~11h~33-h~13h~31. Let(27)ψ=p2+q2;then two groups of possible solutions for θ can be arrived at:(28)θ=atan⁡2rψ,±1-r2ψ2-atan⁡2q,p.Whenh~12 or h~32 in (24) is not zero, substitute (28) into (22)-(23) to get t~12 and t~32 and substitute them back into (24) for verification, so as to determine the unique angle solution. When h~12=h~32=0, it is impossible to determine the unique solution directly from (25). A priori knowledge, such as movement continuity or scene plane normal, needs to be used to eliminate false solutions.When elementsh~12=h~32=0 in the homography matrix H~, it means that the component of n~2=0 in n~. This corresponds to the situation where the scene plane is perpendicular to the floor.


## 2.2.3. Calculation of Translationt′ and Scene Plane Normal nd

After determining the rotation angleθ of the rotation matrix, substitute it into (20) to get (29)a1T0Ta3T=R~-H~=t~1n~T0t~3n~T. We can obtain (30)n~=±a1a12. Right multiply by n~ on both sides of (29) to get (31)t~=R~-H~n~. Equations (30) and (31) can be used to calculate n~ and t~; then R, t′, and nd in (4) can be solved from (19): (32)R=Rk1,θ1TR~e2,-θRk1,θ1t′=Rk1,θ1Tt~nd=Rk1,θ1Tn~.According to Figure 1, the projection of the expected image plane point md on the scene plane normal nd must be smaller than zero, which introduces the constraint (33)mTd·nd<0;that is, when h~12 or h~32 in (24) is not zero, a unique homography decomposition solution can be obtained. When h~12=h~32=0, two groups of possible homography decomposition solutions can be obtained; in this case, the a priori information of nd needs to be used to eliminate the error solution.According to the homography decomposition process described in this section, this decomposition method can avoid the singular value decomposition calculation of the matrix. At the meantime, it also avoids solving for cubic equation with one unknown when determining the scale factor betweenH′ and H. Therefore, it is a fairly good homography matrix decomposition method that is easy to program and operate.


## 3. Controller Design

As shown in Figure3, the main purpose of monocular mobile robot visual servo regulation control is to construct an appropriate visual characteristic set from the image signals provided by visual sensors, through characteristic extraction and position-orientation estimation. Then, construct error signal and consequently solve for the open-loop error fuction of the system. Finally, design appropriate adaptive control law to compensate for the translation extrinsic parameter and the expected imaging depth information of the target, while controlling the mobile robot to move from current position-orientation {E} to the desired {E∗}. Its main steps include image characteristic extraction and visual position-orientation estimation, visual characteristic set construction and open-loop error function solution, and adaptive control law design.Figure 3
Visual servo regulation of the mobile robot with a monocular camera.


### 3.1. Problem Formulation

According to (3),(34)Pc-Pdzd=H-I3×3md;therefore, the image error signal can be defined as(35)ev=exeyezT=RcEH-I3×3md=Pc′zd-RcEmd,where Pc′ is the coordinate of the spacial 3D point P in {c′} coordinate frame. The origin of coordinate frame {c′} overlaps with that of the onboard camera coordinate frame {c}; its x,  y,   and z axes are parallel with the x,  y,  and z axes of the robot coordinate frame {E}, respectively.The linear and angular velocities of the coordinate system{c′} resulting from movement VE=vxωzT of the robot can be easily solved using rigid body kinematics: (36)Vc′=0-ωz0ωz00000tcE+vx00=vx-tyωztxωz0,(37)ωc′=00ωzT.The linear velocity of the 3D point Pc′ resulting from the coordinate system {c′} movement described by (36) and (37) is (38)P˙c′=-Vc′-ωc′×Pc′. Substitute (35)–(37) into (38) and rearrange to have(39)e˙v=e˙xe˙ye˙z=P˙c′zd=-1zdvx+ey+a+bzdωz-ex+c+dzdωz0,where (40)a=RcEmdy,c=RcEmdx,b=ty,d=tx.·x,·y in (40) are the x,y components of the vector, respectively.Define the orientation error signal as(41)eo=θ,where θ is the angle solved from (20). According to (9), the mapping relationship between its rate of change and the rotation angular velocity of the robot is(42)e˙o=-ωz.Let(43)α=zd,r1=-e0,r2=ey,r3=ex;then (39) and (42) can be finally written as(44)r˙1=ωz,αr˙2=-αr3+cωz-dωz,αr˙3=-vx+αr2+aωz+bωz.Equation (44) is the open-loop error function of the system. It is easy to see that, when the constant terms a,  b,  c, and d are zero, this becomes the error function described in [11]; when a, and c are zero, this becomes the error function described in [12]. Therefore, this open-loop error function is more generalized.


### 3.2. Adaptive Controller Design

Whenr1,  r2,  and r3 in the error function (44) are zero, the current {E} and expected {E∗} coordinates of the mobile robot coincide, which means the completion of the regulation control of the robot. Therefore, this section will design the adaptive regulation control law to compensate for the unknown depth and camera translation extrinsic parameter, as well as controlling the mobile robot to realize asymptotic regulation.Reference [11] provides a classic controller design method for nonholonomic constraint system. Inspired by this method, the auxiliary signal is first designed: (45)η=r3+r2cos⁡t. Let(46)ξ=αdbT,(47)Y=r2+a-ccos⁡tωz-r2sin⁡t-ωzcos⁡tωzT;then the differential of η can be calculated according to (44): (48)αη˙=-vx+YTξ-αr3ωzcos⁡t. The control input vx can be written in the following format: (49)vx=k2η+YTξ^.In (49), k2 is a positive real number gain and ξ^ is an estimate of ξ. The designed auxiliary signal is (50)χ=r3r2+ηcos⁡t+ρ^r2+cr2.In (50), ρ^ is an estimation for ρ=d/α. Then, the control input ωz can be written in the following format: (51)ωz=-k1r1-χ.Substituting the control inputs in (49) and (51) into (44) and (48), we can acquire the close-loop error function: (52)αr˙1=-αk1r1-χ,αr˙2=αk1r3+c+ρ^r1-χ-αρ~ωz,αη˙=-k2η+YTξ~+αk1r3r1-χcos⁡t. In (52), ξ~=ξ-ξ^, ρ~=ρ-ρ^.In order to get the dynamic estimates of parametersρ^ and ξ^, define Lyapunov function as (53)V=12αr12+r22+12αη2+12ξ~TΓ1-1ξ~+12αΓ2-1ρ~2. Take its derivative, substitute it into (52), and rearrange to get(54)V˙=-αk1r1r1-χ+αk1r2r3+c+ρ^r1-χ-αρ~r2ωz+η-k2η-YTξ~+αk1r3cos⁡tr1-χ-ξ~TΓ1-1ξ^˙-αρ~Γ2-1ρ^˙,so their dynamic estimates are(55)ξ^˙=Γ1Yη,ρ^˙=Γ2r2ωz. Substitute (55) into (54) to obtain(56)V˙=-αk1r1-χ2-k2η2≤0.


### 3.3. Stability Analysis

According to (53) and (56), r1,η,r3∈L∞; ξ~,ρ~∈L∞. Also according to (47), (49), (50), and (51), vx,ωz,χ,Y∈L∞. Finally, according to (44), (50), and (55), r˙1,r˙2,r˙3,ξ^˙,ρ^˙,χ˙∈L∞.Write functionf(t) as(57)f=αk1r1-χ2+k2r3+r2cos⁡t2≥0.Take its derivative to get(58)f˙=2αk1r1-χr˙1-χ˙+2k2r3+r2cos⁡tr˙3-r2sin⁡t+r˙2cos⁡t∈L∞;therefore, according to the inference of Barbara theorem, (59)limt→∞⁡f,η,r1-χ=0. Consequently, according to (52), (49), (51), and (55), (60)limt→∞⁡r˙1,r˙3,ωz=0,(61)limt→∞⁡ξ^˙,ρ^˙=0.According to (52),(62)αη˙=-k2η-αr3cos⁡tωz+α~r2+a-ccos⁡tωz-b~ωzcos⁡t+d~ωz+α~r2sin⁡t=g·+α~r2sin⁡t,combined with (59) and (60) to get(63)limt→∞⁡g·=0.Because α~,r2 are uniformly continuous and limt→∞⁡η=0, according to the extended Barbara theorem, (64)limt→∞⁡η˙,α~r2=0. Take derivative for r1-χ and rearrange to get(65)ddtr1-χ=ϑt-r22sin⁡t,where (66)ϑt=r3sin⁡t-r˙3cos⁡t+k2r2αη+ωz-r˙2r3+ρ^-c-ρ^˙r2-r3η˙cos⁡t+1αr22α~sin⁡t-1αr2ωzb^cos⁡t+d~+r2+a-ccos⁡tα~+αccos⁡t. Combining (59), (60), and (64), it can be derived from (66) that (67)limt→∞⁡ϑt=0.Because r22sin⁡t is uniformly continuous and r1-χ is bounded, according to (67) and the extended Barbara theorem,(68)limt→∞⁡r22sin⁡t=0,(69)limt→∞⁡r2=0. According to (50), (59), and (69), (70)limt→∞⁡r1,χ,η=0. Finally, according to (45), (69), and (70), (71)limt→∞⁡r3=0.From (69)–(71), it can be eventually derived that, under the effect of designed adaptive regulation control law, the robot can asymptotically converge to the desired position-orientation.


## 3.1. Problem Formulation

According to (3),(34)Pc-Pdzd=H-I3×3md;therefore, the image error signal can be defined as(35)ev=exeyezT=RcEH-I3×3md=Pc′zd-RcEmd,where Pc′ is the coordinate of the spacial 3D point P in {c′} coordinate frame. The origin of coordinate frame {c′} overlaps with that of the onboard camera coordinate frame {c}; its x,  y,   and z axes are parallel with the x,  y,  and z axes of the robot coordinate frame {E}, respectively.The linear and angular velocities of the coordinate system{c′} resulting from movement VE=vxωzT of the robot can be easily solved using rigid body kinematics: (36)Vc′=0-ωz0ωz00000tcE+vx00=vx-tyωztxωz0,(37)ωc′=00ωzT.The linear velocity of the 3D point Pc′ resulting from the coordinate system {c′} movement described by (36) and (37) is (38)P˙c′=-Vc′-ωc′×Pc′. Substitute (35)–(37) into (38) and rearrange to have(39)e˙v=e˙xe˙ye˙z=P˙c′zd=-1zdvx+ey+a+bzdωz-ex+c+dzdωz0,where (40)a=RcEmdy,c=RcEmdx,b=ty,d=tx.·x,·y in (40) are the x,y components of the vector, respectively.Define the orientation error signal as(41)eo=θ,where θ is the angle solved from (20). According to (9), the mapping relationship between its rate of change and the rotation angular velocity of the robot is(42)e˙o=-ωz.Let(43)α=zd,r1=-e0,r2=ey,r3=ex;then (39) and (42) can be finally written as(44)r˙1=ωz,αr˙2=-αr3+cωz-dωz,αr˙3=-vx+αr2+aωz+bωz.Equation (44) is the open-loop error function of the system. It is easy to see that, when the constant terms a,  b,  c, and d are zero, this becomes the error function described in [11]; when a, and c are zero, this becomes the error function described in [12]. Therefore, this open-loop error function is more generalized.


## 3.2. Adaptive Controller Design

Whenr1,  r2,  and r3 in the error function (44) are zero, the current {E} and expected {E∗} coordinates of the mobile robot coincide, which means the completion of the regulation control of the robot. Therefore, this section will design the adaptive regulation control law to compensate for the unknown depth and camera translation extrinsic parameter, as well as controlling the mobile robot to realize asymptotic regulation.Reference [11] provides a classic controller design method for nonholonomic constraint system. Inspired by this method, the auxiliary signal is first designed: (45)η=r3+r2cos⁡t. Let(46)ξ=αdbT,(47)Y=r2+a-ccos⁡tωz-r2sin⁡t-ωzcos⁡tωzT;then the differential of η can be calculated according to (44): (48)αη˙=-vx+YTξ-αr3ωzcos⁡t. The control input vx can be written in the following format: (49)vx=k2η+YTξ^.In (49), k2 is a positive real number gain and ξ^ is an estimate of ξ. The designed auxiliary signal is (50)χ=r3r2+ηcos⁡t+ρ^r2+cr2.In (50), ρ^ is an estimation for ρ=d/α. Then, the control input ωz can be written in the following format: (51)ωz=-k1r1-χ.Substituting the control inputs in (49) and (51) into (44) and (48), we can acquire the close-loop error function: (52)αr˙1=-αk1r1-χ,αr˙2=αk1r3+c+ρ^r1-χ-αρ~ωz,αη˙=-k2η+YTξ~+αk1r3r1-χcos⁡t. In (52), ξ~=ξ-ξ^, ρ~=ρ-ρ^.In order to get the dynamic estimates of parametersρ^ and ξ^, define Lyapunov function as (53)V=12αr12+r22+12αη2+12ξ~TΓ1-1ξ~+12αΓ2-1ρ~2. Take its derivative, substitute it into (52), and rearrange to get(54)V˙=-αk1r1r1-χ+αk1r2r3+c+ρ^r1-χ-αρ~r2ωz+η-k2η-YTξ~+αk1r3cos⁡tr1-χ-ξ~TΓ1-1ξ^˙-αρ~Γ2-1ρ^˙,so their dynamic estimates are(55)ξ^˙=Γ1Yη,ρ^˙=Γ2r2ωz. Substitute (55) into (54) to obtain(56)V˙=-αk1r1-χ2-k2η2≤0.


## 3.3. Stability Analysis

According to (53) and (56), r1,η,r3∈L∞; ξ~,ρ~∈L∞. Also according to (47), (49), (50), and (51), vx,ωz,χ,Y∈L∞. Finally, according to (44), (50), and (55), r˙1,r˙2,r˙3,ξ^˙,ρ^˙,χ˙∈L∞.Write functionf(t) as(57)f=αk1r1-χ2+k2r3+r2cos⁡t2≥0.Take its derivative to get(58)f˙=2αk1r1-χr˙1-χ˙+2k2r3+r2cos⁡tr˙3-r2sin⁡t+r˙2cos⁡t∈L∞;therefore, according to the inference of Barbara theorem, (59)limt→∞⁡f,η,r1-χ=0. Consequently, according to (52), (49), (51), and (55), (60)limt→∞⁡r˙1,r˙3,ωz=0,(61)limt→∞⁡ξ^˙,ρ^˙=0.According to (52),(62)αη˙=-k2η-αr3cos⁡tωz+α~r2+a-ccos⁡tωz-b~ωzcos⁡t+d~ωz+α~r2sin⁡t=g·+α~r2sin⁡t,combined with (59) and (60) to get(63)limt→∞⁡g·=0.Because α~,r2 are uniformly continuous and limt→∞⁡η=0, according to the extended Barbara theorem, (64)limt→∞⁡η˙,α~r2=0. Take derivative for r1-χ and rearrange to get(65)ddtr1-χ=ϑt-r22sin⁡t,where (66)ϑt=r3sin⁡t-r˙3cos⁡t+k2r2αη+ωz-r˙2r3+ρ^-c-ρ^˙r2-r3η˙cos⁡t+1αr22α~sin⁡t-1αr2ωzb^cos⁡t+d~+r2+a-ccos⁡tα~+αccos⁡t. Combining (59), (60), and (64), it can be derived from (66) that (67)limt→∞⁡ϑt=0.Because r22sin⁡t is uniformly continuous and r1-χ is bounded, according to (67) and the extended Barbara theorem,(68)limt→∞⁡r22sin⁡t=0,(69)limt→∞⁡r2=0. According to (50), (59), and (69), (70)limt→∞⁡r1,χ,η=0. Finally, according to (45), (69), and (70), (71)limt→∞⁡r3=0.From (69)–(71), it can be eventually derived that, under the effect of designed adaptive regulation control law, the robot can asymptotically converge to the desired position-orientation.


## 4. Experiments and Analysis

Let the position-orientation relationship between the current{E} and the desired {E∗} coordinate systems of the robot be (72)TE∗E=trans⁡2,1.12,0·rot⁡z-36∘,let the extrinsic parameter between the mobile robot and the camera be (73)TcE=trans⁡0.15,0.03,0·rot⁡k0.5702,-0.6008,0.5603T,56∘,and let the scene plane in the expected camera system {c∗} be (74)nd=0.05230.0697-0.9962T,d∗=2.2361.Then the images of the square on the scene plane in{c} and {c∗} are shown in Figure 4.Figure 4
The current image and the desired image.
(a)
(b)


### 4.1. Fast Decomposition Experiment of the Homography Matrix

Apply different levels (0–2 pixels) of Gaussian noise on the tops of the two images, respectively. For each level of the noise, randomly select 1000 groups of the noise to be applied on the top of the image. Use DLT to estimate the homography matrix and decompose the matrix with traditional SVD-based decomposition method [13] and the proposed fast decomposition algorithm, respectively. The decomposition results are as follows: (75)θsvdi,tsvd′i,nsvdid,θfasti,tfast′i,nfastid,i=1,…,1000. Let the error between the above results and the real values θ,t′,nd be (76)eθsvdi=θsvdi-θ,etsvd′i=acos⁡t′T·tsvd′iensvdid=acos⁡nTd·nsvdid,eθfasti=θfasti-θ,etfast′i=acos⁡t′T·tfast′ienfastid=acos⁡nTd·nfastid.Taking the average value of the 1000 groups of errors, we can have the error maps in Figure5.Figure 5
Comparison of the traditional algorithm and the proposed algorithm with regard to different pixel noise level.
(a)
(b)
(c)According to Figure5(a), the rotation angles calculated from the two methods have extremely close accuracy, with individual cases where the proposed method has even better results than the traditional one. On the other hand, from Figures 5(b) and 5(c), it can be seen that the accuracies of the camera translation and the scene plane normal are obviously higher than those of the traditional algorithm. Therefore, this algorithm effectively avoids the matrix SVD calculation. It requires less operation and is easy for programming, with a higher decomposition accuracy.


### 4.2. Experiment of Adaptive Hybrid Visual Servo Regulation for Mobile Robot

In order to verify the performance of the proposed algorithm, the mobile robot system was simulated in a MATLAB environment. From the simulation results, it can be seen that the proposed algorithm is capable of making the robot gradually converge to the desired location and has good control performance.The gain settings of the controller and adaptive parameter update are(77)k1=0.25,k2=1Γ1=diag⁡0.15,0.2,0.3,Γ2=0.05.Setting the initial guess ofξ^=000T, ρ^=0; the adaptive parameters are illustrated in Figure 6.Figure 6
The adaptive parameters.
(a)
(b)It can be seen from Figure6 that, given an arbitrary guess of the unknown parameters, they are gradually stabilized towards the true value.The control inputs are illustrated in Figure7.Figure 7
Control input.
(a)
(b)The curves of system error changes are shown in Figure8.Figure 8
System error.It can be observed that the control law designed in this study is capable of controlling the robot to gradually stabilize towards the expected position-orientation, with unknown translation extrinsic parameter of the camera.


## 4.1. Fast Decomposition Experiment of the Homography Matrix

Apply different levels (0–2 pixels) of Gaussian noise on the tops of the two images, respectively. For each level of the noise, randomly select 1000 groups of the noise to be applied on the top of the image. Use DLT to estimate the homography matrix and decompose the matrix with traditional SVD-based decomposition method [13] and the proposed fast decomposition algorithm, respectively. The decomposition results are as follows: (75)θsvdi,tsvd′i,nsvdid,θfasti,tfast′i,nfastid,i=1,…,1000. Let the error between the above results and the real values θ,t′,nd be (76)eθsvdi=θsvdi-θ,etsvd′i=acos⁡t′T·tsvd′iensvdid=acos⁡nTd·nsvdid,eθfasti=θfasti-θ,etfast′i=acos⁡t′T·tfast′ienfastid=acos⁡nTd·nfastid.Taking the average value of the 1000 groups of errors, we can have the error maps in Figure5.Figure 5
Comparison of the traditional algorithm and the proposed algorithm with regard to different pixel noise level.
(a)
(b)
(c)According to Figure5(a), the rotation angles calculated from the two methods have extremely close accuracy, with individual cases where the proposed method has even better results than the traditional one. On the other hand, from Figures 5(b) and 5(c), it can be seen that the accuracies of the camera translation and the scene plane normal are obviously higher than those of the traditional algorithm. Therefore, this algorithm effectively avoids the matrix SVD calculation. It requires less operation and is easy for programming, with a higher decomposition accuracy.


## 4.2. Experiment of Adaptive Hybrid Visual Servo Regulation for Mobile Robot

In order to verify the performance of the proposed algorithm, the mobile robot system was simulated in a MATLAB environment. From the simulation results, it can be seen that the proposed algorithm is capable of making the robot gradually converge to the desired location and has good control performance.The gain settings of the controller and adaptive parameter update are(77)k1=0.25,k2=1Γ1=diag⁡0.15,0.2,0.3,Γ2=0.05.Setting the initial guess ofξ^=000T, ρ^=0; the adaptive parameters are illustrated in Figure 6.Figure 6
The adaptive parameters.
(a)
(b)It can be seen from Figure6 that, given an arbitrary guess of the unknown parameters, they are gradually stabilized towards the true value.The control inputs are illustrated in Figure7.Figure 7
Control input.
(a)
(b)The curves of system error changes are shown in Figure8.Figure 8
System error.It can be observed that the control law designed in this study is capable of controlling the robot to gradually stabilize towards the expected position-orientation, with unknown translation extrinsic parameter of the camera.


## 5. Conclusion

This paper studied a fast homography decomposition method based adaptive hybrid visual servo regulation algorithm, with missing depth information of the target and unknown translation extrinsic parameter of the onboard camera. With the construction of auxiliary rotation matrix, a common homography decomposition problem was transformed into a special type of homography decomposition problem, which effectively reduced the decomposition complexity. Compared with traditional homography decomposition methods, this decomposition algorithm has higher decomposition accuracy and robustness. At the meantime, the designed adaptive visual controller is capable of providing online compensation for the unknown imaging depth and translation extrinsic parameter, which enables the mobile robot to gradually move from the original position-orientation to the desired position-orientation, with a good performance.



---

*Source: 739894-2015-09-07.xml*