Posted 02 September 2023

As described in this earlier post, I hatched a plan to use ‘out of range’ (>= 1) steering values to manage ‘open doorway’, ‘open corner’ and other navigation anomalies, but as usual, this has turned out to be ‘easier said than done’. The problem of deciding what to do after an excessive steerval anomaly detection is complicated by the robot’s behavior leading up to the anomaly. As the steering value rises toward the limit, the robot naturally changes its wheel motor speeds in a manner intended to bring the steering value back down again. In the case of an ‘open doorway’ event, this means the robot turns into the open doorway instead of continuing straight. Then, when the robot stops in response to the associated anomaly detection, it is oriented in a way that makes figuring out what to do next very difficult. The following short video, illustrates the problem. In the first part of the video the robot stops at the 45º break and executes the current anomaly response algorithm; it turns parallel to the left wall, turns 90º to the wall and goes forward to the desired offset distance, then makes another 90º turn and continues wall-following. At the end of the video at the next ‘excess steerval’ anomaly, the robot makes an abrupt turn into the open doorway and can’t recover.

So, what to do, what to do? The robot doesn’t have any idea where it is in the house; all it knows is the steering value for the tracked wall first increased sharply (causing the abrupt left turn shown in the video) and then went out of range (causing the robot to stop). What I want the robot to do is figure out that there is a trackable wall opposite the open doorway, and discovering/tracking it is the preferred behavior.

So, how does the robot ‘discover’ that the other wall is a better tracking candidate? The following diagram shows the situation just after the robot stops.

From the robot’s point of view, the nearest wall is still on its left side but the short left-side segment (the side of a bookcase in this particular configuration) isn’t long enough to actually perform a ‘RotateToParallelOrientation’ procedure. In the meantime, the perfectly good wall a meter or so away is totally ignored. Oops! May have found the problem; if the center distance to the other wall is more than 1m – it is outside the current value for ‘MAX_TRACKING_DISTANCE_CM’

07 September 2023 Update:

After further review, it appears the issue wasn’t so much distances > MAX_TRACKING_DISTANCE_CM as having the reported distance to both sides < MAX_TRACKING_DISTANCE_CM, even after the ‘excessive steerval’ anomaly trigger. This occurs, for instance, when the robot successfully navigates the ‘open door’ into my office, transitions to the right-hand wall (kitchen counter) and then runs off the end of the counter wall. This generates an ‘excessive steerval’ anomaly, but when the robot checks again to decide what to do, both side sensors report distance values < MAX_TRACKING_DISTANCE_CM, most likely due to a ‘false’ distance report from a kitchen eat-in counter bar stool, as shown in the following diagram.

In the above diagram, the robot is located at ‘Position B’. There is nothing to track on to its right (except the legs and cross-braces of the bar stool), and a nice wall to the left. However, because the robot can still ‘see’ the bar stool legs on the right, both sides report distances < MAX_TRACKING_DISTANCE_CM.

So, I’m thinking about introducing the ability for the robot to discern the ‘best’ way forward, but this begs the question of what ‘forward’ and ‘best’ mean in this context.

Forward:

I propose that ‘forward’ in this context means – the direction roughly in line with the previous average heading of the robot. In the above diagram, the average robot heading was toward the top of the diagram, so ‘forward’ would mean no more than 90º either side of this heading (note that WallE3 knows nothing about compass headings – so the baseline average heading at the point of decision can be anything).

Best:

I propose that ‘best’ in this context means “the wall that subtends the largest angle in the ‘forward’ (i.e. +/- 90º from average heading from last 2-3 sec before the anomaly) hemisphere.

In order to implement this scheme, I need to also implement a way of determining the average heading value for the last 2-3 sec. This implies an array of heading values that is updated at each measurement interval, with a length that covers the desired time interval. I already do this with front and side distances with aFrontDist, aLeftDistCM, aRightDistCM, and aRearDistCM, with various sizes. The aFrontDist and aRearDistCM sizes are supposed to be large enough so that the front and rear variance calculations will trigger a ‘stuck’ anomaly after about 5 sec of no movement; however, I see from the current code that while the front array size was bumped to 100 just last month to accommodate the new 50mSec cycle time, the rear one is still at 50. It occurs to me that the array size should automatically change based on the cycle time to maintain the desired ‘stuck’ detection time frame (nominally 5 sec). To do this I could create a ‘STUCK_TIME_FRAME_SEC’ constant, and then multiply this by the measurement time interval in Sec in the aFront and aRear array declarations. Here’s what I came up with:

This at least compiles, so I am optimistic that I haven’t created a huge disaster.

To start this implementation, I created a new global variable ‘gl_AvgHeadingDeg’, a new function to initialize a new ‘aAvgHdgDeg’ array, and ‘UpdateAvgHdgArray()’ to update the array and the average heading at each regular measurement interval as shown

Then I added code to UpdateAllEnvironmentalParameters() to update the average heading array, as shown below:

Then I created a ‘ChooseBetterTrackingSide()’ function to actually determine which side to ‘nominate’ for further wall tracking, as shown below:

10 September 2023 Update:

To test the new average heading idea, I modified my existing ‘#ifdef HDG_ONLY’ code block to add the average heading, and then manually rotated the robot back and forth to see if the average heading value was reasonable. After the usual amount of code debugging, I got what I think are reasonable results, as shown in the following Excel plot:

With the actual and average headings stabilized, I very rapidly rotated the robot from ~0º to ~90º and then allowed the average heading value to catch up. As predicted with the current update rate of 50mSec, the average value array of 60 points took approximately 3Sec to catch up.

Now that I have the average heading value feature working, I can start testing the ability to actually use the average value. Here’s a short video and telemetry output from a test in my office showing the ‘ChooseBetterSide’ algorithm in action:

The salient points in the above video:

• At 4sec (3.3sec in telemetry), the robot detects an ‘EXCESS_STEER_VAL’ anomaly
• At about 4.5sec (3.8sec in telemetry) ChooseBetterSide() is called
• 4.5-10.0sec, the robot makes nine 10º CCW turns to measure left/right distances in ‘forward’ direction, and then turns back to the initial average heading.
• As shown in the telemetry, ChooseBetterSide found 7 left and 0 right trackable wall segments, and returned TRACKING_LEFT
• At 12sec, the robot executes a ‘RotateToParallelOrientation(TRACKING_LEFT) to prepare to track the left side wall
• At 13-15sec the robot tracks the left side wall. Note that because the robot was within the tracking distance window at the end of the RotateToParallelOrientation call, it started tracking immediately.

Here’s the telemetry from the above test run:

Next I ran a ‘field’ test in the hallway outside my office, as shown in the following short video:

Interestingly, the normal ‘open door’ algorithm was used for the open doorway into my office, at about 14sec into the video, causing WallE3 to transition to the ‘other’ (kitchen counter) wall. At the end of the kitchen counter (at about 31sec) the ‘ChooseBestSide’ algorithm was used to transition back to the left side wall. Unfortunately he robot failed to make the left turn into the bedroom hallway at the end of the run. More work to go!

Here’s the telemetry from the run:

Stay Tuned!

Frank

13 September 2023 Update:

Yesterday, at the end of the run the robot quit with the output “HandleExcessSteervalCase in ‘else //OpenCorner:’ Can’t decide what to do.” However, when I tried to duplicate this problem in my office with a simple ‘L’ (open corner) configuration, the robot instead chose the ‘ChooseBetterTrackingSide() option – rats.

Looking through the code and the results, I can see this happened because the steervals from both sides were < MAX. The distances from both sides were > MAX_TRACKING_DIST_CM, but I am currently not using this factor for decisions.

Looks like I need to use both distance and steerval to distinguish among the various error configurations (or maybe I need to use the previous steerval, the one that caused the anomaly?

So, just using current steerval, we get the following four possibilities

• 1: Left < MAX, Right < MAX
• 2: Left < MAX, Right > MAX
• 3: Left > MAX, Right < MAX
• 4: Left > MAX, Right > MAX

If we then add the previous steerval (the one that created the anomaly) into the mix we get eight – the above four with PrevLeft > MAX, and four with PrevRight > Max.

So, assuming that the anomaly was EXCESS_STEERVAL on the left side, then the first possibility above was true in the test case, so what should have happened is recognition of an ‘open corner’ case, with the previously tracked wall on the left side. This should produce a left 90º turn, a forward run of 0.5-1.0sec and then either track left or ‘choose best side’.

15 September 2023 Update:

After thinking about this problem for a while, I have come to the conclusion that I’m overthinking the ‘Excess Steerval’ and ‘Open Corner’ conditions. I now think that the ‘Open Corner’ condition can be uniquely defined as the condition where both left and right distances are > MAX_TRACKING_DIST_CM after an ‘Excess Steerval’ anomaly occurs. This means my earlier re-write of HandleExcessSteervalCase() to incorporate steerval information is incorrect – bummer!

Looking at the two wall diagrams above, it looks like there are only a few conditions that are possible after an ‘Excess Steerval’ anomaly:

• An ‘open doorway’ configuration can be identified uniquely because it will have a tracked-side distance > MAX_TRACKING_DIST_CM and a non-tracked side < MAX_TRACKING_DIST_CM.
• An ‘open corner’ configuration can be identified uniquely because it will have tracked-side and non-tracked side distances > MAX_TRACKING_DIST_CM.
• The ‘Which Wall?’ (ChooseBestWall) configuration can be identified uniquely because it will have a tracked-side distance < MAX_TRACKING_DIST_CM.

So I think this all boils down to the following flow chart:

Based on the above, it appears that my original version of HandleExcessSteervalCase() was closer to the mark than my 09/04/23 re-write – oops! Fortunately I kept the old version as a comment block, so I may be able to fix my ‘oops’ without too much agony.

After modifying (once again) my HandleExcessSteervalCase() function, I got the ‘open corner’ case working – at least I think I did. The following short video and telemetry printout shows the action.

Here is the current ‘state of code’ for the ‘HandleExcessSteervalCase’ function:

25 September 2023 Update: Open Corner/Doorway Success!

After quite a bit of yelling, cursing, coding and re-coding, I now believe that WallE3, my autonomous wall-following robot has truly gained the ability to correctly handle ‘open doorway’ and ‘open corner situations. Here’s a short video showing the results of a ‘field test’:

Salient points from the video:

• 0:12: WallE3 encounters an open doorway to the left, with a trackable wall (kitchen counter) to the right. WallE3 stops and transfers to the right wall.
• 0:29: WallE3 encounters an ‘Excess Steerval’ situation, but doesn’t know which wall is better. It utilizes the ‘ChooseBetterTrackingSide()’ function to determine that the left side wall is a better bet, and transfers to it for further progress.
• 0:49: WallE3 encounters an ‘open corner’ situation, where both left and right side distances are > MAX_TRACKING_DISTANCE_CM. Since the last-tracked wall was on the left side, WallE3 turns that way and continues to track around the corner.
• 0:57: WallE3 encounters another ‘open doorway’ situation, and properly transfers to the right wall for onward tracking.
• 1:09: WallE3 encounters another open doorway on the left (non-tracked) side, but because it is tracking the right wall, this is simply ignored.
• 1:14: WallE3 encounters another ‘open doorway’ situation, and properly transfers to the left wall for onward tracking.
• 1:28: WallE3 encounters a WALL_OFFSET_DIST_AHEAD anomaly and quits, because there is no handling code for this yet.

This is by far the ‘cleanest’ run WallE3 has made to date, and gives me some hope for the future. The handling code for the WALL_OFFSET_DIST_AHEAD anomaly has already been written and tested in earlier versions of the code – I just haven’t ported it into the new ‘HandleExcessSteervalCase()’ architecture.

26 September 2023 Update:

Looking at the ‘HandleAnomalousConditions() function in ‘WallE3_Complete_V4.ino’, I see the following:

For the ‘ANOMALY_OBSTACLE_AHEAD’ case, the response was to call ‘BackupAndTurn90Deg(trkcase == TRACKING_RIGHT, true, MOTOR_SPEED_FULL), so it backed up and turned right (CW) if it was tracking the left wall, and left (CCW) if it was tracking the right wall. The ‘BackupAndTurn90Deg()’ function turns all the LEDs ON, calls ‘MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM)’ to move backward, turns all the LEDs OFF, and then calls ‘SpinTurn()’ to do the 90deg turn. Note that this procedure now ignores the ‘uint16_t motor_speed’ parameter in the sig to ‘BackupAndTurn90Deg().

The ‘BackupAndTurn90Deg()’ function is called in only two places; the above HandleAnomalousConditions() block, and in ‘IRHomeToChgStn()’. Both calls specify a speed to use, which ‘BackupAndTurn90Deg()’ ignores because the ‘backup’ part of ‘BackupAndTurn90Deg()’ is controlled by a PID and is limited to MOTOR_SPD_QTR.

So, I have revised BackupAndTurn90Deg() to remove the speed parameter and the front/back boolean from the signature, and ported the code for ANOMALY_OBSTACLE_AHEAD, ANOMALY_STUCK_AHEAD, ANOMALY_STUCK_BEHIND and ANOMALY_WALL_OFFSET_DIST_AHEAD cases from WallE3_Complete_V4

28 September 2023 Update:

After porting the remaining anomaly code handlers to WallE3_Complete_V5, I made another ‘field test’. This time WallE3 travelled all the way down the hallway as before, but this time instead of stopping with an unhandled anomaly condition, the robot backed up from the door, turned around a few times, and then butted its head into the left-side wall – oops! It then backed up and did it again! If I hadn’t turned it off, I suspect it would have continued doing that forever. Here’s the video showing the action:

The telemetry from the above run is shown below:

As shown in the video and telemetry, everything goes swimmingly until about 89sec (90sec & change in the video) where the robot reports a WALL_OFFSET_DIST_AHEAD anomaly. This is expected, but WALLE3 doesn’t back up as far as it should, and then after doing the expected 90º CW turn, instead of starting to track the new left wall, it does another 180º CW turn and runs headlong into the old left wall (the left wall of the hallway, facing the closed door). Regarding the backup maneuver, it appears it started at 18cm and stopped at 19cm, well short of the expected 30cm.

From the code in HandleAnomalousConditions(), The ANOMALY_OBSTACLE_AHEAD case is shown below:

So BackupAndTurn90Deg(false) is called which should result in a backup to 30cm, then a 90º CW turn. Looking at BackupAndTurn90Deg(false), we see

So BackupAndTurn90Deg() calls MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM) which should back up to 30cm.

In MoveToDesiredFrontDistCm() we have:

This function starts with a measured distance of 18cm and a target of 30. The column header line gets printed OK, but then it immediately stops at a distance of 19cm. Obviously the

statement is returning a FALSE value and terminating the block, but I don’t see why

Oops! Using the wrong distance variable – gl_LeftCenterCm instead of gl_FrontCm. Apparently I never ported the correct code from 230918_WallE3_MoveTo_Test.ino to WallE3_Complete_V5.ino.

Stay Tuned,

Frank