Lecture 3B.Supplemental_2
Supplementary Material - Solving for Coefficients of Minimum Jerk Trajectories Presented by Lucy Tang, PhD student. In this segment, we'll talk more about how to find the coefficients of a minimum-jerk trajectory. ecall that we can find the minimum-jerk trajectory by sol!ing the following minimi"ation #roblem. T
x
(
't & arg min x ' t &
T
L' x, x, x, x, t &dt arg min L' x &dt %
x ' t &
$
$
)y sol!ing the *uler-Lagrange e+uation L x
L d % L d - L % - $ dt x dt x dt x d
we find that a necessary condition for the minimum-jerk trajectory is that its thderi!ati!e must be $. This leads us to conclude that a minimum-jerk trajectory takes the form of a / th-order #olynomial, with si0 unknown coefficients. 1ur goal is to find the !alues of these coefficients x't &
c/ t / c 3 t 3 c-t - c % t % c2t c$
To do this we define si0 boundary conditions Position
4elocity
5cceleration
t6$
a
$
$
t6T
)
$
$
Let's write out each of these boundary conditions e0#licitly. The #osition of the trajectory is gi!en by the function 0t&. 5t time $, this function e!aluates to c $. 5ccording to the boundary condition, this must be e+ual to a. 5t time T this function e!aluates to the e0#ression below. 5ccording to the boundary condition, this must be e+ual to b. 7ote that in this e0#ression, the constant T is e0#onentiated to !arious #owers. 8owe!er, because T is known, we can e!aluate these e0#onentials e0#licitly and they become constants. The unknown coefficients are not e0#onentiated, therefore this e0#ression is linear in terms of the coefficients
9e can rewrite the #osition constraints at t 6 $ in matri0 form
c/ c 3 c $ $ $ $ $ 2 - a c % c2 c$
8ere, we are multi#lying a 20 matri0 with a 02 matri0, which will result in a 202 matri0, or sim#ly, a constant. :urther, if we carry out the matri0 multi#lication, c $ will multi#lied by 2 and all the other coefficients will be multi#lied by $. Thus this matri0 e0#ression is e+ui!alent to the constraint c $ 6 a. 9e can similarly re#resent the #osition constraint at time T in matri0 form
T
/
T
3
-
T
T
%
T
c/ c 3 c- 2 b c% c2 c$
5gain, because T is known beforehand, the matri0 on the left contains only constants. 9e see that the second matri0 term on the left-hand side, which contains our unknown coefficients, is the same as the second matri0 term of the left-hand side of the #osition constraint at t 6 $. 7e0t, we consider the !elocity constraints. 9e can find the function for the trajectory;s !elocity by differentiating the #osition function 0t&. The conditions at t 6 $ and t 6 T gi!e us the following constraints
5s before, we can write the constraints in matri0 form. 9e can write the constraint at t 6 $ in the following way
' $& x
c2 $
9e can do the same for the constraint at t 6 T
'T & x
/c 'T &3 3c 3 'T &- -c 'T &% %c 'T & c2 $ /
%
c/ c 3 c $ $ $ $ 2 $ - $ c% c2 c$
/T
3
3T -
-T %
%T
c/ c 3 c- 2 $ $ c% c2 c$
5gain, we see that the matri0 containing the unknown coefficients remains the same in both of these constraints. :inally, we can re#eat this #rocess for the acceleration constraints. Differentiating the !elocity function gets the acceleration function, and from this, we can write the conditions at t 6 $ and t6T
9e can now write these constraints in matri0 form. 5s you #robably e0#ected, these e0#ressions also contain the same matri0 of unknown coefficients we saw earlier
c/ c 3 c $ $ $ % $ $ - $ c% c2 c$
%$T
-
2%T %
.T
c/ c 3 c- % $ $ $ c% c2 c$
)ecause all si0 constraints can be written as a 20 matri0 multi#lied by the same 02 matri0, we can combine them into a single matri0 e0#ression $ $ $ T / 3 T T $ $ $ 3 3T - -T % /T $ $ $ %$T - 2%T % .T
$ T
%
$ T
$
2
%T
2
%
$
%
$
2 c/
a 2 c3 b $ c- $ $ c% $ $ c2 $ $ c$ $
8ere, each row of the 0 matri0 on the left-hand side, in conjunction with the corres#onding row of the 02 matri0 on the right-hand side, re#resents one of the boundary conditions. 5gain, because T, a, and b are known beforehand the only unknown !ariables a##ear in the matri0 of coefficients. 9e can now use one of many techni+ues to sol!e for this matri0 of unknown coefficients. Let's consider an e0am#le. 8ere, T 6 2, a 6 $, and b 6 /. 9e can substitute these !alues into the #re!ious e0#ression to get the following matri0 #roblem
$ $ 2 2 $ $ / 3 $ $ %$ 2%
$
$
$
2
2
2
$
$
2
-
%
2
$
%
$
.
%
$
2 c/
$ $ c3 / $ c- $ $ c% $ $ c2 $ $ c$ $
If we denote the 0 matri0 on the left-hand side as 5, and the 02 matri0 on the right-hand side as b, we ha!e a #roblem in the form 50 6 b. 9here 0 is the matri0 of unknown coefficients. 9e can find 0 by in!erting 5 and multi#lying it by b 0 6 5-2 b
This gi!es us the following matri0 for 0, which contains the !alues of the unknown coefficients in the order that we designated
-$ =/ /$ x $ $ $ This means that the minimum-jerk trajectory for this #roblem is x 't &
-$t / =/t 3 /$t -
9e can !erify that this trajectory does, in fact, satisfy all the boundary conditions. *!aluating the function 0t& at $ and 2, gi!es us a !alue of $ and / res#ecti!ely, as we s#ecified in the boundary conditions
9e can differentiate 0t& to get the !elocity function x 't & . This function is $ at both $ and 2
:inally, we can differentiate once more to get the acceleration function. This function is also "ero at both $ and 2
It is im#ortant to notice that when transforming the condition e0#ressions into matri0 form, the order in which we #lace the unknown coefficients matters. 9e must be sure that all coefficients are matched u# with the right constants. :or e0am#le, in our e0am#le the matri0 e0#ression for the #osition constraint at t 6 T was this
T
/
T
3
-
T
T
%
T
c/ c 3 c- 2 b c% c2 c$
If we re!erse the order of the unknown coefficients in the second matri0 term on the left-hand side, we must also re!erse the order of the terms in the first matri0 term, thus
2
T
T
%
-
T
T
3
c$ c 2 c% / T b c- c3 c/
9e can change the order of the unknown coefficients in the second matri0 term howe!er we want, as long as we make sure the corres#onding coefficient in the first matri0 term match u#. 9hen sol!ing the com#lete #roblem it is im#ortant that we order the unknown coefficient in the second matri0 term in the same manner for all constraints.
