Chapter 7
Image Based Control of Robotic Systems

Chapter introduced some of the fundamental joint space feedback control methods. Many variants of these control strategies have been derived in the literature. This chapter focuses on the derivation of feedback controllers that utilize measurements or observations collected from cameras or in the form of CCD imagery in the task space. The image based visual servo control problem is defined, and its stability and convergence are discussed. General methods to achieve asymptotic stability of task space controllers that utilize the computed torque control strategy are introduced. It is shown that camera and CCD imagery measurements can be employed in task space control formulations. Upon completion of this chapter the student should be able to:

Define the image plane coordinates and pixel coordinates associated with camera measurements of robotic system location and orientation.

Derive the interaction matrix relating the derivatives of image plane coordinates to the velocity of the origin of the camera frame and the angular velocity of the camera frame in the inertial frame.

State the image based visual servo control problem and discuss its stability.

Derive computed torque controllers in terms of task space coordinates.

Derive task space controllers for visual servo problems.

7.1 The Geometry of Camera Measurements

Modernrobotic control systems utilize a wide variety ofsensors to measure the configuration and motion of arobotic system during its operation. These sensors may includeaccelerometers,rate gyros,magnetometers,angle encoders, orglobal positioning system (GPS) sensors, to name a few. This section will discuss a basiccamera model that can be used to represent many of the cameras that are used in robotics for various control tasks. The simplest camera model that is applicable to many of the commercially available cameras is based on thepinhole camera orperspective projection camera.

Figure 7.1 Perspective pinhole camera, rear projected.

Figure 7.2 Perspective pinhole camera, coordinate calculation.

Figure 7.3 Perspective pinhole camera, front projected.

7.1.1 Perspective Projection and Pinhole Camera Models

As shown in Figure 7.1, when a classical pinhole camera creates an image of a point images , a ray is traced from the point images through the pinhole located at the origin of thecamera frame images onto thefocal plane. Thefocal length images is the distance from the origin of the camera frame to the focal plane. This chapter denotes the basis for the camera frame images as images . By convention theline‐of‐sight of the camera is along the images axis. Thecamera coordinates images of afeature point images viewed from a camera are defined in terms of theposition vector images of the point images in the camera frame images ,

The coordinates images of theimage point in the focal plane are known as the thecanonical image plane coordinates,retinal coordinates, orcalibrated coordinates. As shown in Figure 7.1, the location of the the focal plane on the opposite side of the point images being viewed implies that the image in the focal plane is inverted. Figure 7.2 shows a full view of the images – images plane in this case. The relationship between the camera coordinates images and the focal plane coordinates images can be derived by considering similar triangles in Figure 7.2. Knowing the focal length images between the focal plane and camera frame, the relationship is

(7.1)

It is common practice to replace the physically motivated geometry in Figure 7.1 with a mathematical model that places the focal plane between the origin of the camera frame and point images being viewed, as shown in Figure 7.3. While this arrangement is not physically realizable, it is used so that the image is not inverted in the focal plane. In this mathematical model, the negative signs in Equation (7.2) do not appear, and the relationship between the focal plane coordinates images and the camera coordinates images is simply images and images . These equations can be rewritten in terms of thehomogeneous coordinates of the image point in the focal plane and the camera frame coordinates as

(7.2)

Equation (7.2) relates the focal plane coordinates images to the camera coordinates images , but it is often desired to understand how the coordinates of the point images in the inertial 0 frame are related to the focal plane coordinates. The tools derived in Chapters and 3 can be used to obtain the desired expression. Recall that the homogeneous coordinates of the point with respect to the camera images frame are defined as images . These coordinates are related to the inertial frame 0 using thehomogeneous transform images ,

(7.3)

The final homogeneous transformation that relates the inertial frame 0 coordinates and the focal plane coordinates is achieved by combining Equations (7.2) and (7.3),

(7.4)

7.1.2 Pixel Coordinates and CCD Cameras

Most modern cameras are comprised of sensors that make measurements using charge coupled device, or CCD, arrays. Cameras based on CCD arrays return a two dimensional matrix of intensity values, in contrast to the mathematical abstraction used in the last section that considered the image as a continuously varying intensity over the focal plane. Thepixel coordinates are used to represent the locations of the entries in the CCD array of intensity values. The pixel coordinates are introduced in a two step process. First, because the individual pixel elements in the CCD array can have different dimensions in the horizontal and vertical directions, thescaled coordinates images are defined in terms of thefocal coordinates images via the simple diagonal matrix equation

(7.5)

where the parameters images are scale factors in the images and images directions, respectively.

In addition to scaling, it is also common that the pixels in a CCD array are numbered from left to right, starting at the upper left corner of the pixel array. We account for this offset and define the pixel coordinates by shifting the origin with respect to the scaled coordinates,

(7.6)

where images is the location of theprincipal point or the image of the line‐of‐sight of the camera in the CCD array. The process of scaling and translating to obtain the pixel coordinates in terms of the focal plane coordinates can be written by combining Equations (7.5) and (7.6) in terms of a single transformation

(7.7)

Before closing this section, several variants of the equations that relate the camera coordinates,inertial coordinates, focal plane coordinates, and pixel coordinates are derived. These equations introduce theintrinsic orcalibration parameter matrix. With Equation (7.7), Equation (7.4) can be used to relate the pixel coordinates and inertial coordinates of the image point images in

Note that these equations have introduced another scalar parameter, images , that measures how pixels in the CCD array are sheared relative to the vertical and horizontal directions. Combining the focal length with the calibration constants images results in

in which thecamera intrinsic parameter matrix orcalibration matrix may be defined as images ,

By defining theprojection matrix

that extracts the first three coordinates from any 4 images of homogeneous coordinates, a succinct rule that defines the pixel coordinates in terms of the camera coordinates is written as

For completeness the relationships that use the intrinsic parameter matrix images to express the pixel coordinates in terms of the inertial coordinates are summarized,

7.1.3 The Interaction Matrix

This section derives theinteraction matrix orimage Jacobian matrix that plays a critical role in the development of control strategies that use camera measurements for feedback. The interaction matrix relates the derivatives of the focal plane coordinates to the velocity and angular velocity of the camera frame in theinertial frame.

The following theorem specifies an explicit representation of the interaction matrix images when the focal length is equal to 1.

There are several ways to derive the identity in Equation (7.9). This proof will use the derivative Theorem 2.12 that relates the calculation of time derivatives when the basis for the camera frame and the inertial frame are held fixed. By definition, the position vectors can be related via images , where images is the position of the feature point images in the 0 frame, images is the position of the feature point images in the images frame, and images is the vector that connects the origin of the 0 frame to the origin images of the images frame. When the derivative is taken of this vector equation while holding the basis for the 0 frame fixed, it becomes

Note that the definition of the velocity of the point images in the 0 frame and the velocity of the origin images of the images frame in the inertial frame is used in this equation. The troublesome term is the derivative images , which is not known a priori. However, the derivative Theorem 2.12 provides a method of relating the derivatives of an arbitrary vector images with respect to any two frames images , images as

(7.10)

By assumption, the camera frame coordinates are images , so the position images is images , where images is the basis for frame images . Define the velocity of the origin images of the camera frame images and the angular velocity of the camera frame images in the inertial 0 frame as images and images . Applying the transport theorem in Equation (7.10) for frames images and images results in

If this expression is substituted for images in Equation (7.1.3), and the result is solved for images , the matrix equation becomes

(7.11)

This relationship will be used in several of the proofs that follow. It is written in compact form as

(7.12)

Next, a relationship between images , and images will be determined. By definition, images and images . Suppose that the focal length images . Differentiating images and images yields

When the identity above is simplified using the definition of the image plane coordinates, it is possible to obtain a matrix equation that relates the derivatives images of the image plane coordinates to the derivatives images of the camera frame coordinates in

(7.13)

The final expression for the interaction matrix is obtained when Equation (7.11) is substituted into Equation (7.13)

Equation (7.9) shows that the matrix images is a images matrix for a single feature point. In general, Theorem 7.1 will be employed for several feature points. Suppose there are feature points images . Introduce the images vector images and the images vector images that are obtained by stacking the image plane coordinates images and by stacking the camera coordinates images for all the feature points images as

(7.14)

Thesystem interaction matrix is obtained by applying Theorem 7.1 for each feature point images ,

or more concisely as,

(7.15)

When we stack these equations, we obtain the system interaction matrix images

(7.16)

(7.17)

Just as the system interaction matrix images is defined, an equation is also needed for the system of images feature points that is analogous to Equation (7.12). For images

Stacking these equations for images results in

(7.18)

or more succinctly,

(7.19)

7.2 Image Based Visual Servo Control

With the construction of the matrices images and images in Equations (7.17) and (7.19) for a system that includes feature points images that are fixed in the inertial 0 frame, it is straightforward to pose and solve a number of standard problems in the control of robotic systems using camera based measurements. This section will discuss one such control problem, theimage based visual servo (IBVS) control problem.

7.2.1 Control Synthesis and Closed Loop Equations

The IBVS control problem is a specific example of atracking control problem. First, the IBVS control problem will be defined to specify the goals of the strategy and the measurements that will be used for feedback control.

Definition 7.2

Suppose a set of feature points for images are given that are fixed in the inertial frame, and let images denote images desired image point locations that are fixed in the image plane. The tracking error in the image plane for the images th feature point is defined to be

(7.20)

and the image plane tracking error for the system is given by

(7.21)

The IBVS control problem seeks to find the control input vector images that consists of the velocity images of the origin of the camera frame and angular velocity images of the camera frame in the inertial 0 frame,

(7.22)

so that

the control input is a feedback function given in terms the tracking error and perhaps camera extrinsic parameters,
the dynamics of the closed loop system is stable, and
the tracking error approaches zero as .

The derivation of the visual servo control strategy to solve the problem statement in Definition 7.2 is not difficult given the derivation of the matrices images and images . Since the tracking error should approach zero asymptotically, the control law can be defined so that the closed loop system has a tracking error that satisfies the equation

(7.23)

where images is some positive scalar. The solution of Equation (7.23) is given in terms of exponential function

Consequently, if the closed loop tracking error satisfies Equation (7.23), it will approach zero at an exponential rate. The definition of the tracking error can be combined with Equation (7.23) to obtain

(7.24)

Ideally, Equation (7.24) would be uniquely solvable for the velocity images and angular velocity images . However, the system interaction matrix is not square in general since

and it may be that images . Depending on the number of system feature points, the matrix equation

(7.25)

can be underdetermined, overdetermined or exactly determined. If the matrix images has fewer rows than columns, images , the system is said to beunderdetermined. If the matrix images has more rows than columns, images , the system is said to beoverdetermined. If the matrix images has equal numbers of rows and columns, images , the system is said to beexactly determined.

Even if the system is not exactly determined, it is possible to obtain an expression for the control input images by using thepseudo inverse

When both sides of Equation (7.25) are multiplied by images , an expression for the control input vector is obtained as

(7.26)

Before proceeding to the discussion of the closed loop system and its stability, it is important to make several observations about the construction thus far.

The pseudo inverse is not a square matrix. It has dimension .
The pseudo inverse expression

only makes sense provided that the matrix isinvertible. While the expression given for assumes that is invertible, the pseudo inverse always exists for any matrix. The form of the pseudo inverse for an arbitrary matrix can be expressed in terms of thesingular value decomposition of the matrix. While this topic is beyond the scope of this book, the interested reader is referred to [18] for a discussion.
The pseudo inverse is a nonlinear function of the image plane coordinates and the range of each the system feature points. That is,

This implies that the expression in Equation (7.25) is a nonlinear equation.
The set of equations in (7.25) is not a closed set of ordinary differential equations. The left hand side of Equation (7.25) contains the derivatives of the tracking error , while the right hand side of these same equations contains the image plane coordinates and ranges . To put this set of equations in the standard form for a system of ordinary differential equations, it must be rewritten as
(7.27)
for some set of state variables .

The following theorem derives the set of coupled, nonlinear ordinary differential equations that characterizes the dynamics of the closed loop system associated with the feedback control law embodied in Equation (7.25).

These equations follow from the derivation of the closed loop control law in Equation (7.25). From Equation (7.26),

(7.31)

which when combined with Equation (7.24) yields

Moreover, it is known that

When this equation is differentiated with respect to time images , and it follows that images . Finally, Equation (7.19) shows that

which results in

when Equation (7.31) is substituted into Equation (7.19). Thus, each of the equations in Theorem 7.2 holds. It remains to show that this collection of three vector equations constitutes a closed system of ordinary differential equations. Recall from the discussion of Equation (7.27) that this collection of equations must be able to be written in the form images for some images vector images of state variables and some function images . Define the state variable vector to be

Then the left hand side of the collection of governing equations

is simply images , and the right hand side of this set of three vector equations depends on images , which are the entries of images . Thus, the system is a closed set of ordinary differential equations.

7.2.2 Calculation of Initial Conditions

The actual simulation of the collection of nonlinear ordinary differential equations written in the form images , where the vector of state variables images is given by

can use any of a number of standardnumerical integration methods for systems of ordinary differential equations. These numerical methods include the family of linear multistep methods. The linear multistep methods include many popular predictor corrector methods, such as the Adams–Bashforth–Moulton method. Another popular family of numerical integration methods include the self‐starting Runge–Kutta methods. The reader is referred to [2] for a discussion of these methods, as well as other popular alternatives.

To use such a numerical algorithm to obtain an approximate solution of these equations, it is necessary to specify theinitial condition

To describe a general procedure to solve for the initial condition images , the initial and final orientation of the camera frame will need to be described. The basis for the initial camera frame images will be denoted by images , and the basis for the final camera frame images will be denoted by images . The position of a point images relative to the camera frame in its initial configuration is then

and the position of the point images relative to the camera frame in its final configuration is

The following steps can be used to calculate the initial condition images .

Find the coordinates of the offset between the origin of the final camera frame and the origin of the initial camera frame, and find the rotation matrix that maps coordinates with respect to the final camera frame into coordinates with respect to theinitial camera frame. Create the homogeneous transformation that maps the homogeneous coordinates with respect to the final camera frame into the homogeneous coordinates with respect to the initial coordinate frame.
(7.32)
Calculate the camera coordinates with respect to the initial camera frame from the camera coordinates with respect to the final camera frame for each of the points ,
(7.33)

This equation can also be written more compactly as

(7.34)
Use the camera coordinates with respect to the initial camera frame ( ) to calculate the initial focal plane coordinates for
(7.35)
Use the initial focal plane coordinates to calculate the initial focal plane tracking error for each point .
(7.36)

Once these steps have been completed, the initial condition images is constructed by stacking the vectors images for images to form the vectors images ,

and subsequently assembling

Example 7.1

Derive an IBVS controllerusing camera measurements of four feature points that are fixed in the inertial frame. Consider the orientation of the camera frame, focal plane, and camera coordinates of the feature points as shown in Figure 7.4. For this problem the focal length images . Note that the figure depicts the desired or final configuration of the camera frame.

Figure 7.4 Final configuration of the camera frame and feature points.

Solution

The desired focal plane coordinates shown in the figure are given by

The coordinates of the feature points relative to the final orientation of the camera frame are given by

The initial condition will be chosen so that

the origin of the initial camera frame coincides with the origin of the final camera frame, and
the orientation of the initial camera frame is obtained by rotating the final camera frame by about the axis.

The system of ordinary differential equations in Theorem 7.2 is used to simulate the behavior of the control scheme. The calculation of the initial condition for these governing equations is carried out using the steps (i) through (iv) above. In this example,

Rotation about the z axis, ψ=π/4, focal plane trajectories. — Figure 7.5 Rotation about the axis, , focal plane trajectories.

images — Figure 7.5 Rotation about the axis, , focal plane trajectories.

and the rotation matrix that maps the final camera coordinates to the initial camera coordinates is simply

Rotation about the z axis, ψ=π/4, camera coordinate trajectories. — Figure 7.6 Rotation about the axis, , camera coordinate trajectories.

Figure 7.5 depicts the performance of the closed loop system by plotting the trajectories of the focal plane coordinates. The focal plane coordinates of each feature point start at the corners of the red square, which represents the image of the feature points viewed from the initial camera configuration. Thevisual servo image based control law drives the camera so that the focal plane coordinates of each of the feature points approaches the desired locations in the focal plane.

Figure 7.6 illustrates the trajectories taken by the camera coordinates as a function of time when the camera is driven by the image based visual servo control law. Note that in Figure 7.6 the camera moves in the images direction as it is driven by the control law. Even though the target camera configuration is related to the initial camera configuration by a simple rotation about the images basis vector, the set of closed loop equations induces a motion in the images direction: the rotation about the images direction is coupled with the rotation about the images direction.

Example 7.2

As in Example 7.1, derive an IBVS controller using camera measurements of four feature points that are fixed in the inertial frame. The feature points have the same relative geometry with respect to the final camera frame, but the initial camera frame is defined as shown in Figure 7.7.

Figure 7.7 Initial condition configuration.

Solution

As shown in Figure 7.7, the position and orientation of the initial camera frame are defined to satisfy two conditions.

The origin of the initial camera frame is offset from the origin of the final camera frame by
(7.37)
The orientation of the initial camera frame is obtained via a rotation of the final camera frame about the axis by an angle of , so that
(7.38)

As before, the initial condition images is calculated using the updated rotation matrix images and the origin offset images in Equations (7.38) and (7.37).

Figures 7.8 and 7.9 illustrate the results of the numerical simulation in these cases. Note that in contrast to the last example, the image of the feature points viewed from the initial camera configuration is significantly skewed. The red polygons in Figures 7.8 and 7.9 denote the images of the feature points viewed from the initial camera configuration. The controller drives the trajectories of the feature points in the focal plane to the desired locations, as shown in Figures 7.8 and 7.9. The trajectories of the camera coordinates are shown in Figure 7.9. Again, the camera coordinates of all the feature points converge to the desired locations as a function of time.

Rotation about the x axis, φ=π/4, focal plane trajectories. — Figure 7.8 Rotation about the axis, , focal plane trajectories.

Rotation about the x axis, φ=π/4, camera coordinate trajectories. — Figure 7.9 Rotation about the axis, , camera coordinate trajectories.

Rotation about the z axis, ψ=π/2, focal plane trajectories. — Figure 7.10 Rotation about the axis, , focal plane trajectories.

Rotation about the z axis, ψ=π/2, camera coordinate trajectories. — Figure 7.11 Rotation about the axis, , camera coordinate trajectories.

Rotation about the z axis, ψ=3π/4, focal plane trajectories. — Figure 7.12 Rotation about the axis, , focal plane trajectories.

Rotation about the z axis, ψ=3π/4, camera coordinate trajectories. — Figure 7.13 Rotation about the axis, , camera coordinate trajectories.

Rotation about the z axis, φ=7π/8, focal plane trajectories. — Figure 7.14 Rotation about the axis, , focal plane trajectories.

Rotation about the z axis, φ=7π/8, camera coordinate trajectories. — Figure 7.15 Rotation about the axis, , camera coordinate trajectories.

Rotation about the z axis, ψ=99π/100, focal plane trajectories. — Figure 7.16 Rotation about the axis, , focal plane trajectories.

Example 7.3

Consider again the case studied in Example 7.1 where the origin of the initial camera frame coincides with the origin of the final camera frame, and the initial camera frame is obtained by rotating the final camera frame by images about the images axis. This example utilizes this same setup but chooses the rotation angles to be images and images .

Solution

Figures 7.10 through 7.17 depict the focal plane trajectories and camera coordinate trajectories for these four test cases. Figures 7.10, 7.12, 7.14, and 7.16 show that the focal plane trajectories behave exactly as expected in these four cases. The focal plane trajectories start at the corners of the red polygon associated with the image of the feature points viewed from the initial camera frame, and they end at the corners of the blue polygon corresponding to the view of the feature points from the final camera frame. However, Figures 7.11, 7.13, 7.15, and 7.17 exhibit striking behavior. As the rotation angle approaches images , the range of motion along the images direction increases dramatically. Table 7.1 summarizes these results.

Rotation about the z axis, ψ=99π/100, camera coordinate trajectories. — Figure 7.17 Rotation about the axis, , camera coordinate trajectories.

Rotation about the z axis, ψ=π/4, minimum singular value. — Figure 7.18 Rotation about the axis, , minimum singular value.

Rotation about the z axis, ψ=π/2, minimum singular value. — Figure 7.19 Rotation about the axis, , minimum singular value.

Rotation about the z axis, ψ=3π/4, minimum singular value. — Figure 7.20 Rotation about the axis, , minimum singular value.

Rotation about the z axis, ψ=99π/100, minimum singular value. — Figure 7.21 Rotation about the axis, , minimum singular value.

Recall that the origin of the final camera frame is required to be located at a images axis distance of 8 units from all of the feature points. Consider in particular the case when the rotation angle is images . It is clear in this case that the performance of the control method is entirely unsatisfactory. The control law drives the camera so that its maximum range is 600 units from the feature points.

The reason for this aberrant behavior can be deduced from the last column in the table. This column lists the minimum singular value of the system interaction matrix images over the entire range of motion. A plot of these minimum singular values is given for simulations associated with the various rotation angles in Figures 7.18 through 7.21. This minimum singular value can be viewed as a measure of how close the matrix images is to being singular. The image Jacobian is said to be rank deficient, or singular, in the neighborhood of the configuration associated with the rotation angle images . This difficulty is well understood and has counterparts in many robotic control problems.

7.3 Task Space Control

Chapter describes several techniques for deriving feedback controllers for fully actuated robotic systems. All of these control strategies are derived so that the generalized coordinates images and their derivatives images approach a desired trajectory as images , images and images . For setpoint controllers the desired trajectory is a constant trajectory for which images , while trajectory tracking controllers consider time varying desired trajectories. Since all of the formulations in this book have selected the generalized coordinates to be either joint angles or joint displacements, the techniques derived in Chapter are sometimes referred to as methods ofjoint space control. That is, the performance criteria or objective of the controllers discussed in Chapter is the minimization of an error expressed explicitly in terms of the joint degrees of freedom.

Frequently the goal or objective to be achieved via feedback control is naturally expressed in terms of variables associated with the problem at hand, but are not easily written in terms of the joint variables. For example, designing a controller that locates the tool or end effector at some position in the workspace. In this case suppose that the position of the tip of the tool is given by the vector

The control strategy should guarantee that images , images , images as images . However, the coordinates images are not the joint variables for the robotic manipulator, and the strategies discussed in Chapter are not directly applicable. In principle, it is usually possible to reformulate the equations of motion in terms of the task space variables, but this can be a tedious problem for a realistic robotic system. The set of variables images are an example of a collection of task space variables for the problem at hand. Fortunately, there are many techniques that can be used to derive task space controllers. One approach employs the task space Jacobian matrix and is summarized in the following theorem.

Theorem 7.3

Suppose that images are the images generalized coordinates of a robotic system whose governing equations have the form in Equation (6.1). Suppose further that images task space coordinates images are given. The images th entry of the task space Jacobian matrix images is defined to be

and the task space tracking error is defined to be images with images the desired task space trajectory. Assume the following:

The generalized mass matrix and nonlinear right hand side satisfy the hypothesis of Theorem 6.1.
The task space Jacobian and its derivative are continuous functions on and , respectively.
The task space Jacobian is uniformly invertible in the sense that there exist constants and such that

for all and .

Then the computed torque control law images with

yields a closed loop system for which the origin of the task space tracking error and its derivative are asymptotically stable.

Example 7.4

Consider the spherical robotic manipulator studied in Examples 6.6, 6.7, 6.8, and 6.10 in Chapter . Derive a task space controller based on Theorem 7.3 where the task space coordinates images are defined as the coordinates in the 0 frame of the origin images of frame 3. In other words, define

The controller must be designed so that the task space coordinates track the desired trajectories

(7.39)

where images and images rad images . Choose the gain matrices to be images and images , where images or images . Assume the robot's initial configuration is as shown in Figure 7.22.

Initial Configuration: θ1=π2 rad, θ2=π2 rad, dp,q=1 m — Figure 7.22 Initial Configuration: rad, rad, m.

Evaluate the performance of this controller by plotting the state trajectories, tracking error and control inputs as a function of time.

Solution

The change of coordinates images for images is needed that defines the task space coordinates images in terms of the generalized coordinates images . From kinematic analysis of the robot,

The task space Jacobian matrix can be calculated either by taking partial derivatives of this this expression, or by involving the tools developed in Chapter for the evaluation of Jacobian matrices. In either case, the Jacobian is found to be

For this specific choice of the task space coordinates, the Jacobian matrix is precisely equal to the velocity Jacobian matrix images introduced in Chapter . The time derivative images of the task space Jacobian matrix is straightforward, although lengthy, to evaluate; its derivation is omitted here. Figures 7.23 and 7.24 depict the tracking errors images and images , respectively, when the initial condition is selected to be

Tracking error, ex1(t) — Figure 7.23 Tracking error, , Example 7.4.

Tracking error, ex2(t) — Figure 7.24 Tracking error, , Example 7.4.

Tip trajectory in inertial coordinates, (x1(t),x2(t),x3(t)) — Figure 7.25 Tip trajectory in inertial coordinates, , Example 7.4.

Actuation force, f(t) — Figure 7.26 Actuation force, , Example 7.4.

Actuation moment, m1(t) — Figure 7.27 Actuation moment, , Example 7.4.

Actuation moment, m2(t) — Figure 7.28 Actuation moment, , Example 7.4.

and the configuration of the robot initially is depicted in Figure 7.22. The graph of the tracking error images is identical to that of images and is not shown. It is evident from Figures 7.23 and 7.28 that the tracking error converges to zero at an exponential rate, as predicted by the theory. The trajectory of the point images in the inertial coordinates is depicted in Figure 7.25. The trajectory converges to the straight line described parametrically by Equations (7.39). The actuation force and moments required to drive the robot are shown in Figures 7.26, 7.27 and 7.28. It is clear from the figures that there is a substantial transient torque required at startup as the gains are increased.

7.4 Task Space and Visual Control

The previous section discussed how the inner loop–outer loop architecture of the computed torque control strategy introduced in Section 6.6 of Chapter can be used to derive tracking or setpoint controllers in task space variables. This section will illustrate that this approach can be used to design controllers based on typical camera observations. The design of controllers based on task space coordinates associated with camera measurements yields stability and convergence results that are more realistic than those described in Section 7.3 of this chapter. Recall that Section 7.3 discusses the visual servo control problem where it is assumed that the input to the controller is the vector of velocities and angular velocities of the camera frame. The value of this input vector is selected so that

(7.40)

where images is the tracking error in the image plane of the feature points. However, the velocities and angular velocities of the camera frame are not the output of an actuator; they are response variables. Their values depend on the forces and moments delivered by the actuators. If the input torques and forces can be chosen such that the velocities and angular velocities satisfy Equation (7.40) exactly, then the discussion of stability and convergence in Section 7.3 applies. In practice, it is not possible to achieve the exact specification of the velocities and angular velocities in Equation (7.40).

Example 7.5

Suppose that the spherical robot manipulator studied in Example 7.2 is equipped with a camera and laser ranging system. The camera is mounted on the robot so that the camera frame coincides with the 3 frame. The camera focal length may be taken as images . A reflective target images is located as depicted in Figure 7.29, having coordinates in the 0 frame given by

Spherical robot configuration (θ1,θ2,dq,r)=(π2,π2,dq,r) and inertially fixed feature point t. — Figure 7.29 Spherical robot configuration and inertially fixed feature point .

Let images denote the coordinates relative to the camera frame of the point images so that images . Assume that the laser ranging system enables the real time measurement of the range images of the point images . Derive a controller that will orient the robot so that the image of point images approaches the origin of the image plane and the range images approaches some desired constant value images as images . Evaluate the performance of the controller through simulation. Discuss potential limitations or drawbacks of the proposed controller.

Solution

The controller will be designed using the computed torque images , where the vector images is given in terms of task space coordinates

(7.41)

as specified in Theorem 7.3. The task space coordinates are chosen as images , where the image plane coordinates images of the point images are images and images . The task space Jacobian images and its time derivative images are required to implement the control law in Equation (7.41). Note that the Jacobian images in Equation (7.41) is defined to be

and can be identified from the equation images .

It will be shown that the matrix images can be written as a product of two matrices, images , where images and images are defined in the equations

The matrix images is by definition the joint space Jacobian matrix studied in Chapter . It is possible to derive this matrix from first principles by calculating the velocity and angular velocity directly, or by using the specialized procedures derived in Chapter . Using either approach, this is found to be

The determination of the matrix images is also direct using the interaction matrix images and the matrix images introduced in Section 7.2 of this chapter. Since

the matrix images is given by

The time derivative of the Jacobian in task space variables is now computed via

where

and

Two sets of initial conditions have been used to evaluate the controller designed in this example. Figures 7.32 through 7.36 depict the results obtained when the initial condition is selected to be

The configuration of the robot for this initial condition is depicted in Figures 7.30 and 7.31.

Schematic of initial configuration of robot for case 1. — Figure 7.30 Schematic of initial configuration of robot for case .

Isometric view of initial configuration of robot for case 1. — Figure 7.31 Isometric view of initial configuration of robot for case .

Trajectory θ1(t) — Figure 7.32 Trajectory , Example 7.5, case .

Figures 7.32, 7.33, and 7.34 depict the trajectories of images and images starting with this initial condition. It is seen that

as images , which implies that the task coordinates must satisfy

as images . Figures 7.35 and 7.36 illustrate the corresponding tracking errors for images and images , respectively, which converge asymptotically to zero. The tracking error images has a magnitude on the order of the numerical accuracy images of the simulation and is not shown. Figures 7.37 and 7.38 show the actuation force and moment that drives joint 1. The magnitude of the actuation moment about joint 2 is numerically equal to zero and is also not shown.

Trajectory θ2(t) — Figure 7.33 Trajectory , Example 7.5, case .

Trajectory dq,r(t) — Figure 7.34 Trajectory , Example 7.5, case .

Tracking error ev(t) — Figure 7.35 Tracking error , Example 7.5, case .

Tracking error eZ(t) — Figure 7.36 Tracking error , Example 7.5, case .

The results of the simulation when the initial condition is selected to be

are depicted in Figures 7.41 through 7.48. The configuration of the spherical robotic manipulator associated with this initial condition is depicted in Figures 7.39 and 7.40.

Figures 7.41, 7.42, and 7.43 depict the state trajectories images and images , respectively, generated by this initial condition. In contrast to test case 1, it is the angle images that remains at images , while images and images vary substantially before they reach steady state. Figures 7.44 and 7.45 illustrate that images and images approach zero asymptotically, as expected. The tracking error images likewise approaches zero, but its amplitude is less than images for the duration of the simulation and it is not shown. The actuation force is plotted in Figure 7.46, and the actuation torque that drives joint 2 is shown in Figure 7.48. The magnitude of the torque that drives joint 1 never exceeds images N m and is therefore not shown.

7.5 Problems for Chapter 7

Problem 7.1 Figure 7.49a depicts the configuration of a camera and a calibration pattern. Assume the the axis of the camera is perpendicular to the plane of the calibration pattern and that it intersects the exact center of the calibration pattern. Further assume that the and axes of the camera are parallel to the and axes of the retinal plane. The image produced by this configuration is shown in Figure 7.49b. Given a focal length of 2.98 mm, an image resolution of 1080 1920 pixels, and a imaging sensor size of mm, determine the calibration matrix that relates the pixel coordinates ( , ) to retinal coordinates for this image.

Figure 7.49 Camera Calibration Pattern Configuration and Generated Image. (a) Configuration. (b) Generated image.

Figure 7.50 PUMA robot with attached camera.
Problem 7.2 Suppose that a camera is mounted so that its camera frame is rigidly fixed to link 3 of the PUMA robot as shown in Figure 7.50. Assume that the location and orientation of the camera relative to the 3 frame of the PUMA robot is given by the constant homogeneous transformation ,

For the following two problems let the PUMA robot be configured with

(a) Suppose that is the rotation matrix in which the frame is rotated by about the axis, and that

Which points in the world are mapped to the point in the canonical focal plane?

(b) Suppose that is the rotation matrix in which the frame is rotated by about the axis, and that

To what point in the canonical image plane is the point having inertial coordinates

mapped?
Problem 7.3 Suppose that a camera is mounted so that the camera frame is fixed rigidly to link 3 of the spherical wrist as shown in Figure 7.51.

Figure 7.51 Spherical wrist with mounted camera

Suppose that the location and orientation of the camera relative to the 3 frame of the spherical wrist is given by the constant homogenous transformation ,

(a) Suppose that is the rotation matrix parametrized by the 3‐2‐1 Euler angles that is used with to map frame 0 into the frame, where the yaw , pitch , and roll are

Suppose further that m. Which points in the are mapped to the point in the canonical retinal plane?

(b) Suppose that is the rotation matrix parametrized by the 3‐2‐1 Euler angles used to map frame 0 into the frame, where the yaw , pitch and roll are

Suppose further that . To what point in the canonical retinal plane is the point having inertial coordinates

mapped?
Problem 7.4 An ideal pinhole camera is mounted on the SCARA robot shown in Figure 7.52 so that the camera axes coincide with the 3 frame and the optical center is located at point . Find the transformation that maps the inertial coordinates of point , , to the canonical focal plane coordinates . Find the transformation that maps the inertial coordinates of to the pixel coordinates .

Figure 7.52 SCARA robot with mounted camera.
Problem 7.5 An ideal pinhole camera is mounted on the cylindrical robot shown in Figure 7.53 so that the camera axes coincide with the 4 frame and the optical center is located at point . Find the transformation that map the inertial coordinates of the point , , to the canonical focal plane coordinates . Find the transformation that maps the inertial coordinates of to the pixel coordinates .

Figure 7.53 Cylindrical robot with mounted camera.
Problem 7.6 Derive a controller so that the point on the cylindrical robot depicted in Figure 7.54 tracks the trajectory of the point . The point has coordinates relative to the inertial frame 0, . Base the controller on the task space computed torque control law studied in Theorem 7.3.

Figure 7.54 Cylindrical robot and task space trajectory tracking.

Figure 7.55 SCARA robot and task space trajectory tracking.
Problem 7.7 Derive a controller so that the point on the SCARA robot depicted in Figure 7.55 tracks the trajectory of the point . The point has coordinates relative to the inertial frame 0, . Base the controller on the task space computed torque control law studied in Theorem 7.3.
Problem 7.8 Suppose that the robot depicted in Figure 7.52 is equipped with a camera whose axes coincide with the 3 frame and laser ranging system. Let denote the coordinates relative to the camera frame of the point so that . Assume the laser ranging system enables the measurement of the coordinate of the point . Derive a controller that will actuate the robot so that the canonical focal plane coordinates of the point approach the origin of the focal plane and the depth approaches some fixed value .
Problem 7.9 Suppose that the robot depicted in Figure 7.53 is equipped with a camera whose axes coincide with the 4 frame and a laser ranging system. Let denote the coordinates relative to the camera frame of point , . Assume the laser ranging system enables the measurement of the coordinate of the point . Derive a controller that will actuate the robot so that the canonical retinal plane coordinates of point approach the origin of the focal plane and the depth approaches some fixed value .
Problem 7.10 Show that the interaction matrix for focal length is given by

Rotation angle	Maximum range	Minimum singular value
	8.8
	11.5
	22
	45
	600