裝配圖機(jī)器人基礎(chǔ)教程,裝配,機(jī)器人,基礎(chǔ)教程 Chapter 3 Geometric description of the robot mechanism The geometric description of the robot mechanism is based on the usage of translational and rotational homogenous transformation matrices. A coordinate frame is attached to the robot base and to each segment of the mechanism, as shown in Figure 3.1. Then, the corresponding transformation matrices between the consecutive frames are determined. A vector expressed in one of the frames can be transformed into another frame by successive multiplication of intermediate transformation matrices. Vector a in Figure 3.1 is expressed relative to the coordinate frame x 3 , y 3 , z 3 , while vector b is given in the frame x 0 , y 0 , z 0 belonging to the robot base. A mathe- matical relation between the two vectors is obtained by the following homogenous transformation bracketleftbigg b 1 bracketrightbigg = 0 H 1 1 H 2 2 H 3 bracketleftbigg a 1 bracketrightbigg . (3.1) 3.1 Vector parameters of a kinematic pair Vector parameters will be used for the geometric description of a robot mechanism. For simplicity we shall limit our consideration to the mechanisms with either par- allel or perpendicular consecutive joint axes. Such mechanisms are by far the most frequent in industrial robotics. In Figure 3.2, a kinematic pair is shown consisting of two consecutive segments of a robot mechanism, segment i?1 and segment i. The two segments are connected by the joint i including both translation and rotation. The relative pose of the joint is determined by the segment vector b i?1 and unit joint vector e i , as shown in the Figure 3.2. The segment i can be translated with respect to the segment i?1 along the vector e i for the distance d i and can be rotated around e i for the angle ? i .The coordinate frame x i , y i , z i is attached to the segment i, while the frame x i?1 , y i?1 , z i?1 belongs to the segment i?1. T. Bajd et al., Robotics, Intelligent Systems, Control and Automation: Science 23 and Engineering 43, DOI 10.1007/978-90-481-3776-3_3, c?Springer Science+Business Media B.V. 2010 24 3 Geometric description of the robot mechanism z 0 y 0 b x 0 z 1 z 2 z 3 z 4 y 1 y 2 y 3 y 4 x 1 x 2 x 3 x 4 a 0 H 1 1 H 2 2 H 3 3 H 4 Fig. 3.1 Robot mechanism with coordinate frames attached to its segments e i z i–1 y i–1 x i–1 e i+1 z i y i J i e i–1 d i b i b i–1 x i i–1 i Fig. 3.2 Vector parameters of a kinematic pair The coordinate frame x i , y i , z i is placed into the axis of the joint i in such a way that it is parallel to the previous frame x i?1 , y i?1 , z i?1 when the kinematic pair is in its initial pose (both joint variables are zero ? i = 0andd i = 0). The geometric relations and the relative displacement of two neighboring segments of a robot mechanism are determined by the following parameters: e i – unit vector describing either the axis of rotation or direction of translation in the joint i and is expressed as one of the axes of the x i , y i , z i frame. Its components are the following e i = ? ? 1 0 0 ? ? or ? ? 0 1 0 ? ? or ? ? 0 0 1 ? ? ; b i?1 – segment vector describing the segment i?1 expressed in the x i?1 , y i?1 , z i?1 frame. Its components are the following 3.1 Vector parameters of a kinematic pair 25 b i?1 = ? ? b i?1,x b i?1,y b i?1,z ? ? ; ? i – rotational variable representing the angle measured around the e i axis in the plane which is perpendicular to e i (the angle is zero when the kinematic pair is in the initial position) d i – translational variable representing the distance measured along the direction of e i (the distance equals zero when the kinematic pair is in the initial position) If the joint is only rotational (Figure 3.3), the joint variable is represented by the angle ? i , while d i = 0. When the robot mechanism is in its initial pose, the joint angle equals zero ? i = 0 and the coordinate frames x i , y i , z i and x i?1 , y i?1 , z i?1 are parallel. If the joint is only translational (Figure 3.3), the joint variable is d i , while ? i = 0. When the joint is in its initial position, then d i = 0. In this case the coordinate frames x i , y i , z i and x i?1 , y i?1 , z i?1 are parallel irrespective of the value of the translational variable d i . By changing the value of the rotational joint variable ? i , the coordinate frame x i , y i , z i is rotated together with the segment i with respect to the preceding segment i? 1 and the corresponding x i?1 , y i?1 , z i?1 frame. By changing the translational variable d i , the displacement is translational, where only the distance between the two neighboring frames is changing. e i z i–1 y i–1 e i+1 z i y i x i d i e i+1 e i–1 z i–1 e i y i–1 z i y i x i J i b i–1 b i b i–1 x i–1 e i–1 x i–1 b i i–1 i–1 i i Fig. 3.3 Vector parameters of a kinematic pair 26 3 Geometric description of the robot mechanism The transformation between the coordinate frames x i?1 , y i?1 , z i?1 and x i , y i , z i is determined by the homogenous transformation matrix taking one of the three possible forms regarding the direction of the joint vector e i . When the unit vector e i is parallel to the x i axis, there is i?1 H i = ? ? ? ? 10 0 d i + b i?1,x 0cos? i ?sin? i b i?1,y 0sin? i cos? i b i?1,z 00 0 1 ? ? ? ? , (3.2) when e i is parallel to the y i axis, we have the following transformation matrix i?1 H i = ? ? ? ? cos? i 0sin? i b i?1,x 010d i + b i?1,y ?sin? i 0cos? i b i?1,z 000 1 ? ? ? ? (3.3) and when e i is parallel to the z i axis, the matrix has the following form i?1 H i = ? ? ? ? cos? i ?sin? i 0 b i?1,x sin? i cos? i 0 b i?1,y 001d i + b i?1,z 01 ? ? ? ? . (3.4) In the initial pose the coordinate frames x i?1 , y i?1 , z i?1 and x i , y i , z i are parallel (? i = 0andd i = 0) and displaced only for the vector b i?1 i?1 H i = ? ? ? ? 100b i?1,x 010b i?1,y 001b i?1,z 000 1 ? ? ? ? . (3.5) 3.2 Vector parameters of the mechanism The vector parameters of a robot mechanism are determined in the following four steps: Step 1 – The robot mechanism is placed into the desired initial (reference) pose. The joint axes must be parallel to one of the axes of the reference coordinate frame x 0 , y 0 , z 0 attached to the robot base. In the reference pose all values of joint variables equal zero, ? i = 0andd i = 0, i = 1,2,...,n Step 2 – The centers of the joints i = 1,2,...,n are selected. The center of joint i can be anywhere along the corresponding joint axis. A local coordinate frame x i , y i , z i is placed into the joint center in such a way that its axes are parallel to the axes of the reference frame x 0 , y 0 , z 0 . The local coordinate frame x i , y i , z i is displaced together with the segment i 3.2 Vector parameters of the mechanism 27 Step 3 – The unit joint vector e i is allocated to each joint axis i = 1,2,...,n.Itis directed along one of the axes of the coordinate frame x i , y i , z i . In the direction of this vector the translational variable d i is measured, while the rotational variable ? i is assessed around the joint vector e i Step 4 – The segment vectors b i?1 are drawn between the origins of the x i , y i , z i frames, i = 1,2,...,n. The segment vector b n connects the origin of the x n , y n , z n frame with the robot end-point Sometimes an additional coordinate frame is positioned in the reference point of a gripper and denoted as x n+1 , y n+1 , z n+1 . There exists no degree of freedom between the frames x n , y n , z n and x n+1 , y n+1 , z n+1 , as both frames are attached to the same segment. The transformation between them is therefore constant. The approach to geometric modeling of robot mechanisms will be illustrated by an example of a robot mechanism with four degrees of freedom shown in Figure 3.4. The selected initial pose of the mechanism together with the marked positions of the joint centers is presented in Figure 3.5. The corresponding vector parameters and joint variables are gathered in Table 3.1. The rotational variables ? 1 , ? 2 and ? 4 are measured in the planes perpendicular to the joint axes e 1 , e 2 and e 4 , while the translational variable d i is measured along h 0 h 1 l 1 l 2 h 3 d 3 l 3 l 4 0 1 1 2 2 3 3 4 4 J 4 J 2 J 1 Fig. 3.4 Robot mechanism with four degrees of freedom 28 3 Geometric description of the robot mechanism y 0 x 0 z 0 x 4 x 3 x 2 x 1 y 4 y 3 y 2 y 1 z 4 z 3 z 2 z 1 l 2 y 0 x 0 z 0 e 2 b 4 e 3 b 2 e 4 e 1 b 1 b 3 b 0 Fig. 3.5 Positioning of the coordinate frames for the robot mechanism with four degrees of freedom the axis e 3 . Their values are zero when the robot mechanism is in its initial pose. In Figure 3.6 the robot manipulator is shown in a pose where all four variables are positive and nonzero. The variable ? 1 represents the angle between the initial and 3.2 Vector parameters of the mechanism 29 Table 3.1 Vector parameters and joint variables for the robot mechanism in Figure 3.5 i 1234 ? i ? 1 ? 2 0 ? 4 d i 00d 3 0 i 1234 0100 e i 0010 1001 i 123 4 5 000 00 b i?1 0 l 1 l 2 l 3 l 4 h 0 h 1 0 ?h 3 0 d 3 J 1 J 2 J 4 x 0 y 0 z 0 x 1 y 1 z 1 x 2 y 2 z 2 x 3 y 3 z 3 x 4 y 4 z 4 l 2 Fig. 3.6 Determining the rotational and translational variables for the robot mechanism with four degrees of freedom 30 3 Geometric description of the robot mechanism momentary y 1 axis, the variable ? 2 the angle between the initial and momentary z 2 axis, variable d 3 is the distance between the initial and actual position of the x 3 axis, while ? 4 represents the angle between the initial and momentary x 4 axis. The selected vector parameters of the robot mechanism are inserted into the ho- mogenous transformation matrices (3.2)–(3.4) 0 H 1 = ? ? ? ? c1 ?s10 0 s1 c100 001h 0 0001 ? ? ? ? , 1 H 2 = ? ? ? ? 10 0 0 0 c2 ?s2 l 1 0 s2 c2 h 1 00 0 1 ? ? ? ? , 2 H 3 = ? ? ? ? 100 0 010d 3 + l 2 001 0 000 1 ? ? ? ? , 3 H 4 = ? ? ? ? c4 ?s40 0 s4 c40 l 3 001?h 3 0001 ? ? ? ? . An additional homogenous matrix describes the position of the gripper reference point where the coordinate frame x 5 , y 5 , z 5 can be allocated 4 H 5 = ? ? ? ? 1000 010l 4 0010 0001 ? ? ? ? . This last matrix is constant as the frames x 4 , y 4 , z 4 and x 5 , y 5 , z 5 are parallel and displaced for the distance l 4 . Usually this additional frame is not even attached to the robot mechanism, as the position and orientation of the gripper can be described in the x 4 , y 4 , z 4 frame. When determining the initial (home) pose of the robot mechanism we must take care that the joint axes are parallel to one of the axes of the reference coordinate frame. The initial pose should be selected in such a way that it is simple and easy to examine, that it corresponds well to the anticipated robot tasks and that it mini- mizes the number of required mathematical operations included in the transforma- tion matrices. As another example we shall consider the SCARA robot manipulator whose geometric model was developed already in the previous chapter and is shown in Figure 2.10. The robot mechanism should be first positioned into the initial pose 3.2 Vector parameters of the mechanism 31 y 0 x 0 z 0 e 1 e 2 b 1 b 0 e 3 b 2 Fig. 3.7 The SCARA robot manipulator in the initial pose in such a way that the joint axes are parallel to one of the axes of the reference frame x 0 ,y 0 ,z 0 . In this way the two neighboring segments are either parallel or perpendic- ular. The translational joint must be in its initial position (d3 = 0).TheSCARA robot in the selected initial pose is shown in Figure 3.7. The joint coordinate frames x i ,y i ,z i are all parallel to the reference frame. Therefore, we shall draw only the reference frame and have the dots indicate the joint centers. In the centers of both rotational joints, unit vectors e 1 and e 2 are placed along the joint axes. The rotation around the e 1 vector is described by the variable ? 1 , while ? 2 represents the angle about the e 2 vector. Vector e 3 is placed along the translational axis of the third joint. Its translation variable is described by d 3 . The first joint is connected to the robot base by the vector b 0 . Vector b 1 con- nects the first and the second joint and vector b 2 the second and the third joint. The variables and vectors are gathered in the three tables (Table 3.2). In our case all e i vectors are parallel to the z 0 axis, the homogenous transfor- mation matrices are therefore written according equation (3.4). Similar matrices are obtained for both rotational joints. 32 3 Geometric description of the robot mechanism Table 3.2 Vector parameters and joint variables for the SCARA robot manipulator i 123 ? i ? 1 ? 2 0 d i 00d 3 i 123 000 e i 000 11-1 i 123 000 b i?1 0 l 2 l 3 l 1 00 0 H 1 = ? ? ? ? c1 ?s100 s1 c100 001l 1 0001 ? ? ? ? . 1 H 2 = ? ? ? ? c2 ?s200 s2 c20l 2 0010 0001 ? ? ? ? . For the translational joint, ? 3 = 0 must be inserted into equation (3.4), giving 2 H 3 = ? ? ? ? 100 0 010 l 3 001?d 3 000 1 ? ? ? ? . With postmultiplication of all three matrices the geometric model of the SCARA robot is obtained 0 H 3 = 0 H 1 1 H 2 2 H 3 = ? ? ? ? c12 ?s12 0 ?l 3 s12?l 2 s1 s12 c12 0 l 3 c12 + l 2 c1 001l 1 ?d 3 000 1 ? ? ? ? . We obtained the same result as in previous chapter, however in a much simpler and more clear way. Chapter 7 Robot control The problem of robot control can be explained as a computation of the forces or torques which must be generated by the actuators in order to successfully ac- complish the robot task. The appropriate working conditions must be ensured both during the transient period as well as in the stationary state. The robot task can be presented either as the execution of the motions in a free space, where position con- trol is performed, or in contact with the environment, where control of the contact force is required. First, we shall study the position control of a robot mechanism which is not in contact with its environment. Then, in the further text we shall up- grade the position control with the force control. The problem of robot control is not unique. There exist various methods which differ in their complexity and in the effectiveness of robot actions. The choice of the control method depends on the robot task. An important difference is, for ex- ample, between the task where the robot end-effector must accurately follow the prescribed trajectory (e.g. laser welding) and another task where it is only required that the robot end-effector reaches the desired final pose, while the details of the trajectory between the initial and the final point are not important (e.g. palletizing). The mechanical structure of the robot mechanism also influences the selection of the appropriate control method. The control of a cartesian robot manipulator in general differs from the control of an anthropomorphic robot. Robot control usually takes place in the world coordinate frame, which is defined by the user and is called also the coordinate frame of the robot task. Instead of world coordinate frame we often use a shorter expression, namely external coordinates. We are predominantly interested in the pose of the robot end-effector expressed in the external coordinates and rarely in the joint positions, which are also called internal coordinates. Nevertheless, we must be aware that in all cases we directly control the internal coordinates i.e. joint angles or displacements. The end-effector pose is only controlled indirectly. It is determined by the kinematic model of the robot mechanism and the given values of the internal coordinates. Figure 7.1 shows a general robot control system. The input to the control system is the desired pose of the robot end-effector, which is obtained by using trajectory interpolation methods, introduced in the previous chapter. The variable x r represents T. Bajd et al., Robotics, Intelligent Systems, Control and Automation: Science 77 and Engineering 43, DOI 10.1007/978-90-481-3776-3_7, c?Springer Science+Business Media B.V. 2010 78 7 Robot control x r q + + – x Actuators Sensors q r Trajectory planning uInverse kinematics Control t Robot mechanism q ～ Fig. 7.1 A general robot control system x y z y J j Fig. 7.2 The RPY description of the orientation the desired, i.e. the reference pose of the robot end-effector. The x vector, describing the actual pose of the robot end-effector in general comprises six variables. Three of them define the position of the robot end-point, while the other three determine the orientation of the robot end-effector. Thus, we write x = bracketleftbig xyz??ψ bracketrightbig T .The position of the robot end-effector is determined by the vector from the origin of the world coordinate frame to the robot end-point. The orientation of the end-effector can be presented in various ways. One of the possible descriptions is the so called RPY notation, arising from aeronautics and shown in Figure 7.2. The orientation is determined by the angle ? around the z axis (Roll), the angle ? around the y axis (Pitch) and the angle ψ around the x axis (Yaw). By the use of the inverse kinematics algorithm, the internal coordinates q r , corresponding to the desired end-effector pose, are calculated. The variable q r rep- resents the joint position, i.e. the angle ? for the rotational joint and the distance d for the translational joint. The desired internal coordinates are compared to the ac- tual internal coordinates in the robot control system. On the basis of the positional error ?q, the control system output u is calculated. The output u is converted from a digital into an analogue signal, amplified and delivered to the robot actuators. The actuators ensure the forces or torques necessary for the required robot motion. The robot motion is assessed by the sensors which were described in the chapter devoted to robot sensors. 7.1 Control of the robot in internal coordinates 79 7.1 Control of the robot in internal coordinates The simplest robot control approach is based on controllers where the control loop is closed separately for each particular degree of freedom. Such controllers are suitable for control of independent second order systems with constant inertial and damp- ing parameters. This approach is less suitable for robotic systems characterized by nonlinear and time varying behavior. 7.1.1 PD control of position First, a simple proportional-derivative (PD) controller will be analyzed. The basic control scheme is shown in Figure 7.3. The control is based on calculation of the positional error and determination of control parameters, which enable reduction or suppression of the error. The positional error is reduced for each joint separately, which means that as many controllers are to be developed as there are degrees of freedom. The reference positions q r are compared to the actual positions of the robot joints q ?q = q r ?q. (7.1) The positional error ?q is amplified by the proportional position gain K p . As a robot manipulator has several degrees of freedom, the error ?q is expressed as a vector, while K p is a diagonal matrix of the gains of all joint controllers. The calculated control input provokes robot motion in the direction of reduction of the positional error. As the actuation of the robot motors is proportional to the error, it can occur that the robot will overshoot instead of stopping in the desired position. Such over- shoots are not allowed in robotics, as they may result in collisions with objects in the robot vicinity. To ensure safe and stable robot actions, a velocity closed loop is introduced with a negative sign. The velocity closed loop brings damping into the system. It is represented by the actual joint velocities ˙q multiplied by a diagonal matrix of velocity gains K d . The control law can be written in the following form K p q r q Robot+ + K d + – + – uq ~ q . Fig. 7.3 PD position control with high damping 80 7 Robot control K p q r q Robot+ + K d + + + – + + – u q r . q . q . ~ q ~ Fig. 7.4 PD position control u = K p (q r ?q)?K d ˙q, (7.2) where u represents the control inputs, i.e. the joint forces or torques, which must be provided by the actuators. From equation (7.2) we can notice that at higher veloc- ities of robot motions, the velocity control loop reduces the joint actuation and, by damping the system, ensures robot stability. The control method shown in Figure 7.3 provides high damping of the system in the fastest part of the trajectory, which is usually not necessary. Such behavior of the controller can be avoided by upgrading the PD controller with the reference velocity signal. This signal is obtained as the numerical derivative of the desired position. The velocity error is used as control input ˙ ?q = ˙q r ? ˙q. (7.3) The control algorithm demonstrated in Figure 7.4 can be written as u = K p (q r ?q)+K d (˙q r ? ˙q). (7.4) As the difference between the reference velocity ˙q r and ˙q is used instead of the total velocity ˙q, the damping effect is reduced. For a positive difference the control loop can even accelerate the robot motion. The synthesis of the PD position controller consists of determining the matrices K p and K d . For fast response, the K p gains must be high. By proper choice of the K d gains, critical damping of the robot systems is obtained. The critical damping ensures fast response without overshoot. Such controllers must be built for each joint separately. The behavior of each controller is entirely independent of the controllers belonging to the other joints of the robot mechanism. 7.1.2 PD control of position with gravity compensation In the chapter on robot dynamics we found that the robot mechanism is under the influence of inertial, Coriolis, centripetal and gravitational forces (4.46). In general 7.1 Control of the robot in internal coordinates 81 also friction forces, occurring in robot joints, must be included in the robot dynamic model. In a somewhat simplified model, only viscous friction, being proportional to the joint velocity, will be taken into account (F v is a diagonal matrix of the joint fric- tion coefficients). The enumerated forces must be overcome by the robot actuators which is evident from the following equation, similar to equation (4.46) B(q)¨q + C(q, ˙q)˙q + F v ˙q + g(q)=τ. (7.5) When developing the PD controller, we did not pay attention to the specific forces influencing the robot mechanism. The robot controller calculated the required actu- ation forces solely on the basis of the difference between the desired and the actual joint positions. Such a controller cannot predict the force necessary to produce the desired robot motion. As the force is calculated from the positional error, this means that in general the error is never equal to zero. When knowing the dynamic robot model, we can predict the forces which are necessary for the performance of a par- ticular robot motion. These forces are then generated by the robot motors regardless of the positional error signal. In quasi-static conditions, when the robot is moving slowly, we can assume zero accelerations ¨q ≈ 0 and velocities ˙q ≈ 0. The robot dynamic model is simplified as follows τ ≈ g(q). (7.6) According to equation (7.6), the robot motors must above all compensate the gravity effect. The model of gravitational effects ?g(q) (the circumflex denotes the robot model), which is a good approximation of the actual gravitational forces g(q), can be implemented in the control algorithm as shown in Figure 7.5. The PD controller, showninFigure7.3, was upgraded with an additional control loop, which calculates the gravitational forces from the actual robot position and directly adds them to the controller output. The control algorithm shown in Figure 7.5 can be written as follows u = K p (q r ?q)?K d ˙q + ?g(q). (7.7) K p q r q + u+ + Robot+ + K d + – + – q g(q) ^ q . ~ Fig. 7.5 PD control with gravity compensation 82 7 Robot control By introducing gravity compensation, the burden of reducing the errors caused by gravity, is taken away from the PD controller. In this way the errors in trajectory tracking are significantly reduced. 7.1.3 Control of the robot based on inverse dynamics When studying the PD controller with gravity compensation, we investigated the robot dynamic model in order to improve the efficiency of the control method. With the control method based on inverse dynamics, this concept will be further upgraded. From the equations describing the dynamic behavior of a two-segment robot ma- nipulator (4.46), we can clearly observe that the robot model is nonlinear. A linear controller, such as the PD controller, is therefore not the best choice. We shall derive the new control scheme from the robot dynamic model described by equation (7.5). Let us assume that the torques τ, generated by the motors, are equal to the control outputs u. Equation (7.5) can be rewritten B(q)¨q + C(q, ˙q)˙q + F v ˙q + g(q)=u. (7.8) In the next step we will determine the direct robot dynamic model, which describes robot motions under the influence of the given joint torques. First we express the acceleration ¨q from equation (7.8) ¨q = B ?1 (q)(u?(C(q, ˙q)˙q + F v ˙q + g(q))). (7.9) By integrating the acceleration, while taking into account the initial velocity value, the velocity of robot motion is obtained. By integrating the velocity, while taking into account the initial position, we calculate the actual positions in the robot joints. The direct dynamic model of a robot mechanism is shown in Figure 7.6. In order to simplify the dynamic equations, we shall define a new variable n(q, ˙q) comprising all dynamic components except the inertial component n(q, ˙q)=C(q, ˙q)˙q + F v ˙q + g(q). (7.10) The robot dynamic model can be described with the following shorter equation B(q)¨q + n(q, ˙q)=τ. (7.11) In the same way also equation (7.9) can be written in a shorter form ¨q = B ?1 (q)(u?n(q, ˙q)). (7.12) Let us assume that the robot dynamic model is known. The inertial matrix ? B(q) is an approximation of the real values B(q), while ?n(q, ˙q) represents an approximation of n(q, ˙q) as follows 7.1 Control of the robot in internal coordinates 83 B –1 (q) + + – q F g(q) u Robot + + + + + + n(q, q) . q¨ q . C(q, q) . n Fig. 7.6 The direct dynamic model of a robot mechanism B –1 (q) +y ++ – q q n(q, q) u + + Robot B(q) ^ ^ q ¨ q . q . . n(q, q) . Fig. 7.7 Linearization of the control system by implementing the inverse dynamic model ?n(q, ˙q)= ? C(q, ˙q)˙q + ? F v ˙q + ?g(q). (7.13) The controller output u is determined by the following equation u = ? B(q)y + ?n(q, ˙q), (7.14) where the approximate inverse dynamic model of the robot was used. The system, combining equations (7.12)and(7.14),isshowninFigure7.7. Let us assume the equivalence ? B(q)=B(q) and ?n(q, ˙q)=n(q, ˙q). In Figure 7.7 we observe that the signals ?n(q, ˙q) and n(q, ˙q) subtract, as one is presented 84 7 Robot control B(q) y + n(q, q) u + + Robot y ^ . ^ q q q . q . Fig. 7.8 The linearized system with a positive and the other with a negative sign. In a similar way, the product of matrices ? B(q) and B ?1 (q) results in a unit matrix, which can be omitted. The simplified system is shown in Figure 7.8. By implementing the inverse dynamics (7.14), the control system is linearized, as there are only two integrators between the input y and the output q. The system is not only linear, but is also decoupled, as e.g. the first element of the vector y only influences the first element of the position vector q. From Figure 7.8 it is also not difficult to realize that the variable y has the characteristics of acceleration, thus y = ¨q. (7.15) In an ideal case, it would suffice to determine the desired joint accelerations as the second derivatives of the desired joint positions and the control system will track the prescribed joint trajectories. As we never have a fully accurate dynamic model of the robot, always a difference will occur between the desired and the actual joint positions and will increase with time. The positional error is defined by ?q = q r ?q, (7.16) where q r represents the desired robot position. In a similar way also the velocity error can be defined as the difference between the desired and the actual velocity ˙ ?q = ˙q r ? ˙q. (7.17) The vector y, having the acceleration characteristics, can be now written as y = ¨q r + K p (q r ?q)+K d (˙q r ? ˙q). (7.18) It consists of the reference acceleration ¨q r and two contributing signals which de- pend on the errors of position and velocity. These two signals suppress the error arising because of the imperfectly modeled dynamics. The complete control scheme isshowninFigure7.9. By considering equation (7.18) and the equality y = ¨q, the differential equation describing the robot dynamics can be written as ¨ ?q + K d ˙ ?q + K p ?q = 0, (7.19) 7.2 Control of the robot in external coordinates 85 B(q) y q + n(q, q) u + + Robot K d + + ++ + + + – – Inverse dynamics Position control u Robot Inverse dynamics y Position control q r ¨ q r q r . q r ¨ q r q r . q ~ . q ~ K p ^ ^ . q . q q . + Fig. 7.9 Control of the robot based on inverse dynamics where the acceleration error ¨ ?q = ¨q r ? ¨q was introduced. The differential equation (7.19) describes the time dependence of the control error as it approaches zero. The dynamics of the response is determined by the gains K p and K d . 7.2 Control of the robot in external coordinates All the control schemes studied up to now were based on control of the internal coordinates, i.e. joint positions. The desired positions, velocities and accelerations were determined by the robot joint variables. Usually we are more interested in the motion of the robot end-effector than in the displacements of particular robot joints. At the tip of the robot, different tools are attached to accomplish various robot tasks. In the further text we shall focus on the robot control in the external coordinates. 7.2.1 Control based on the transposed Jacobian matrix The control method is based on the already known equation (4.17) connecting the forces acting at the robot end-effector with the joint torques. The relation is defined by the use of the transposed Jacobian matrix 86 7 Robot control τ = J T (q)f, (7.20) where the vector τ represents the joint torques and f is the force at the robot end- point. It is our aim to control the pose of the robot end-effector, where its desired pose is defined by the vector x r and the actual pose is given by the vector x. The vectors x r and x in general comprise six variables, three determining the position of the robot end-point and three for the orientation of the end-effector, thus x = bracketleftbig xyz??ψ bracketrightbig T . Robots are usually not equipped with sensors assessing the pose of the end-effector; robot sensors measure the joint variables. The pose of the robot end-effector must be therefore determined by using the equations of the direct kinematic model x = k(q) introduced in the chapter on robot kinematics (4.4). The positional error of the robot end-effector is calculated as ?x = x r ?x = x r ?k(q). (7.21) The positional error must be reduced to zero. A simple proportional control sys- tem with the gain matrix K p is introduced f = K p ?x. (7.22) When analyzing equation (7.22) more closely, we find that it reminds us of the equa- tion describing the behavior of a spring, where the force is proportional to the spring elongation. This consideration helps us to explain the introduced control principle. Let as imagine that there are six springs virtually attached to the robot end-effector, one spring for each degree of freedom (three for position and three for orientation). When the robot moves away from the desired pose, the springs are elongated and pull the robot end-effector into the desired pose with the force proportional to the positional error. The force f therefore pushes the robot end-effector towards the de- sired pose. As the robot displacement can only be produced by the motors in the joints, the variables controlling the motors must be calculated from the force f.This calculation is performed by the help of the transposed Jacobian matrix as shown in equation (7.20) u = J T (q)f. (7.23) The vector u represents the desired joint torques. The control method based on the transposed Jacobian matrix is shown in Figure 7.10. J T (q) x r q Robot+ x + – K p k(?) uf x ~ Fig. 7.10 Control based on the transposed Jacobian matrix 7.2 Control of the robot in external coordinates 87 7.2.2 Control based on the inverse Jacobian matrix The control method is based on the relation between the joint velocities and the velocities of the robot end-point (4.10), which is given by the Jacobian matrix. In equation (4.10) we emphasize the time derivatives of external coordinates x and internal coordinates q ˙x = J(q)˙q ? dx dt = J(q) dq dt . (7.24) As dt appears in the denominator on both sides of equation (7.24), it can be omitted. In this way we obtain the relation between changes of the internal coordinates and changes of the pose of the robot end-point dx = J(q)dq. (7.25) Equation (7.25) is valid only for small displacements. As with the previously studied control method, based on the transposed Jacobian matrix, also in this case we first calculate the error of the pos