Method of tolerance-based trajectory planning

A method of controlling movement of a body at spaced apart instances along a trajectory in a multi-dimensional environment, including the steps of:

• determining a current position of the body in the multi-dimensional environment at a current instance;
• determining an ideal point on the trajectory, the ideal point being a point on the trajectory that is closest to the current position;
• determining a vector difference between the current position and the ideal point;
• and
• adjusting a velocity of the body for a subsequent instance based on the vector difference, wherein the velocity is adjusted based at least on a tangential tracking error of the ideal point and on a distance from the ideal point to an endpoint of the trajectory.

A method of controlling movement of a body at spaced apart instances along a trajectory in a multi-dimensional environment, including the steps of:

• determining a current position of the body in the multi-dimensional environment at a current instance;
• determining an ideal point on the trajectory, the ideal point being a point on the trajectory that is closest to the current position;
• determining a vector difference between the current position and the ideal point;
• adjusting a velocity of the body for a subsequent instance based on the vector difference;
• calculating a first tracking error in a normal direction which corresponds to a distance from the current position to the ideal point;
• and
• calculating a second tracking error in a tangential direction which corresponds to a path length that the ideal point lags behind.

A method of controlling movement of a body at spaced apart instances along a trajectory in a multi-dimensional environment, including the steps of:

• determining a current position of the body in the multi-dimensional environment at a current instance;
• determining an ideal point on the trajectory, the ideal point being a point on the trajectory that is closest to the current position;
• determining a vector difference between the current position and the ideal point;
• adjusting a velocity of the body for a subsequent instance based on the vector difference, wherein the trajectory is comprised of a series of arcs each of which is tangent to a preceding arc and to a succeeding arc.

A method of controlling movement of a body along a trajectory, including the steps of:

• determining whether a status flag is set to accelerate or decelerate;
• if the status flag is set to accelerate, then
• determining a stop distance required for a current state of the body, the current state including position and velocity, and
• comparing the stop distance required to a second distance between an ideal current position of the body and the ending position;
• if the second distance is less than the stop distance required, then changing the status flag to decelerate;
• if the second distance is greater than the stop distance required, then determining a desired acceleration and a desired velocity for the body;
• and
• adjusting a velocity of the body based on the desired acceleration and the desired velocity for the body.

A method of controlling movement of a body through a series of motion commands, for a representative motion command of the series of motion commands the method including the steps of:

• obtain a motion command having a contour and an endpoint;
• determine a stopper location associated with the motion command, the stopper location being spaced apart from the endpoint;
• generating a speed profile for the motion command which has a zero velocity at the stopper location and passes through the endpoint of the motion command at a desired speed, the desired speed being greater than zero;
• and
• executing the motion command.