Manipulation Planning

Manipulation planning on a planetary vehicle produces a joint-space or Cartesian trajectory that reaches a target without touching anything else. The split between ground and flight is sharp: the trajectory is planned on Earth and the flight software’s job is to check and execute it. Nothing on Mars has yet planned its own arm motion from a science goal.

The two collision problems

An arm collides with two different things and the checks cost differently. Self-collision, arm against arm and arm against vehicle, is a fixed geometry problem answered from the kinematic model. Terrain collision is answered against a mesh built from stereo imagery taken from the vehicle that sol, and is far more expensive [1].

Measured inside the Perseverance ground planner:

Function	Average duration
Forward kinematics	0.7 ms
Self-collision check	1.3 ms
Terrain clearance computation (Proximeter)	11.8 ms

Source: [1], Table I. Terrain clearance costs nine times a self-collision check and 17 times a forward kinematics evaluation, which is why the planner’s structure is organized around avoiding it. Flight software on Perseverance does not run this query at all; the clearance is settled on the ground before the sequence is uplinked [2].

Flight software uses a coarser model. The InSight Instrument Deployment Arm flight software, inherited from the Phoenix robotic arm software which inherited it from the MER Instrument Deployment Device, composes each collision link from cylinders and boxes and checks each generated via point against a table of link pairs [3]. Checking can be enabled and disabled per pair, and static link positions are parameters the ground can update. On InSight that mechanism carried the science instruments: with SEIS, the wind and thermal shield, or HP3 on the deck, that instrument’s static link was checked and its dangling link was not; once grappled, the dangling link that hangs from the grapple attachment point in the gravity direction was checked and the static link was not; once set on the terrain, the static link’s position parameters were rewritten to the new Mars location and checking switched back.

Planning on the ground: RRT-Connect and viapoint reduction

Perseverance arm activities are planned in ArmSketch. Inputs are the rover’s initial joint angles and location, terrain meshes from rover stereo imagery, and a set of science activities; the tool computes motion plans between every pair of activities, simulates each in the SSim flight software simulator to get its duration, and searches combinatorially for the activity ordering with the shortest total execution time. Rover planners are expected to produce a rough plan in about 10 minutes [1], inside the same tactical timeline that constrains the sampling assessments Proximeter automates [2].

Motion planning uses RRT-Connect [1]. The operational constraint is unusual: each viapoint in the path becomes a separate arm motion command that a rover planner must review before uplink, so the number of viapoints, not the path length, is the quantity to minimize, and in the scenarios studied a single viapoint suffices to clear any obstacle. Standard RRT-Connect does not minimize viapoints, and its raw output depends strongly on the step-size parameter:

RRT-Connect step size	Mean viapoints	Mean path length
0.25	15.73	4.05
0.5	7.83	4.16
1.0	3.86	4.36
2.0	2.16	5.32
5.0	1.37	8.56

Source: [1], Table III, averaged over 50 random seeds.

A rapidly-exploring random tree is probabilistically complete and not optimal, and the rate of that convergence is stated by its author to be an open problem, so an RRT planner carries no bound on planning time and no bound on solution quality [10]. Its path length in bounded convex two-dimensional free space with a holonomic model and a Euclidean metric runs 1.3 to 2.0 times the optimal path to the tree root, the most favorable case measured, with no figure given for cluttered or nonholonomic problems. The metric that would be ideal for a kinodynamic problem is the cost of the optimal trajectory between two states, which is as expensive as the planning problem, so every implementation substitutes a metric whose effect on convergence is unquantified [10]. Planning on the ground absorbs that: the 10 minute budget is a rover planner’s, not a flight timeline’s.

Path simplification recovers both. At step size 1.0, binary shortcutting takes the path from a mean of 3.86 viapoints and length 4.36 to 1.08 viapoints and length 3.71, at a cost of 234 collision checks [1]. Greedy graph shortcutting at step size 1.0 gives 1.40 viapoints and 197 collision checks. The evaluation weighted viapoints at 10/15, collision checks at 3/15 and path length at 2/15 [1]. The weighting follows from the review cost: each viapoint is a command a rover planner must check [1][5].

The collision checking itself was restructured around a safety bubble. The baseline splits an edge into uniform steps, orders them by binary subdivision so a collision is found early, and checks self-collision and terrain clearance at each; if the edge is clear it has still paid for every step [1]. The adaptive version calls the terrain clearance query once, takes the returned clearance distance as the radius of a bubble in Cartesian space around each joint, and then uses forward kinematics alone, at 0.7 ms, to confirm that neighboring steps have not moved any joint outside its bubble, calling the expensive query again only when one has. Over a benchmark of 6 scenarios, 334 RRT plans and 802 edges, 528 of them collision-free, the adaptive algorithm is 3.3 times faster on average [1]. On unsafe edges it is slightly slower, because it makes more forward kinematics calls, and both algorithms agreed on every edge’s safety across the benchmark plus 23 added edge cases including long motions needing two or more viapoints, self collisions, paths hugging the terrain, wide and narrow obstacles, and turret rotation near the ground.

Proximeter: what the clearance query does

Proximeter computes the worst-case minimum distance between triangular meshes of the robot hardware and of the local terrain, over the volume of the placement’s position and orientation uncertainty, which is resolved anisotropically into normal, lateral, clocking and angular components because normal uncertainty is generally the smaller [2]. Distance checks use the Proximity Query Package, and the search over the uncertainty volume is a grid search accelerated by Lipschitz optimization, which uses the bound on the function’s rate of change to skip evaluations that cannot beat the current minimum. Queries complete in under 50 ms [2].

Before Proximeter, operators selected points by hand on the rover collision model and on the terrain, measured the distance between them, and repeated for further point pairs, a process that was slow, subjective and conservative enough that valid arm placements were rejected [2]. TargetWizard, which drives Proximeter through a gRPC server using protocol buffers, performs sampling and caching evaluations that take human operators 30 to 40 minutes in 15 seconds, removing the need to pause operations for a sol to assess a complex science activity. Applied by command line to Curiosity’s Feed-Extended Sample Dropoff procedure, developed after the loss of the drill feed actuator and requiring collision checks to be disabled so the flight software cannot assess it, Proximeter reproduced the manual assessment and saved 10 to 20 minutes of operator time [2].

Terrain meshes have defects from poor stereo range data or lighting, so Proximeter can miss a surface feature absent from the mesh or alarm on one that does not exist, and in those cases operators are responsible for detecting the defect because the clearance cannot be trusted [2]. Mesh hole and surface roughness checks were being added before any use in a fully autonomous mode. Running Proximeter onboard would remove the ground loop from contact science target evaluation entirely [2][5].

Force as the terminal condition

Placement and preload are not position problems. Perseverance’s arm is about 2 m long with 5 degrees of freedom, carries a 45 kg turret, applies several hundred newtons of preload to surface targets and over 700 N to its dock, and senses through a 6 degree of freedom force torque sensor between the arm and the turret with redundant A and B strain gauge sets of which the software can use only one at a time [4].

Preload uses Iterative Force Control rather than the continuous force control common in terrestrial robotics. The arm holds still, reads the force sensor, computes the error to the preload goal, uses a compliance model of the arm at the current pose to compute the small Cartesian motion that would produce the missing force, converts that to joint commands, executes, and reads again [4]. It terminates on the goal within a parameterized deadband, or on a parameterized iteration limit, in which case failure is declared. The compliance matrices are computed analytically per link and tool and tuned by kinematic calibration with laser metrology.

Docking, the one autonomous arm operation

The corer must mate with the bit carousel to exchange drill bits and sample tubes. Four posts on the corer face enter four concave cones on a rotating ring; each cone has a contact switch triggering within 0.5 mm of its bottom, and the ring’s hardstops sit at +3.13 and -10.08 degrees from center [4]. ARM_DOCK runs the whole sequence autonomously from free space with no operator monitoring: move to standoff, move to a teach point on a slower straight-line Cartesian path, move to contact until the force sensor exceeds its noise threshold, apply an open-loop initial preload, then lateral alignment driving the forward preload to its goal while minimizing lateral loads until at least one contact switch triggers, then angular alignment raising the preload while also nulling moments until at least three switches trigger, then a final preload for margin during bit exchange. It is the only arm operation on Perseverance that closes a loop onboard without a ground-planned trajectory [1][4].

Fault protection runs against the same signals: motor monitoring at 64 Hz for current, stalls, shorts, temperature, position discrepancy and joint limits; a fault if any contact switch is pressed before contact is expected; the measured preload constrained to the target plus a margin, for example a 700 N limit against a 600 N target; lateral forces and moments held inside deadbands; dock rotation checked against softstops more conservative than the hardstops; and a per-phase iteration limit [4].

Ground testing was extensive because the operation is irreversible. The Controls and Autonomy for Sample Acquisition and Handling testbed, whose software mimicked the flight architecture with a faster development cycle, performed more than 1200 dockings, testing cone and post materials and surface finishes and characterizing force sensor inaccuracy and arm positioning uncertainty by design of experiments [4]. Qualification model dirty testing added more than 400 dockings and reached three times life, with per-docking maximum forces flat over time. Assembly testing found under 1 mm of clearance between a drill stabilizer and the bit carousel housing, mitigated by removing material and adding a flight rule, and thermal testing found a limit-cycling condition fixed by an algorithm change [4], the class of defect a ground mesh check cannot reach [2]. The experimental preload-to-dock-rotation slope, -0.138 mrad/N, matched the analytical -0.131 mrad/N [4]. Perseverance docked 16 times to the adaptive caching assembly in the first 210 sols [5].

Docking was first performed on Mars on sol 87 and repeated successfully eight more times through sol 211, on sols 150, 164, 189, 195, 196 and 211 [4]. Docking itself takes about 2 minutes of a roughly 20 minute deposit-bit sequence or 25 minute retrieve-bit sequence. Sampling sequences including docking are simulated before uplink in SSim, which runs the ARM, DRILL, MOT and ACA flight software modules and models the dock as a rigid flat plane whose reaction forces are written into simulated strain gauge channels, a model that misses the post and cone contact physics but catches sequencing errors. The same simulator supplies the activity durations ArmSketch orders its plans by [1].

What the ground actually spends

Over the first 210 sols Perseverance issued 20,764 arm and sampling commands [5]. The arm was unstowed 27 times, the contact sensor touched the surface 17 times, the drill was placed on the surface 9 times, and the instruments were placed 152 times for WATSON, 10 for SHERLOC and 8 for PIXL, the ordering following how tight each instrument’s standoff requirement is. A typical sampling operation including docking, bit exchange, acquisition and processing uses over a hundred command products; the sol 196 core used 27 scripts, 73 strategic sequences and 11 tactical sequences [5].

The Three Forks sample depot is the largest single manipulation campaign flown. Ten returnable sample tube assemblies were dropped between sols 652 and 693, 42 sols, over 207.93 m of driving, documented by about 4000 images [6]. The drop is from roughly 90 cm above the ground and the requirement is that each tube land within a 70 or 95 cm radius drop zone, with tubes at least 5.5 m apart so each has a hazard-free circle for the retrieval helicopter. Achieved radial offsets were 38.4, 29.1, 48.8, 40.0, 9.6, 24.2, 14.7, 48.6, 19.7 and 12.3 cm [6]. Every tube landed horizontal, so the tube topple contingency, which would have approached at 25 degrees above the ground plane in 1 cm increments over 20 cm with a 30 cm alternating lateral sweep of the stabilizer shaft, was never used [6].

Two faults interrupted it. On sol 672 a transient spike on the shoulder azimuth resolver at motion start pushed the encoder-to-resolver mismatch past its persistence limit; recovery took sols 673 and 674, closing the WATSON cover and wiggling the joint with a wider persistence setting [6]. On sol 687 the sample handling arm’s internal force exceeded its 23 N positive limit in the home position, traced to a sequencing error in which forces were not tared across the nine previous drops, so false internal force accumulated; the drop succeeded on sol 690 after the sequencing was corrected. The ground test did not find either, because the testbed cleared the persistent volatile state on every power cycle. Post-drop imaging before driving away was made mandatory, adding a sol to each drop, because a tube near a wheel is unrecoverable.

Autonomy that was demonstrated and not flown

Single cycle instrument placement was demonstrated in the field. MER practice required three to four sols per target: approach, refine the rover position, then deploy the arm [7][8]. The demonstrated alternative takes a target designated once in initial imagery, then autonomously drives to it, tracks it through the traverse, hands the track off from the mast cameras to the body cameras, refines the base placement, and deploys the arm, all from one command.

Tracking is the hard part. The mast is pointed by an exact inverse kinematics solution to the target’s 3D position, and the target is matched by a combination of global normalized cross-correlation and a local affine transform over multi-size templates at several pyramid levels, holding under a pixel between frames and a few pixels over a 10 m traverse [8]. The camera handoff, projecting the target from mast stereo into the body camera view across a large viewpoint change, is the largest single error contributor. Two dimensional tracking was validated to 4 cm at 3 sigma, and image-based hazard-camera instrument placement reduces the final error from several centimeters to under 1 cm [8].

Over 43 runs on the Athena rover in the JPL Mars Yard, with targets 5 to 15 m away, targets from 7 cm to 0.5 m in radius, 28 small-target runs and 15 runs at 10 m each involving at least one major obstacle, 93 percent of placements landed within 2 to 3 cm of the designated target [7]. Terrain was mapped by quarter-resolution dense stereo into a 20 cm grid, with 20 cm the traversable step height, 80 percent of wheel diameter. Three consecutive tracking failures aborted the run.

Real-time onboard collision avoidance was also built and not flown to a planetary surface. A perturbation approach for the two 7 degree of freedom arms of the Ranger telerobotic flight experiment adds virtual repulsive forces when any arm segment enters an obstacle’s safety zone, perturbing the Cartesian command into the existing position loop rather than replacing the controller [9]. On a MIPS R4600 at 100 MHz, line-segment to face obstacle detection took about 1.23 ms, the perturbation computation under 0.2 ms, and the full cycle including forward and inverse kinematics about 2.5 ms, against a 400 Hz servo interface to the arm control unit. Maximum measured intrusion into the safety zone was about 0.28 cm at the arm angle and under 0.2 cm at the end effector, against 3.15 cm with avoidance disabled, at stand-off distances of 10 to 20 cm [9].

Comparison

	InSight IDA	Perseverance arm
Degrees of freedom	4	5
Collision primitives	cylinders and boxes	triangular meshes on the ground [1][2], flight model not published
Where the trajectory is planned	ground, expanded onboard to actuator commands	ground, in ArmSketch [1]
What flight software checks	every via point against the enabled link pairs [3]	via points, plus force, moment, contact switch and dock rotation limits during docking [4]
Force sensing	current limits and torque checks on guarded moves [3]	6 degree of freedom force torque sensor, redundant gauges [4]
Autonomous closed loop operations	none; grapple state machine only [3]	docking, from free space to preload, no operator monitoring [4]
Fault response	stop all motors and heaters, announce, mark safed, accept only recovery commands [3]	per-phase iteration and force limits, then fault [4]