Improving obstacle recognition level for HC-SR04 sonar sweeper

[NightMoth]

Hello all

Sharing here pseudocode of my algorythm that improves obstacle recognition level for HC-SR04 Ultrasonic sensor sonar-sweeper

I'm not sure wether I'm reinventing a weel, but I didn't find similar hobbyst level projects on the web,
and I spent more than month to finish this project...so The World needs to know about it!

1) Problem:
HC-SR04 detects any obstacle that is within 15-30degrees sector along its orientation.
Any obstacle that is detected during sweep appears much wider than it is in real.
There is "early" detection of an obstacle after the edge of the sensor's detection sector just touches it,
and "late" detection when the sensor already pass it, but an obstacle is still within its detection sector.

Real setup


Sensor data



2) Solution method:
Suppose you already have a data table that contains sonar data (Call it SDT - "Sonar data table")
This is a table of angle-distance obstacle detection records and detection confidence levels.
It has m rows - each row corresponds to some discrete sensor orientation angle.
And n=3 colums:
Column 0 - "Angle, deg" - column with values of sensor orientation angles.
Column 1 - "Dist, mm" - column with distances to an obstacle detected for each angle
Column 2 - "Confidence" - column with confidence level of detection for each angle (0 - no detection).

Main idea is:
For each angle-distance obstacle detection pair check wether it was another "mirror" detection at the same distance (+/- some room) but at the edges of detection sector.
If yes (it means possible early or late detection of current obstacle) - increase confidence level of current obstacle detection.

3) Pseudocode:
- Define angle between sensor heading direction and the edge of detection sector as "deviation angle" (devAng)
   For my sensor devAng = 8deg
- Define allowable room to detect "mirror" image of an obstacle as "deviation distance" (devDist)
   For my sensor devDist = 2*sensor accuracy = 2*5mm = 10mm (found experementally)

// for each row in SDT (Sonar Data Table)
for i = 0 to m-1
   // For current row with index i check wether it was an obstacle detected
   if SDT(i,2) > 0     // if detection confidence level >0
      // If yes
      // Check right edge of the detection sector
         // find value of right edge angle:
         angRight = SDT(i,0) - devAng;
         // find idxRight - index of a row that contains such angle value that is  equal or as close as possible to angRight
         // such that SDT(idxRight,0) ~= angRight
         idxRight = findIdxByAng(angRight)   // some function that can make it
         // For the found row with index = idxRight check wether there is another (possibly "mirror")
         // obstacle detected at the same distance as for row i,
         // but within some room +/- devDist
         if SDT(idxRight,1)>=SDT(i,1)-distDev AND SDT(idxRight,1)<=SDT(i,1)+distDev
            // If yes
            // increase confidence level for current detection
            SDT(i,2)++
         endif
      // Repeat for the left edge of detection sector
         // find value of left edge angle:
         angLeft = SDT(i,0) + devAng;
         // find idxLeft - index of a row that contains such angle value that is equal or as close as possible to angLeft
         // such that SDT(idxLeft,0) ~= angLeft
         idxLeft = findIdxByAng(angLeft)   // some function that can do it
         // For row with index = idxLeft check wether it was aother obstacle detected at same distance as for row i,
         // but within +/- distDev
         if SDT(idxLeft,1)>SDT(i,1)-distDev AND SDT(idxLeft,1)<SDT(i,1)+distDev
            // increase confidence level for current detection
            SDT(i,2)++
         endif
   endif
endfor

4) Results attached

Sensor data after analysis


Clear image

[moffy]

An interesting project thanks for sharing.