Home > Back-end >  Heading angle for waypoint navigation (fails at bounds)
Heading angle for waypoint navigation (fails at bounds)

Time:01-15

In a Cartesian coordinate system / Euclidian plane, a vehicle travels clockwise around the origin with radius 1 and currently at angle θ = PI/4 (45°), so its heading is 7/4*PI (315°). To reach its new destination (the origin) it should change course (instantly) to -5/4*PI (-135°).

This angle can be obtained with e.g. atan2 on the components of the vector from position to target (with the target at origin, it's the the inverted position vector)b:

However, due to inertia, the vehicle cannot change its course instantly and crosses the X-axis (x=1, y=0, θ=0), where a new calculation results in a course of PI (180°).

The vehicle tries to achieve this new (positive angle) course by a positive turn (counterclockwise, CCW), which takes it back across the X-axis, and so on (it "wiggles" away from the target along the x-axis).

It breaks also if negative angles are "wrapped" into the absolute positive range 0...2PI - example data of the southbound vehicle passing west of the target:

course -0.00477
adjust  6.27840

course  0.00034
adjust  0.00034

The proportional control then basically causes a negative (clockwise) turn, which is the wrong direction.

Do I eliminate this "overflow" somehow (not limit to 0...2PI but map absolute angles to the vehicle's) or should I use a different strategy altogether (which)?

  • Related