Robotics 10 Trajectory Planning - 4s

11/04/2023
-2-
Trajectory planning
Chương 10.
THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG
Trajectory planning
điểm tựa
(trung gian)
A
điểm đầu Vật cản
nhanh không gian

tự do
Nguyễn Quang Hoàng Vật cản
Email: hoang.nguyenquang@hust.edu.vn B
chậm không gian chật hẹp, chậm
có rào bảo vệ
điểm cuối
Các điểm tựa khi thiết kế quỹ đạo quanh các Chuyển động trong không gian tự do và trong
vật cản không gian chật hẹp, có rào bảo vệ
Nguyen Quang Hoang - Department of Applied Mechanics - SME Nguyen Quang Hoang - Department of Applied Mechanics - SME
1 2
-3- -4-
Trajectory planning Trajectory planning – Motion planning
1. Khái niệm đường di chuyển (path) và quỹ đạo chuyển động (trajectory) Động học: giải quyết các mối quan hệ hình học về vị trí / vận tốc giữa không gia khớp
2. quỹ đạo nối điểm đầu điểm cuối q(t) và không gian thao tác (làm việc) x.
3. quỹ đạo qua các điểm tựa
Động lực học: giải quyết mối quan hệ giữa mômen xoắn/lực tác dụng tại các khớp và
4. Tạo luật chuyển động nhờ đa thức bậc 3, 4, 5, 6, 7
các chuyển động của các khâu.
5. Tạo luật chuyển động nhờ các hàm điều hòa (sin, cos)
6. Thiết kế luật chuyển động trong không gian khớp Điều khiển: tính toán lực/mômen khớp cần thiết để thực hiện một chuyển động mong
7. Thiết kế quỹ đạo trong không gian thao tác (đề các) muốn.
8. Đường spline bậc 3 (cubic S-pline) [remind Polynomial interpolation; Thiết kế (hay lập kế hoạch) quỹ đạo: tạo ra chuyển động mong muốn của các tay máy,
Lagrange polynomials are used for polynomial interpolation, Newton đáp ứng yêu cầu nhiệm vụ đặt ra.
interpolation]
Thông thường, người dùng sẽ được yêu cầu để xác định một số điểm và đặc tính
chung của quỹ đạo (ví dụ như điểm ban đầu / cuối cùng, thời gian, vận tốc tối đa, vv),
Làm sao để robot thực hiện chuyển động theo và thực tế tính toán của quỹ đạo được yêu cầu đến hệ thống điều khiển.
mong muốn của người sử dụng?
3 4
11/04/2023
-5- -6-
Trajectory planning – Motion planning Trajectory planning – Motion planning
Thiết kế quỹ đạo: là vấn đề quan trọng trong lĩnh vực robot, rất Các phương thức thiết kế quỹ đạo có thể là hoàn toàn khác nhau:
quan trọng trong việc đo đạc kích thước, điều khiển, và sử dụng điểm-điểm
động cơ điện trong máy tự động (ví dụ: đóng gói bao bì). với con đường được xác định trước
hoặc:
Bài toán này ban đầu xuất hiện trong ngành cơ khí (thiết kế biên trong không gian khớp (biến q);
dạng cam): với công nghệ robot thì ta thay thế cam cơ khí bằng trong không gian làm việc, hoặc xác định một số điểm quan tâm
cam điện (thiết kế quỹ đạo). (đầu tiên và cuối cùng điểm, điểm trung gian) hoặc toàn bộ đường
hình học x = x(t).
Tài liệu tham khảo: Việc thiết kế quỹ đạo mong muốn, sự cần thiết phải xác định hai
1. Siciliano – Sciavicco: Modeling and Control of Robot Manipulators. Chapter 4. khía cạnh:
2. Reza N. Jazar: Theory of Applied Robotics: Kinematics, Dynamics, and Control. đường di chuyển hình học (path)
Chapter 13.
3. B. Heimann+: Mechatronik: Komponenten-Methoden-Beispielen Chapter 7.
luật chuyển động (motion law)
4. L. Biagiotti · C. Melchiorri: Trajectory Planning for Automatic Machines and cùng với các ràng buộc về tính liên tục (trơn tru) của quỹ đạo và
Robots. Springer 2008. tính liên tục của đạo hàm với cấp xác định (bậc 1, 2, 3, …).
5 6
-7- -8-
Đường hình học có thể được xác định trong không gian làm việc hoặc trong không gian
khớp. Thông thường, nó được thể hiện trong một hình thức tham số như Ví dụ về đường di chuyển hình học trong không gian thao tác: đoạn thẳng,
-Trong không gian làm việc: đường hình học là quỹ tích các điểm mà điểm tác động cuối đoạn của parabol, cung tròn, …
của tay máy đi chuyển qua. Trong không gian khớp, quỹ đạo hình học nhận được bằng cách gán các giá trị
p  p(s )   3 , s  , s  [0, L ] đầu/cuối (có thể có các giá trung gian) cho các biến khớp, tương ứng với luật
- Khái niệm đường hình học trong không gian khớp được hiểu một cách trừu tượng, là chuyển động mong muốn.
một hàm
q  q()  n ,   ,   [min , max ] B, s=L

Liên quan đến luật chuyển động, là cần thiết để xác định các hàm liên tục đến
z0 cấp đạo hàm nhất định (thường ít nhất là đạo hàm cấp 1 và cấp 2, tức là liên
Nếu biết thêm quy luật theo tục về vận tốc và gia tốc).
thời gian  z(s)
=0 =1 Thông thường, các đa thức bậc N đối với biến thời gian được sử dụng để mô
s  s(t ),   (t ) A, s=0
đường di chuyển sẽ được gọi

p(s) tả luật chuyển động:
p(s)
t=0 [t0 tf ] s
là quỹ đạo chuyển động t
y(s)
(Trajectory) O0
p  p(s )  p(s(t ))   3 , s x(s)

y0
S (t )  PN (t )  a 0  a1t  a2t 2  ...  aN 1t N 1  aN t N
s=0 s= L

t   , t  [t 0 , t f ] x0
7 8
11/04/2023
-9- -10-
Bài toán thiết kế quỹ đạo có đầu vào thường là:
Thông thường, người sử dụng chỉ xác định một số lượng tối thiểu
 dữ liệu xác định đường di chuyển (qua các điểm), các thông tin về quỹ đạo, chẳng hạn như điểm ban đầu và cuối
 ràng buộc hình học về đường di chuyển (ví dụ như chướng ngại vật), cùng, thời gian chuyển động, vận tốc tối đa, và v.v...
 ràng buộc trên các động học (giới hạn khớp, vận tốc cực đại, )
 ràng buộc do hệ thống truyền động (lực, mô men cực đại ) • Quỹ đạo trong không gian làm việc cho phép xem xét trực tiếp
Và đầu ra thường là
nhất các ràng buộc có thể có đối với đường dẫn di chuyển
• quỹ đạo trong không gian khớp, hoặc không gian làm việc, được đưa ra như là một (chướng ngại vật, tính chất hình học của đường di chuyển, ...) mà
chuỗi (theo thời gian) của gia tốc, vận tốc và vị trí các giá trị: có nhiều khó khăn hơn khi xem xét chúng trong các không gian
khớp (do tính phi tuyến của động học)
a(kT), v(kT), q(kT) hoặc a(kT), v(kT), p(kT) , k = 0,1,2,…..N_
• Quỹ đạo trong không gian khớp đơn giản hơn về tính toán và cho
T là một thời gian trích mẫu (phù hợp với thời gian tính toán và thời gian xử lý các công
việc liên quan như truyền số liệu đến các thiết bị truyền động. phép xem xét các vấn đề về cấu hình kỳ dị, dư dẫn động, các ràng
buộc về vận tốc và gia tốc.
9 10
-11- -12-
Trajectory planning – Motion planning Joint space trajectory planning
Các quỹ đạo được xác định bằng cách định nghĩa các điểm đặc trưng: Trong các trường hợp đơn giản nhất, (một phần) một quỹ đạo được xác định
 gán trực tiếp các thông số kỹ thuật bằng cách cho trước điều kiện ban đầu và điều kiện cuối cùng về: thời gian
 gán bằng cách định nghĩa các cấu hình mong muốn x trong không gian làm việc, sau chuyển động, vị trí, vận tốc, gia tốc, ....
Sau đó, vấn đề là xác định một hàm
đó chúng được chuyển đổi sang trong không gian khớp nhờ sử dụng mô hình động
học ngược.
q  q(t ) hay q  q(s ), s  s(t )
Các thuật toán để tính toán một hàm q(t) nội suy từ các điểm được đã cho được đặc
trưng bởi các tính năng sau: để những điều kiện ràng buộc được thỏa mãn.
 quỹ đạo phải hiệu quả về mặt tính toán (thời gian tính toán nhỏ)
 profile vị trí và vận tốc (ít nhất) phải là các hàm liên tục theo thời gian Đây là bài toán điều kiện biên, bài toán này có thể được giải quyết dễ dàng bằng
 tác dụng không mong muốn (như độ cong thay đổi đột ngột) phải được giảm thiểu cách lựa chọn hàm đa thức như:
hoặc hoàn toàn tránh được.
Trong phần này, lập trình quỹ đạo cho một biến khớp duy nhất được xem xét. S (t )  PN (t )  a 0  a1t  a2t 2  ...  aN 1t N 1  aN t N
Nếu số khớp nhiều hơn, một chuyển động có phối hợp cần được thiết kế (lên kế hoạch),
ví dụ như cho tất cả các khớp đồng thời chuyển động [cùng thời điểm ban đầu và thời Việc chọn đa thức bậc N = 3, 5 hay 7, … phụ thuộc vào các điều kiện biên cần
điểm cuối], hoặc thiết kế cho khớp nào di chuyển nhiều nhất và sau đó điều chỉnh bằng được thỏa mãn và phụ thuộc vào sự trơn tru của quỹ đạo.
một hệ số thích hợp cho các khớp còn lại [hệ số tỷ lệ ở đây <=1].
11 12
11/04/2023
-13- -14-
Joint space trajectory planning Joint space trajectory planning - đa thức bậc 3
Quỹ đạo đa thức Quỹ đạo đa thức bậc 3
Nói chung, bên cạnh những giá trị ban đầu và cuối cùng, các ràng buộc khác
Nếu chỉ quan tâm đến tính chất liên tục của luật chuyển động, thì việc sử dụng đa
có thể được xác định trên các giá trị của đạo hàm theo thời gian (vận tốc, gia
thức bậc 3 cho phép tạo ra được một quỹ đạo trơn tru về di chuyển và vận tốc
tốc, giật, ...) tại thời điểm tj. Nói cách khác, người ta có xác định một đa thức
thỏa mãn các điều kiện đầu và điều kiện cuối.
q(t), mà đạo hàm bậc k của nó có giá trị xác định tại thời điểm tj.
Xét đa thức bậc ba
Về mặt toán học, những điều kiện này có thể được thể hiện như:
q(t )  a 3t 3  a2t 2  a1t  a 0 ,
n!
k ! ak  (k  1)! ak 1t j  ...  a t n k  q (k ) (t j )
(n  k )! n j q(t )  3a 3t 2  2a2t  a1
Hay ở dạng
Ở đây có bốn hệ số cần được xác định, ngoài các điều kiện đầu và cuối về vị trí
Ma  b
các điều kiện về vận tốc cũng cần được xem xét
M  (n 1)(n 1) M, b đã biết từ số liệu đầu bài, a là véc tơ cần tìm
q(t 0 )  qi , q (t f )  q f , q(t0 )  qi , q(t f )  qf
b  (n 1)1
1
a (n 1)1
aM b
13 14
-15- -16-
Joint space trajectory planning - đa thức bậc 3 Joint space trajectory planning - đa thức bậc 3
Quỹ đạo đa thức bậc 3 Trường hợp thời điểm đầu t0 = 0, ta có
q(t )  a 3t 3  a2t 2  a1t  a 0 , a 0  qi , a1  qi ,
3(q f  qi )  (2qi  qf )t f 2(q f  q i )  (qi  q f )t f
a2  , a3  .
q (t 0 )  qi , q (t f )  q f , q(t 0 )  qi , q(t f )  q f t f2 t f3
Ví dụ:
Phương trình xác định các hệ số:
3 2
Ma  b ti  0, t f  4,
50
q(t0 )  a t  a t  a1t 0  a 0  qi
40
3 0 2 0 q (0)  10, q (t f )  45, o

1 30
aM b
q[ ]
2
q(t0 )  3a t  2a2t 0  a1  qi
3 0 q(0)  0, q(t f )  0 20
vt [ o/s]
o 2
gt [ /s ]
q(t f )  a t  a 2t f2  a1t f  a 0  q f
3 f
3
Kết quả
10
2
q(t f )  3a t  2a 2t f  a1  qf , 1 t t 02 t 03  a 0  qi 
0
3 f
 0     a  [10.00, 0, 6.5625, 1.0938]T -10
0 1 2t0 3t02  a1  qi 
 -20
    
0 1 2 3 4 5
hay
1 t f t f2 t f3  a2  q f 
t [s]
      2
q (t )  10  6.5625t 1.0938t 3
0 1 2t f 3t f2  a 3  q f 
 
15 16
11/04/2023
-17- -18-
Ví dụ: Ví dụ:
ti  0, t f  4, 50
80
q(0)  10, q(t f )  45, 40
ti  0, t f  4, 60
o
q(0)  20, q(t f )  0
q[ ]
q(0)  10, q(t f )  45, 40
30 vt [o/s]
gt [o/s2] q(0)  30, q(t f )  30 20
20
0
10
-20
Ma  r 0 -40 q [o]
-10
Ma  r -60
vt [o/s]
gt [o/s 2]
Kết quả 0 1 2 3 4 5
Kết quả -80
t [s]
0 1 2 3 4 5
a  [10.00, 20.00, 3.4375, 0.1563]T t [s]
a  [10.00, 30.00, 32.8135, 5.4688]T

2 3
q (t )  10  20t  3.4375t  0.1563t
q(t )  10  30t  32.8135t 2  5.4688t 3
17 18
-19- -20-
Xét trường hợp tổng quát, một đa thức bậc 3 cần thỏa mãn hai điều kiện về vị trí và vận Quỹ đạo đa thức bậc 3 đi qua nhiều điểm (đảm bảo liên tục về dịch chuyển và vận tốc)
tốc tại hai đầu ti và tf. Tìm đa thức q(t) bậc 3 trên đoạn [ti, tf] thỏa mãn:
q(ti )  qi , q(t f )  q f , q(ti )  qi , q(t f )  q f Cho chuỗi thời gian t(k) cùng các giá trị về vị trí và vận tốc tương ứng:
Ta viết đa thức cần tìm ở dạng sau: t  [0  t 0  t1  t2  ......  tN  t f ]

q (t )  a  b(t  ti )  c(t  ti )2  d (t  ti )3 , ti  t  t f q  [ q 0 , q 1 , q 2 ,. .. , q N ]
q  [ q0 , q1 , q2 ,. .. , qN ]
q(t )  b  2c(t  ti )  3d (t  ti )2 ,
Coi quỹ đạo là sự nối ghép liên tiếp N đoạn với các điều kiện đầu và cuối sau :
Từ các điều kiện biên với h  t f  ti
a  qi , • Thời điểm đầu và cuối [tk , tk 1 ],
q(ti )  a  qi , b  qi , • Vị trí đầu và cuối [q k , q k 1 ], k  0, 1, ...N - 1
q(ti )  b  qi , 3(qi  q f )  (2qi  qf )(t f  ti ) • Vận tốc đầu và cuối [qk , qk 1 ],
c ,
q(t f )  a  hb  h 2c  h 3d  q f , (t f  ti )2
q(t f )  b  2hc  3h 2d  q f , 2(qi  q f )  (qi  qf )(t f  ti ) Áp dụng N đa thức bậc 3 cho N đoạn quỹ đạo nối hai điểm liên tiếp, với các điều kiện biên
d , cần thỏa mãn
(t f  ti )3
19 20
11/04/2023
-21- -22-
Ví dụ: N = 5; Ví dụ: N = 5;
60
t = [ 0 2 4 6 8 10]; t = [ 0 2 4 6 8 10]; 40
q = [-10 20 0 30 40 40]; q = [ 0 20 40 60 40 40];
qv = [ 0 10 -10 10 0 0]; 60 qv = [ 0 0 0 0 0 0]; 20
40 0
q [ o]
-20
20 vt [ o/s]
gt [ o/s 2]
-40
0 0 1 2 3 4 5 6 7 8 9 10
t [s]
q [ o]
-20
vt [o/s]
gt [o/s 2]
-40
0 2 4 6 8 10
t [s]
Rõ ràng là tính liên tục của vị trí và vận tốc được đảm bảo, tuy nhiên gia tốc có bước nhảy
Rõ ràng là tính liên tục của vị trí và vận tốc được đảm bảo, tuy nhiên gia tốc khi chuyển qua các điểm tựa.
có bước nhảy khi chuyển qua các điểm tựa.
21 22
-23- -24-
Giả sử cần tìm đa thức bậc 5, là hàm của thời gian t trên đoạn [ti  t tf]. Thỏa Từ các điều kiện đầu và cuối ta nhận được q(ti )  a 0  qi ,
mãn điều kiện biên tại hai đầu
q(ti )  a1  qi ,
q(ti )  2a2  qi ,
q (t f )  a5h 5  a 4h 4  a 3h 3  a2h 2  a1h  a0  q f ,
t  ti : q(ti )  q i , q(ti )  qi , q(ti )  qi ,
q(t f )  5a5h 4  4a 4h 3  3a 3h 2  2a2h  a1  qf ,
t  t f : q(t f )  q f , q(t f )  q f , q(t f )  qf .
q(t f )  20a5h 3  12a 4h 2  6a 3h  2a2  qf ,
Xét đa thức bậc 5 cùng các đạo hàm của nó 1 0 0 0 0 0 a  q 

   0  i 
0 1 0 0 0 0 a  q 
   1  i 
q(t )  a 5 (t  ti )5  a 4 (t  ti )4  a3 (t  ti )3  a2 (t  ti )2  a1(t  ti )  a 0 0
 0 2 0 0 0     
 a 2  q i 
Ax  b
 5    
q(t )  5a 5 (t  ti )4  4a 4 (t  ti )3  3a 3 (t  ti )2  2a2 (t  ti )  a1 2 3 4
1

0
h h h
3h 2
h
4h 3
h  a 3  q f 
4 
   
5h  a 4  qf 

q(t )  20a 5 (t  ti )3  12a 4 (t  ti )2  6a 3 (t  ti )  2a2 1 2h
    
0
 0 2 6h 12h 2 20h 3  a 5  qf 
 x  A1 b
23 24
11/04/2023
-25- -26-
Ví dụ Đa thức bậc 5 tìm được là:
ti  0, q(0)  10o , q(0)  0, q(0)  0,
q(t )  210t 5  525t 4  350t 3  10
t f  1, q(t f )  45o , q(t f )  0, q(t f )  0
50
Ma trận A, véc tơ vế phải r và véc tơ hệ số a: 40
30
1 0 0 0 0 0  a 0  10  a   10 
  0   20
0 1 0 0   
0 0  a1   0    a   0 
  1   10
0   
0 0  a 2   0      
0 2 0 a2   0 
0

            -10 q [o]
1 1 1 1 1 1  a 3  45 a 3   350  v/2 [ o/s]
         -20
a/10 [ o/s 2]
0 1 2 3 4 5  a 4   0  a 4  525
         -30
0 0.2 0.4 0.6 0.8 1
0 0 2 6 12 20 a 5   0  a 5   210  t [s]
        
Đa thức bậc 5 tìm được là: Rõ ràng là tính liên tục của vị trí, vận tốc và gia tốc được đảm bảo, tuy nhiên
đạo hàm của gia tốc [jerk= giật] có bước nhảy.
5 4 3
q(t )  210t  525t  350t  10
25 26
-27- -28-
Tương tự như trường hợp sử dụng chuỗi các đa thức bậc 3 để thiết kế một quỹ Phương trình xác định hệ số của đa thức trên đọa thứ k
đạo liên tục về vị trí và vận tốc qua N+1 điểm tựa.
hk  tk 1  tk  0, k  0,1, 2,..., N  1
Với đa thức bậc 5 ta hoàn toàn làm tương tự, ở đây ta cần cho thêm điều kiện về
gia tốc tại các điểm tựa. Do đó gia tốc sẽ là hàm liên tục.
1 0 0 0 0 0  a   q 
   0,k   k 
t  [0  t 0  t1  t2  ......  tN  t f ] 0 1 0 0 0 0  a   q 
   1,k   k 
0 0 2 0 0 0  a 2,k   qk 
q  [ q 0 , q1, q2 ,. .. , q N ] 
 5   
q  [ q0 , q1, q2 ,. .. , qN ] 1 hk hk2 hk3 hk4 hk  a 3,k  qk 1 
    
q  [ q0 , q1, q2 ,. .. , qN ] 0 1 2hk 3hk2 4hk3 5hk4  a 4,k  qk 1 
    
0 0 2 6hk 12hk2 20hk3  a 5,k  qk 1 
    
Phương trình xác định hệ số của đa thức trên đọa thứ k
Quỹ đạo thiết kế q(t) trên đoạn: tk  t  tk 1 , k  0,1,..., N  1
q(t )  qk (t )  a 5,k (t  tk )5  a 4,k (t  tk )4  a 3,k (t  tk )3  a 2,k (t  tk )2  a1,k (t  tk )  a 0,k ,
q(t )  q k (t )  a 5,k (t  tk )5  a 4,k (t  tk )4  a 3,k (t  tk )3  a 2,k (t  tk )2  a1,k (t  tk )  a 0,k , q(t )  qk (t )  5a 5,k (t  tk )4  4a 4,k (t  tk )3  3a 3,k (t  tk )2  2a2,k (t  tk )  a1,k ,
4 3 2
q(t )  qk (t )  5a 5,k (t  tk )  4a 4,k (t  tk )  3a 3,k (t  tk )  2a 2,k (t  tk )  a1,k , q(t )  qk (t )  20a 5,k (t  tk )3  12a 4,k (t  tk )t 2  63,k (t  tk )  2a2,k ,
q(t )  qk (t )  20a 5,k (t  tk )3  12a 4,k (t  tk )t 2  6 3,k (t  tk )  2a 2,k ,
Yêu cầu viết thuật toán để đưa ra quỹ đạo dạng số và đồ thị.
27 28
11/04/2023
-29- -30-
Joint space trajectory planning - đa thức bậc 5 Joint space trajectory planning – profile vận tốc
Ví dụ
t = [ 0 2 4 6 8 10]; Thiết lập quỹ đạo có profile vận tốc dạng hình thang
q = [ 10 20 0 30 40 40];
qv = [ 0 -10 20 3 0 0]; Một cách tiếp cận khác là sự kết hợp các đoạn cong thể hiện bằng các đa thức (như đường
qa = [ 0 5 10 3 0 0]; bậc 2,3,5,7) hay các hàm lượng giác (sin, cosin) với các đoạn thẳng để tạo ra quỹ đạo di
chuyển trơn (là hàm liên tục theo thời gian). Phương án này hay được sử dụng trong công
nghiệp do tính đơn giản của nó.
60 Trường hợp đơn giản nhất là sử dụng profile vận tốc có dạng hình thang cân, vận tốc tăng
50 đều từ 0 đến vận tốc cực đại, duy trì vận tốc cực đại trong một khoảng thời gian, và sau đó
40 giảm đều về 0. Theo cách này ta dễ dàng áp đặt các giá trị cực đại về vận tốc và gia tốc.
30 q [ o]
o
20 vt [ /s]
gt [o/s 2] Với profile vận tốc dạng hình thang quá trình chuyển động sẽ được chia thành ba pha :
10
1. Pha tăng tốc: [chuyển động nhanh dần đều, gia tốc hằng số, vận tốc tăng tuyến tính, và vị
0
trí tăng theo hàm bậc 2]
-10
2. Pha vận tốc hằng: giai đoạn di chuyển với vận tốc không đổi [gia tốc bằng 0, dịch chuyển
-20
là tuyến tính với thời gian]
-30
3. Pha hãm (phanh, giảm tốc): [chuyển động chậm dần với gia tốc hằng số, vận tốc là tuyến
-40
0 1 2 3 4 5 6 7 8 9 10 tính, và dịch chuyển là hàm parabol]
t [s]
29 30
-31- -32-
Joint space trajectory planning – profile vận tốc Joint space trajectory planning – profile vận tốc
Profile vận tốc là hình thang cân. Các hàm gia tốc, vận tốc, và di chuyển như sau:
amax Xét trường hợp thời gian tăng tốc = thời amax Pha tăng tốc: Pha vận tốc hằng: Pha giảm tốc:
tf -ta tf gian giảm tốc, độ lớn gia tốc như nhau. tf -ta tf 0  t  ta : ta  t  t f  ta : t f  ta  t  t f :
ta t ta t q (t )  a q (t )  0 q (t )  c
a 2  b  d 2
q (t )  a t  a qb (t )  b1 qd (t )  c2t  c1
-amax -amax
q (t )  1 a t 2  a t  a q (t )  b t  b 
a 2 1
 a  b qd (t )   12 c2t 2  c1t  c0

1 0

0  t  ta
2 2 1 0
v Tăng tốc v
vmax vmax Các điều kiện để di chuyển được liên tục, từ điểm đầu đến điểm cuối:
ta  t  t f  ta Vận tốc hằng
q(0)  q i , q(0)  0  a 0  q i , a1  0
ta tf -ta tf t t f  ta  t  t f Giảm tốc ta tf -ta tf t
q q qa (ta )  v max  a 2ta  v max  a 2  v max / ta
qf qf qb (ta )  v max  b1  v max
qm qm
Đồ thị luật chuyển động gồm ba đoạn: 1
thẳng và 2 parabol qa (ta )  qb (ta )  12 a 2ta2  b1ta  b0
qi qi
ta tm tf -ta tf t ta tm tf -ta tf t  b0  a t  b1ta  12 a 2ta2  v maxta ,
1
2
2
2 a
 b0   12 v maxta
31 32
11/04/2023
-33- -34-
Phương trình chuyển động tìm được như sau:

amax c2  a2  v max / ta amax
 1 v max 2
tf -ta tf tf -ta tf  qi  t , 0  t  ta
 2 ta
ta t qc (t f  ta )  vmax ta t 
 1
-amax  c2 (t f  ta )  c1  vmax t f  ta  t  t f : -amax q(t )   qi  v maxta  v max (t  ta ), ta  t  t f  ta

q (t )   v max
 2
v  c v  1v

ta
vmax
 c1  v max  c2 (t f  ta )  vmaxt f / ta  vmax
q f  max (t f  t )2 , t f  ta  t  t f
vmax (t f  t ) 2 ta

c q (t )  
 ta
q(t f )  q f , 
ta tf -ta tf t v (t  t )2 ta tf -ta tf t ta , v max
q (t )  q  1 max f Lưu ý rằng, các thông số
q  21 c2t f2  c1t f  c0  q f  c f
2 ta q

qf   12 v 2
t / ta  v maxt f t f / ta  c0  q f qf
max f cần được chọn sao cho, tồn tại thời gian cho pha vận tốc hằng, tức là
qm qm
 c0  q f  12 vmaxt f2 / ta
qi qi
0  ta  t f / 2
ta tm tf -ta tf t ta tm tf -ta tf t
33 34
-35- -36-
Cho biết gia tốc q0 Ta có q0  vmax  q0ta Để biểu thức trong dấu căn không âm, ta cần thêm điều kiện
4 | q f  qi |
amax Từ đồ thị dịch chuyển ta có  amax q02t f2  4q0 (q f  qi )  0  q0 
t  1 t , t f2
tf -ta tf  m 2 f tf -ta tf
q m  qa 
ta t vmax  , q  q(t )  1 (q  q ), ta t Từ đây cho phép tính được giá trị nhỏ nhất của gia tốc.
m
tm  ta  m
2 f i
-amax  1 1 -amax
Từ đó suy ra: qa  q(ta )  qi  q0ta2  qi  vmaxta Nếu lấy gia tốc Profile vận tốc dạng tam giác cân
 2 2
v v
4 | q f  qi | q  1 q t 2 , 0  t  12 t f
vmax vmax 1 
q0   ta  tf q(t )   i 21 c
(q f  qi ) / 2  qi  v maxta / 2 qm  qa (q f  qi ) 2
2 q f  qc (t f  t )2 , 1
t  t  tf
vmax   vmax   t f  2 2 f
tf / 2  ta tm  ta t f  ta
ta tf -ta tf t ta tf -ta tf t
q q Như thế sẽ không còn pha q t, 0  t  12 t f
Với gia tốc a thì thời gian tăng tốc là bao lâu? q(t )   c
qf qf vận tốc hằng qc (t f  t ), 1
t  t  tf
(q f  qi )  2 f
q0ta  vmax   q0ta (tf  ta )  (q f  qi )

qm t f  ta qm
q 0  t  12 t f
(q f  qi ) q02t f2  4q0 (q f  qi ) q(t )   c
qi 1 qi  q t  t  tf
1
2
t  t f ta 
a
0  ta  t f   c 2 f
ta tm tf -ta tf t q0 2 2q0 ta tm tf -ta tf t
35 36
11/04/2023
-37- -38-
Quỹ đạo với thời gian ngắn nhất: Gia tốc chữ nhật
Tóm lại • Chuyển động chia thành 2 pha: pha tăng ta = tf/2
amax 4 | q f  qi | tốc và pha hãm.
qi , qf , tf q0  amax
tf -ta tf Cho Chọn • pha đầu tăng tốc với gia tốc cực đại
t f2 • pha cuối hãm với gia tốc cực đại
ta t ta=tf/2 tf
• Profile vận tốc dạng tam giác. Nếu gia tốc
-amax 1 q02t f2  4q0 (q f  q i ) của hai pha có trị số bằng nhau thì profile t
Tính ta  t f  , v max  q0ta vận tốc sẽ là tam giác cân. Thời gian cho 2
2 2q0 -amax
v pha bằng nhau = ½ tf.
vmax • Thời gian chuyển động sẽ là nhỏ nhất. vmax
ta tf -ta tf t Cho qi , qf , q0 4 | q f  qi | qi , qf , qmax

q Chọn tf  Cho
q0 ta=tf/2 tf t
qf 4 | q f  qi | qf
qm
2 2
q t  4q0 (q f  q i ) Chọn t f 
1 0 f qmax
Tính ta  t  , v max  q0ta
qi 2 f 2q0 1 1
Tính ta  t f , v max  qmaxt f qi
ta tm tf -ta tf t 2 2
37 38
-39- -40-
Một số dạng cải tiến Trường hợn đặc biệt: gia tốc hình sin
Profile vận tốc hình thang cân đơn giản về mặt tính toán, tuy nhiên gia tốc không liên tục. Dưới đây là một số dạng cải Gia tốc hình sin
tiến, với mục đích làm trơn vận tốc và gia tốc. Một sự trả giá cho điều đó là độ phức tạp tính toán tăng lên. Gia tốc hình sin ta = tf/2
ta < tf/2
amax amax
Gia tốc hình chữ nhật Gia tốc hình thang cân ta=tf/2 tf
amax amax tf -ta tf
t
tf -ta tf tf -ta tf ta t
-amax
ta t ta t -amax
v
-amax -amax v vmax
vmax
v v
vmax vmax
ta tf -ta tf t ta=tf/2 tf t
ta tf -ta tf t ta tf -ta tf t q q
q q
39 40
11/04/2023
-41- -42-
Joint space trajectory planning – gia tốc hình sin Joint space trajectory planning – gia tốc hình cos
Trường hợp đặc biệt có thể xảy ra khi, không có thời gian chuyển động với vận tốc hằng số Trường hợn đặc biệt: gia tốc hình cos
Gia tốc hình sin
a sin(2t / t ), ta = tf/2 2
0  t  tf a0 q(t )  (q f  q i ) cos(t / t f ),
q(t )   0 f
2t f2
0, tf  t
 1
ta=tf/2 tf q(t ) 
2t f f
(q  qi ) sin(t / t f ), q(t )  (q  qi )  (q f  qi ) cos(t / tf )
2 f
a (t / 2)[1  cos(2t / t )], 0  t  tf t
q(t )   0 f f
0, tf  t -a0

 t f  tf   v
  2t 
q  a 0 t  sin  , 0  t  tf
q(t )   i 2  2  t 
  f 
q f , tf  t
Như thế ta có ta=tf/2 tf t

2(q f  qi )
q f  qi  a 0t f (t f / 2)  a0  q
2
t f
2(q f  qi )
qmax  q(t f / 2)  a 0t f /  
tf
41 42
-43- -44-
Lập quỹ đạo qua một tập điểm Lập quỹ đạo qua một tập điểm
Cho tập điểm theo trật tự thời gian: Yêu cầu: q(k) = S(tk) & đạo hàm S(t) tại tk bằng nhau từ
hai phía (trái và phải)
(tk, qk), k = 0,1,…, N; t(k) < t(k+1),
==> h(k) = t(k+1) - t(k) > 0 Phương án giải quyết:
Tìm hàm trơn S(t) [đường cong trơn, liên tục ít nhất đến đạo hàm cấp 1] đi qua • Sử dụng nội suy đa thức, tìm được đa thức bậc N
các điểm trên: N
S (t )  PN (t )  a 0  a1t  a 2t 2  ...  a N 1t N 1  a N t N   ait i
q(k) = S(tk) & i 0
• Sử dụng đa thức nội suy Lagrange S(t) sẽ là đa thức bậc N

đạo hàm S(t) tại tk bằng nhau từ hai phía (trái và phải)
N (t  t j )
S (t )   q k Lk (t ), Lk (t )  
k 0 0 j N (tk  t j )
j k
đa thức Lagrange cơ sở
43 44
11/04/2023
-45- -46-
• Sử dụng nội suy đa thức, tìm được đa thức bậc N Tập điểm cho
S (t )  PN (t )  a 0  a1t  a2t 2  ...  aN 1t N 1  a N t N
• Từ điều kiện đa thức đi qua các điểm tựa
S (tk )  qk  a 0  a1tk  a2tk 2  ...  aN 1tk N 1  a N t N  qk ,
Kết quả nội suy đa thức Lagrange
k  1, 2,..., N  1
• Tìm được hệ phương trình ĐSTT
1 t t 02 ... t 0N 1 t 0N   a 0   q 0 
 0    
1 t t12 ... t N 1
t1N   a1   q1 
 1 1
    
 ...   ...    ..   Ta  q
 N     
 1 tN 1 tN2 1 ... tNN11 tN 1  a N 1  q N 1  Kết quả nội suy dùng đường spline
     (kết nối các đa thức bậc 3 trên từng đoạn)
1 t tN2 ... tNN 1 tNN   aN   qN 
 N 
• giải hệ phương trình đại số tuyến tính tìm các hệ số
 a  T1q
45 46
-47- -48-
Do đó, bài toán tạo ra một quỹ đạo từ N+1 điểm cho trước, gọi là đường di chuyển qua (tiếp) các nhược điểm
các điểm tựa, được xác định và phải được tay may với tới tại những thời điểm xác định. • số lượng các phương trình ràng buộc tăng lên, gây khó khăn cho việc
Đối với mỗi biến khớp ta có N ràng buộc, và do đó ta có thể sử dụng một đa thức bậc N tìm nghiệm.
để mô tả quỹ đạo đi qua các điểm cho trước đó [như sử dụng nội suy Lagrange, Nội suy • các hệ số của đa thức phụ thuộc vào tất cả các điểm cho trước, do đó,
Newton, …]. Tuy nhiên, sự lựa chọn này, có những nhược điểm sau đây: nếu nó cần thay đổi một điểm, tất cả đều phải được tính toán lại.
• không thể gán trước được vận tốc tại thời điểm ban đầu và thời điểm cuối cùng.
• khi bậc của đa thức tăng lên, ứng xử dao động cũng tăng lên, và điều này sẽ làm cho
các quỹ đạo không còn phù hợp cho các tay máy.
• khi bậc của đa thức tăng lên, thì độ chính xác khi tính toán hệ số của đa thức giảm.
Tìm quỹ đạo đi qua N+1 điểm
• NO: một đa thức bậc N
• YES: N đa thức bậc p thấp hơn N, (p<N), mỗi đa thức nội suy một đoạn
của quỹ đạo. Đơn giản nhất là chọn p = 3 (tất nhiên có thể chọn p =
5,7,..).
47 48
11/04/2023
-49- -50-
Cubic Spline Cubic Spline
Để thiết lập bài toán xây dựng đường spline, ta xét bài toán cho một N+1 các điểm q Sk(t) qN
tựa và chuỗi thời gian tương ứng qk
qk+1 qN-1
ti  t0  t1  .....  tN 1  tN  t f , hk  tk 1  tk  0 q1
qi  q 0 , q1, ..., qN 1, qN  q f , (initial, final ) q0 S0(t)
Đường spline S(t) bậc 3 là một hàm xác định từng khúc thỏa mãn các điều kiện sau
t0 t1 tk tk+1 tN-1 tN t
S (t )  S k (t ) là đa thức bậc 3 trên mỗi đoạn t  [tk , tk 1 ] k  0,1,..., N  1
Ta viết N đoạn của hàm spline S(t) bậc 3 như sau
S (tk )  qk , k  0, 2,..., N S(t) đi qua các điểm tựa Sk (t )  ak  bk (t  tk )  ck (t  tk )2  dk (t  tk )3 , t  [tk , tk 1 ]
k  0,1,..., N  1
S (t ), S (t ) & S(t ) liên tục trên toàn bộ miền thời gian t  [t1 , tN 1 ]=[0, t f ] Cần tìm 4N hệ số
ak , bk , ck & dk , k  0,1, 2,..., N  1
 S(t) là hàm liên tục đến đạo hàm bậc 2 (trơn tru). để các điều kiện liên tục của đường spline được thỏa mãn
49 50
-51- -52-
Thiết lập phương trình xác định các hệ số của đường spline
Ta nhận được 4N-2 phương trình sau:
1. S(t) đi qua các điểm tựa: Sk (tk )  qk , (1)
Sk (tk 1 )  qk 1, k  0,1,..., N  1 ak  q k ,
k  0,1,..., N 1
(2)
ak  hkbk  hk2ck  hk3dk  qk 1 , k  0,1,..., N 1
2. S(t) có các đạo hàm trái và phải tại các điểm tựa bằng nhau (có đạo hàm (3)
bk  2hkck  3hk2dk  bk 1  0, k  0,1,..., N 2
liên tục tại các điểm tựa):
q 2ck  6hk dk  2ck 1  0, k  0,1,..., N 2 (4)
Sk(t) qN
Sk (tk 1 )  Sk 1 (tk 1 ), qk
q
S (t )  S (t ),
k k 1 k 1 k 1
k  0,1,..., N  2 qk+1 qN-1
qk
Sk(t) qN
q1
Hệ 4N-2 pt cho 4N ẩn. qk+1 qN-1
Cụ thể với q0 S0(t) q1
Sk (t )  ak  bk (t  tk )  ck (t  tk )2  dk (t  tk )3 , q0 S0(t)
Sk (t )  bk  2ck (t  tk )  3dk (t  tk )2 , t0 t1 tk tk+1 tN-1 tN
Sk (t )  2ck  6dk (t  tk ), t  [tk , tk 1 ], k  0,1, 2,..., N  1
t0 t1 tk tk+1 tN-1 tN t
51 52
11/04/2023
-53- -54-
Cần thêm 2 pt bổ sung: nhận được bằng cách áp đặt điều kiện tại hai điểm đầu và cuối Biến đổi hệ trên về dạng đơn giản hơn
Từ phương trình (1) ta có ngay ak  qk , k  0,1, 2,..., N  1
S0 (t0 )  0  S0 (ti ), Spline tự nhiên (Natural or Free Spline) là Đặt ck  12 mk từ phương trình (4) suy ra
SN 1 (tN )  0  SN 1 (t f ) đường spline mà độ cong tại hai đầu bằng 0
(zero curvature). mk 1  mk
mk  6hk dk  mk 1  0  dk 
6hk
S0 (t 0 )  A  S 0 (ti ), Spline kẹp 2 đầu (Clamped spline) là đường Thế (ak,ck,dk) vào phương trình (2), sau đó giải tìm
SN 1 (tN )  B  SN 1 (t f ) spline mà hệ số góc tại hai đầu được cho qk 1  ak  hk2ck  hk3dk qk 1  qk hk hk
trước bk   bk   mk  (mk 1  mk )
hk hk 2 6
Not-A-Knot spline với điều kiện tại hai đầu Thay thế (ak,bk,ck,dk) vào phương trình (3) và đơn giản hóa ta nhận được
S0 (t1 )  S1 (t1 ),
như sau (third derivative matching)
S (t )  S (tN 1 ) q  q q  qk 
hk mk  2(hk  hk 1 )mk 1  hk 1mk 2  6  k 2
N 2 N 1 N 1
k 1
 k 1  k=0,…,N-2; N-1 pt; N+1 ẩn.
 h k 1
hk 
53 54
-55- -56-
Cụ thể với đường spline tự nhiên (gia tốc tại thời điểm đầu và cuối bằng 0) Hoặc việt lại thành
S (t )  0
0 0
 2c  0  m 0
0 0
1 0 0 0 0   m 0   0 
SN 1 (tN )  0  SN (tN )  2cN  0  mN  0 
h  m   r 
 0 2(h0  h1 ) h1 0  1   1 
q  q 0   ..   r 
Cùng với q  qk   h1 2(h1  h2 ) h2 0    2 
hk mk  2(hk  hk 1 )mk 1  hk 1mk 2  6  k 2 k 1
 k 1 

0 0 h2 ..  hN 3 0 0 
   
   .. 
 hk 1 hk  k  0,1,..., N  2 
 hN 3 2(hN 3  hN 2 ) hN 2

0   ..  
  

    
Ta thu được  0 hN 2 2(hN 2  hN 1 ) hN 1  mN 1  rN 1 
    
2(h  h )  m   r  0 0 0 0 1   mN   0 
 0 1
h1 0 0  1   1      
 h1 2(h1  h2 ) h2 0 0  m   r 
  2   2 

 0 h 2(h  h3 ) 0   ..   .. 
    rk 1  6  h 1 (q k 2  qk 1 )  h1 (qk 1  q k ) , k  0,2,..., N  2

2 2
    k 1 k 
 .. .. ..  hN  3 0   ..   .. 
    
 0 hN 3 2(hN 3  hN 2 ) hN 2  mN 2  rN 2 
    


0 0 hN 2 2(hN 2  hN 1 ) mN 1  rN 1 
   
Am  r, A  (N 1)(N 1), m & r,  (N 1)1
Giải được
rk 1  6  h 1 (qk 2  qk 1 )  h1 (qk 1  qk ) , k  0,2,..., N  2
 k 1 k  Am  r, A  (N 1)(N 1), m & r,  (N 1)1
m0  0, m1, m2 ,..., mN 1, mN  0
m1 , m2 ,..., mN 1 ; & m 0  0, mN  0
Giải được  ak , bk , ck , dk ; k  0,1,..., N  1
 ak , bk , ck , dk ; k  0,1,..., N  1
55 56
11/04/2023
-57- -58-
Ràng buộc tại thời điểm đầu q  q 
Cụ thể với đường spline clamped (cho trước vận tốc tại thời điểm đầu và cuối) S 0 (t 0 )  q0 :  2h 0m 0  h 0m 1  6  1 0
 q 0  : r0
 h 0 
S0 (t 0 )  q0  S0 (ti ), Clamped Spline là đường spline mà hệ số góc Tại các điểm tựa
k  0,1,..., N  2
SN 1 (tN )  qN  SN 1 (t f ) tại hai đầu được cho trước (trung gian)
q  q q  qk 
hk mk  2(hk  hk 1 )mk 1  hk 1mk 2  6  k 2 k 1
 k 1 
tại thời điểm đầu  hk 1 hk 
Ràng buộc tại thời
S0 (t )  b0  2c0 (t  t0 )  3d 0 (t  t0 )2 , q  q  điểm cuối SN 1 (tN )  qN :
 2h0m 0  h0m1  6  1 0
 q0  : r0  q  q N 1 
 S (t )  b  q
0 0 0 0  h0 
hN 1m N 1  2hN 1m N  6 q N  N  : r
 hN 1  N
Kết hợp lại ta được hệ N+1 pt cho N+1 ẩn

tại thời điểm cuối
 r0 
SN 1 (tN )  bN 1  2cN 1 (tN  tN 1 )  3dN 1 (tN  tN 1 )2 2h
 0 h0 0 0   m0   

h     6  h1 (q2  q1 )  h1 (q1  q0 ) 
 bN 1  2cN 1hN 1  3h 2
d  qN .  0 2(h0  h1 ) h1 0 0   m1    1 0  
N 1 N 1
 0 h1 2(h1  h2 ) 0   ...   6  1 (q  q )  1 (q  q ) 
     h2 3
 2 h1 2 1 
 
với cuối     
 .. ..   ..    ... 
mN  mN 1 1      

dN 1  , cN 1 
m ,  .. .. ..  hN 2 0   ..  6 1 (q  q )  1
(q  q )
     hN 2 N 1 N 2 h N 3 N 2 N 3  

6hN 1 2 N 1  0 hN 2 2(hN 2  hN 1 ) hN 1  mN 1    1  
q N  q N 1 hN 1 hN 1  q  q N 1     6 (q  qN 1 )  h (qN 1  qN 2 ) 
1
 2hN 1mN  6 qN  N


 0 0 hN 1 2hN 1   mN    hN 1 N  
 : rN
N 2
bN 1   mN 1  (mN  m N 1 ) hN 1m N 1    
hN 1 2 6  hN 1  
rN 
57 58
-59- -60-
Cubic Spline: clamped spline Cubic Spline: Thuật toán
Kết hợp lại ta được hệ N+1 pt cho N+1 ẩn Cho tập N+1 gồm điểm theo trật tự thời gian
Am  r, A  (N 1)(N 1) , m & r,  N 11
ti  t0  t1  .....  tN 1  tN  t f (initial, final )
Giải hệ tìm được
q i  q 0 , q1 , ..., q N 1 , qN  q f
m  A1 r, m  [m 0 , m1 ,..., mN ]T
Từ đó tính được các hệ số của đa thức Sk(t) 1. Tính các bước thời gian
a k  qk
hk  tk 1  tk , k  0,1, 2,..., N  1
qk 1  qk hk hk
bk   mk  (mk 1  mk ) 2. Xây dựng ma trận hệ số A và véc tơ vế phải r (tùy thuộc loại spline)
hk 2 6
ck  12 mk 3. Giải hệ phương trình đại số tuyến tính: Am = r = > m
mk 1  mk k  0,1, 2,..., N  1
dk  4. Giải các hệ số ak, bk, ck, dk = > Sk(t), k = 0,1,…, N-1
6hk
59 60
11/04/2023
-61- -62-
Cubic Spline: Thuật toán Cubic Spline
Ví dụ: cho bộ số liệu sau, N = 5.
4. Tính các hệ số từ véc tơ m
q  qk hk h i 0 1 2 3 4 5
ak  qk , bk  k 1  mk  k (mk 1  mk ),
hk 2 6 t=x(i) 4.00 4.35 4.57 4.76 5.26 5.88
m 
 m q(i) 4.19 5.77 6.57 6.23 4.90 4.77
ck  12 mk , dk  k 1 k
6hk h(i) 0.35 0.22 0.19 0.50 0.62

5. Xây dựng đa thức bậc 3 cho mỗi đoạn t  [tk , tk 1 ] Sử dụng spline tự nhiên ta nhận được
2 3
S k (t )  ak  bk (t  tk )  ck (t  tk )  dk (t  tk ) ,
t  [tk , tk 1 ], k  0,1,..., N  1
k  0,1, 2,..., N  1
spline_natural.m; % natural spline, S"(0) = 0; S"(tf) = 0

spline_clamped.m; % clamped spline, S'(0) = A, S'(tf) = B;
spline_cubic.m
61 62
-63- -64-
Tính được các hệ số Sử dụng spline kẹp hai đầu ta nhận được (clamped spline)
Tính được các hệ số

Chẳng hạn đa thức bậc 3 trên hai đoạn 2 và 3 được viết
ra như sau
Câu hỏi: hãy kiểm tra tính liên tục của hai hàm trên tại x = 4.76
Checking
63 64
11/04/2023
-65- -66-
Cubic Spline Multisegment linear path with blends
Nội suy quỹ đạo dạng LSPB –[Linear Segments with Parabolic Blends]
Sử dụng các đoạn chuyển động tuyến tính + kết nối các đoạn tuyến
tính bằng các đoạn uốn parabol
65 66
-67- -68-
Multisegment linear path with blends Multisegment linear path with blends
Profile vận tốc dạng hình thang là một ví dụ về dạng này. q
a = const qk v = const
Gia tốc hình chữ nhật
amax
tf -ta tf
ta t q0
-amax qk+1
v
vmax
t0 t1 tk tk+1 tN t
ta tf -ta tf t
q
Yêu cầu tính toán: thời điểm tiếp nối thẳng-parabol,
Lưu ý: quỹ đạo chuyển động sẽ không đi qua các điểm tựa.
Cần thiết có sự điều chỉnh nếu muốn v(t0)=v(tf)=0
Điều chỉnh t0 và tN để có được 2 đoạn uốn ở đâu và cuối,
(đảm bảo v0 = vf =0)
67 68
11/04/2023
-69- -70-
Thiết kế quỹ đạo trong không gian thao tác Thiết kế quỹ đạo trong không gian thao tác
Bài 1. Cần di chuyển điểm cuối E từ A(xA,yA) đến B (xB,yB), trong khoảng Bài 2a. Di chuyển từ A đến B theo đường thẳng: Cho A, B, và khoảng tg
thời gian [0 = t0, tf] . Xác định chuyển động không gian khớp q1(t), q2(t). [0=t0, tf] . Xác định chuyển động không gian khớp:
Hai khớp chuyển động đồng thời. q1(t), q2(t)
B, t2 = tf 1. Xác định L = AB = …
B1. Giải động học ngược
từ A(xA,yA) ==> q1(t0), q2(t0) 2. Thiết kế s(t) sử dụng đa thức bậc 3 (5,7,….)
từ B(xB,yB) ==> q1(tf), q2(tf) s(0)  0, s(t f )  L B
s(t)
s(0)  0, s(t f )  0 P
q2 q2 y(s)
3. Xác định điểm P trên quỹ đạo A
B2. Thiết kế luật chuyển động
A, t0 = 0 rB  rA x  Q
(vận tốc ban đầu và cuối = 0)
rP (t )  rA  s(t ) r   
y
L  
[tf, q1(t0), q1(tf)] ==> q1(t) q1 q1
4. Giải động học ngược vị trí (xem chương 5).
O
[tf, q2(t0), q2(tf)] ==> q2(t) x P (t ), yP (t )  q1(t ), q2 (t ) x(s)
69 70
-71- -72-
Thiết kế quỹ đạo trong không gian thao tác
Bài 2b. Cần di chuyển điểm cuối E từ A(xA,yA,zA) đến B (xB,yB,zB), trong khoảng Bài 3. Di chuyển từ A qua điểm tựa C đến B:
thời gian [0 = t0, tf] . Cho phương trình đường thẳng Cho A, C, B, và chuỗi 0 = t0 < t1 < t2 = tf.
Xác định chuyển động không gian khớp: q1(t), q2(t)
rB  rA
rP (t )  rA  s(t ) [x P (t ), yP (t ), z P (t )]
L B, t2 = tf
Sử dụng đa thức bậc 3
s(t )  a 0  a1t  a 2t 2  a 3t 3 , ak ? B C, t1
s(t)
s(0)  0, s(t f )  L y(s)
P
s(0)  0, s(t f )  0 A
Q A, t0 = 0
Cho biết: A[2, 1, 0] đến điểm B[0, 2, 0], tf

= 2 s. Tìm các hệ số ak?
O
x(s)
Cho L1 = 1.5 m, L2 = 1 m == > q1(t), q2(t) ?
71 72
11/04/2023
-73- -74-
Lập trình quỹ đạo trong không gian khớp đảm bảo, bàn kẹp di
chuyển từ vị trí đầu đến vị trí cuối, nhưng không đoán được bàn Nếu mong muốn khâu cuối cần di chuyển theo một đường hình
kẹp sẽ di chuyển theo đường như thế nào do tình phi tuyến của học xác định nào đó [cong hoặc thẳng] trong không gian thao tác,
bài toán động học vị trí. ta cần phải lập kế hoạch chuyển động trực tiếp trong không gian
Một quỹ đạo tránh vật cản sẽ rất khó khăn nếu thự hiện trong đó. Tất nhiên, sau đó sẽ giải bài toán động học ngược để xác định
không gian khớp. các biến khớp tương ứng. Việc lập kế hoạch có thể được thực hiện
bằng cách nội suy thông qua một chuỗi các điểm mà đường di
Theo yêu cầu của bài toán: chuyển cần đi qua hoặc bằng cách tạo ra các chuyển động giải tích
Khâu tác động cuối (bàn kẹp, que hàn, đầu vòi phun sơn, … cần di (thô) nguyên thủy và quỹ đạo tương đối một cách đúng giờ.
chuyển theo một đường nhất định, cùng với hướng xác định
73 74
-75- -76-
Nếu quỹ đạo được xác định trong không gian làm việc, thì cần thiết phải giải bài toán Một vấn đề khác: các vị trí trong tọa độ Descartes thực tế đạt được trong chuyển
động học ngược để tìm quỹ đạo trong không gian khớp (nơi mà các động cơ làm
động thu được bằng cách nội suy điểm q(tk) không còn đúng với các giá trị mong
việc). Vì điều này sẽ làm tăng thời gian tính toán trong việc lập trình quỹ đạo, nên
muốn như thiết kế ban đầu.
việc tính toán quỹ đạo trong kg làm việc và chuyển đổi sang không gian khớp được
thực hiện với một tần số thấp hơn so với tần số điều khiển. Do đó, cần thiết phải nội
suy các dữ liệu trước khi chuyển nó cho bộ điều khiển cấp thấp: thông thường, sử
dụng nội tuyến tính cho đơn giản.
Ví dụ: Đường thẳng là thể

hiện các giá trị mong muốn
như thiết kế ban đầu. Tuy
nhiên khi thực hiện lại thu
được các giá trị thay đổi
xung quanh đường mong
muốn.
75 76
11/04/2023
-77- -78-
x 0 , x1,..., xN Đối với việc tính toán quỹ đạo không gian làm việc, có thể áp dụng một trong các kỹ thuật sử dụng cho
các không gian khớp (thay thế các biến khớp q(t) bằng x(t), tức là một vị trí / hướng trong không gian
(3) t0 , t1,..., tN (2) Descartes) hoặc xác định đường hình học bằng hàm giải tích (ví dụ như một hình elip) là hàm của thời
gian (tức là p = p(t)), hoặc tốt hơn là sử dụng biểu diễn tham số phương trình của đường di chuyển p
= p(s), với s = s(t) là luật chuyển động thích hợp.
q 0 , q1,..., qN xd (t ), Việc gì ở đâu ?
t0 , t1,..., tN t  [t0 , tN ] B, s=L
Nội suy trong không gian khớp (1) z0
(1) (4)
Nội suy trong không gian thao tác (2) z(s)

q(t ), t  [t0 , tN ] =0 =1
A, s=0
Động học ngược tại các điểm tựa (3) p(s)
p(s)
t=0 [t0 tf ] s
(5) Động học ngược cả quá trình (4) t
O0 y(s)
y0
x(t ), t  [t0 , tN ] Động học thuận (5)
s=0 s= L s x(s)
(6)
xk  x(tk ) So sánh các giá trị vào và ra (6)
x0
77 78
-79- -80-
Xét bài toán vị trí điểm tác động cuối (end-effector): Với quỹ đạo tham số ta có các liên hệ sau
B, s=L
z0
Lập trình một quỹ đạo trong không gian làm việc, thường là một đường
hình học p (đoạn thẳng, cung tròn, elip,. . . ) được định nghĩa là một hàm z(s)
của một tham số s(t): p = p(s). A, s=0
p(s)
p(s)
Tham số s = s(t) được tính bằng cách sử dụng một trong những phương s
y(s)
pháp đã trình bày trong phần quỹ đạo trong không gian khớp. O0
y0
Một cách tiếp cận cổ điển là tạo hàm s(t) là kết hợp các đoạn tuyến tính Quỹ đạo không chứa điểm kỳ dị nếu: x(s)
với các hàm bậc 2, sin, cos, [lưu ý việc hạn chế giật, thay đổi gia tốc đột (đường cong liên tục, trơn tru)
ngột, tránh kích thích dao động kết cấu, ...). x0
dp dx (s ) dy(s ) dz (s )
0   0,  0, 0
ds ds ds ds
79 80
11/04/2023
-81- -82-
Mô tả đường cong trong không gian Hệ trục tọa độ tự nhiên với
Xét đường cong trơn nối từ điểm đầu pi= p(s=0) đến điểm cuối pf = p(s=L), với L • Véc tơ đơn vị tiếp tuyến t tại điểm p(s).
là chiều dài của đường cong. • Véc tơ pháp tuyến n (nằm trong mặp phẳng mật tiếp của đường cong tại điểm
Điểm p(s) thuộc đường cong có tọa độ trong hệ trục đề các Oxyz: p(s), [mặt phẳng này chứa cung pp’ , với p’ là điểm ở lân cận p, chính xác là
p  f (s )   3
 [x  x (s ), y  y(s ), z  z (s )], s  [0, L ] mặt phẳng giới hạn khi cho p’ tiến lại p. Trường hợp quỹ đạo là đường cong
phẳng mặt phẳng mật là mặt chứa đường cong.]
Tại điểm p(s) thuộc đường cong, ta dựng được hệ trục tọa độ tự nhiên với 3 véc • Véc tơ trùng pháp tuyến b, được chọn để tạo ra hệ trục tọa độ thuận, b = tn.
tơ đơn vị: t,n,b: pf
pf
s=L
z z ba vectơ đơn vị trên có liên quan
n
z(s)
n t
pi
đến phương trình tham số của
t
b s=0 đường cong
p(s) p
pi b
1
s
r(s)
dp d 2p d 2p
O
y(s) O t , n ,
ds ds 2 ds 2
y y
x(s)
x x b  t  n.
81 82
-83- -84-
Đường di chuyển thẳng, Quỹ đạo thẳng Đường di chuyển là cung tròn
Xét đường di chuyển là cung tròn có tâm C, điểm xuất phát pi (x,y,z)i và điểm cuối pf (x,y,z)f.
Xét đoạn thẳng nối hai điểm đầu pi (A) và cuối pf (B). Điểm p(s) trên đoạn thẳng Dựng hệ C(zxy)’ với các véc tơ đơn vị {x’,y’,z’}:
này, cách điểm đầu A một đoạn có chiều dài s được xác định bởi phương trình
• Cz’ là trục của cung tròn (qua tâm C & vuông góc mặt chứa cung)
s • Cx’ là trục nối từ C đến điểm đầu pi,
p(s )  pi  (p  pi ) • Cy’ suy ra từ Cz’ & Cx’, y’ = z’ cross x’.
|| p f  pi || f B
s(t)
Trong hệ C(zxy)’, điểm M trên cung tròn cách pf
P
p(0)  pi điểm xuất phát một khoảng s, có tọa độ là: z’
y(s) z
A y’ d
  cos (s ) pi x’
p(L)  p f L || p f  pi ||   p C
p (s )    sin (s )
M M
.   r p’ s=arc(piM) y’
pf  pi 0
d d 2p    p
t p(s )  , 0   cos(s / )
ds || pf  pi || ds 2
  C
pi x’ c
x(s)    sin(s / )
  O y
Do đó hệ tọa độ tự nhiên là không duy nhất.  0  pf
x
83 84
11/04/2023
-85- -86-
Tọa độ điểm M trong hệ cố định O(zxy), được tính z0 B, s=L
=0 =1 
p(s )  rM  rC  Rp (s ) z(s)
A, s=0
với R là ma trận cosin chỉ hướng của hệ C(xyz)’ đối t=0 [t0 tf] t p(s)
với hệ O(zxy), đó là ma trận chứa tọa độ của 3 véc p(s)
s
tơ đơn vị x’y’z’ trong hệ Oxyz, đó là ma trận hằng pf y(s)
z’ s=0 s= L s O0
(do cung tròn cố định): z y0
d
pi
R   x  y  z 
x’ x(s)
  s p C
M rM  p(s ) x0
Tính được các đạo hàm: Có phương trình đường cong dp
 sin(s / ) y’ Có luật di chuyển (chuyển động) t ,
  p s  s(t ) ds
d d
p(s )  R p (s )  R  cos(s / )  1
ds ds  
c Ta tính được vận tốc, gia tốc của điểm: d 2p d 2p
 0  n 2
,
y dp ds ds 2
 1 cos(s / ) O vM  rM  p (s )  s  st
2 2   x ds b  t  n.
d d
p(s )  R 2 p (s )  R 1 sin(s / ) dp d 2p d 2p
ds 2
ds   (s )  s  s2 2  st  s 2
a M  rM  p n
 0  ds ds ds 2
85 86
-87- -88-
Ví dụ: cho điểm đầu điểm cuối và biết đường di chuyển là thẳng. 1(t ), 2 (t ) ?
Suy ra luật di chuyển của điểm tác
Y (t )  1.5 động cuối
a1  a 2  1 m X (t )  1  s(t )  1  6t 2  4t 3 ,
Y (t )  1.5
Xác định luật chuyển động s(t): chọn
đa thức bậc 3
s(0)  0, s(0)  0, Giải bài toán động học ngược
s(1)  2, s(1)  0
 s(t )  6t 2  4t 3
Suy ra luật di chuyển của điểm tác động cuối
X (t )  1  s(t )  1  6t 2  4t 3 ,
Y (t )  1.5
87 88
11/04/2023
-89- -90-
Bài tập: a1  a2  0.25 m Bài tập: biết đường di chuyển là cung tròn
biết đường di chuyển là thẳng p(t)
tâm, bán kính, và góc mở pf
y  0.25998x  0.3705 B
A r y’
cho điểm đầu & điểm cuối
C (0.50; 0.50), R  0.7, 
A(0.41122; 0.26359)
B(0.0282; 0.37783) 0  60, f  90
C //x
Biết thời gian chuyển động: 10 giây -60o
Hoặc tâm, điểm đầu & điểm cuối:
Xác định : s(t )
• luật chuyển động s(t) bằng đa thức bậc 3;
x (t ), x(t ) C (0.50; 0.50), p0 x’
• hàm di chuyển của tọa độ điểm tác động cuối;
• Giải bài toán ngược xác định các tọa độ khớp; y(t ), y(t ) P0 (0.85; 0.1062), Pf (0.50; 1.20)
• Vẽ đồ thị các đại lượng: x(t), y(t), theta1(t), theta(2) và
đạo hàm cấp 1 và 2 của chúng 1 (t ),  (t )
1 Biết thời gian chuyển động: 1 giây
• Vẽ các cấu hình của tay máy theo thời gian chuyển động. 2 (t ), 2 (t ) a1  a2  1.0 m
89 90
-91- -92-
Xác định : Ví dụ: chuyển động trên một cung tròn

• luật chuyển động s(t) bằng đa thức bậc 3; s(t )
• hàm di chuyển của tọa độ điểm tác động cuối;
x (t ), x(t )
• Giải bài toán ngược xác định các tọa độ khớp;
• Vẽ đồ thị các đại lượng: x(t), y(t), 1(t), 2(t) và y(t ), y(t )
đạo hàm cấp 1 và 2 của chúng 1 (t ),  (t )
1
• Vẽ các cấu hình của tay máy theo thời gian chuyển
2 (t ), 2 (t )
động.
91 92
11/04/2023
-93- -94-
Xét bài thiết kế luật thay đổi hướng của bàn kẹp: Xét bài thiết kế luật thay đổi hướng của bàn kẹp:
Bây giờ ta đến hướng của khâu tác động cuối (end-effector). Thông thường, hướng của
khâu cuối được xác định bởi ma trận quay R (thay đổi theo thời gian) của hệ trục tọa độ Thiết kế luật thay đổi hướng phức tạp hơn so với
gắn với khâu cuối đối với hệ cố định. Như đã biết, ba cột của ma trận quay thể hiện cho bài toán vị trí. Trong bài toán vị trí việc đưa ra
ba vector đơn vị của hệ tọa độ gắn liền khâu cuối trong hệ cố định. Tuy nhiên, để tạo ra luật chuyển động giữa điểm đầu pi và cuối pf là
một quỹ đạo, phép nội suy tuyến tính dựa trên các véc tơ đơn vị (n,s,a) mô tả hướng khá đơn giản. Điều này khó áp dụng trực tiếp đối
ban đầu và hướng cuối cùng không đảm bảo tính trực giao của các véc tơ đơn vị này tại với việc nội suy giữa hai hướng đầu và cuối xác
mỗi thời điểm. định bởi hai ma trận (3x3) Ri và Rf. Nếu ta áp
dụng các phép tính nội suy đã nêu đối với các
phần tử của ma trận R, mặc dù R(t) thỏa mãn
hướng đầu và hướng cuối nhưng trong quá trình
chuyển động một số tính chất của ma trận nội
suy R(t) không còn được đảm bảo như tính chất
trực giao của các hàng cột.
93 94
-95- -96-
Thông thường, các góc Euler hoặc RPY được sử dụng hoặc, có thể Xét bài thiết kế luật thay đổi hướng của bàn kẹp:
sử dụng cách biểu diễn trục quay/góc quay để giải quyết bài toán
s(t ) s(t )
này. e  i  (  i ), e  i  (  i ),
|| f  i || f || f  i || f
Với các góc Euler hoặc góc RPY, hai bộ ba i = [i, i, i ]T và f =     s(t )
e i
(  i ),
[f, f, f ]T được xác định ứng với Ri và Rf. Sau đó thực hiện phép || f  i || f
nội suy dựa trên một trong những kỹ thuật trình bày (nên sử dụng
các phương pháp để có được tính liên tục ít nhất là của vận tốc Có thể sử dụng ba hàm s1,2,3(t) khác nhau để nội suy độc lập từng
góc). góc Euler, [, ,  ]T.
Có thể sử dụng một hàm s(t) để nội suy đồng thời cho cả ba góc:
[, ,  ]T
s(0)  0, s(0)  0
s(t ) :  e (t )  i  (f  i )s(t )

s(t f )  1, s(t f )  0

95 96
11/04/2023
-97- -98-
Sơ đồ thuật toán: Góc xoay và trục quay = Angle and axis
R0 , R1,..., RN
t0 , t1,..., tN  t f Một cách khác để tạo ra một luật di chuyển của hướng là sự biểu
Xác định các góc Euler diễn rõ ràng hơn trong không gian Descartes bằng góc quay và trục
0 , 1,..., N , từ Rk quay.
k  [k ,k ,k ]T Cho hai hệ trục tọa độ trong không gian Descartes có cùng gốc
nhưng hướng khác nhau, ta luôn có thể xác định một vector đơn vị
Nội suy tương tự như
trong không gian khớp (trục quay, r) và một góc quay (J) để chuyển hệ thứ nhất tới trùng
(t )  [(t ),(t ),(t )]T với hệ thứ hai bằng cách xoay hệ thứ nhất quanh trục quay r một
t  [t0 , tN ]
góc J. [định lý Euler].
Tính ma trận quay nội

R(t ), t  [t0 , tN ] suy R(t)
Rk  R(tk )
97 98
-99- -100-
Gọi Ri và Rf là hai ma trận cô sin chỉ hướng tương ứng là hướng Do đó, ta sẽ biểu diễn ma trận Rif dưới dạng phép quay quanh một trục
ban đầu và hướng cuối của hệ bàn kẹp so với hệ cố định (cả hai trong không gian có véc tơ đơn vị r và góc quay J, các đại lượng này
đều so với hệ cố định). Ma trận cô sin chỉ hướng của tư thế cuối được tính từ Rif như sau:
đối với tư thế ban đầu (hướng tương đối của vị trí cuối so với vị trí
 r  r  r  1
ban đầu) được ký hiệu Rif. r r r13  f  cos1  11 22 33 ,
Ta có mối liên hệ sau:  11 12  2 
r  Rif  r21 r22 r23  . r  r 
 11 r12 r13  
r31 r32 r33 
  32 23 
R f  Ri R fi
 R f  Ri R f  r21 r22 r23  .
i T   i 1 r  r  , sin   0
r   13 31  f
  2 sin f  
r31 r32 r33  r21  r12 
Nếu sử dụng ma trận R(t) để mô tả phép dịch chuyển hướng bàn Sau đó sử dụng hàm nội suy như đã sử dụng trong không gian khớp (đa
kẹp từ Ri đến Rf, thì nó phải thỏa mãn điều kiện đầu và cuối sau: thức bậc 3, 5,..) để nội suy góc quay theo thời gian:
s(0)  0, s(0)  0

R i (0)  I & Ri (t f )  Rif (t )  s(t )f với s(t ) : 
s(t f )  1, s(t f )  0

99 100
11/04/2023
-101- -102-
Tính ma trận cosin chỉ hướng trận Ri(t) theo thời gian Sơ đồ thuật toán:
R0 , Rf , t0 , t f
 r 2 (1  c )  c  r r (1  c )  r s  r r (1  c )  r s  
 x y x z z x y 
  Xác định ma trận quay
Ri (t )  R(r, (t ))  rx ry (1  c )  rz s  ry2 (1  c )  c  rz ry (1  c )  rx s   , Rfi  RiT Rf 
  tương đối, từ đó tính trục
rx rz (1  c )  rys  ryrz (1  c )  rx s  rz2 (1  c )  c   r i  ..., f  ...
  quay và góc quay
Nội suy tương tự như trong

  (t )  f s(t ),
Có thể tính được vận tốc góc và gia tốc góc của bàn kẹp: không gian khớp
t  [t0, tN ]
i   ri , Re (t )  Ri Ri (t ),
R(t )  R[r i , (t )], Tính ma trận quay nội suy
t  [t0, tN ] R(t)
e  Ri  (t ), i
 e  Ri  (t ). i
101 102
-103- -104-
Sơ đồ khối Kết luận
Các thông số vào: Một số kỹ thuật cho lập trình quỹ đạo trong không gian khớp và trong không gian làm việc có
x 0, x1,..., xN , ti  t0  t1  ...  tN  t f đã được minh họa.
xk  {pk , Rk }  {pk , k }
Nếu quỹ đạo được thiết kế trong không gian làm việc, việc cho khâu cuối di chuyển dọc theo
một đường cho trước là rất quan trọng trong nhiều ứng dụng công nghiệp.
Giải bài toán ngược vị trí xác định Sử dụng các phép nội suy tìm ra
các vị trí biến khớp tương ứng hàm liên tục theo thời gian
Mặt khác, gánh nặng tính toán cao hơn khi lập trình quỹ đạo trong không gian làm việc. Vì lý do
q 0 , q1,..., qN x(t )  x (t ), x(t ), ti  t  t f này, tần số sử dụng khi tính toán quỹ đạo thường thấp hơn tần số điều khiển, và như vậy một
hàm (vi) nội suy tiếp theo là cần thiết.
Sử dụng các phép nội suy tìm ra Giải bài toán động học ngược xác
hàm liên tục theo thời gian định các vị trí biến khớp tương ứng Hơn nữa, do vận tốc / gia tốc / mô-men xoắn bị giới hạn nên có thể dẫn đến một kết quả không
khả thi về vật lý trong không gian khớp (ví dụ như trong các truyền động không gian) nên việc
tính toán lại có thể là cần thiết [ví dụ có thể tăng tf] để giảm vận tốc và gia tốc, dãn đến giảm
được mô men, ..]
Kết quả: chuyển động của biến khớp
q(t )  q(t ), q(t ), ti  t  t f Cần thiết phải xét đến vận đề động lực học khi thiết kế quỹ đạo.
103 104
11/04/2023
-105- -106-
Kết luận Một số vấn đề liên quan
Phương trình chuyển động Một số chỉ tiêu đánh giá quỹ đạo tối tối ưu:
Các cấu hình kỳ dị có thể xuất hiện khi thiết kế quỹ đạo trong không gian
làm việc. M(q)q  C(q, q )q  g(q)  D(q, q )q  u (tối ưu thời gian chuyển động),
Như một vấn đề của thực tế, nếu chuyển động được xác định trong Các ràng buộc về mặt kết cấu và động học tf
không gian làm việc với tới các điểm gần cấu hình kỳ dị, nó cần phải
q min  q  q max q min  q  q max
 t0
dt  min
tránh. Do đó, quỹ đạo au khi thiết kế cần được kiểm tra trước và nếu cần
thiết phải chỉnh sửa. (tối ưu ‘mức tiêu hao năng lượng’),
q min  q  q max
tf
Rõ ràng tất cả những vấn đề này không xuất hiện nếu quỹ đạo dự
kiến trong không gian khớp.
Các ràng buộc về mômen/lực động cơ t0
uT Wudt  min
u min  u  u max
(tối ưu ‘mức tiêu hao năng lượng’)
Tránh cấu hình kỳ dị, tránh vật cản
 q 
tf
T
q  q singular , x  x obstacle W1q  uT W2 u dt  min
t0
105 106

Robotics 10 Trajectory Planning - 4s

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robotics 10 Trajectory Planning - 4s

Uploaded by

Copyright:

Available Formats

11/04/2023

điểm đầu Vật cản

nhanh không gian

Nguyễn Quang Hoàng Vật cản

q  q()  n ,   ,   [min , max ] B, s=L

đường di chuyển sẽ được gọi

p  p(s )  p(s(t ))   3 , s x(s)

3 0 2 0 q (0)  10, q (t f )  45, o

a  [10.00, 20.00, 3.4375, 0.1563]T t [s]

a  [10.00, 30.00, 32.8135, 5.4688]T

Ta viết đa thức cần tìm ở dạng sau: t  [0  t 0  t1  t2  ......  tN  t f ]

Xét đa thức bậc 5 cùng các đạo hàm của nó 1 0 0 0 0 0 a  q 

Ma trận A, véc tơ vế phải r và véc tơ hệ số a: 40

 a  b qd (t )   12 c2t 2  c1t  c0

Phương trình chuyển động tìm được như sau:

q0ta  vmax   q0ta (tf  ta )  (q f  qi )

ta tm tf -ta tf t q0 2 2q0 ta tm tf -ta tf t

ta tf -ta tf t Cho qi , qf , q0 4 | q f  qi | qi , qf , qmax

Như thế ta có ta=tf/2 tf t

==> h(k) = t(k+1) - t(k) > 0 Phương án giải quyết:

• Sử dụng đa thức nội suy Lagrange S(t) sẽ là đa thức bậc N

Kết hợp lại ta được hệ N+1 pt cho N+1 ẩn

 2hN 1mN  6 qN  N

6hk h(i) 0.35 0.22 0.19 0.50 0.62

spline_natural.m; % natural spline, S"(0) = 0; S"(tf) = 0

Tính được các hệ số

Cho biết: A[2, 1, 0] đến điểm B[0, 2, 0], tf

Ví dụ: Đường thẳng là thể

Xác định : Ví dụ: chuyển động trên một cung tròn

Tính ma trận quay nội

Nội suy tương tự như trong

You might also like