Software Design Overview

The control system first reads a user-supplied configuration file, then initializes components according to the specified settings. When initilization is successfully completed, the system enters the main event loop. In this loop, events are generated or captured, and dispatched to the handlers associated with the current behavior of the robot. When the main processing loop is finished, the system releases all components and exits.

The configuration information loaded at startup is stored in memory in a form that all components understand. Each component knows how to initialize and release itself as well as how to operate while the vehicle is in action. The propulsion system, DVL, DAC, altimeter, acoustics, cameras, pressure sensor, and dropper are each associated with a component object. With the exceptions of the DAC, pressure sensor, and dropper, each component begins its own thread of execution during initialization. The threads that handle the DVL, altimeter, acoustics and cameras are each responsible for continuously reading from those input sources and either updating global shared state (in the case of the DVL and altimeter) or generating events to be handled according to the current behavior (in the case of the acoustics and cameras). Thus, the system is highly concurrent, and a processing or I/O delay in one component typically will not affect the operations of other components. All concurrent access to shared data is protected with synchronization primitives implemented with the POSIX threading API.

Each component logs any pertinent information during runtime to its own log. All errors are written to a shared log, as well as to standard error. All log entries contain the exact execution and entry timestamps for debugging purposes. The configuration state of the vehicle (position, velocity, altitude, altitudinal velocity, heading, and angular velocity) are logged at each main processing loop pass, making replay of the mission possible from the logs. Any events, such as behavior changes and decisions, are also appended to a separate log. The logs are protected by synchronization primitives, so that multiple threads may safely write to the same log.

Software Platform

The vehicle's PC104 stack runs the Ubuntu Server 7.04 distribution of Linux with the 2.6.20 kernel. The main board's onboard ethernet controller is fully supported by kernel drivers . The two USB web cameras are supported by the USB Video Class (UVC) driver. The system interfaces with the WinSystems PCM-MIO data acquisition card via the manufacturer's driver. The doppler velocity logger and altimeter connect to the system's two serial ports, and communication with the propulsion, dropper, and acoustics hardware is performed via the DAC.

The control system itself is written primarily in Java. Libraries against which the software is linked, via the Java Native Interface (JNI), include the PCM-MIO and UVC drivers.

Software: Interfacing with Sensors

As described in the design overview, the sensor component objects each handle their own I/O.

Software: Propulsion and Navigation

The propulsion component takes request for navigation and translates them to thruster movements. It first reads the umpteen navigational control parameters from the configuration settings, then begins its own thread of execution to handle requests. Requests are made from behaviors running in other threads, so all access is synchronized. Requests are almost always in the form of a target waypoint, consisting of a position and heading. The propulsion thread continuously performs the following calculations: It checks the current mode of propulsion. If there is no request, it idles. If there is a waypoint request, it calculates the distance from the current position and heading to the target. It then finds the instantaneous linear and angular that brings the vehicle's configuration closer to the target. These accelerations are converted to force and torque based on the mass and moment of inertia of the vehicle, as well as its current linear and angular velocity. Next, the propulsion component converts the force and torque to desired thrust forces from each of the four thrusters. This is a linear mapping, because the force and torque equations are linear and there are four inputs (three one-dimensional forces and one torque) and four unknowns (the thrust delivered by each of the four thrusters). The delivered thrusts are scaled according to input parameters that determine how depth, position, and heading progress are weighted against each other. The thrusts are scaled and clamped to within the range possible given the thrusters, and the resulting forces are converted to control voltages using a cubic model of the thrust-to-control-voltage relationship derived for a single thruster. These control voltages are sent to the thrusters through the DAC analog outputs. The propulsion system can deliver arbitrary direction forces and torques independently of each other -- the robot can move in any direction while facing any direction.

The conversion algorithm described above changes according to input parameters. For instance,the short_distance parameter determines the distance between current state and target waypoint inside which the vehicle's acceleration will be proportional to the distance. Thus with short_distance equal to one meter, the vehicle will not attempt to slow down until it is within one meter of its waypoint. Furthermore, the vehicle will not attempt to assume its target heading until it is within short_distance of the target, instead turning towards its target for most efficient travel up to that proximity. Variables such as kp_depth and kp_heading change how the propulsion system weights differences in depth and heading (between current state and target state) over differences in position. A high kp_depth results in navigation that sacrifices linear velocity for correct direction.

Software: Behavior Control

The control system is event based, and the behavior of the vehicle is determined by reactions to events. The system groups sets of event handlers into objects called behaviors. A behavior is simply an object with arbitrary internal state that handles events generated by the system. The following events are generated:

enter

think

leave

frame

ping

Consider, as an example, a Waypoint behavior. This might handle events as follows:

enter

think

Behaviors can be implemented in compiled C++ or in interpreted code via the scripting engine.

Software: Scripting Engine

The control system includes a scripting engine, written from scratch, for a language similar to C++ or Java. The language is loosely typed and supports primitive types (real numbers, strings, 3d vectors, and lists of arbitrary objects) as well as user-defined compound object types with methods. Scripts are parsed and virtual machine code is generated from them at runtime, so changes to scripts require no recompiles of the C++ code. The scripting engine supports both interpreted and native types and functions, so any expensive processing can be implemented in C++ and called from scripts.

The real usefulness of scripts lies in the ability of the user to declare and implement behaviors in the scripts themselves. This means that arbitrary behaviors can be expressed and tested without a single recompile of any code. For example, consider the hypothetical Waypoint behavior discussed here. Here is a possible implementation of this behavior in script: 

# This is a comment line.
# Declare the behavior type.

behavior Waypoint {
	# Members
	real startTime;
	real targetDirection = 0;
	vector targetPosition;
	real timeLimit;
	vector target;

# Constructors
method Test() {}
method Test(vector t, real time) { targetPosition = t; timeLimit = time; }
method Test(vector t, real time, real d) { target = t; timeLimit = time; 
	targetDirection =d; }

# Handle the enter event by setting our waypoint.  Once the waypoint is set,
# the propulsion component will continuously move us closer to it.
# Also turn on the forward-facing camera and capture color frames.

method onEnter(robot, t) {
	startTime = t;
	setWaypoint(targetPosition, targetDirection);
	startFrontCamera(1);
}

# Handle the think event
method onThink(robot, t) {
	# How far away are we?
	diff = targetPosition - robot.position;
	dirDiff = targetDirection - robot.heading;
	# Output some debug info.
	print("Current pos: ",robot.position, "\tDistance to target: ",
		 diff.abs(),"\n");

	# Are we close enough to be done? 
	if (diff.abs() < 0.2 && abs(dirDiff) < PI/36 ) {
	   print("Attained waypoint: ", targetPosition, "\n");
	   leave();
	} else if (now()-startTime > timeLimit) {  # Timed out
	   print("Timed out.\n");
	   leave();
	}
}

# Save each frame to a file

method onFrame(robot, t, camera, image) {
	print("onFrame: t = ", t, "\n");
	saveImage(image, "frame_at_time_"+t+".jpg");
}

# This is how we identify this behavior object.

method toString() { 
	return "Waypoint["+targetPosition+","+targetDirection+"]"; }
}

# This is executed after initialization
main()
{
	# Add a new waypoint to the queue of behaviors.
	queueBehavior(new Waypoint([0,2,-1], 30, PI));
}

The scripting engine is also used to parse and evaluate configuration settings, allowing for arbitrary expressions in the configuration file. Here's an example configuration file: 

devices {
	dmm32 = "on";
	depth = "on";
	dvl = "on";
	altimeter = "on";
	propulsion = "on";
	frontcam = "on";
	downcam = "off";
}

frontcam {
	device = "/dev/video0";
	brightness = 0.5;
	contrast = 0.5;
	hue = 0;
}

downcam {
	device = "/dev/video1";
	brightness = frontcam.brightness;
	contrast = 0.5;
	hue = 0;
}

depth {
	a = 0.4;
	b = 0.5/(0.5-a);
	channel = 13;
	use_altimeter = "no";
	bottom = 4.58;
}

propulsion {
	check_supply_period = 1000000;
	supply_voltage = 33;
	channels = { 2, 3, 0, 1 };
	inversions = { 0, 0, 0, 1 };
	no_dac = 0;
	max_forward_thrust = 10*NEWTONS_PER_POUND;
	max_reverse_thrust = -4*NEWTONS_PER_POUND;
	long_distance = 1.5;
	short_distance = 0.5;
	big_turn = 3.149;
	kp_heading = 1;
	kp_depth = 4;
}

dvl {
	zero_heading = -3*PI/4;
	angle_to_forward = 3*PI/4;
	offset_from_center = [0,0.25,0];
	start_pos = [0,0,0];
	device = "/dev/ttyS0";
}

altimeter {
	device = "/dev/ttyS1";
	window = 1;
}

robot {
	no_log = 0;
}

Software: Vision Processing

Vision processing algorithms operate in real time on the system's single processor. Detecting the bins on the floor of the pool works as follows. First, the image's gradient field is calculated (both horizontal and vertical). Then, edge pixels are selected by thresholding on the magnitude of the gradient, around the mean gradient level plus one standard deviation. Then, a fast queue-based flood fill algorithm computes non-edge connected regions. Regions that are too small are discarded. Each surviving region's star-shaped boundary is determined by scanning its edge pixels rotationally from the center of mass of the region. The resulting boundary is simplified by merging nearly-collinear consecutive segments. The longest segments of the resulting coarsened boundary are selected, and their intersections are found. These intersections are used as the vertices of a candidate shape. The shape's angles are checked, and if they fall within specified bounds, the object is considered a bin.

Copyright © Duke University Robotics 2005-2006