Vision Guided Navigation For A Nonholonomic Mobile Robot ...
Proceedings of the 36th Conference on Decision & Control San Diego, California USA December 1997
TP07 4:50
Vision Guided Navigation for A Nonholonomic Mobile Robot Yi Mal
Jana KoGeckA
Shankar Sastry
Electronics Research Laboratory University of California at Berkeley Berkeley, CA 94720-1774 {mayi, janka, sastry}@robotics.eecs.berkeley.edu Abstract Visual servoing, i.e. the use of the vision sensor in feedback control, has been of increasing interest. Work has been done by applications in autonomous driving, manipulation, mobile robot navigation and surveillance. This paper studies the navigation task for a nonholonomic ground mobile base tracking an arbitrarily shaped ground curve using vision sensors. This tracking problem is formulated as one of controlling the shape of the curve in the image plane. The study of the controllability of the system characterizing the image curve dynamics shows that the shape of the image curve is controllable only up to its “linear” curvature parameters. A stabilizing control law is presented for tracking (piecewise) analytic curves, and we propose to track arbitrary curves by approximating them by piecewise linear curvature curves. Simulation results are given for these control schemes. The observability of the image curve dynamics using direct measurements from vision sensors as the outputs is studied and an extended Kalman filter is proposed to dynamically estimate the image parameters needed for the feedback control from the actual noisy image sequences.
1 Introduction Sensing of the environment and subsequent control are pertinent for the navigation of an autonomous mobile agent. In spite of the fact that there has been an increased interest in the use of visual servoing in the control loop, the sensing and control problems have usually been studied separately [5, 6, 16, 171. The dynamic vision approach proposed by Dickmanns et QZ [l, 21 makes the connection between the estimation and control tighter by setting up a dynamic model of the evolution of the curvature of the road in a driving application. However the curvature estimates are ‘This work was supported by ARO under the MURI grant DAAH04-96-1-0341. We would like t o thank Dr. Stefan0 Soatto for t h e formulation of this problem given in the AI/Robotics/Vision seminar at U C Berkeley, October 1996.
0-7803-3970-8197 $10.00@ 1997 IEEE
used only for the estimation of the state of the vehicle with respect to the road frame where the control objective is formulated. Control for steering along a curved road directly using the measurement of the projection of the road tangent and it’s optical flow has been previously considered by Raviv and Herman [13]. Stability and robustness issues have not been addressed, and no statements have been made as to what extent these cues are sufficient for general road scenarios. A visual servoing framework proposed in [3, 14, 121 by Espiau et al addresses the control issues directly in the image plane and outlines the dynamics of certain simple geometric primitives. Generalization of the curve tracking and estimation problem outlined in Dickmanns for arbitrarily shaped curves addressing both the estimation of the shape parameters as well as control has been explored in [4] by Frezza and Picci. They used an approximation of an arbitrary curve by a spline, and proposed a scheme for recursive estimation of shape parameters of the curve, and designed control laws for tracking the curve. For a theoretical treat,ment of the problem, the understanding of the dynamics of the image of an arbitrary ground curve is crucial. In spite of the fact that the system characterizing the image curve seems to be infinitedimensional, this system is only controllable up to its linear curvature terms regardless of the kinematics of the mobile robot base. It is also shown that linear curvature curves has finite dimensional dynamics and such dynamics can be transformed to a canonical chainedform.
We design stabilizing feedback control laws for tracking general (piecewise) analytic curves. We also propose to approximately track general curves by using piecewise linear curvature curve models. Simulation results are presented. We study the observability of the image curve dynamics from the direct measurements of the vision sensor. Based on the sensor models, an extended Kalman filter is proposed to dynamically estimate the image parameters needed for the feedback control. We thus obtain a complete closed-loop vision-guided navigation system for the considered non-holonomic mobile robot.
3069
/'
2 Curve Dynamics Fm
i / F~
ImagePlane
This section is to derive image curve dynamics under the motion of a ground-based mobile robot. In the following, only the unicycle model is studied in detail. One can refer to Ma, Kosecka and Sastry [S] for generalized results to other mobile robot models.
Let p f m E R3 be the position vector of the origin of the mobile frame F, (attatched to the unicycle) from the origin of a fixed spatzal frame F f , and 0 E R be the rotation angle of F, with respect to F j , defined in the counter-clockwise sense about the y-axis, as shown in Figure 1.
Figure 2: The side-view of the unicycle mobile robot with a camera facing downward with a tilt angle 4 > 0.
curve r is analytic; 2. the ground curve r is such that it can be parameterized by y in the camera coordinate frame F,. Assumption 1 means that r can be locally expressed by its convergent Taylor series expansion. Assumption 2 guarantees that the task of tracking the curve can be solved using a smooth control law, since it avoids the critical case that the curve is orthogonal to the heading of the mobile robot.
Figure 1: Model of the unicycle mobile robot. For unicycle kinematics, one has:
where the steering input w controls the angular velocity; the driving input v controls the linear velocity along the direction of the wheel. A unit focal length monocular camera is mounted on the mobile robot facing downward with a tilt angle q5 > 0 and elevated above the ground by a distance d , as shown in Figure 2. The camera coordznate frame F, is such chosen for the camera that the z-axis of F, is the optical axis of the camera, the x-axis of F, is that of F,, and the optical center of the camera coincides with the origins of F, and F,. From (l),the velocity of a point q attached to the camera frame F, is given in the (instantaneous) camera frame by:
(i) ( =
si;Q)v+ cos Q
(
y sin Q + z cos 4 -xsin4 -x cos Q
)
2.1 Relations between Orthographic and Perspective Projections According t o Assumption 2, a t any time t , the curve r can be expressed in the camera coordinate frame as ( y = ( y , t ) y, , ~ ~ ( y , tE) R3. ) ~ Since r is a planar curve on the ground, y Z ( y , t )is given by:
which is a function of only y. Thus only T~( y ,t ) changes with time and determines the dynamics of the ground curve. In order to determine the dynamics of the image curve we consider both orthographic and perspective projection cases and show that under certain conditions they are equivalent. The orthographic projection image curve of I' on the image plane z = 1 is given by (yz(9,t ) ,Y ) E~ R2, denoted by f', as shown in Figure 3. On the other hand,
t Y
w . (2)
In order to simply the notation, from time to time in the rest of the paper, we use the abbreviations sq5, cq5, ctq5 and tq5 to represent sin 9, cos q5, cot q5 and tan 6, respectively. For the rest of this paper, unless otherwise stated, we make the following assumptions: 1. the given ground
Figure 3: The orthographic projection of a ground curve on the z = 1plane. Here 61 = -yz and = By,. BY the perspective projection image curve, denoted by A ,
3070
0 and y # -d/cosd, then Vn E N,Cn = A n ( y ) t n where An(y) E Rnxnis a nonsingular lower triangular matrix.
2.2 Dynamics of General Curves While the mobile robot moves, a point attached to the spatial frame Ff moves in the opposite direction relative to the camera frame Fc. Thus, from (2), for points on the ground curve r = (yx(y,t), y,~ ~ ( y )we) ~ have: ,
,
The ground curve r is a called linear curvature curve if the derivative of its curvature k ( s ) with respect to the arc-length parameter s is a non-zero constant, i.e. k’(s) E c # 0. Suce curves are also referred to as clothoids. If k’(s) =: 0, the curve is just a constant curvature curve which is a degenerate case of linear curvature curves.
C E C”.
TP07 4:50
Vision Guided Navigation for A Nonholonomic Mobile Robot Yi Mal
Jana KoGeckA
Shankar Sastry
Electronics Research Laboratory University of California at Berkeley Berkeley, CA 94720-1774 {mayi, janka, sastry}@robotics.eecs.berkeley.edu Abstract Visual servoing, i.e. the use of the vision sensor in feedback control, has been of increasing interest. Work has been done by applications in autonomous driving, manipulation, mobile robot navigation and surveillance. This paper studies the navigation task for a nonholonomic ground mobile base tracking an arbitrarily shaped ground curve using vision sensors. This tracking problem is formulated as one of controlling the shape of the curve in the image plane. The study of the controllability of the system characterizing the image curve dynamics shows that the shape of the image curve is controllable only up to its “linear” curvature parameters. A stabilizing control law is presented for tracking (piecewise) analytic curves, and we propose to track arbitrary curves by approximating them by piecewise linear curvature curves. Simulation results are given for these control schemes. The observability of the image curve dynamics using direct measurements from vision sensors as the outputs is studied and an extended Kalman filter is proposed to dynamically estimate the image parameters needed for the feedback control from the actual noisy image sequences.
1 Introduction Sensing of the environment and subsequent control are pertinent for the navigation of an autonomous mobile agent. In spite of the fact that there has been an increased interest in the use of visual servoing in the control loop, the sensing and control problems have usually been studied separately [5, 6, 16, 171. The dynamic vision approach proposed by Dickmanns et QZ [l, 21 makes the connection between the estimation and control tighter by setting up a dynamic model of the evolution of the curvature of the road in a driving application. However the curvature estimates are ‘This work was supported by ARO under the MURI grant DAAH04-96-1-0341. We would like t o thank Dr. Stefan0 Soatto for t h e formulation of this problem given in the AI/Robotics/Vision seminar at U C Berkeley, October 1996.
0-7803-3970-8197 $10.00@ 1997 IEEE
used only for the estimation of the state of the vehicle with respect to the road frame where the control objective is formulated. Control for steering along a curved road directly using the measurement of the projection of the road tangent and it’s optical flow has been previously considered by Raviv and Herman [13]. Stability and robustness issues have not been addressed, and no statements have been made as to what extent these cues are sufficient for general road scenarios. A visual servoing framework proposed in [3, 14, 121 by Espiau et al addresses the control issues directly in the image plane and outlines the dynamics of certain simple geometric primitives. Generalization of the curve tracking and estimation problem outlined in Dickmanns for arbitrarily shaped curves addressing both the estimation of the shape parameters as well as control has been explored in [4] by Frezza and Picci. They used an approximation of an arbitrary curve by a spline, and proposed a scheme for recursive estimation of shape parameters of the curve, and designed control laws for tracking the curve. For a theoretical treat,ment of the problem, the understanding of the dynamics of the image of an arbitrary ground curve is crucial. In spite of the fact that the system characterizing the image curve seems to be infinitedimensional, this system is only controllable up to its linear curvature terms regardless of the kinematics of the mobile robot base. It is also shown that linear curvature curves has finite dimensional dynamics and such dynamics can be transformed to a canonical chainedform.
We design stabilizing feedback control laws for tracking general (piecewise) analytic curves. We also propose to approximately track general curves by using piecewise linear curvature curve models. Simulation results are presented. We study the observability of the image curve dynamics from the direct measurements of the vision sensor. Based on the sensor models, an extended Kalman filter is proposed to dynamically estimate the image parameters needed for the feedback control. We thus obtain a complete closed-loop vision-guided navigation system for the considered non-holonomic mobile robot.
3069
/'
2 Curve Dynamics Fm
i / F~
ImagePlane
This section is to derive image curve dynamics under the motion of a ground-based mobile robot. In the following, only the unicycle model is studied in detail. One can refer to Ma, Kosecka and Sastry [S] for generalized results to other mobile robot models.
Let p f m E R3 be the position vector of the origin of the mobile frame F, (attatched to the unicycle) from the origin of a fixed spatzal frame F f , and 0 E R be the rotation angle of F, with respect to F j , defined in the counter-clockwise sense about the y-axis, as shown in Figure 1.
Figure 2: The side-view of the unicycle mobile robot with a camera facing downward with a tilt angle 4 > 0.
curve r is analytic; 2. the ground curve r is such that it can be parameterized by y in the camera coordinate frame F,. Assumption 1 means that r can be locally expressed by its convergent Taylor series expansion. Assumption 2 guarantees that the task of tracking the curve can be solved using a smooth control law, since it avoids the critical case that the curve is orthogonal to the heading of the mobile robot.
Figure 1: Model of the unicycle mobile robot. For unicycle kinematics, one has:
where the steering input w controls the angular velocity; the driving input v controls the linear velocity along the direction of the wheel. A unit focal length monocular camera is mounted on the mobile robot facing downward with a tilt angle q5 > 0 and elevated above the ground by a distance d , as shown in Figure 2. The camera coordznate frame F, is such chosen for the camera that the z-axis of F, is the optical axis of the camera, the x-axis of F, is that of F,, and the optical center of the camera coincides with the origins of F, and F,. From (l),the velocity of a point q attached to the camera frame F, is given in the (instantaneous) camera frame by:
(i) ( =
si;Q)v+ cos Q
(
y sin Q + z cos 4 -xsin4 -x cos Q
)
2.1 Relations between Orthographic and Perspective Projections According t o Assumption 2, a t any time t , the curve r can be expressed in the camera coordinate frame as ( y = ( y , t ) y, , ~ ~ ( y , tE) R3. ) ~ Since r is a planar curve on the ground, y Z ( y , t )is given by:
which is a function of only y. Thus only T~( y ,t ) changes with time and determines the dynamics of the ground curve. In order to determine the dynamics of the image curve we consider both orthographic and perspective projection cases and show that under certain conditions they are equivalent. The orthographic projection image curve of I' on the image plane z = 1 is given by (yz(9,t ) ,Y ) E~ R2, denoted by f', as shown in Figure 3. On the other hand,
t Y
w . (2)
In order to simply the notation, from time to time in the rest of the paper, we use the abbreviations sq5, cq5, ctq5 and tq5 to represent sin 9, cos q5, cot q5 and tan 6, respectively. For the rest of this paper, unless otherwise stated, we make the following assumptions: 1. the given ground
Figure 3: The orthographic projection of a ground curve on the z = 1plane. Here 61 = -yz and = By,. BY the perspective projection image curve, denoted by A ,
3070
0 and y # -d/cosd, then Vn E N,Cn = A n ( y ) t n where An(y) E Rnxnis a nonsingular lower triangular matrix.
2.2 Dynamics of General Curves While the mobile robot moves, a point attached to the spatial frame Ff moves in the opposite direction relative to the camera frame Fc. Thus, from (2), for points on the ground curve r = (yx(y,t), y,~ ~ ( y )we) ~ have: ,
,
The ground curve r is a called linear curvature curve if the derivative of its curvature k ( s ) with respect to the arc-length parameter s is a non-zero constant, i.e. k’(s) E c # 0. Suce curves are also referred to as clothoids. If k’(s) =: 0, the curve is just a constant curvature curve which is a degenerate case of linear curvature curves.
C E C”.
