Move Server Design Specification

Achut Reddy

Team Overbot

January 9, 2004

Introduction

The Move server is at an intermediate level in the software hierarchy. It accepts commands from the higher level Map/Steer server, and issues commands in turn to the lower level control servers, such as Speed and Transmission. It is generally responsible for moving the vehicle as directed, while maintaining vehicle safety.

Open questions are marked with: *** Q:

Design Goals

• Independence and isolation from higher layers

The Move server is mostly self contained. Other than receiving MOVE commands it does not interact with the higher layers. It does not ever refer to the terrain map or make any high-level movement or planning decisions.

• Adaptive self-tuning

The Move server must be highly adaptive to changing conditions. In particular, the Move server design must dynamically adapt to:

• changing actuator performance levels

• changing vehicle dynamics

• software performance/bandwidth usage

• changing terrain and environmental conditions

The general methodology for making the Move server algorithms adaptive is as follows. The key performance statistics are identified and continuously monitored. Then these are compared to the expected or predicted values. The deviance between predicted value and actual value is computed and factored in to subsequent commands.

As a concrete example, the direction server (steering) takes a target steering angle theta and starts the steering wheel turning until the wheels have reached the target angle. This action takes an expected amount of time t(theta). The actual time taken to complete the action is measured and compared to the predicted value. The actual value might be different for any number of reasons: the torque may have increased due to softer terrain, the actuator performance may be degrading, the wheel position sensor error may be increasing, and others. Whatever the reason, the predicted value is adjusted by the actual value, and this new predicted value is used in any subsequent operations. This may result for example in over specifying the command to compensate for an underperforming actuator.

A similar adaptive method is applied to all other actuators as well.

• Simulation/Debuggability

The Move server will support simulation modes so that it can be tested offline. In particular it will be possible to run it without the map server by replacing it with a fake command generator. A "-s" command line option will be supported to run in simulation mode.

Thread Architecture

A block diagram of the Move server is shown below in Figure 1. Each box within the Move server represents a separate thread within the Move server process. In the QNX message architecture, each function which could possibly block must execute in a separate thread, so that other threads are not blocked.

FIGURE 1 - Move server block diagram

Receive Thread

This thread simply loops waiting for command messages. When a command is received, it signals the command thread (using SignalKill()) and passes the command to it for execution. The Receive thread shares a data structure (protected with a mutex) with the Command thread for communication. It then Replies to the message so the sender can unblock. The reply contain an error code which is used to communicate state information back to the Map server. In some instances (detailed below), the Move server may go into a state where it is temporarily busy, or the vehicle is not in the expected position (from the Map server's point of view). In these cases it will set an error code in the reply to indicate that.

Command Thread

This thread idles until it receives a MOVE command from the Receive Thread. The command is checked for feasibility and safety, based on a Vehicle Dynamics Model (see below). If the command is deemed safe and feasible, it proceeds to execute the command which results in moving the vehicle. The Move command specifies a distance, maxspeed, and curvature (see Messages below). If no subsequent command is received then the Move server will simply bring the vehicle to a stop. If a subsequent command is received before the current one has completed, the new command replaces the current one and gets immediately executed. Thus, as long as new commands continue to be received before the current one completes, the vehicle will be kept in smooth continuous motion. In general the Move server will try to keep the speed as high as possible without exceeding maxspeed. However several factors can cause that speed to be reduced:

• too bumpy terrain
• too steep terrain
• too sticky terrain (mud)
• too large errors in actuator
• obstacle undetected by map server
• e-stop

*** Q: any others?

The Move server needs to monitor the current position of the vehicle. It does this by querying the GPSINS server in order to determine how close the vehicle is to the target distance.

In addition, the Move server continually monitors the VORAD as a failsafe against collision. In the event that the map server did not see an obstacle but the VORAD does, the Move server will make an emergency stop.

*** Q: What happens after an emergency stop? Do we simply do a warm reset and start over, or do we keep the state that we made an emergency stop and let the higher level servers decide what to do about it?

Messages

// Move command message
struct MsgMOVE: public MsgBase {

	    static const uint32_t k_msgtype = char4('M','V','M','O');
	    float	distance;		// distance to travel (in meters)
	    float	maxspeed;		// maximum speed limit (in meters/sec)
	    float	curvature;		// 1/r, r=radius of curvature (in meters^-1)
					   	// by convention:
					   	//	zero means straight ahead
					   	//	negative means turn left
					   	//	positive means turn right
	};


// Move in REVERSE command message

struct MsgREVERSE: public MsgBase {
       static const uint32_t k_msgtype = char4('M','V','R','E');
	    float	distance;		// distance to travel (in meters)
	    float	maxspeed;		// maximum speed limit (in meters/sec)
	    float	curvature;		// 1/r, r=radius of curvature (in meters^-1)
				    	// by convention:
					   	//	zero means straight ahead
					   	//	negative means turn left
					   	//	positive means turn right
	};


// Stop command message
struct MsgSTOP: public MsgBase {

	    static const uint32_t k_msgtype = char4('M','V','S','T');
	};

FIGURE 2 - Move server messages

Dynamics Model

The Move server must incorporate at least a simple model of the vehicle dynamics in order to determine movement safety. At the very least we need pitch limits, maximum acceleration/deceleration rates, maximum turning rates. A more sophisticated model would include these as a function of speed, vehicle weight (vehicle gets lighter as fuel is consumed), lateral acceleration, etc.

Speed Server

The Move server does not issue commands to the Brake and Throttle servers directly, but through the Speed server, which provides a slightly higher level interface. The Speed server continually monitors its current speed.

	// Set vehicle speed        

	//        

	//		If "speed" is _higher_ than the current speed, vehicle will 
	//			accelerate at "rate" until its current speed matches "speed".
	//		If "speed" is _lower_ than the current speed, vehicle will 
	//			decelerate at "rate" until its current speed matches "speed".
	//			For speed enough values of "rate", the deceleration can be
	//			accomplished merely by reducing the pressure on the accelerator.
	//			For higher values, the deceleration must be accomplished by
	//			braking.
	//	 	If "speed" is the _same_ as the current speed, the message is ignored 

	struct MsgSetSpeed: public MsgBase {        
          static const uint32_t k_msgtype = char4('S','P','S','S');        
            float        speed;        // new speed (in meters/sec).  Always nonnegative.        
            float        acceleration; // acceleration (or deceleration) in meters/sec^2      
                                       // rate at which the vehicle should change its   
                                       // current speed to match the new speed.  
                                       // Always positive.        

	};

FIGURE 3 - Speed server messages