1 Introduction. Control 1:

Transcription

1 1 1 Introduction Hierarchical Control for Mobile Robots Motive Autonomy Control Level We build controllers because: Real hardware ultimately responds to forces, energy, power, etc. whereas we are concerned with positions and velocities. Controllers map between the two using models. We use feedback because: Inevitable disturbances mean real systems do not, in fact, do exactly what they are told. Our models of how the system operates are imperfect due to approximations and lots of missing information. We use predictive modelling because: Real systems are low pass filters and modelling this allows us to anticipate their response and compensate beforehand. Underactuated systems cannot be moved in arbitrary directions but we can effect parallel parking using prediction.

2 1.1The Grand Hierarchy 2 Most real controllers for complex systems exhibit hierarchical structure. For a mobile robot, there is a hierarchy of control problems. 1.1 The Grand Hierarchy One reason for hierarchy is that its tough to be both smart and fast. Higher levels tend to be more abstract and deliberative while lower levels tend to be more quantitative and reactive. Another is its good software design practice. Humans find it easier to understand. Here is a hierarchy at the level of the entire system: Deliberative Autonomy Perceptive Autonomy Motive Autonomy Figure 1 Grand Hierarchy. The entire system is described here. Motive autonomy. Responsible for controlling the motion of the vehicle with respect to the environment. Requires feedback only of the motion state (position, heading, attitude, velocity) of the vehicle. Path following fits here. Perceptive Autonomy. (aka Reactive Autonomy). Responsible for responding to the immediate environment.

3 Requires feedback of the state of the environment (e.g. perception). May only need relative position estimates. Obstacle Avoidance fits here. Deliberative Autonomy. Responsible for achieving goals (aka the mission ). May use expansive models of the environment. Often needs absolute position estimates. Definitely uses a capacity to predict the conseqences of actions. Tends to do optimization. Global path planning fits here. 2 Motive Autonomy 3 1.1The Grand Hierarchy 2 Motive Autonomy Controllers can be classified along several axes of attributes. One disinction is what are you controlling: Articulation Control. Controls internal articulations. Examples include control of wheel velocities, steer angles, throttles, brakes, pan/tilt mechanisms. Based on internal motion feedback. Mobility Control. Controls the state of motion of the vehicle with respect to the environment. Based on bodily motion feedback. Another is at what level does the algorithm operate: Independent control level (SISO). This is the control of actuators as independent entities. Based on axis level feedback. Coordinated control level (MIMO). This is the control of the entire vehicle considered as one entity. Based on composite feedback generated from several components.

4 2.1 Controlled Variable The controlled variable varies by situation. position control forms and responds to errors in position coordinates. Used for pointing sensors and controlling steer angles, etc. differential or incremental position control accomplishes a measured change in position. Used for achieving small motions, perhaps based only on odometric feedback (because its better for measuring small motions than the alternatives). rate control forms and responds to errors in rate coordinates. As a separate matter, the positions and rates under control are not necessarily of the robot with respect to the world. For example, in formation control, the spacing between vehicles is measured and tweaked. 2 Motive Autonomy 4 2.1Controlled Variable Because many servos are implemented in software, it is possible to switch back and forth between speed and position control. This can be advantageous. You use velocity control for the long haul and then enter a position acquire and hold loops near the terminal position. 2.2 Requirements If it is necessary to move a precise distance or move to a precise location, then position control is likely called for. Speed control is normally fine for gross motions between resting positions. Velocity control can be important to critical in rare situations. For example lawn care and, floor care, care alot about the quality of the treatment process. On occasion, following a specified path is the whole issue. On others the path used to get somewhere is not important but the endpoint is critical.

5 On others, the path cannot be controlled because the end state predetermines the path through nonholonomic constraints. On others, massive deviation from the endpoint is possible (e.g. forktruck side shift tolerates crosstrack error) 2.3 Typical Motive Autonomy Hierarchy One hierarchy for motive autonomy that captures many of these ideas is like so: Path Following Control Trajectory Control Path Endpoint Control Instantaneous Ref. Pt. Control Instantaneous Axis Servos Level Path Control Level Endpoint Control Level Coordinated Control Level Independent Control Level Figure 2 A Control Hierarchy. Higher levels produce the reference signals for lower levels. Each layer in the hierarchy is implemented by supplying the reference signals (commands) to the immediately subordinate layer. 2 Motive Autonomy 5 2.3Typical Motive Autonomy Hierarchy It is also possible to access each layer directly as required by the application. The layers perform the following functions: Independent Control Level - Control the instantaneous state of each axis, where state may mean position or speed etc. Coordinated Control Level - Control the instantaneous state of the vehicle control point. May or may not be closed loop. Endpoint Control Level - Control the position of the vehicle at the end of a trajectory without regard for the path taken to get there. Used for goal acquisition and corrective trajectories. Path Control Level- Control the crosstrack error of the vehicle as it follows a path by servoing it to zero at some lookahead.

6 3 Independent Control Level The simplest positional capabilities of a vehicle can be implemented with PID (proportional-integral-derivative) loops. These loops are simple in the following respects: They consider only the instantaneous value of their reference inputs conducting no lookahead. They react simply to the current error signal. They are not coordinated with other servos that execute simultaneously. It is important to be clear on the difference between the controlled axes and the associated vehicle state. Often, for example, the position of an axis (such as a throttle or a steer angle) maps onto the linear or angular speed of the vehicle. 3.1 Axis Actuators Axis actuators include such things as engine throttles, motors, hydraulic jack valves, and lead screws. 3 Independent Control Level 6 3.1Axis Actuators There is usually a calibration required which maps the computed output onto quantity actuated. There can also be kinematics required to convert coordinates in minor ways. It is normally best to hide such kinematics here rather than to propagate them up into higher level control loops. 3.2 Axis Servo Control Laws A generic PID servo loop can be constructed and several instances used for each axis. The generic characteristics of the loops themselves are as shown below: x d x d x d k i k p kd u x x System Figure 3 Generic Axis Servo. This structure covers alot of real cases.

7 In the figure, x and its time derivatives denote the actual state of the system whereas x d and its derivatives denote the commanded or desired state of the system. A control law of the following PID form is most general for a system of second order open loop dynamics: ut () = x d () t + k e d () t + k p et () + k i et () dt The error terms are as follows: e () t = x d () t x () t e () t = x d () t x () t et () = x d () t xt () Suppose the system has open loop dynamics of the form: ut () x () t which is to say the input is a force applied to a unit mass. (1) (2) = (3) 3 Independent Control Level 7 3.3Integral Term Substituting the control into this permits cancelling ut () to get: e () t + k d e () t + k p et () + k i et () dt 0 With the integral term, this is a 3rd order system. Without it, it is a second order system which can be tuned to achieve critical damping. Without the integral term, the characteristic equation is: This is critically damped when: 3.3 Integral Term While it may seem that an integrated position error is meaningless, note that this term continues to grow if the system has stopped slightly away from its goal position. = (4) s 2 + k d s+ k p = 0 (5) k d 2 4k p = 0 (6)

8 Without this term, and the position error gain may be insufficient to overcome friction, since the speed and acceleration terms generate no output. The integral term will cause the system to continuously seek its goal position and oscillate around it at very low amplitude. 3.4 Proportional Term Note that the proportional gain k p is analogous to a spring constant since the output is proportional to the error deflection. Hence, it is often called servo stiffness. 3.5 Derivative Term The derivative gain generates a control signal (lets say a torque) whose amplitude is proportional to a velocity difference. Hence it is often described in terms of viscous effects. 3.6 Missing Commands and Feedback In a pinch, the equation for et () can be differentiated numerically to generate the derivative error terms but consistent command 3 Independent Control Level 8 3.4Proportional Term derivatives and real measurements of rates are better. 3.7 Profile Generation At times, it is useful to generate an entire command trajectory in the single axis case rather than asking for some distant terminal state. This is the case where a consistent set of demands can be generated easily. A common example is the generation of a trapezoidal velocity profile: Speed Short Time Duration Speed Long Time Duration Figure 4 Trapezoidal Profile Generation. Such a trajectory permits analytic computation of consistent position, rate, and acceleration demands.

9 4 Coordinated Control Level 4.1 A Generic Coordinated Control Hierarchy Coordinated control is also called MIMO (multi-input multi-output) control. Here all elements of the state vector are controlled as a unit. Here is a more detailed cut at a generic hierarchy for this level and those above it. All of these elements are based only on vehicle position and velocity feedback. Example nterpolate Between Waypoints Stay on the path Control Formation Hit recovery point on path Ramp speed to stop soon Squeeze between two trees Hold at 30 mph Move wheels at same speed Stop at waypoint Crosstrack Servos Trajectory Generation Trajectory Follower Coordinated Control Hierarchy Path Interpolation Alongtrack Servos Speed Profile Generation Cruise Control Position Hold Level Waypoint Seeking Level Path/Object Followe Level Goal Elaboration Level Goal Acquisition Level Coordinated Contro Level 4 Coordinated Control Level 9 4.1A Generic Coordinated Control Hierarchy Again, higher levels may use lower levels to do their job. The three levels in the earlier diagram are five here. Coordinating actuators requires that their responses be: consistent: so that their net effect is what is desired. synchronized: so that they have the right values at the right times. 4.2 Basic Coordinated Control This part implements at least some form of kinematics to keep actuators moving in relative lockstep. One example is driving all the wheels at potentially different rates in order to achieve some precise rigid body motion. See kinematics of wheeled mobile robots in next lecture for how to do this. Figure 5 Coordinated Control Hierarchy. All of these elements are based only on vehicle position and velocity feedback.

10 4.3 Position Hold This is a special very tight loop that is entered when near the end of a trajectory where the terminal position matters alot. It measures position error and stops the vehicle at just the right spot. 4.4 Trajectory Follower Another aspect may be synchronization. Suppose for example that the curvature profile of some intended motion is given: κ( s) = given (7) It is often the case that error along the path (alongtrack) can be ignored only the crosstrack error matters. The throttle and steering actuators can be coordinated to achieve open loop following by sacrificing speed following for curvature following. Whatever the speed is, right or wrong, compute the distance s actually travelled and use this quantity, rather than time, to compute the associated curvature command. 4 Coordinated Control Level Position Hold 4.5 Cruise Control Just like it sounds, this is a servo on vehicle speed-over-the-ground. Just maintaining motion is not so easy on rough terrain - let alone maintaining constant speed. Often used when precise path following is not important (e.g. waypoint seeking). 4.6 Trajectory Generation An elaborative control. Computes the trajectory to a desired goal state when the path to the state is unimportant or unspecified from above. See upcoming lecture on this. 4.7 Speed Profile Generation The speed equivalent of trajectory generation. Elaborates a desired distance into a way to get there. 4.8 Crosstrack Servo Our friend the path follower. Does its best to stay on a path by driving position error transverse to some desired path to zero.

11 Perhaps by using recovery trajectories generated one level down. 4.9 Alongtrack Servos Tries to drive alongtrack position error, perhaps measured with respect to some moving object, to zero. Formation control in convoys. Docking a robot with a power receptacle. Picking up a pallet with a forktruck. When path shape is controlled separately from speed, this is called the kinodynamic approach Path Interpolation An elaborative element. If waypoints are specified very sparsely and lower level algorithms require dense waypoints, this element fills in the spaces with more waypoints using some interpolation scheme. 5 Trajectory Following Alongtrack Servos 5 Trajectory Following Let a trajectory be defined as a complete and precise specification of required motion provided over some time interval. We would like in many situations to use feedback control to ensure that the robot stays on this precisely specified path. For the present purpose, let a trajectory be represented as a specified curvature function over an interval of distance: x d ( s) κ d ( s) = (8) In the plane (or even in 3D), this is a complete description of a path through space because: θ( s) = θ 0 + κds s xs ( ) = cos[ θ( s) ] ds 0s ys ( ) = sin[ θ( s) ] ds 0 s 0 (9)

12 5.1 Open Loop Execution The simplest way to execute a trajectory is to establish a correspondence between the distance travelled by the vehicle s' and the trajectory parameter s. As the vehicle moves, the curvatures associated with the curve can be computed and issued to the hardware. As shown in Figure 6, this may only be a viable option if the initial vehicle pose is sufficiently close to the initial trajectory pose. 5 Trajectory Following Open Loop Execution 5.2 Closing a Heading Loop The previous loop required that the curvature be correct at corresponding points. A more complete solution would require that the heading be correct at corresponding points. Once an error is identified, a corrective curvature can be computed and added to the present curvature. Trajectory Vehicle Path Trajectory Vehicle Path θ = θ( s) θ' ( s' ) Figure 6 Open Loop Execution of Trajectories. Trajectories provide a good mechanism for implementing coordinated control. However, initial position (and especially heading) error may lead to large terminal errors even if the curvature commands are followed perfectly. Figure 7 Heading Servo. The error between the present vehicle heading and the heading of the corresponding point on the trajectory can be used to create a corrective trajectory. Let θ( s) refer to the heading of the trajectory at some point along the trajectory.

13 Let θ' ( s' ) refer to the heading of the vehicle at the corresponding point. A proportional controller can be used to reduce this error thus: κ = θ L (10) where L is a length whose inverse serves as the proportional gain. It is important that the corrective curvature κ be added to the open loop curvature command so that additional errors are not generated as a result of ignoring the predictable curvature profile of the trajectory. The correction amounts to increase or decrease in the bending of the curve over and above the bending encoded in the open loop curvature. This controller can be expected to drive the dc component of heading error exponentially to zero. The vehicle trajectory is effectively warped in order to make this occur. A certain amount of heading error can also be expected just because of delays and rate limitations in actuation. 5 Trajectory Following Closing a Predictive Position Loop 5.3 Closing a Predictive Position Loop Crosstrack error can be reduced by explicitly steering toward a goal point on the trajectory. A simple technique is pure pursuit which generates a corrective arc designed to reaquire the trajectory at a point slightly ahead of the vehicle. However, the availability of an explicit representation of the future shape of the desired trajectory permits a computation of a corrective curvature in a more predictive fashion. The position error at some point in the future can be predicted, and a corrective addition to

14 the curvature command can be issued now to prevent it. Trajectory Vehicle Path v 2 θ Figure 8 Predictive Crosstrack Error Servo. The predicted crosstrack error in the future is used to compute a corrective curvature to be issued now. This approach can be more stable than reacting to the present crosstrack error. In this case, the corrective curvature is: κ = θ v (11) 1 v 1 5 Trajectory Following Closing a Pose Loop 5.4 Closing a Pose Loop When trajectories are very short, as is commonly the case (particularly at the end of a otherwise long one), the use of a lookahead point is not viable. In this case, it is more convenient (although less stable) to servo based on the present error. Rather than use distance travelled in order to determine corresponding points, it is useful to compute the closest point to the present vehicle pose on the goal trajectory. This mechanism can be used to correct for alongtrack error by terminating when the closest point first becomes the terminal point on the trajectory. More generally, the coordinates of this point in the vehicle frame can be used to both determine signed crosstrack error (x The control amounts to an incremental curvature impulse which, if issued now, would correct the predicted crosstrack error at the lookahead distance

15 coordinate) and issue a stop command in time to stop at the terminal point (y coordinate). Trajectory Vehicle Path v 2 θ 5 Trajectory Following Closing a Pose Loop κ = x L 2 (13) will correct for the crosstrack error by increasing heading by the angle x L over a distance of L. Closest Point Figure 9 Instantaneous Pose Error Servo. The coordinates of the closest point, expressed in vehicle coordinates, can be used to both correct for crosstrack error with a corrective curvature and predict when to stop in order to hit the terminal point. Two corrective terms can be generated and both added to the trajectory curvature. For heading error: κ is still used. For crosstrack error: = θ L (12)

16 6 Path Following This is very similiar to trajectory following with these differences: The path is often represented as just a sequence of waypoints, so the desired curvature, etc. is not available. Paths are often specified over much larger distances. Nonetheless the basic elements are the same: Determine a measure of error. It may be present error or predicted future error. It may be a single scalar or a vector of them. Doing this often requires some way to determine some corresponding point on the path. Formulate a control which reduces the measured error. Here, a steering input is required. 6 Path Following Closing a Pose Loop A simple scheme is to steer toward some goal point which is ahead of the vehicle on the path. ψ e ψ L ψ d Path Figure 10 Pure Pursuit. The heading error with respect to the line to the goal point is the error measure.

17 The error measure is the heading error between the current vehicle heading and the heading of the line to the goal point: ψ e = ψ d ψ (14) The control is a corrective arc which removes this error after travelling a distance L: κ = ψ e L (15) Clearly, the proportional gain is: 1 K p -- L = (16) In practice, tuning L is tough. When its too small, following is poor and when its too large, the system is unstable. 6.1 Path Following with Obstacle Avoidance When path following coexists with obstacle avoidance, two issues pertain: Avoidance maneuvers can cause massive following errors which may induce instability in path following. 6 Path Following Path Following with Obstacle Avoidance Only certain steering commands are being checked for obstacles and path following must use one of these in order to be following a proofed path through the environment. The first problem can be managed by gain scheduling. When the crosstrack error is large due to obstacle avoidance, increase the lookahead by the same amount. The second problem actually leads to a much better tracker. Because high fidelity simulation is used in obstacle avoidance, a model based controller can be constructed

18 which analyses the response trajectories to pick the best based on some utility functional. Note, the vehicle is in a sharp left turn and happens to have zero heading at t=0. Figure 11 Model Referenced Pure Pursuit. A simulation is used to pick the best path the hard way. good 7 Summary Path Following with Obstacle Avoidance 7 Summary There are many forms of mobile robot controls and they can be arranged in a rough hierarchy. There is a kind of generic PID loop that covers alot of cases. Coordinated control has many different variants in mobile robots. Synchronizing curvature with speed is a very good idea. It is work to get a robot to follow an intricate path. The stability/following error tradeoff is a constant concern. Life is worse when obstacle avoidance must co-exist with path following. 8 Notes Make figures 5 and 2 more consistent. Need more pictures in these noes. 9 References

19 9 References Path Following with Obstacle Avoidance

20 9 References Path Following with Obstacle Avoidance