Page 62 - Sensing, Intelligence, Motion : How Robots and Humans Move in an Unstructured World

FEEDBACK CONTROL 37

D 11 = m 1 k 2 122

2
2
+ m 2 k 2 s θ 2 + k 2 c θ 2 + r 2 (2y 2 + r 2 )
211 233

2
2
2
+ m 3 k 2 322 s θ 2 + k 2 333 c θ 2 + r 3 (2z 3 + r 3 )s θ 2 + r 2 2

2
2
2
1 2 s θ 2 (2s θ 4 − 1) + s θ 4 + k (1 + c θ 2 + s θ 4 )
2
1 2
2
k
+ m 4
2 411 2 422

2
1 2
2
2
2
2 2
2
+ k s θ 2 (1 − 2s θ 4 ) − s θ 4 + r s θ 2 + r − 2y 4 r 3 s θ 2 + 2z 4 (r 2 sθ 4 + r 3 sθ 2 cθ 2 cθ 4 )
2 433 3 2

1 2 2 2 2 2 2
+ m 5 (−k + k + k ) (sθ 2 sθ 5 − cθ 2 sθ 4 cθ 5 ) + c θ 4 c θ 5
2 511 522 533
1
2
2
2
+ (k 2 511 − k 2 522 + k 2 533 )(s θ 4 + c θ 2 c θ 4 )
2
1 2 2 2
2 2 2 2 2 2
+ (k + k − k ) (sθ 2 cθ 5 + cθ 2 sθ 4 sθ 5 ) + c θ 4 s θ 5 + r s θ 2 − r
2 511 522 533 3 2

2
+ 2z 5 r 3 (s θ 2 cθ 5 + sθ 2 sθ 4 cθ 4 sθ 5 ) − r 2 cθ 4 sθ 5

1 2 2 2 2 2
+ m 6 (−k + k + k ) (sθ 2 sθ 5 cθ 6 − cθ 2 sθ 4 cθ 5 cθ 6 − cθ 2 cθ 4 sθ 6 ) + (cθ 4 cθ 5 cθ 6 − sθ 4 sθ 5 )
2 611 622 633

1
2
+ (k 2 − k 2 + k 2 ) (cθ 2 sθ 4 cθ 5 sθ 6 − sθ 2 sθ 5 sθ 6 − cθ 2 cθ 4 cθ 6 ) + (cθ 4 cθ 5 sθ 6 + sθ 4 cθ 6 ) 2
2 611 622 633
1 2 2 2
2 2 2
+ (k + k − k ) (cθ 2 sθ 4 sθ 5 + sθ 2 cθ 5 ) + c θ 4 s θ 5
2 611 622 633

2 2
− r 6 cθ 2 sθ 4 sθ 5 + (r 6 cθ 5 + r 3 )sθ 2 + (r 6 cθ 4 sθ 5 − r 2 )
2
2
2
2

2
2
2
+ 2z 6 r 6 (s θ 2 c θ 5 + c θ 4 s θ 5 − c θ 2 s θ 4 s θ 5 + 2sθ 2 cθ 2 sθ 4 sθ 5 cθ 5 )

2
− r 3 (sθ 2 cθ 2 sθ 4 sθ 5 + s θ 2 cθ 5 ) − r 2 cθ 4 sθ 5
Figure 2.6 The full expression for one term, D 11 , of the matrix (2.15).
2.4 FEEDBACK CONTROL
In the jargon of control theory, the system under control is called a plant.In
our case the plant is a robot arm manipulator. In doing its job of controlling
the motion of arm motors, the robot control system realizes an appropriate
control law, which is the relationship between the system’s input and output.
For the arm control, the input is the arm’s desired position(s), and the output
is the arm’s corresponding actual position. One can distinguish three types of
control: