Collision Avoidance FSM
Overview
Implemented in Visual Studio 2010 / Microsoft's XNA Game Studio 4 extending the Academic Graphics MonoGames (XNA) Starter Kit
Collision spheres have a fixed radius of 50, and after considerable experimentation the most effective layout of 5 sensors was created as shown in the image above. The two outside sensors, “Left Side” and “Right Side” are used primarily for wall following, by keeping a sensor “active” the default action of turning towards the destination is negated. 49 out of 50 frames using a random number generator nothing happens when only this outside sensor is active, that 1/50th frame turns slightly away from the wall. Combined with the default action of turning towards the destination which keeps the agent against the wall the interplay between these 2 commands creates a very smooth motion when running along a long, straight wall.
|
Sensor |
Size |
Location |
Description |
|
Left Side |
50 |
200 forward, 180 left |
wall following |
|
Left |
50 |
200 forward, 60 left |
trigger to turn right |
|
Middle |
50 |
200 forward |
amplify turns |
|
Right |
50 |
200 forward, 60 right |
trigger to turn left |
|
Right Side |
50 |
200 forward, 180 right |
wall following |
|
Sensors * (left to right) |
Description |
Action |
Notes |
|
O XXX X |
wall-following |
turn right 1/50 frames |
Randomly skipping 49 out of 50 updates is used to smooths jitter when wall-following |
|
X XXX O |
wall-following |
turn left 1/50 frames |
Randomly skipping 49 out of 50 updates is used to smooths jitter when wall-following |
|
_ OXX X |
obstacle hit |
medium speed turn right |
|
|
X XXO _ |
obstacle hit |
medium speed turn left |
|
|
O OO_ X |
blocked on left |
fast speed turn right |
|
|
X _OO O |
blocked on right |
fast speed turn left |
|
|
O OOO O |
completely blocked |
** |
If last turn was to the right, very fast turn right. else very fast turn to the left |
|
X XXX X |
no sensors hit |
** |
Slowly turn towards last valid destination |
* O = triggered X = not triggered _ = sensor state ignored
Obstacle Avoidance Issues
The inner three sensors are used for turning to avoid obstacles. When the “left”/”right” sensor alone are triggered, the agent turns at a medium speed in the opposite direction. When the middle sensor and a side sensor are triggered, the turning rate is increased to fast.
One problem exists when all of the sensors are triggered simultaneously. This condition when the pathway is completely blocked uses a simple Boolean flag “rightTurn” to look at the recent history in order to determine which direction to turn. If the agent was previously turning left, continuing to turn left when all the sensors are triggered is usually the ideal action. If the agent was previously turning right, continuing to turn right … etc… This Boolean value is only used for this one special case and nothing else.
Another issue is returning to a previous destination. A new vector3 variable “destination” was added to the Object3D class to keep track of this information and code throughout the rest the program was modified to set this value every time the agent turns towards another treasure or nav point. Turning slowly back towards this destination every time no sensors were impacted keeps the agent moving towards its real goal whenever possible. To prevent nearby treasures from interfering with obstacle avoidance, the change path method will not trigger if obstacle avoidance is running.
Code
float fX = sphereDistance * orientation.Forward.X; // shorthand
float fZ = sphereDistance * orientation.Forward.Z;
float rX = sphereDistance * orientation.Right.X;
float rZ = sphereDistance * orientation.Right.Z;
float slowTurn = 0.005f; // Turning
rates
float mediumTurn = 0.01f;
float fastTurn = 0.04f;
float veryFastTurn = 0.08f;
//Sensors: X ... . // Wall
following, makes an adjustment only every 50 frames roughly
if (SensorLeftSide
&& !SensorLeft && !SensorMiddle && !SensorRight
&& !SensorRightSide)
{
if (random.NextDouble()
< 0.02) // 1/50th of the time
{
turnToFace(Translation + new
Vector3(fX + fastTurn * rX, 0.0f,
fZ
+ fastTurn * rZ)); // turn right
} rightTurn = true;
}
//Sensors: . ... X // Wall
following
if (!SensorLeftSide
&& !SensorLeft && !SensorMiddle && !SensorRight
&& SensorRightSide)
{
if (random.NextDouble()
< 0.02) // 1/50th of the time
{
turnToFace(Translation + new
Vector3(fX - fastTurn * rX, 0.0f,
fZ
- fastTurn * rZ)); // turn left
}
rightTurn = false;
}
//Sensors: _ X.. . // Obstacle
hit on left
if (SensorLeft
&& !SensorMiddle && !SensorRight && !SensorRightSide)
{
turnToFace(Translation + new Vector3(fX + mediumTurn
* rX, 0.0f, fZ + mediumTurn * rZ)); // right
rightTurn = true;
}
//Sensors: . ..X _ // Obstacle hit on right
if (!SensorLeftSide
&& !SensorLeft && !SensorMiddle && SensorRight)
{
turnToFace(Translation + new Vector3(fX - mediumTurn
* rX, 0.0f, fZ - mediumTurn * rZ)); // left
rightTurn = false;
}
//Sensors: X XX_ . // Blocked the left, turn right quickly
if (SensorLeftSide
&& SensorLeft && SensorMiddle
&& !SensorRightSide)
{
turnToFace(Translation + new Vector3(fX + fastTurn
* rX, 0.0f, fZ + fastTurn * rZ)); // right
rightTurn = true;
}
//Sensors: . _XX X // Blocked on
the right, turn left quickly
if (!SensorLeftSide
&& SensorMiddle && SensorRight && SensorRightSide)
{
turnToFace(Translation + new Vector3(fX - fastTurn
* rX, 0.0f, fZ - fastTurn * rZ)); // left
rightTurn = false;
}
//Sensors: X XXX X // All sensors are triggered, look at RightTurn variable to
// determine which way to continue turning based on last
turn direction
if (SensorLeftSide
&& SensorLeft && SensorRight
&& SensorRightSide)
{
if (rightTurn)
turnToFace(Translation + new
Vector3(fX + veryFastTurn * rX, 0.0f,
fZ
+ veryFastTurn * rZ)); // right
else
turnToFace(Translation + new
Vector3(fX - veryFastTurn * rX, 0.0f,
fZ
- veryFastTurn * rZ)); // left
}
// no sensor data then slowly turn to treasure
if (!SensorMiddle &&
!SensorLeft && !SensorRight
&& !SensorLeftSide && !SensorRightSide)
{
turnToFace(Vector3.Lerp(Translation + new
Vector3(fX, 0.0f, fZ),
destination,
slowTurn));
}