Skip to content

Vehicle Software#

The simulated Polaris runs the command chain of the vehicle program. The chain has a route, a driver or a proposer, the trajectory checker, the shield and the drive-by-wire. The run-time library acres_runtime compiles the chain one time for the simulator and for the vehicle.

What the Model Does#

Inputs Outputs
The prepared path of a mission: points, speed profile, ground class, corridor The command of each control step: curvature and speed, the gear
The vehicle state: pose, speed, yaw rate, gear The verdict of each proposal: accepted or the first rule that refuses it
The planar scan of the shield (360 beams) The intervention of the shield and its rule codes
Chunks of a proposer (optional) The stop and the hold of the vehicle on a slope
The command chain in motion: the default driver steers onto the path with pure pursuit, and the shield limits the speed so that the stop distance stays inside the free distance to an object on the path. Pure pursuit and the stop rule of the shield Gravel, the class limits of the speed envelope: d_m = 2.4 m, T = 1.0 s, a = 1.2 m/s². The vehicle starts 2.2 m beside the path at 3 m/s. reference path object orange: stop distance d_stop(v) = d_m + v T + v² / (2a). The shield caps the speed so that it stays inside the free distance. blue: look-ahead point at L_d = clip(4 + 0.6 v, 4, 9) m, curvature κ = 2 y_b / (x_b² + y_b²) The vehicle stops 2.4 m before the object, beyond the obstacle margin d_m.
From a simulation with the rules and the class limits of this page. The default driver steers onto the path with pure pursuit (blue). The shield limits the speed so that the stop distance (orange) stays inside the free distance to the object. The vehicle stops at the obstacle margin. Open the diagram

The chain has these layers. Each layer passes its result to the next one.

  1. The mission planner selects the field visits and the routes (Reference: Planner).
  2. A skill runs the phase of the mission: go to a field, scout it, return home.
  3. The default driver gives a command. A proposer can give candidate chunks.
  4. The trajectory checker accepts or refuses each chunk. A refused, late or missing chunk leaves the default driver in control.
  5. The shield limits each command, from the default driver and from an accepted chunk.
  6. The gear logic, the launch hold and the actuator limits complete the command row.
  7. The drive-by-wire model and the vehicle model apply the command (Drive-by-Wire and ULC).

The sources are in Runtime/Source and Runtime/Include/acres_runtime. The trajectory checker is a compiled Lean model (Formal Verification).

Symbols#

Symbol Quantity Unit
\(v\) Speed of the vehicle m/s
\(\kappa\) Curvature command, positive to the left 1/m
\(L_d\) Look-ahead distance of the driver m
\((x_b, y_b)\) Look-ahead point in the body frame m
\(d_{obs}\) Free distance to the nearest return in the swept corridor m
\(d_m\) Obstacle margin of the ground class m
\(T\) Stop time: latency and response of the speed controller s
\(a\) Deceleration of a stop m/s²
\(w\) Half width of the swept corridor m
\(m_v\), \(G\), \(E\) Vehicle margin, geofence margin, extra side margin of the obstacle rule m
\(a_{lat}\) Lateral acceleration limit m/s²
\(d_c\) Stored clearance bound of a corridor cell m

How It Works#

Default Driver#

The default driver is pure pursuit (Runtime/Source/driver.cpp). It aims at the path point one look-ahead distance ahead of the nearest path point.

\[ L_d = \operatorname{clamp}(L_{min} + k_L\,\lvert v \rvert,\ L_{min},\ L_{max}), \qquad \kappa = \operatorname{clamp}\!\left(\frac{2\,y_b}{x_b^2 + y_b^2},\ -\kappa_{max},\ \kappa_{max}\right) \]

\(L_{min} = 4\) m, \(k_L = 0.6\) s and \(L_{max} = 9\) m (scouting_v1.json). In reverse, the look-ahead is a constant. \(\kappa_{max}\) is 0.2 1/m, the curvature limit of the shield. The speed command is the lowest value of the speed profile between the vehicle and a preview point. At the end of a path the command decreases to 0 before the last point, thus the vehicle comes to a stand at the end.

Proposals and Chunks#

A proposer gives chunks: offsets from the route and speeds at fixed stations along the route. The library converts a chunk into the input of the checker. Fleet.propose and a registered learned skill give chunks (Python: Learning). The C types are in Runtime/Include/acres_runtime/types.h. A chunk keeps the current start state and ends with a possible stop. A direction change needs separate parts.

Corridor#

The route and the ground-class raster of the map give the corridor (Runtime/Source/corridor.cpp). Lane and verge cells can join it. Other classes join it only with the class of the route and the local reach rules. Obstacle cells never join it. Each cell stores a lower bound \(d_c\) of the distance from its center to excluded ground. A query at the point \(p\) takes the largest valid bound of the cells \(c\) near it:

\[ d(p) = \max_c \left(d_c - \lVert p - c \rVert\right) \]

The distance function changes by at most the distance between two points, thus each term stays a lower bound. The stored values round down to float32. Runtime/Source/corridor_query.h gives the queries. Verification/AcresVerification/Trajectory/Raster.lean has the proof of the bound.

Trajectory Checker#

The checker gives a verdict, a reason and the first failing index for each chunk. The limits are in Learning/acres_learn/configs/trajectory_v1.json.

Reason Check
malformed Shape, finite values, sample count and station step
step Distance between samples
speed Configured and ground-class speed limits
direction One direction of travel
start Start pose, heading and speed
stop Stop distance inside the rest of the trajectory
acceleration, deceleration Speed change between samples
curvature, curvature_rate Turn geometry and curvature change
steering_rate, start_curvature Steering response and the current curvature
lateral_acceleration Lateral acceleration limit
obstacle, point, corridor Obstacle distance and footprint containment

The stop rule of the checker and of the shield has the same distance:

\[ d_{stop}(v) = d_m + \lvert v \rvert\,T + \frac{v^2}{2a} \]

The checker and the shield use one margin model: the footprint cover has the vehicle body and the turn margins of the model. A checker acceptance is not a physical safety guarantee. It applies to the geometric assumptions of the model, not to an error of localization or perception. Without a loaded checker plug-in, each proposal gets ACRES_RT_REASON_NO_CHECKER, and the default driver stays in control.

Shield#

The shield (Runtime/Source/shield.cpp) limits the command of each control step with four rules: the actuator limits, the geofence, the corner rule and the obstacle rule. It changes a command only when the command breaks a rule. It never increases the speed and never reverses it.

Obstacle rule. The shield measures the free distance \(d_{obs}\) to the nearest return of the scan in the swept corridor. It takes the smallest of three distances. The first is along the arc of the command curvature, the second along the arc of the reference curvature, the third along the reference path. The speed limit is the largest speed that stops inside that distance:

\[ \lvert v \rvert \le \operatorname{stopCap}(d_{obs}), \qquad \operatorname{stopCap}(d) = \max\{v \ge 0 : d_m + v\,T + v^2/(2a) \le d\} \]

The response distance \(\lvert v \rvert \times\) corridor.response_s (0.5 s) shortens the free distance. Thus the speed cap accounts for the delay and the rate of the brake.

Swept corridor. The half width of the corridor of the obstacle rule grows with the measured speed (Runtime/Source/sweep.cpp):

\[ w = m_v + \min\!\left(G + E,\ \max\!\left(f,\ t_n\,\frac{G + E}{G}\,\lvert v \rvert\right)\right) \]

\(m_v = 0.8\) m, \(G = 0.3\) m, \(E = 0.32\) m, \(t_n = 0.075\) s and \(f = 0.1\) m (control_v1.json). The full side margin of 0.62 m comes at 4 m/s. Narrow means slow, not stop: on a narrow lane the shield lowers the speed. The geofence rule keeps the margin \(G\).

Corner rule. The speed obeys the lateral acceleration limit at the output curvature: \(v^2 \lvert \kappa \rvert \le a_{lat}\), with \(a_{lat} = 1.5\) m/s².

Scan of the shield. The shield reads a planar scan of 360 beams with a reach of 40 m. In the simulator the scan comes from the simulated LiDAR sweep through the scan filter of the vehicle (Scan of a Vehicle). A beam with no return is safe: each entry of the speed envelope stops inside 40 m (no_return_is_safe).

Speed Envelope#

The speed envelope gives the shield and the checker their limits for each ground class. The limits are a speed, a deceleration, an obstacle margin and a lateral limit. A measurement of the simulated Polaris in ACRES Core gives the limits (Calibration/Polaris/speed_envelope.py). The table is Learning/acres_learn/configs/envelope_v1.json.

Class Speed Command (m/s) Speed of the Vehicle (m/s) Deceleration \(a\) (m/s²) Obstacle Margin \(d_m\) (m) Lateral Limit (m/s²)
Asphalt 6.9 7.8 1.2 2.5 1.5
Concrete 6.9 7.8 1.2 2.5 1.5
Gravel 6.9 7.8 1.2 2.4 1.5
Grass lane 6.9 7.8 1.2 2.6 1.5
Dirt 7.1 7.8 1.2 2.1 1.5
Grass 4.9 5.6 1.2 2.6 1.5
Field 5.0 5.6 1.2 2.6 1.5

The table shows the grip level unknown: the worst value of each limit over all ground states of the model. This is the default, because the vehicle has no soil sensor. The level dry gives 0 m/s to 0.1 m/s more speed command. A path point with no known class gets the worst value of each limit over the classes.

The limits come from these rules.

  • Deceleration. 0.8 of the lower of the measured stop deceleration and the limit of the speed controller (1.5 m/s²): 1.2 m/s² for each class.
  • Stop time. \(T = 1.0\) s: the latency of a stop (0.5 s) and the response of the speed controller (0.5 s).
  • Obstacle margin. 1.5 m, plus the longest measured tail of a stop, plus the addition that a follow test needs to keep 1.5 m from a wall.
  • Speed. The lowest of three values:
    • the speed cap of the project: 7.8 m/s on roads and lanes, 5.6 m/s on soft ground,
    • the largest speed with \(d_{stop} \le 40\) m,
    • the largest speed with no wheel lift in a steering step.
  • Speed command. The speed of the vehicle less the largest overshoot of the speed controller.

Stop and Hold on a Slope#

A stop command (a speed command of 0) brings the vehicle to a stand also on a slope. Two parts of the model do this.

  • End of a commanded stop. The speed command and the speed reference are 0. The brake demand of the ULC then gets an integral of the speed that is left (dbw.ulc.stop_integral_per_s, 2.0). The brake stays while the vehicle stands, and not below the stop brake of 20 %.
  • Standstill hold. The tire model has no static friction at zero speed. The hold acts below dbw.ulc.hold_speed_mps (0.05 m/s) while the ULC holds the stop brake. It removes the horizontal velocity of the chassis (Polaris Ranger Dynamics).

Worked Example#

Stop distance on asphalt. The fastest entry of the envelope is 7.8 m/s with \(d_m = 2.5\) m, \(T = 1.0\) s and \(a = 1.2\) m/s²:

\[ d_{stop}(7.8) = 2.5 + 7.8 \times 1.0 + \frac{7.8^2}{2 \times 1.2} = 35.65\ \text{m} \]

This is inside the reach of 40 m of the scan, with 4.35 m to spare.

Speed cap for an object. On gravel (\(d_m = 2.4\) m) an object is 20 m ahead. The cap solves \(2.4 + v + v^2/2.4 = 20\):

\[ v = \frac{-1 + \sqrt{1 + 4 \times 17.6 / 2.4}}{2 / 2.4} = 5.41\ \text{m/s} \]

Corridor half width. At 2 m/s: \(w = 0.8 + \min(0.62,\ \max(0.1,\ 0.155 \times 2)) = 1.11\) m. At 4 m/s and faster: \(w = 1.42\) m.

Pure pursuit. At 5 m/s the look-ahead is \(L_d = 4 + 0.6 \times 5 = 7\) m. A look-ahead point at \((x_b, y_b) = (6.9, 1.0)\) m gives:

\[ \kappa = \frac{2 \times 1.0}{6.9^2 + 1.0^2} = 0.041\ \text{1/m}, \qquad R = 24.3\ \text{m} \]

Code#

Item File
Plan check, routes Runtime/Source/plan_check.cpp, Runtime/Source/route_api.cpp
Default driver Runtime/Source/driver.cpp
Command chain: gear logic, launch hold, command row Runtime/Source/command_chain.cpp
Shield, obstacle distance, swept corridor Runtime/Source/shield.cpp, Runtime/Source/sweep.cpp
Corridor rasters and queries Runtime/Source/corridor.cpp, Runtime/Source/corridor_query.h
Trajectory checker Runtime/Lean/ (the compiled Lean model), Runtime/Source/band_checker.cpp (the batch form for training)
Speed envelope, grip level, class limits Runtime/Source/parameters.cpp, Runtime/Source/route_limits.cpp
Skills and proposals Runtime/Include/acres_runtime/skills.h, Runtime/Include/acres_runtime/proposed.h
Configuration Learning/acres_learn/configs/control_v1.json, trajectory_v1.json, envelope_v1.json, scouting_v1.json

Validation#

Formal proofs. The rules of the shield, the speed envelope and the checker have Lean proofs (Formal Verification). The proofs are about the command, not about the motion of the vehicle.

Stop on a slope. The test hold_tests of the run-time library starts at 3 m/s and keeps the stop command for 45 s. It uses planes of 3.2 %, 5 % and 8 %, uphill and downhill, and 16 steep windows of the lanes of the map (to 9.0 %). Each of the 114 runs in the three condition sets is at rest in 3.7 s or less. It moves 9 mm or less in the 30 s after it.

Measured stops. In ACRES Core a stop from 7 m/s takes 14.4 m to 18.4 m on all classes and ground states. The grip does not limit a stop: the speed controller limits the deceleration to 1.5 m/s².

Steering step. A curvature step to 0.2 1/m at the held speed rolls the vehicle over at 7.8 m/s on each class. With the corner rule of the shield no step rolls the vehicle over, and the largest lateral acceleration is 3.9 m/s².

Parameters#

Block shield of control_v1.json#

Name Type Unit Default Description
curvature_max_per_m number 1/m 0.2 The curvature limit of the command.
speed_min_mps, speed_max_mps number m/s -1.5, 7.8 The speed limits of the command.
vehicle_margin_m number m 0.8 Half of the vehicle width with a margin, \(m_v\).
geofence_margin_m number m 0.3 The side margin of the geofence rule, \(G\).
obstacle_extra_m number m 0.32 The extra side margin of the obstacle rule, \(E\).
lateral_accel_max_mps2 number m/s² 1.5 The lateral limit of the corner rule. The envelope can replace it.
obstacle_margin_m, latency_s, brake_decel_mps2 number m, s, m/s² 1.5, 0.5, 0.8 The base stop rule. The class limits of the envelope replace it.

Block corridor of control_v1.json#

Name Type Unit Default Description
beams, range_m integer, number , m 360, 40 The planar scan of the shield.
narrow_time_s, narrow_floor_m number s, m 0.075, 0.1 The growth of the side margin with the speed, and its floor.
response_s number s 0.5 The response time of the speed controller in the free distance.
scan_source, scan_crops string, boolean sweep, false The source of the scan in the simulator.

Block limits of trajectory_v1.json#

Name Type Unit Default Description
accel_max_mps2, decel_max_mps2 number m/s² 1.1, 1.5 The speed change between samples.
curvature_rate_max_per_m2 number 1/m² 0.1 The curvature change along the path.
steering_rate_max_per_m_s number 1/(m s) 0.059 The curvature change in time.
follower_error_m number m 0.32 The side margin of the narrow-lane rule.
start_position_m, start_heading_deg, start_speed_mps number m, deg, m/s 0.5, 15, 1.0 The tolerances of the start state.

Limitations#

  • The shield has no model of a moving object. At the speeds of the envelope, the default driver collides with a crossing person in 12 of 30 scenarios. It collides with an oncoming vehicle in 1 of 30.
  • The distance along the reference path supposes that the vehicle follows its path. An object that moves into the path has no such guarantee.
  • No component makes a live grip estimate. The chain uses the conservative grip level unknown.
  • Some reference loops of the map have more than the curvature limit of 0.2 1/m. A proposer needs a feasible trajectory inside the corridor. Do not increase the checker limits to accept a reference path.
  • The end of a commanded stop uses an estimated gain. No recording of the real speed controller gives it.
  • A fit to drives on roads gives the vehicle model. No recording gives the grip of wet grass or of a water film on a road.

References#

  • Coulter, R. C. (1992). Implementation of the Pure Pursuit Path Tracking Algorithm. Technical Report CMU-RI-TR-92-01, Robotics Institute, Carnegie Mellon University.
  • Dataspeed Inc. Drive-by-Wire System Overview: the control modes of the Polaris Ranger and the ULC.