Page 70 - Human Inspired Dexterity in Robotic Manipulation

66 Human Inspired Dexterity in Robotic Manipulation

1 2 T
H ¼ u ðtÞ + λ ðAx + buÞ, (5.14)
2
_
where λ is the costate vector. From λ ¼ ∂H=∂x one obtains the costate
_
T
T
dynamics λ ¼ A λ, and establishes λ(t) ¼ e A t λ(0), where the vector
λ(0) is to be determined from the boundary conditions. Next, from ∂H/∂
u ¼ 0 one defines the structure of the optimal control
T
T A t
uðtÞ¼ λ ðtÞb ¼ b e T λð0Þ: (5.15)
Upon substituting Eq. (5.15) into Eq. (5.10), one defines
ð T
T A τ
At
xðTÞ¼ e xð0Þ e AT e Aτ bb e T dτ λð0Þ, (5.16)
0
and, therefore,
1

ð T
T A τ
λð0Þ¼ e Aτ bb e T dτ ð xð0Þ e AT xðTÞÞ: (5.17)
0
The optimal trajectory is thus determined as

ð t
T A τ
At
xðtÞ¼e xð0Þ e At e Aτ bb e T dτ
0
(5.18)
ð T 1
T
Aτ T A τ AT
e bb e dτ xð0Þ e xðTÞ :
0
To proceed further, one needs to define the structure of the matrix expo-
At
nential e . Note that the matrix A has the upper triangular block form, and
so does its matrix exponential:

A o A oh At E o E oh
A ¼ , e ¼ , (5.19)
A h E h
O O
where O is the 3 4 zero matrix, and the block components are defined as
e
E o ¼ e A o t , E h ¼ e A h t , and E oh ¼ Ð t A o ðt sÞ A oh e A h s ds [18]. The specific calcu-
0
lations yield
sinω 1 t
2 3
cosω 1 t 0 0
ω 1
6 7
6 ω 1 sinω 1 t cosω 1 t 0 0 7
E o ¼ 6 7 , (5.20)
6 sinω 2 t 7
0 0 cosω 2 t
4 5
ω 2
0 0 ω 2 sinω 2 t cosω 2 t