Sensing Intelligence Motion - How Robots & Humans Move - Vladimir J. Lumelsky Part 3 pot

36 A QUICK SKETCH OF MAJOR ISSUES IN ROBOTICS External Torques (Terms 5, 6, and 7). These are due to external forces and torques that come from the arm’s interaction with other objects; they appear when the arm physically touches objects, such as in assembly or cleaning. During a free arm motion these torques are zeros. Then there is another classification of dynamic torques, also with three types: Inertial Torques. These are proportional to accelerations in the arm joints, and they arise from normal action/reaction forces of an accelerating body. Centripetal Torques. These torques arise from a constrained rotation about a point, and they are proportional to the squares of joints’ velocities. For example, the arm’s forearm must rotate about the arm’s shoulder joint, and so the centripetal acceleration is aimed at the shoulder joint along link l 1 (Figure 2.1). Coriolis Torques. These torques arise from vortical forces, as a result of inter- action between two rotating systems (in our case two arm links), and they are proportional to the product of joint velocities of two different links. Notice the remarkable growth in the complexity of equations as we proceed from kinematics to statics to dynamics equations. Then there is another natural source of complexity—the arm complexity, measured by the number of robot degrees of freedom. The reader is reminded that in our example, Figure 2.1, we are dealing with the simplest two-link planar manipulator: In its analysis we started with modest kinematic equations (2.2) and (2.3) and arrived at rather complex dynamic equations in (2.14). Will the equations complexity grow as quickly with the growth in the number of robot DOF? Indeed they will. As an example, if we write only the acceleration-related coefficients for an arm with six DOF, they form this 6 × 6 matrix (note that a great many of today’s industrial robot arm manipulators have six or more DOF): D =        D 11 D 12 D 13 D 14 D 15 D 16 D 12 D 22 D 23 D 24 D 25 D 26 D 13 D 23 D 33 D 34 D 35 D 36 D 14 D 24 D 34 D 44 D 45 D 46 D 15 D 25 D 35 D 45 D 55 D 56 D 16 D 26 D 36 D 46 D 56 D 66            (2.15) The diagonal terms in this matrix represent uncoupled terms—that is, terms caused by a single joint—and off-diagonal terms represent pairwise interaction effects for all six joints. Each such term is itself a rather complex relationship. Out of curiosity, if the very first term in the matrix above, D 11 , is written in full, it looks as shown in Figure 2.6 [9]. As you glance at this formula, try to imagine whatthewholematrixD must look like, and imagine what kind of complexity a control system based on such expressions must involve. Consequently, all kind of simplifications are done in real-world systems when designing robot control schemes. Simplifications in equations bring, of course, imprecision, and so the design process involves carefully studied trade-offs. FEEDBACK CONTROL 37 D 11 = m 1 k 2 122 + m 2  k 2 211 s 2 θ 2 + k 2 233 c 2 θ 2 + r 2 (2y 2 + r 2 )  + m 3  k 2 322 s 2 θ 2 + k 2 333 c 2 θ 2 + r 3 (2z 3 +r 3 )s 2 θ 2 +r 2 2  + m 4  1 2 k 2 411  s 2 θ 2 (2s 2 θ 4 −1) +s 2 θ 4  + 1 2 k 2 422 (1 +c 2 θ 2 +s 2 θ 4 ) + 1 2 k 2 433  s 2 θ 2 (1 −2s 2 θ 4 ) − s 2 θ 4  + r 2 3 s 2 θ 2 +r 2 2 −2y 4 r 3 s 2 θ 2 +2z 4 (r 2 sθ 4 + r 3 sθ 2 cθ 2 cθ 4 )  + m 5  1 2 (−k 2 511 + k 2 522 + k 2 533 )  (sθ 2 sθ 5 − cθ 2 sθ 4 cθ 5 ) 2 + c 2 θ 4 c 2 θ 5  + 1 2 (k 2 511 −k 2 522 + k 2 533 )(s 2 θ 4 +c 2 θ 2 c 2 θ 4 ) + 1 2 (k 2 511 +k 2 522 − k 2 533 )  (sθ 2 cθ 5 + cθ 2 sθ 4 sθ 5 ) 2 +c 2 θ 4 s 2 θ 5  + r 2 3 s 2 θ 2 −r 2 2 + 2z 5  r 3 (s 2 θ 2 cθ 5 + sθ 2 sθ 4 cθ 4 sθ 5 ) − r 2 cθ 4 sθ 5  + m 6  1 2 (−k 2 611 + k 2 622 + k 2 633 )  (sθ 2 sθ 5 cθ 6 − cθ 2 sθ 4 cθ 5 cθ 6 − cθ 2 cθ 4 sθ 6 ) 2 + (cθ 4 cθ 5 cθ 6 −sθ 4 sθ 5 ) 2  + 1 2 (k 2 611 −k 2 622 + k 2 633 )  (cθ 2 sθ 4 cθ 5 sθ 6 −sθ 2 sθ 5 sθ 6 − cθ 2 cθ 4 cθ 6 ) 2 +(cθ 4 cθ 5 sθ 6 +sθ 4 cθ 6 ) 2  + 1 2 (k 2 611 +k 2 622 − k 2 633 )  (cθ 2 sθ 4 sθ 5 +sθ 2 cθ 5 ) 2 +c 2 θ 4 s 2 θ 5  −  r 6 cθ 2 sθ 4 sθ 5 + (r 6 cθ 5 +r 3 )sθ 2  2 + (r 6 cθ 4 sθ 5 −r 2 ) 2 +2z 6  r 6 (s 2 θ 2 c 2 θ 5 +c 2 θ 4 s 2 θ 5 − c 2 θ 2 s 2 θ 4 s 2 θ 5 +2sθ 2 cθ 2 sθ 4 sθ 5 cθ 5 ) − r 3 (sθ 2 cθ 2 sθ 4 sθ 5 + s 2 θ 2 cθ 5 ) − r 2 cθ 4 sθ 5  Figure 2.6 The full expression for one term, D 11 , of the matrix (2.15). 2.4 FEEDBACK CONTROL In the jargon of control theory, the system under control is called a plant.In our case the plant is a robot arm manipulator. In doing its job of controlling the motion of arm motors, the robot control system realizes an appropriate control law, which is the relationship between the system’s input and output. For the arm control, the input is the arm’s desired position(s), and the output is the arm’s corresponding actual position. One can distinguish three types of control: 38 A QUICK SKETCH OF MAJOR ISSUES IN ROBOTICS • Open-loop control, where the control action is applied regardless the system errors • Linear control, in which the control law is a linear relationship • Nonlinear control, in which the control law is a nonlinear relationship Because there are a great number of nonlinear relationships, the term “nonlinear control” calls for further precision. Typically, the nonlinear control is more com- plex to realize than linear control, so a need for nonlinear control suggests that the system in question is quite complex. One finds in literature many terms related to different methods of nonlinear control: switching controls (which are further divided into a bang-bang control, duty-cycle modulation, logic control), globally nonlinear feedback mapping (e.g., saturating controls), adaptive control (with its own division into, for example, model-based reference control and self-tuning control), and so on. Both linear and nonlinear control are typically realized as a feedback-based control, as opposed to open-loop control. In a feedback con- trol system the control law becomes a feedback control law, and is calculated based on the desired system behavior and the contemplated error (i.e. difference between the system’s input and output), forming a feedback loop (Figure 2.7). Assume that each of the two joints of our arm (Figure 2.1) has a torque motor with a unity input-to-torque conversion coefficient. As before, denote the motor torques at links l 1 and l 2 as n 0,1 and n 1,2 , respectively. To demonstrate how the control system is synthesized, we will use an independent joint controller, called also a linear decentralized feedback law. Assume that the desired joint angles are given. The control law we choose is of proportional-integral-derivative (PID) type, a widely used type of controller. In Figure 2.8, K is position (error) gain, L is integral (error) gain, and H is derivative (error) gain. In simple terms, the controller’s position feedback component improves the speed of response of the control system; the integral feedback component ensures a steady-state tracking Feedback law Desired output Actua l output Disturbances Plant Figure 2.7 A sketch of a feedback-based control system. FEEDBACK CONTROL 39 C ommande d j o i nt ang l e D i stur b ance s f, n M otor S ensorRo b ot K L H q d – q q d q + − + Σ + + q e Figure 2.8 A PID control system. of the desired output; and the derivative feedback component enhances the system stability and reduces oscillation due to control. Equations characterizing the control law here will come out as (see Ref. 7), n 0,1 = K 01 (θ 1 − θ 1d ) + L 01 e 1 + H 01 θ 1 ˙e 1 = θ 1 − θ 1d (2.16) and n 1,2 = K 12 (θ 2 − θ 2d ) + L 12 e 2 + H 12 θ 2 ˙e 2 = θ 2 − θ 2d (2.17) Steady-State Analysis. Assume that the commanded angles (θ 1d ,θ 2d ) are con- stant, the disturbances are constant, and the control system is stable. System stability implies that eventually all time derivatives approach zero, which, using Eq. (2.14), gives θ 1 = θ 1d θ 2 = θ 2d e 2 = 1 L 12  1 2 m 2 l 2 g 2 cos(θ 1d + θ 2d ) + n 2,3  + 1 L 12  l 2 cos(θ 1d + θ 2d )f 2,3y − l 2 sin(θ 1d + θ 2d )f 2,3x  e 1 = 1 L 01  L 12 e 2 +  1 2 m 1 + m 2  l 1 g 2 cos θ 1d  + 1 L 01  l 1 cos θ 1d f 2,3y − l 1 sin θ 1d f 2,3x  (2.18) Analysis of these equations shows that steady-state tracking will occur regardless of disturbances. Note that the value of error e 1 must compensate for the value 40 A QUICK SKETCH OF MAJOR ISSUES IN ROBOTICS of e 2 but not vice versa, which is understandable given the links’ sequential connection. It can be shown that if the gains L 01 and L 12 are zeroes, then joint values θ 1 and θ 2 differ from their desired values by amounts proportional to the disturbance values and inversely proportional to the feedback gains K 01 and K 12 . If, in turn, both gains K are zero, then the system’s steady state is independent of the commands and reflects only the balance of gravitational and disturbance forces. This of course indicates the importance of the feedback. Dynamic Stability Analysis. This is done to verify the stability hypothesis. Suppose that n 2,3 = 0, f 2,3x = 0, and f 2,3y = 0. Assume a small initial error δθ 1 (t), δθ 1 (0) = θ 1 − θ 1d δθ 1 (0) = θ 1 − θ 1d and constant θ 1d and θ 2d for t>0. Then, assuming that stability can be achieved via feedback, linearized equations in terms of δθ 1 (t) and δθ 2 (t) are written. Stability of those equations can be assessed by applying to them the Laplace transform and studying the characteristic polynomial [7]. Tests for stability are in general difficult to apply, so simpler necessary conditions are used, followed by a detail experimental verification. 2.5 COMPLIANT MOTION When the robot is expected to physically interact with other objects, additional care has to be taken to ensure a smooth operation. Imagine, for example, that the robot has to move its hand along a straight line, on a flat surface, say a table. It is easy to program such motion, but what if the table has a tiny bump right along the robot’s path? The robot will attempt to produce a straight line, effectively trying to cut through the bump. Serious forces will develop, with a likely unfortunate outcome. What is needed is some mechanism for the robot to “comply” with deviations of the table’s surface from the expected surface. Two types of motion are considered in such cases: • Guarded motion, when the arm is still moving in free space, before it con- tacts an object. Position control similar to the one above is used. • Compliant motion, when the arm is in continuous contact with the object’s surface. Position control and force control are then used simultaneously. Consider an example: Let us say that our task requires the robot to grasp an object A (see Figure 2.9) that is initially positioned on top of object B 1 , move it first into contact with the surface of table T , then slide it along T until it contacts an object B 2 , and stop there. Let us assume that the grasping operation itself presents no difficulty and that the grasp is a rigid grasp; that is, for all COMPLIANT MOTION 41 T A x y B 1 B 1 B 1 B 1 B 2 B 2 B 2 B 2 A T A T A T (a) (b) (c) (d) p p Figure 2.9 In this task, as object A is moved by the robot arm along the surface of table T , between positions (c) and (d), the control system has to maintain a compliant contact between A and T . practical purposes the grasped object A becomes the rigid continuation of the robot hand. The contact relation between object A and objects B 1 , T ,andB 2 is each a point relation and easy to accomplish. Our primary concern is the contact relation between object A and surface T during the motion of A along T .This contact relation is an external constraint on the arm’s position control during this part of the path. The PID controller in our example in Figure 2.8 realizes positioning control; that is, it concerns itself only with the robot arm endpoint position (θ 1 ,θ 2 ) [or, equivalently, (x, y)]. In the example in Figure 2.9, such a controller would work as long as the arm moves in free space. But when the arm attempts to move its endpoint along the surface of table T , a contact constraint comes in, affecting the y component of the arm motion and acting as another “control.” With two simultaneous constraints affecting the same motion components, one from the control system and the other from a contact constraint, the system is overspecified. In other words, we have a potential conflict. Imagine that at some moment a little bump on the table will require the arm to move for an instant up and then down again. As the robot control will “insist” on moving along the straight line, the arm’s positioning system will likely fail because of inconsistency of the simultaneous constraints. Besides deviations of surface T from its model, there may be a conflict due to modeling uncertainty, controller errors, and so on. We need to resolve this conflict. The solution is to (a) add a force control at this part of the path and (b) limit the force and positioning controls so that each type never creates an overspecified 42 A QUICK SKETCH OF MAJOR ISSUES IN ROBOTICS system. In the task in Figure 2.9 the motion will be planned in two stages: 1. A guarded motion in the y direction will be used for the part of the path in free space, namely from the arm initial position (Figure 2.9a) to grasping the object A (Figure 2.9b) to the position when object A contacts surface T (Figure 2.9c). Only position control will be used at this stage. (Since object A is immobile during the grasping operation, let us assume that such control will suffice for grasping.) 2. Compliant motion control will be done during the part of motion where object A is in continuous contact with table T , between positions shown in Figures 2.9c and 2.9d. Both position and force control will be used at this stage: position control in the direction x and force control in direction y. Here is why this control is called compliant. During this part of the path the control system will only attempt to maintain a set force pressure in the y direction. If a little bump is encountered on the table, the arm’s attempt to maintain the same y coordinate as before will instantaneously develop a stronger reaction force from the table. As the arm’s control measures and reacts to forces in this y direction, it will then comply, gently raising the arm enough to keep the same action/reaction forces in the y direction. As the bump is passed, the reaction force will quickly decrease, and the arm’s control will move the arm endpoint a notch down, just to maintain the force at the set value. This hybrid controller therefore has two feedback loops (see Figure 2.10): one for position control and one for force control. (Each loop may of course have its own complications; for example, each can be built as a PID controller shown in Figure 2.8.) Remember that the controller shown in Figure 2.10 can provide a successful compliance control only specifically along the y axis, which is what is needed for the task in Figure 2.9. In reality the direction of the compliance line may differ from case to case, so for the general case a more general scheme is needed. The controller shown in Figure 2.11 can handle such cases. Its main difference from the controller in Figure 2.10 is that instead of specific matrices M 1 and M 2 in Figure 2.10, a generalized constraint frame 2 ×2rotationmatrixQ is used. Matrix Q describes orientation of the constraint axes. Other inputs in the scheme are as follows: Axis s specifies the position versus control differentiation of axes, s i =  1, where 0, where s i = 1ifaxisi of constraint frame is position-controlled s i = 0ifaxisi of constraint frame is force-controlled p d = (x d ,y d ) is the desired position vector. f d = (f xd ,f yd ) is the desired force vector. R is the coordinate transformation of the force control loop. T is the coordinate transformation of the position control loop. COMPLIANT MOTION 43 x e y e f ey Σ Σ Σ Position controller Force controller − − + + x y 1 0 0 0 0 0 0 1 f ex f dx f dy x d y d x e 0 0 f ey f x f y Robot M 1 M 2 Figure 2.10 In the task shown in Figure 2.9, this compliance control scheme will pro- vide position control during the motion between the positions shown in Figures 2.9a and 2.9c and will then provide force control between the positions shown in Figures 2.9c and 2.9d. Σ Σ Σ Position controller Force controller − − + + Robot Q Diag(s) Q −1 Q (I - Diag(s)) Q −1 p d f d p f R T Figure 2.11 This scheme generalizes the scheme in Figure 2.10 to any orientation of the compliance line. 44 A QUICK SKETCH OF MAJOR ISSUES IN ROBOTICS Suppose that the line of compliant motion forms an angle δ with the horizontal. Define a unit vector along that line as u(δ). Then: • The position loop projection becomes u(δ)(p e · u(δ)). • The force loop projection becomes u(δ + π 2 )(f e · u(δ + π 2 )). These operations will be implemented properly if matrix Q is defined to align the constraint frame with the known compliance line, and vector s differentiates the directions of control loop actions: Q =  cos δ −sin δ sin δ −cos δ  s =  1 0  (2.19) 2.6 TRAJECTORY MODIFICATION Robot trajectories (equivalently, robot paths) are generated in many ways. For example, as explained in Section 1.2, not rarely a path is obtained manually: A technician brings the arm manipulator to one point at the time, he or she presses a button, and the point goes into the trajectory database. A sufficient number of those points makes for a path. Or, the path can be obtained automatically via some application-specific software. Either way, if the robot goes through the obtained path, it is very possible that the motion would be less than perfect; for example, it may be jerky or make corners that are too sharp. For some applications, path smoothness may be very critical. Then, the set of collected path points has to be further processed into a path that satisfies additional requirements, such as smoothness. Depending on the application, more requirements to the path quality may appear: a continuity of its second and even third derivatives (which relates to the path smoothness), precision of its straight line segments, and so on. That is, techniques used for modifying the robot path often emphasize appropriate mathematical properties of the path curves. The path preprocessing will likely include both position and orientation information of the path. If, for example, such work is to be done for a six-DOF arm manipulator, the desired properties of the path are expected from all DOF curve components. Common trajectory modification techniques are polynomial trajectories,which amount to the satisfaction of appropriate constraints, and straight-line interpola- tion. Polynomial Trajectories (Satisfaction of Constraints). Consider an example in Figure 2.12. We want to obtain a mathematical expression for a simple path that would bring this two-link planar arm from its initial point (position) p a to the destination point p b . Positional constraints are defined by the joint angle vectors TRAJECTORY MODIFICATION 45 p b q 2 p a q 1 Figure 2.12 By constraining the path to only starting and finishing points (p a ,p b ),a simple straight-line path between p a and p b can be used. Adding additional constraints on velocities along the path requires going to more complex path shapes. θ a and θ b at positions p a and p b , respectively. Here and below, θ, ω, and so on, are vectors whose dimensionality is equal to the number of system DOF; for a two-DOF robot arm, θ = (θ 1 ,θ 2 ), and so on. The constraints can be met by a trajectory θ (t) = (1 −f(t))θ a + f(t)θ b where function f is defined on the segment [0,1] and converts the arm’s path into a trajectory; f(a)= 0, f(b)= 1. In its simplest, the function f(t)= t produces a linear trajectory, θ(t) = (1 −t)θ a + tθ b (2.20) While geometrically a straight line is a nice trajectory, it has drawbacks: (a) A simple differentiation shows that the angular velocity of this trajectory is a con- stant, which means that if we want to connect together two such path segments, large (formally, infinite) accelerations will develop at the joint point of those segments. (b) From our path expression it is hard to know if the whole trajectory lies in the robot workspace. Recall that the workspace of our arm may have a circular dead zone around its base (Figure 2.1): If our straight line passes through that zone, it would mean that the path is not physically realizable. This suggests that we may want to add other constraints to the path. Let us add constraints on velocity, ˙ θ (a) = ω a , ˙ θ(b) = ω b . Two constraints can be met by a cubic trajectory, θ (t) = (1 −t) 2 [ θ a + (2θ a + ω a )t ] + t 2 [ θ b + (2θ b − ω b )(1 −t) ] (2.21) In terms of its execution, this trajectory is significantly more realistic than the one in (2.20). But, it still has drawbacks: (a) The trajectory does not take into account [...]... computed all at once before the motion starts; in principle, an optimal path can be found in this way Motion planning with incomplete information, also called sensor-based motion planning or on-line motion planning, or path planning with uncertainty, or the Sensing Intelligence Motion (SIM) paradigm Here the decision-making is done continuously as the robot moves along, based on on-line information, such as... by Canny [32 ] A variety of special cases shown to lead to simpler algorithms were described by Hopcroft et al [20] 2.9 MOTION PLANNING WITH INCOMPLETE INFORMATION By the mid-1980s it became clear that the inherent uncertainty of a realistic robot environment and the subsequent need for real-time sensing called for a paradigm of motion planning that would fundamentally differ from the Piano Mover’s paradigm... robot motion The problem can be fixed via additional constraints, namely by specifying accelerations α at the path’s beginning and end, α(a) and α(b) Now we have a total of six constraints; to meet them, a minimum of fifth-order polynomial is needed: θ (t) = (1 − t )3 θ a + (3 a + ωa )t + (α a + 6ωa + 12θ a ) t2 2 + t 3 θ b + (3 b − ωb )(1 − t) + (α b − 6ωb + 12θ b ) (1 − t)2 2 (2.22) Straight-Line... pb in Figure 2.12 are pa = (0, π ) and pb = ( π , π ) This corresponds, 6 3 2 respectively, to these Cartesian coordinates: √ pa = pb = 3 1 l2 , l2 2 2 √ √ √ 3 3 3 1 l1 − l2 , l1 + l2 2 2 2 2 l1 + If we provide the robot with only starting and ending points (pa , pb ) of the desired straight-line path (pa , pc , pb ) (Figure 2. 13) , then, given the robot’s joint space linear interpolation procedure, it... uncertainty and sensing were not some small irritating “engineering details” but major factors in the theoretical foundation of motion planning algorithms As it turned out, uncertainty and sensing became the very center around which the new paradigm would be built The result was the theory and practice of robot motion planning with incomplete information, or the SIM (Sensing Intelligence Motion) paradigm... during the motion then, the C-space for a 2D workspace becomes a 3D space of parameters (x, y, θ ), where θ is the robot orientation angle (Figure 2.16a) In this case, even the original polygonal robot and/or obstacles produce nonpolygonal and, in general, nonlinear grown 3D obstacles in C-space (Figure 2.16b) As complexity of the robot and obstacles increases—for example, in cases with 3D multilink... will further explore differences between these two paradigms for motion planning, the Piano Mover’s model and the Sensing Intelligence Motion model Provable Versus Heuristic Algorithms Another important distinction between algorithms is between provable (other terms: nonheuristic, exact, algorithmic) and heuristic approaches A provable motion planning algorithm is one for which there is a guarantee... becomes a very practical issue 2.8 MOTION PLANNING WITH COMPLETE INFORMATION In this type of motion planning, input information is processed before the actual motion starts This means that the input information must exist beforehand The model with complete information is formulated as follows .3 Given a solid object (robot), or a combination of solid objects, in two- or three-dimensional space, whose size,... Straight-Line Interpolation Achieving a straight-line motion with the arm shown in Figure 2.12 may be tricky The reason for that is that the arm’s rotating joints move links’ endpoints on circular curves This means that we cannot have an ideal straight line, and so we need to synthesize it from curves The more those curves and the shorter they are, the better our straight-line approximation To accomplish this,... available beforehand The way the Piano Movers strategies proceed is as follows Before the motion planning proper is attempted, the task’s configuration space (C-space) is calculated Assume for now that the robot—or, in general, an object for which the motion planning task is contemplated—is a rigid body moving in twodimensional (2D) space (see object A, Figure 2.15a) In C-space the robot shrinks to a point, . =        D 11 D 12 D 13 D 14 D 15 D 16 D 12 D 22 D 23 D 24 D 25 D 26 D 13 D 23 D 33 D 34 D 35 D 36 D 14 D 24 D 34 D 44 D 45 D 46 D 15 D 25 D 35 D 45 D 55 D 56 D 16 D 26 D 36 D 46 D 56 D 66            (2.15) The. trade-offs. FEEDBACK CONTROL 37 D 11 = m 1 k 2 122 + m 2  k 2 211 s 2 θ 2 + k 2 233 c 2 θ 2 + r 2 (2y 2 + r 2 )  + m 3  k 2 32 2 s 2 θ 2 + k 2 33 3 c 2 θ 2 + r 3 (2z 3 +r 3 )s 2 θ 2 +r 2 2  + m 4  1 2 k 2 411  s 2 θ 2 (2s 2 θ 4 −1). this way. • Motion planning with incomplete information, also called sensor-based motion planning or on-line motion planning,orpath planning with uncer- tainty,ortheSensing Intelligence Motion (SIM)