Charging Station Initial Approach Algorithm Improvement

Posted 20 March, 2021

In order to realize my long-term goal of a fully autonomous wall-following robot, Wall-E2 has to be able to reliably mate to its charging station when it gets low on go-juice. Unfortunately, Wall-E2 occasionally fails to mate properly, usually due to an initial misalignment with the center of the IR homing beam. I haven’t worried too much about this, as there have been more pressing problems, but as these have been solved, the mating problem has risen to the top of the to-do list.

The basic geometry for the charging station is shown below:

Tilted gate option. The tilt decreases the minimum required IR beam capture distance from about 1.7m to about 1.0m

As long as the robot starts its approach on or near the boresight of the IR beam, all goes swimmingly. However, if Wall-E2 detects the IR beam while tracking the wall at right angles to the one depicted above, it can easily start its approach before getting to the center of the beam, resulting in it getting stuck on the outside guide-in rail (upper rail in the above diagram). In addition, if Wall-E2 is tracking too close to the wall above, it can actually get stuck on the inside guide-in rail (lower rail in the above diagram).

So, what is needed here is a way to force the robot to line up on the IR beam centerline before committing to the mating approach. To investigate this, I created a part-task version of Wall-E2’s operating system that does just one thing; it detects the IR homing signal, and then takes action to position itself in the center of the IR homing beam, aligned with the beam’s boresight. In the aviation instrument (blind) flying world, this is known as the ‘IAP’ (Initial Approach Point), so I needed to create an algorithm so Wall-E2 could navigate to the Charging Station IAP, and start it’s final approach from the same place every time.

In previous work I have gotten Wall-E2 smart enough to track walls at a constant offset. so this is where I started with the current effort. When Wall-E2 starts to track a wall, the first thing it does is use the near-side array of VL53L0X IR laser TOF sensors to orient parallel to the wall, without regard to the absolute offset. It then angles toward or away from the wall to achieve tracking at the desired offset.

The starting position for the current effort is with the robot placed close to the wall leading to the charging station, pointed generally toward the charging station. When the robot wakes up, it sees that there is an active IR homing beacon, and takes action to navigate to the IAP.

  • First it uses the parallel orientation algorithm to align itself parallel to the near wall so it can measure it’s offset from the wall, and also to ensure that the front distance measurement accurately reflects the distance from the robot to the charging station.
  • Next, it compares the wall offset and front distance measurements to the known values for the IAP, i.e. a 50 cm offset at a distance of 180 cm. It then calculates how much additional offset it needs to place itself in the center of the beam.
  • If necessary, the robot turns 90º away from the wall and moves away to achieve the desired offset. If the robot is already far enough away from the wall, it skips this step
  • After getting far enough away, the robot turns in place (a ‘Spin Turn’) until the signal strength of the received IR homing beacon rises above a set threshold. This gets the robot oriented generally in the direction of the charging station
  • The last step is to fine-tune the robot’s orientation so that it is centered in the beam and also well aligned with the beam boresight.

The following photograph shows the robot at the IAP, ready to start the final approach to the charging station.

Wall-E2 at the Initial Approach Point, ready to start the final approach to the charging station.

And the following video shows the entire process, up to the point where the robot would actually start the final approach.

15 April 2021 Update:

One of the issues with the current initial approach algorithm is the lack of accuracy in achieving the desired wall offset, due to Wall-E2’s tendency to ‘coast’ past the desired distance. I could just lower the offset target by a fixed amount to account for the ‘coast’ effect, but since that changes significantly depending on whether Wall-E2 is on carpet or hard flooring, that doesn’t sound like a good ide.

Instead, I decided to use yet another PID object to manage offset distance acquisition, using the following algorithm:

Using this code, I got the following output:

And a short video showing the offset acquisition process:

Here’s the same process, but starting from farther away than desired

21 April 21 Update:

After some additional work on the initial approach algorithm, I arrived at a pretty nice spot; Wall-E2 will reliably detect the IR homing beacon, offset the proper amount from the wall using a 90º turn and a PID engine-driven rear-distance controlled movement, and then rotate to orient to the IR beam boresight. The ‘rotate-to-boresight’ operation takes place in two stages. In the first stage, the robot turns toward the beacon in 10deg steps until the beacon is re-acquired (this is necessary because the robot loses the beam signal when it turns 90º to the wall) and then uses another PID-driven algorithm to center up on the beam boresight. Here’s a short video showing the process.

IR Homing with Initial Approach Phase added. 2-sec pauses are inserted to delineate sub-phases

As can be seen from the above video, the robot successfully navigates to the initial approach point (IAP), rotates to orient with the homing beacon boresight, and then homes to the charging station. This all works, but it is pretty clunky and inelegant. The initial 90º turn away from the wall is in itself a bit problematic, as it can easily overshoot, and then the robot loses the beacon signal, which means that after the appropriate wall offset has been reached, the robot has to turn back toward the charging station to re-acquire the signal, and it has to do so ‘gently’ so as not to overshoot.

I think it would be much better if the initial turn away from the wall was just 45º so the robot won’t lose the beacon signal while navigating to the IAP, and potentially eliminating the first part of the ‘rotate to boresight’ phase. Here is the relevant geometry:

Charging station initial approach and homing geometry

In the above figure, the robot currently makes a 90º utilizes the line labelled ‘Offset =…’ to offset out to the IR beam boresight. I’m thinking that the line labelled ‘x = …’ would work better, as the robot only has to make a 45º turn initially, and then the robot might not lose the beam signal as it offsets out to the IR boresight line. Here’s the supporting math.

Initial Approach Point math

In the above figure, an example is worked out for d = 120cm, where the perpendicular offset is found to be 34.4cm and the 45º turn distance is found to be 1.09*Offset = 37.8cm.

23 April 2021 Update:

The change from 90º to 45º IAP approach angle turned out to be pretty easy to do – really just a matter of changing ‘SpinTurn(90)’ ‘SpinTurn(45)’ and the offset value to 1.09 x offset. Here’s a short video showing the result.

After a few more runs (with some failures due to the robot hanging up on the outside rail), I realized my basic beam geometry estimate was significantly off. Instead of a beam angle of about 16deg, it was more like 11, yielding a distance::offset ratio of about 0.18 instead of 0.27. Revising the program to use the more accurate ratio resulted in the following much nicer homing run.

Homing run using a distance::offset ratio of 0.18 vs 0.27

And here is the telemetry from the run:

Much nicer!

Stay tuned!

Frank

Leave a Reply

Your email address will not be published. Required fields are marked *