(FIXME, uploaded image does not seem to work)|
Figure 1 illustrates the kinematics of a trapezoidal velocity profile Trajectory planner.
In such a scheme waypoints are parameterized in time- that is each trajectory point is given as a physical location to be reached at a particular time. If the input for this is gcode, a destination and velocity is given. This is easily converted to time by the simple formula time= distance/feed.
The chart shows results for one axis only. A coordinated set of these parameterized points can be one for multi-axis motion.
Note the shape of the upper velocity profile. That is where the term trapeziodal comes from. There are regions of constant velocity that are horizontal...we call those areas cruise regions. They are connected by periods of linearly changing velocity. We call those the blend regions.
If we integrate the velocity with respect to time we get the lower chart. That is position versus time. Since the blend phase is linear, the path in that area becomes a parabola with respect to time as shown in red.
Thus motions are generated with smooth constant acceleration near waypoints rather that an abrupt corner that is physically unrealizable with continuous speed.
For this reason the method is also widely known as the method of linear paths with parabolic blends.
The method results in motions with piecewise continuous accelerations and infinite jerk. It can be further smoothed with cubic or quintic sub interpolation for more controlled paths.
The calculations are as follows:
Given trajectory waypointa Xi, Xj, Xk....Xn and desired signed feedrates between points Fij, Fjk,Fkl... we will calculate the blends and cruises at and about point Xk.
Only three points, Xj, Xk, and Xl and the desired feed Fjk and Fkl are required.
tmjk= desired time interval to go from point Xj to Xk
tmkl= desired time interval to go from point Xk to Xl
dXjk/dt = (position of Xk - position ofXj)/tmkl=Fjk
dXkl/dt = (position of Xl - position ofXk)/tmjk=Fkl
blend accel = SGN(dXkl/dt - dXjk/dt)|desired acceleration|
blendtime k= (dXkl/dt-dXjk/dt)/|desired acceleration|
cruisetime jk= tmjk - 1/2 blendtime j - 1/2 blendtime k
This process can be continued along the path for each point using the point before and the point after. It is not shown in this example, but the initial and final points of a complete move are calculated slightly differently since an entire blend region at one end of the segment must be counted in the total segment's time duration.
The results of these calculations for cruise velocities and cruise and blend times can be used to create a queue of planned motions to execute the entire path.
Current acceleration and velocity can be calculated for any point on the path at time t using these formulas:
X=Xj + tdXjk/dt
tinb= t-(tj/2 = tjk)
X = Xj + dXjk/dt (t-tinb) +(tinb^2 )(desired acceleration)/2
dX/dt = dXjk/dt + (desired acceleration)tinb
This continues with t being reset to tj/2 when a new linear segment is entered, until we have worked our way through all the data sets reperesenting the path segments.
Since this is just a rearrangement of one command list (gcode program) to another (stored motion queue) it can be done at any time - either before motions are executed or during.