Waypoint steering geometry for a mobile robot

waypoint stearing dia

Fig 1 shows the geometry that I will use in the waypoint steering algorithms that will be discussed in future posts.  The field geometry is North (N) along the y-axis and East (E) along the x-axis.   The heading angle , psi is defined positive clockwise from North as a compass rose.

Waypoint:   The waypoint is defined with both location (X_p, Y_p) and direction psi_p.   The slope of the center track line m is  1/tan(psi_p).

Cross track and Along track Ranges:

The two parameters of interest are the along track range R_y and cross track range , R_x.   The steering is determined by R_x = R*sin(delta_psi) where delta_psi is the angle between a line drawn from the robot to the waypoint and the actual track line.

Derivation of R_x  and  R_y without sin and cos functions:

We know that the track can be expressed as a line that goes through (X_p,Y_p) and has a slope m.   So we can write the track line as  y= mx + y_0 form but we need y_0.    Plugging in for point p gives  y_0  = Y_p – m*X_p .

Hence the line eq  is   0 =  -y +m*x + Y_p – m*X_p.

It can be shown that the distance R_x  between a point  (x_r, y_r)  and a line  a*x + b*y  + c  = 0  is

|R_x|= | (a*x_r+b*y_r +c)|/sqrt(a^2 + b^2)    (see  wikipedia)

where a = m ,  b= -1 ,  c= Y_p – m*X_p ,    m  = 1/tan(psi_p)

This form can lead to division by zero when psi_p = 0 but can be avoided by using the tan(90 – psi_p)  = 1/ m  .  We  divide the starting equation

0 =  -y +m*x + Y_p – m*X_p

by m   to change its form and use it whenever m >1 or psi_p > 45 degrees.

0 = -y/m + x + Y_p/m – X_p.

Now we have new definitions of a,b,c

a = 1, b = -1 /m, c = Y_p/m – X_p  , 1/m = tan(90 – psi_p).

R_y derivation

We can use a similar method to find R_y.

|R_y| =  |(a’*x_r+b’*y_r +c’)|/sqrt(a’^2 + b’^2) )

where we use the perpendicular line to the track passing  through (X_r,Y_r) with slope  m’ =-1/m and the point as (X_p,Y_p).    This leads to

a’ = -1  , b’ = m’ , c’ = Y_r  – m’*X_r  , m’ = -1/m = -tan(psi_p)  .  We use this when psi_p  < 45 degrees and the modified form

a’ = m , b’ = 1 , c’ = -Y_r*m – X_r    for psi_p > 45 degrees.

Making R_x and R_y signed values

By convention, I want displacement to the right as positive R_x.  (X_r,Y_r) will be to the right of the track if  0< delta_psi  < 180 otherwise we change the sign of R_x.

Similarly, we want the along track distance, R_y,  to be positive if we are heading in the direction of the waypoint along the track and we have not reached the waypoint. This occurs when  0<90 – delta_psi < 180 otherwise R_y is negative.


Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

%d bloggers like this: