5.5.1.3. Hybrid Avoid Obstacles and Go-to-Goal

When an obstacle is near by, but not an immediate threat for a collision, then the robot does not need to go directly away from the obstacle as the Avoid Obstacles behavior would, but it should veer to the side of the obstacle while moving towards the goal. The hybrid algorithm calculates an weighted angle heading that favors \alpha_g when \alpha_{ao} is small while favoring larger values of \alpha_{ao}.

The input to the behavior is the set of angles:

  • Angle to goal in the global coordinate frame, \phi_g.
  • The avoid obstacle angle in the local coordinate frame, \alpha_{ao}, which comes from the sonar data.
  • The robot’s orientation in the global coordinate frame, \theta.
  • The threshold angle, Max_{\alpha\_Hybrid}, for switching to the Avoid Obstacles behavior.

The variable that we call h is the weighting variable.

(5.5)h = \left\{ \begin{array}{ll} \frac{|\alpha_{ao}|}{Max_{\alpha\_Hybrid}} & \mbox{if} \: |\alpha_{ao}| < Max_{\alpha\_Hybrid} \\ 1 & \mbox{otherwise} \end{array} \right.

\alpha = h \cdot \alpha_{ao} + (1 - h) \cdot \alpha_g

\alpha = h \cdot \alpha_{ao} + (1 - h) \cdot (\phi_g - \theta)

Thus, for |\alpha_{ao}| \geq Max_{\alpha\_Hybrid}, we have pure collision avoidance:

\alpha = \alpha_{ao}