Reaching ENVISAT

ENVISAT was the largest civil Earth observation satellite ever flown, and it has been dead since 2012.

It launched in March 2002 and ESA lost contact with it on 8 April 2012. It weighs 8,211 kg, spans about 26 m with the solar array deployed, and sits in a sun-synchronous orbit near 790 km, which is one of the busier altitudes in low Earth orbit. Estimates put it up there for roughly another 150 years.

The problem is that nothing on it responds any more, so it cannot be moved out of the way of anything. If something does hit it, the debris ends up spread through an altitude band that a lot of other satellites are using.

Any mission that goes up to remove it needs a manipulator, and I wanted to work out what that manipulator actually has to do. Specifically: if you choose a point on the satellite to grab and a direction to come in from, what angle does each of the six joints need to be at. The rest of the page is that question worked through.

The arm is a serial chain. Six revolute joints with a link between each pair, the base fixed to the station, and a gripper on the far end. Nothing branches off, so each joint has to carry everything outboard of it, and the pose of the gripper is just the product of six transformations taken in order.

Two things about the layout matter more than the rest. The first is that the last three joint axes all pass through a single point, which makes the wrist spherical. I did that because it lets the inverse problem split into a position part and an orientation part that can each be solved directly. Without a spherical wrist you have to run a numerical solver at every waypoint, and then you still have to work out which of the several possible answers it landed on.

The second is that the joints do not have equal travel and none of them are symmetric about zero.

Joint 2 is the shoulder and it is the most restricted joint on the arm. It has 126.7° of travel where the base has 350°, and only 36.7° of that is forward of vertical. In practice this limit sets the shape of the reachable workspace far more than the link lengths do. I had to move the capture point in twice before the shoulder could reach it, and even now it ends up within 8° of the stop.

Forward kinematics means going from a set of joint angles to the position and orientation of the gripper. It is the easier of the two directions, and it is also what I check everything else against.

The Denavit-Hartenberg convention describes the relationship between two consecutive links using four parameters, and only four, which is the reason it is worth using at all:

• a_i · length of the link along the common normal
• α_i · twist from the old z axis to the new one about that normal
• d_i · offset along the previous z axis to the common normal
• θ_i · joint angle about the previous z axis, from the old x axis to the new one

Each row of four gives you one transformation matrix:

Multiplying all six together gives the transformation from the base frame to the gripper:

These are the parameters the arm runs on. Three of the a values come out at zero, which is what you get when the frames are assigned sensibly.

Rows 4, 5 and 6 all have a_i = 0, which is the spherical wrist written out. The three axes meet at the origin of frame 4, and the only thing between that point and the tool tip is a 23.7 mm offset along the tool’s own z axis. Section 04 uses that.

Inverse kinematics is the direction you actually need for planning. You know where you want the gripper and which way you want it pointing, and you have to work backwards to the six joint angles.

Because the wrist is spherical this splits cleanly in two. The position of the gripper is set almost entirely by the first three joints and its orientation by the last three, so the two halves can be solved one after the other rather than together.

Inverse position problem

If you back the tool tip off along its own z axis by the 23.7 mm from the table, you arrive at the wrist centre, which is the one point on the arm that the last three joints cannot move:

Joint 1 comes directly from where that point sits in plan view. Joints 2 and 3 are then a planar two-link problem in the vertical plane joint 1 has just swept out, with the shoulder at one end and the wrist centre at the other.

The part that is easy to get wrong is that the forearm is not a straight link. There is a 41.5 mm offset at the elbow and then 180 mm out to the wrist centre, so the effective link is 184.7 mm long and it sits 12.98° off the line you would draw between the two joints. If you ignore that, every solution comes back a couple of centimetres short, and the error looks like a rounding problem rather than a geometry problem.

Inverse orientation problem

Once the first three angles are known, the rotation they produce is known as well, so you can take it off the target and whatever is left is what the wrist has to supply:

I take R₃₀ from the forward kinematics pass rather than deriving it separately. If you derive the same rotation twice you have two things that can disagree with each other, and when they do the symptom is a pose that is a few degrees off with nothing obviously wrong in either derivation.

Choosing between the solutions

Any reachable pose has eight solutions: elbow up or down, shoulder reaching forwards or leaning back over the base, and the wrist folded either way. All eight put the gripper in the same place, but they are not interchangeable.

So the solver works out all eight and ranks them. Joint limits come first, since a solution that breaks a stop is not a solution. After that it ranks on how far each one sits from the previous waypoint, and that second test matters more than it sounds. Two waypoints only a few centimetres apart can easily be solved on different branches, and if you then interpolate between them the arm swings through most of its workspace and comes back. The joint angles are valid at both ends. The elbow still goes through the payload on the way.

There is a related problem at the wrist. When joint 5 passes through zero the axes of joints 4 and 6 line up, only their sum is determined by the target, and the solver is free to return any split that adds up to it. My first version of the capture approach ran almost straight into that. Joint 4 came back at −50° at the capture waypoint against +6° at the waypoint 40 mm before it, and both were correct as poses. The interpolation between them was not. The fix was to search the approach direction rather than to patch the angles afterwards, because a couple of degrees of joint 5 is enough to make the two splits genuinely different orientations. The approach the arm uses now comes down at 74° below horizontal and keeps joint 5 about 55° clear of zero the whole way in.

Checking the solver is straightforward once you trust the forward direction. I generate 3,000 random joint configurations inside the limits, run forward kinematics on each to get a pose, hand that pose back to the solver, and compare what comes out. The worst position error over the run was 5×10⁻¹⁶ m.

A capture is a sequence of poses rather than a single one, and the arm has to be somewhere sensible at every point in between.

Waypoints are specified as homogeneous transforms, which pack a rotation and a translation into one matrix, so each waypoint says both where the gripper goes and which way it faces:

Each waypoint goes through the inverse solver once. After that the arm is described by six numbers per waypoint, and the path between them is interpolated:

I interpolate in joint space and the reason is the joint limits. The limits are a box, six independent intervals, and a straight line between two points inside a box stays inside the box. So if both waypoints are legal then everything between them is legal as well, and there is nothing left to check. Interpolating the gripper along a straight line in space gives you no such guarantee, and you end up testing the limits at every sample and backing off whenever one trips.

What it costs is that the tip does not travel in a straight line, it bows out slightly. That is why standoff, approach and capture are three separate waypoints along one line rather than a single long move. Over the last 40 mm the bow is small enough to ignore, and the gripper goes in along the axis of the post instead of sliding sideways across it.

All in degrees. Joint 2 is the one to watch at capture, 28.9° against a 36.7° stop. Joint 1 holds at −26° through all four approach waypoints because the approach runs along a single line and the base does not have to turn to follow it, then it does 104° on its own during the carry. Joint 6 does not move at all through the approach, which is what you get by solving the four collinear waypoints on one shared tool roll instead of letting each one choose its own.

Watch the approach and grasp on their own ↗

All of the above is kinematics. There is no mass anywhere in it.

That is the honest limit of the work. The arm knows what angles its joints need to be at and nothing about what it costs to get them there. No inertias, no joint torques, no reaction back into the station, no contact model at the gripper. For a half-metre arm on a bench that is a reasonable thing to leave out. For eight tonnes of tumbling satellite it is the part that decides whether the capture works at all, because as soon as the gripper closes the arm and the debris are one body and whatever angular momentum ENVISAT had becomes the station’s problem.

There are two smaller gaps as well. Interpolating in a straight line through joint space means every joint runs at a constant rate and then reverses direction instantly at each waypoint, which no real actuator will do. Putting a trapezoidal or S-curve velocity profile over the same joint path would fix that without changing anything above it. And nothing here checks the arm against itself or against the payload. The branch continuity rule in section 04 avoids the obvious collisions by keeping consecutive waypoints in the same family of solutions, but that is working around the problem rather than solving it.

The original MATLAB and the CoppeliaSim scene are both lost. The parameters in section 03 came back out of the manufacturer’s published geometry, and I rewrote the kinematics from scratch and checked them against themselves, which is why section 04 quotes a round-trip residual rather than showing a screenshot. The simulation figures are from the original work.