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).
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.