Navigation System
Purpose:Navigation system is responsible for taking in a path as a set of waypoints from path planner, determining the safe speed and steering limits and computing required steering commands to safely drive the vehicle on the path. It also reacts to the control signals like pause, and kill signals.
Input:
Path to follow in 'i'th iteration, wpi, is a sequence of waypoints, where each waypoint is a 3-tuple. The x element of the jthwaypoint in wpi is denoted as wpi[j].x. Similarly, for y and speed element.
Speed of the vehicle at iteration cycle 'i' =
Position of the vehicle at iteration cycle 'i' =
Heading of the vehicle at iteration cycle 'i' =
External pause and kill signal =
Output:
Steering commands =
Throttle commands =
Next global waypoint to travel to = next_waypoint
Some abbreviations:
rddf – Input file specifying the route for the vehicle to follow. Route is specified as a set of waypoints to touch, each waypoint has its (x, y) position, width of the path segment starting from this waypoint point to next waypoint, max speed over this path segment.
PP – Path planner
cbln path – Path (wpi) provided by path planner to follow ('CajunBot Local Navigation')
1) Abstract description of present navigation system:
Every time in the control loop the latest path to follow, wpi, is read and a control point on or towards the path is picked using x-track guide mode (explained later in the section). The heading error to reach this control point is computed as difference between present heading of the vehicle and desired heading towards this control point. This heading error is used as an input for the PID controller to produce steering commands (taking values between -1 to +1 for steering wheel position, where -ve values imply steering wheel to left, +ve values imply steering wheel to right). A Look-up table is used for using different 'proportional' (kp) PID parameter at different vehicle speeds. (ki, kd) are constant for a run and are read from a configuration file.
Similarly the max speed limit for the vehicle is computed by analyzing the path and other safety constraints. This max speed limit is used to compute the desired speed for the vehicle based on the maximum acceleration or deceleration that the vehicle can take. Speed error to reach this speed is computed as difference between present speed of the vehicle and desired speed. This speed error is used as an input for the PID controller to produce throttle commands which can take values between -1 to +1, where -ve value imply active braking and +ve value imply throttle. The PID parameters (kp, ki, kd) are constant for a run and are read from a configuration file.
The max speed limit for the vehicle is based on following factors:
a) Track speed limit set in the path for each path segment in rddf
b) Overall speed limit for the run as specified in the config file.
c) Speed limits for approaching waypoints based on the amount of turn to make at the waypoint and the waypoint speed limit set in input path. Kinematic equations are used to reduce the speed limit for the immediate waypoint in order to be able to respect the speed limit of following waypoints by using the their speed limits, distance between waypoints and achievable acceleration and deceleration by the vehicle.
d)Speed limit based on vehicle's heading error.
e) Speed limit based on vehicle's present rate of turning.
f) Stop the vehicle, when:
1)external hardware pause signal is received,
2)external kill signal received,
3)no new path for specified amount of time,
4)reached the last waypoint.
g) Reduced speed limit based on time gap between consecutive new paths to follow.
3) Navigation system:
Following are the steps followed by the navigation system in computing the steering commands to follow the input path.
a) Read and pre-process input data
b) Decide what to do (stop, drive, quit)
c) Determine desired instantaneous speed and direction to drive:
c.1) Determine instantaneous direction to drive
c.2) Determine instantaneous speed to aim
d) Compute Navigation commands:
d.1) Compute PID steering commands to drive to desired direction
d.2) Compute PID throttle commands to reach desired speed
e) Combine steering & throttle commands and publish the navigation command
Figure-1: Navigation System Flowchart
a) Read and pre-process input data
Read latest signal_line (pause and kill signals), vehicle's position, vehicle's heading, vehicle's speed and cbln path to follow from data queues.
Process input data to:
➢Check if the path has minimum number of waypoints, greater than zero, if not trigger the software pause. (software pause is the means to stop the vehicle in case of inconsistent state)
➢Check how old is the input path and cap the max speed of the vehicle proportional to the delay. If the path is older than certain amount then trigger software pause.
➢Check the cbln waypoints are already reached and drive to the next cbln waypoint (some waypoints may have already covered). If all cbln waypoints are reached trigger software pause.
➢Update the speed limits of all waypoints in cbln path by capping the max speed limit of a waypoint such that speed limit of the next waypoint's can be respected (check appendix D).
b) Decide what to do (stop, drive, quit)
Attempt to stop the vehicle (bring to zero speed) if any of the following conditions happens:
➢If input path does not have minimum number of waypoints which is 1 (software pause)
➢If input path is too old which can be detected from time stamp when the path was published (software pause)
➢If all the waypoints from input cbln path are already reached (software pause)
➢If completed the whole track (software kill)
➢If external hardware pause or hardware kill signal is received
➢Hardware un-pause delay: If vehicle was in hardware pause mode and now a hardware un-pause signal is received then wait for a certain amount of time before changing the hardware pause status with in the program. This provides enough time for obstacle detection which is not active during the pause mode to acquire enough sensor data to detect obstacles and provide this information to the path planner to provide a path to following.
If vehicle is stopped due to hardware or software kill signal then quit the program execution. And in rest all condition drive the vehicle.
c) Determine instantaneous desired speed and direction to drive:
c.1) Determine instantaneous direction to drive: Use xtrack guide mode to determine instantaneous desired heading that vehicle should try to achieve in order to smoothly come on to the input cbln path and to follow it. Following is a short explanation on 'xtrack' guide mode (Check Appendix A for two other guide modes that were earlier used)
In xtrack guide mode the vehicle's position is projected onto the current cbln path segment, from this projected point a 'x-track' point is marked at 'look_ahead_distance' along the PRESENT path segment (refer figure -2). if the length of the remaining path segment is shorter than 'look-ahead-distance' then 'x-track' point is marked on the projected part of the PRESENT path segment. And the vehicle is driven to this 'x-track' point by setting the desired heading pointing to the 'x-track'.
Figure-2: X-track Guide Mode
c.2) Determine instantaneous speed to aim: Compute the instantaneous speed to aim by the vehicle based on following constraints:
➢If the vehicle is paused (hardware/software) then set lower desired speed in order to smoothly bring the vehicle to stop. This desired speed (dsi)value is computed using max pause deceleration allowed, time since pause was enabled and speed of the vehicle when pause was enabled.
➢Else set the desired speed (dsi) higher or lower than the present speed (si) depending upon the speed limit at the immediate waypoint (wpi[j].speed_limit), distance to the waypoint (di), present speed of the vehicle(si) and allowed acceleration (max_a) and deceleration (max_d) that vehicle can attempt. Speed limit of the immediate waypoint is updated to satisfy the constraints listed in Appendix B.
sdi = Safe distance to decrease present speed si to wpi[j].speed_limit
l_t = look ahead time
If (disdi)
then //accelerate
dsi = min (max_speed, si + max_a * l_t)
else
dsi = max ( si + max_d, SAS (wpi[j].speed_limit, di))
end
d) Compute Navigation commands:
d.1) Compute PID steering commands to drive to desired direction:
Based on present speed of the vehicle proportional parameter 'P' of the PID controller is set by looking at the look-up table. 'I' and 'D' terms are constants for a run and are read from a configuration file at the start of the program.
Vehicle's heading at control loop 'i' =
Desired heading at control loop 'i' =
Heading error at control loop 'i' =
Time since last control loop =
Steering command at control loop 'i' =
Bound the steering value between -1 and + 1
d.2) Compute PID throttle commands to reach desired speed:
'P', 'I', 'D' are constant for a run and are read from a configuration file at the start of the program.
Desired speed at control loop 'i' =
Vehicle's speed at control loop 'i' =
Speed error at control loop 'i' =
Time since last control loop =
= min (+1, max (-1, )) --- {Bound the throttle value between -1 and +1}
e) Combine steering & throttle commands and publish them
If vehicle is attempting a turn i.e.then throttle is limited by a max throttle value to avoid skidding, i.e. .
Appendix:
(A) Guide modes:
1) Waypoint guide mode:
In this mode the vehicle is aimed to travel directly towards the immediate cbln waypoint. And the desired heading is set to point to the immediate cbln waypoint.
2) Follow-the-carrot guide mode:
In this mode the vehicle's position is projected onto the current cbln path segment just like in x-track guide mode, from this projected point a 'carrot' point is marked at 'look_ahead_distance' ON the path. The vehicle is aimed to travel to this point by setting the desired heading towards this 'carrot' point.
(B) Setting the track and waypoint speed limits:
Track speed limits:
➢Speed limit as mentioned in immediate cbln waypoint.
➢Speed limit as mentioned in the config file (speed limit for the whole run).
➢Speed limit as mentioned in the control_data_t queue to dynamically reduce the speed limit.
➢Speed limit based on instantaneous heading error that controller is trying to cover.
➢Speed increment limit based on immediate heading error (dont try to increase speed by more than certain amount when you trying to turn so much).
➢Speed increment limit based on the rate of turn the vehicle is experiencing.
Waypoint speed:
➢Waypoint's speed wpi[j].speed_limit is capped by the track speed computed above
➢If next waypoint is the last waypoint then waypoint speed is set to zero.
➢If critical_waypoint_speed limit is enabled then limit the speed of the waypoint to critical_waypoint_speed if its critical waypoint. (this feature is most of the time disabled in config file).
➢Determine the orientation difference between present path segment and next path segment and limit the waypoint speed by the max safe speed for making such a turn.
➢Speed limit as mentioned in the control_data_t queue to dynamically reduce the speed limit (No module populates this queue so this step is disabled).
(C) Known problems with present system:
Path planner is tested to provide reasonable paths, and so is the navigation system tested to show proper performance when following a fixed path (dummy path planner), but navigation system is not designed to follow a path that keeps changing as vehicle moves.
The path planner does not consider the current state of the control hardware, while computing a path to follow.
Navigation system is very naïve in analyzing the safe speed limit for the path. It just marks the max speed at every waypoint based on orientation difference happening at that waypoint without considering how close are these turns happening, when there are closely placed waypoint on a curved path, it find minimal turn at every waypoint and hence fails to set safe speed at these waypoints. It needs to look at the radius of curvature of the curved path and set the safe speed.
(D) Consistent speed limit:Update the speed limits for all the waypoints in the cbln path so that speed limits of all waypoints are achievable. As an example speed limits should be updated to avoid situations like a immediate waypoint is having a high speed limit (say 20 mph) and next waypoint which is at short distance (say 2 meters) is having a very low speed limit (say 5mph), in this situations if controller tries to reach 20mph at immediate waypoint later it would have enough distance to reduce the speed to 5mph on time.
= Safe Approaching Speed: Computes the maximum speed for the vehicle so as to safely reduce its speed to s0 within a distance of d.
For j starting from num_waypoints till j > 0
d = distance_between (wpi [j], wpi[j – 1])
wpi[j-1].max_speed = min (wpi[j-1].max_speed, SAS(wpi[j].max_speed, d)
ps: Code is not this compact.