Luận án Xây dựng Robot tự hành dạng Nonholonomic và tổng hợp bộ điều khiển bám quỹ đạo

ng (X,Y) là tâm của 
 khung bám, diện tích 
 khung là kích thước của Đặt tâm khung bám 
 khung bám vào tâm đối tượng và 
 tính diện tích lại 
 Kết quả X, Y, Z và Hội tụ 
 Roll 
 Hình 2.4. Lưu đồ thuật toán Camshift 
2.2.3. Kết hợp bộ lọc Kalman với thuật toán bám ảnh Camshift 
 Khi robot hoạt động trong những điều kiện môi trường khác nhau của mục tiêu 
chuyển động, nền ảnh và sự che khuất có thể cản trở việc theo dõi và bám theo ảnh của 
đối tượng. Trong phần này sẽ trình bày việc sử dụng kết hợp bộ lọc Kalman với thuật 
toán bám ảnh Camshift nhằm mục đích nâng cao chất lượng bám ảnh. Trong thuật 
 36 
toán bám ảnh Camshift được giới thiệu ở phần trên có thể thấy đối tượng được bám tốt 
khi nền ảnh không ảnh hưởng đến đối tượng, tuy nhiên khi đối tượng bám bị che khuất 
hay lẫn màu với nền ảnh, sẽ gây ra hiện tượng mất bám. Để khắc phục nhược điểm này 
và nâng cao chất lượng bám, bộ lọc Kalman được sử dụng [40, 73, 76, 77]. 
 Sau khi tiến hành thuật toán Camshift ta sẽ có được trạng thái hệ thống xk,, yk, vị 
trí x và y của đối tượng tại thời điểm k. Với những đánh giá được đề cập phía trên ta 
có thể sử dụng bộ lọc Kalman để tạo ra phương pháp giới hạn vị trí của đối tượng hiệu 
quả hơn, điều đó là để nói thay vì tìm kiếm đối tượng trong toàn bộ mặt phẳng ảnh ta 
sẽ định nghĩa một cửa sổ tìm kiếm (hay cửa sổ mục tiêu được dề cập trong mục 2.1) 
  
tập trung vào giá trị dự đoán xk của bộ lọc. 
 Ước lượng trạng thái Mô hình chuyển 
 Dữ liệu ảnh và phương sai động 
 Camsifht Kalman 
 Vector đo lường và tín hiệu không 
 chắc chắn 
 Hình 2.5. Sơ đồ kết hợp bộ lọc Kalman và Camshift 
 Các bước sử dụng bộ lọc Kalman cho việc bám đối tượng là: 
 Bước 1: Khởi tạo (k=0). Trong bước này nó sẽ tìm kiếm đối tượng trong toàn bộ 
bức ảnh do chúng ta không biết vị trí trước của bức ảnh. Theo cách này ta nhận được 
x0. Tương tự ta có thể đánh giá một lượng dung sai lớn ban đầu (P0). 
 Bước 2: Dự đoán (k>0). Trong giai đoạn này sử dụng bộ lọc Kalman ta dự đoán 
  
vị trí tương đối của đối tượng, như vậy vị trí xk được coi như trung tâm tìm kiếm để 
tìm kiếm đối tượng. 
 Bước 3: Hiệu chỉnh (k>0). Trong phần này ta đặt đối tượng (là một chuỗi điểm 
  
được dự đoán trong trạng thái tiên nghiệm xk ) và ta sử dụng vị trí thực của nó (đo đạc) 
  
để thực hiện việc hiệu chỉnh trạng thái bằng cách sử dụng bộ lọc Kalman tìm kiếm xk 
 Bước 2 và 3 được thực hiện trong khi việc bám đối tượng vẫn đang tiến hành. 
 - Mô phỏng kết hợp bộ lọc Kalman với thuật toán bám ảnh Camshift 
 37 
 Để minh họa những kết quả của việc dùng bộ lọc Kalman trong việc bám đối 
tượng [77], ta lựa chọn cho bám theo một quả bóng và xem xét các trường hợp sau: 
 
 x0 E[x ]
 Khởi tạo 0 
 PEEE [(x [x])(x [x])]T
 0 0 0 0 0
 Ước lượng xˆk F k, k 1x ˆ k 1 
 trạng thái 
 PFPFQ T 
 k k, k 1 k 1 k , k 1 k
 PH T
 G k k 
 k HPHR T 
 k k k k
 xˆ x ˆ GH (y x ˆ ) 
 Ước lượng với k k k k k k
 hiệu chỉnh đo 
 PIGHPk () k k k 
 Hình 2.6. Sơ đồ tóm tắt của bộ lọc Kalman 
 a) Trong thử nghiệm này, ta thực hiện bám theo một quả bóng, quả bóng sẽ di 
chuyển tương đương với một đường thẳng tuyến tính, được miêu tả bởi hệ phương 
trình sau: 
 XFXk 1 k 1, k k W k (2.8) 
 38 
 xk 1 1010 x k 
 y 0101 y
 k 1 k 
 Wk (2.9) 
 xk 1 0010 x k 
 yk 1 0001 y k 
 YHXVk k k k (2.10) 
 xk 
 xm 1000 y
 k k v (2.11) 
 k
 ymk 0100 x k
 yk 
 - w k , vk là vector nhiễu hệ thống, nhiễu đo lường dạng ồn trắng, Gaussian, với kỳ 
vọng bằng 0. Chúng giải thích cho sự không chắc chắn trong gia tốc của đối tượng. 
 - Yk là vector đo lường 
 1000 
 - Hk là ma trận quan sát được viết là: Hk ; 
 0100 
 Trong hình 2.7 là kết quả mô phỏng cho thấy vị trí ước lượng bám rất sát với vị 
trí thực của đối tượng bám. 
 Hình 2.7. Kết quả dự đoán vị trí với bộ lọc Kalman 
 b) Một lợi thế của bộ lọc Kalman cho việc bám bắt đối tượng là nó có thể ước 
lượng được vị trí đối tượng ngay cả khi bị che khuất nhỏ. Cách để thực hiện việc này 
là xem xét hai giai đoạn của bộ lọc: dự đoán và hiệu chỉnh. Có nghĩa là nếu việc giới 
hạn vị trí của đối tượng là không trong một chuỗi của trạng thái dự đoán của bộ lọc 
 39 
(trong thời điểm k), ta có thể coi rằng đối tượng bị che khuất bởi một vài đối tượng 
khác, do đó ta sẽ không thể dùng hiệu chỉnh đo và sẽ chỉ lấy giá trị lọc tiên nghiệm vị 
trí của đối tượng. Hình 2.8 chỉ ra hoạt động của bộ lọc khi đối tượng bị che khuất. Hệ 
thống này được mô tả với những biểu thức tương tự được dùng trong trường hợp a). 
 Hình 2.8. Bộ lọc Kalman khi bị che khuất 
 c) Hầu hết các quỹ đạo chuyển động của đối tượng là thay đổi phức tạp (thay đổi 
cả vận tốc và gia tốc) không thể mô hình hóa bởi các hệ thống tuyến tính, khi đó chúng 
ta phải dùng các phương trình phi tuyến, do đó trong những trường hợp này ta sẽ dùng 
bộ lọc kalman mở rộng EKF (Extended Kalman Filter). Hình 2.10 cho thấy rõ chất 
lượng bám sử dụng bộ lọc Kalman mở rộng cho việc bám đối tượng với quỹ đạo phức 
tạp tốt hơn hẳn của bộ lọc Kalman thông thường. Với bộ lọc Kalman mở rộng được 
mô hình hóa sử dụng các phương trình không giới hạn chuyển động của Brownian 
(trong khi bộ lọc Kalman thông thường, mô hình hệ thống được sử dụng các phương 
trình của trường hợp a). 
 xk 1 f ( k , x k ) w k (2.12) 
 1 
 exp( (x 1.5 x ))
 4 k k 
 x 
 k 1 1 
 exp( (yk 1.5 y k ))
 yk 1 4 
 w (2.13) 
 x 1 k
 k 1 
 exp( xk )
 4 
 yk 1 
 1 
 exp( x ) 
 4 k 
 yk h(,) k x k v k (2.14) 
 40 
 xk 
 xm 1000 y
 k k v (2.15) 
 k
 ymk 0100 x k
 yk 
 
 Khởi x0 E [x0 ]
 tạo T
 PEEE0 [(x 0 [x])(x 0 0 [x])] 0
  
 xk f(k,x k 1 ]
 Ước Tuyến 
 f(k,x) h (k,x) tính hóa 
 lượng FHk 1, k 
 x x 
trạng thái x tx x t x
 T
 PFPFQk k, k 1 k 1 k , k 1 k 
 T
 PHk k
 Gk T 
 HPHRk k k k
 Ước lượng 
 xˆ x ˆ G y h ( k ,x ˆ ) 
 với hiệu k k k k k
 chỉnh đo 
 PIGHPk () k k k 
 Hình 2.9. Sơ đồ tóm tắt của bộ lọc Kalman mở rộng 
 41 
 Hình 2.10. Việc theo dõi các chuyển động phức tạp sử dụng bộ lọc Kalman mở rộng 
 - Thực nghiệm kết hợp bộ lọc Kalman ứng dụng bám ảnh Camshift 
 Để tiến hành chạy thực nghiệm, NCS đã lập một chương trình xử lý ảnh bám 
bắt mục tiêu tự động trên nền Visual C 6.0 và có sử dụng thư viện OpenCV của Intel. 
OpenCV là thư viện mã nguồn mở chuyên dùng cho các ứng dụng xử lý ảnh và video 
số. Ở đây sử dụng các hàm hỗ trợ như cvCamshift() để thực hiện thuật toán bám 
Camshift và các hàm liên quan hỗ trợ bộ lọc Kalman. Tất cả các thực nghiệm được 
tiến hành trong phòng thí nghiệm với khung nền tương đối phức tạp. 
 Hình 2.11. Chụp các frame chạy thực nghiệm thuật toán bám Camshift không có Kalman 
 42 
 Hình 2.12. Chụp các frame chạy thực nghiệm với thuật toán bám Camshift có Kalman 
 Các kết quả cho thấy khi kết hợp thêm bộ lọc Kalman chất lượng bám của hệ 
thống tốt hơn, khi đối tượng bị che khuất hay lẫn vào nền ảnh thì hệ vẫn duy trì bám 
tốt. 
 Một hạn chế của việc kết hợp thêm bộ lọc Kalman vào thuật toán bám đó là khối 
lượng tính toán lớn hơn do đó thời gian xử lý lâu hơn, dẫn đến tốc độ bám chậm: khi 
không có bộ lọc Kalman tốc độ bám tầm 15÷20 frame/s, khi có bộ lọc Kalman tốc độ 
là 7÷10 frame/s. Tuy nhiên hạn chế này có thể khắc phục bằng cách tăng tốc độ xử lý 
của CPU máy tính hoặc nhúng thuật toán trên các DSP, hay FPGA chuyên dụng. 
 Một hạn chế khác là khi đối tượng bị che khuất với thời gian đủ lâu (trên 3s), 
hoặc khi có đối tượng khác chuyển động cùng chiều che khuất dần cũng dẫn đến hiện 
tượng mất bám. 
2.3. Kết luận chương 2 
 Chương 2 đã trình bày các nghiên cứu về xử lý ảnh bám bắt và xác định vị trí 
mục tiêu bằng xử lý ảnh nhận được từ camera. Các nghiên cứu tập trung vào thuật toán 
xử lý ảnh bám bắt mục tiêu di động: KLT và CamShift, đề xuất thuật toán kết hợp 
CamShift với bộ lọc Kalman mới để nâng cao chất lượng và tốc độ bám ảnh khi có 
nhiễu hoặc ảnh bị che khuất một phần. Các kết quả nghiên cứu đã được công bố trong 
bài báo “Một phương pháp sử dụng bộ lọc Kalman kết hợp với thuật toán bám ảnh 
Camshift nhằm nâng cao chất lượng bám trong các hệ thống robot tự động tìm kiếm 
và bám bắt mục tiêu” ở Hội nghị Toàn Quốc về Điều khiển và Tự động hóa 
VCCA2011. ISBN 978-604-911-020-7., 11/2011. 
 43 
 CHƯƠNG 3: MÔ HÌNH HÓA HỆ ROBOT TỰ HÀNHGẮN 
 CAMERA 
 Cấu trúc của điều khiển robot tự hành có thể chia ra 3 giai đoạn: lập phương án 
chuyển động (motion planning), thiết kế quỹ đạo (trajectory generation), và điều khiển 
bám quỹ đạo (trajectory tracking). Trong giai đoạn lập phương án chuyển động, bằng 
kinh nghiệm và hiểu biết của mình người thiết kế lập ra đường chuyển động cho robot 
chưa cần quan tâm đến thời gian chuyển động. Sang giai đoạn thiết kế quỹ đạo người 
thiết kế phải lập quỹ đạo chuyển động theo đường chuyển động mong muốn đã chọn ở 
giai đoạn trước, vận tốc, gia tốc chuyển động theo một hàm thời gian. Giai đoạn cuối 
là thiết kế điều khiển bám theo quỹ đạo chuyển động mong muốn. Chương 3 trình bày 
về mô hình hóa và mô phỏng động học, động lực học, động học ngược của mô hình 
robot tự hành có gắn camera đã xây dựng ở Chương 1 và cũng là sản phẩm của luận án. 
Quy đổi tọa độ của mục tiêu về tọa độ tâm của robot để xây dựng quỹ đạo đặt cho bài 
toán điều khiển bám quỹ đạo sẽ được trình bày ở Chương 4. 
3.1. Mô hình hóa hệ thống Pan/Tilt. 
 Robot của luận án có phần đỡ camera là một đế xoay Pan/Tilt có hai bậc tự do 
quay theo hai hướng phương vị (Pan) và góc tà (Tilt). Cấu trúc này được ứng dụng 
nhiều làm bệ radar (cố định hay di động đặt trên xe, tàu) hay bệ quay các thiết bị 
quang học theo dõi, kiểm tra không gian [4,5,6,7, 26, 27, 28, 29]. 
 Hình 3.1. Mô hình hóa hệ đế xoay Pan/Tilt 
 44 
- Bảng thông số DH: 
 Khâu i i ai di 
 1 1 90 0 l1 
 2 2 0 l2 0 
 i 1
- Tính các ma trận Ai . 
 cos1 0 s in  1 0 c os  2 sin  2 0 l 2 c os  2 
 sin 0 c os  0 sin  c os  0 l c os  
0 1 1 1 2 2 2 2 
 AA1 , 2 
 0 1 1l1 0 0 1 0 
 0 0 0 1 0 0 0 1 
 cos1 c os  2 c os  1 sin  2 sin  1 l 2 c os  1 c os  2 
 sinc os  sin  sin  c os  l sin  c os  
 0 0 1 1 2 1 2 1 2 1 2 
 AAA2 1. 2 (3.1) 
 sin2c os  2 0 l 1 l 2 sin  2 
 0 0 0 1 
- Tính các ma trận Uij : 
 sin1 0c os  1 0 
 0 cos 0 sin  0 
  A1 1 1 
 U11 
 1 0 0 0 0 
 0 0 0 0 
 sin1c os  2 sin  1 sin  2 c os  1 l 2 sin  1 c os  2 
 0 cos c os  c os  sin  sin  l c os  c os  
  A2 1 2 1 2 1 2 1 2 
 U21 
 1 0 0 0 0 
 0 0 0 0 
 sin2c os  1 c os  1 c os  2 0 l 2 sin  2 c os  1 
 0 sin sin  sin c os  0 l sin  sin  
  A2 1 2 1 2 2 1 2 
 U22 (3.2) 
 1 cos2 sin  2 0 l 2 c os  2 
 0 0 0 0 
- Mô men quán tính của hai khâu: 
+ Khâu 1: 
 45 
 0 0 0 0 
 1 1 
 0m l2 0 m l 
 31 1 2 1 1
 J (3.3) 
 1 0 0 0 0 
 1
 0 m l 0 m 
 2 1 1 1 
+ Khâu 2: 
 1 1 
 m l2 0 0 m l
 32 2 2 2 2 
 0 0 0 0
 J (3.4) 
 2 0 0 0 0 
 1
 m l0 0 m 
 2 2 2 2 
- Tính ma trận D( ): 
 1 1
 D Trace( U J UTT ) Trace ( U J U ) 0 m l2 C 2 m l 2 C 2
 11 11111 212213 222 3 222
 T
 D12 D 21 Trace( U 22 J 2 U 21 ) 0 
 1
 D Trace() U J UT m l 2
 22 22 2 223 2 2
- Tính ma trận h(,)  : 
 n n
  
 hi  h ikm1  2 
 k 1 m 1
 n
 U pk T
 hikm  Trace() J p U pi 
 p max( i , m , k ) m
 UU11TT  21
 h111 Trace( J 1 U 11 ) Trace ( J 2 U 21 ) 0 0 0 
 1   1
 U 21 T 1 2
 h112 Trace() JU 2 21 mlCS 2 2 2 2 
 2 3
 U22 T 1 2
 h121 Trace() J 2 U 21 m 2 l 2 C 2 S 2 
 1 3
 U22 T
 h122 Trace( J 2 U 21 ) 0 
 2
 U 1
 h Trace()21 JUT mlCS2
 211 2 223 2 2 2 2
 1 
 46 
 U 21 T
 h212 Trace( J 2 U 22 ) 0 
 2
 U 22 T
 h221 Trace( J 2 U 22 ) 0 
 1
 U 22 T
 h222 Trace( J 2 U 22 ) 0 
 2
 Suy ra: 
 1
 h h2 h     h     h   2 m l 2 C S     
 1 1111 11212 12112 12223 22 2212
 1
 h h2 h     h     h   2 m l 2 C S   2 
 2 2111 21212 22112 22223 22 221
 1 
 m l2 C S   
 3 2 2 2 2 1 2 
 h(,)  
 1
 m l2 C S  2 
 3 2 2 2 2 1 
 - Tính ma trận véc tơ lực trọng trường C( ): 
 n
 j
 Ci () m i gUji r j 
 j i
 TT
 1 1 
C1 m 10 g 000 U 11 l 1 01 m 2 0 g 00 U 21 l 1 001 
 2 2 
 1
 gm l C C
 2 2 2 1 2
 T
 1 1
 C2 m 20 g 0 0 U 22 l 2 0 0 1 gm 2 l 2 S 1 S 2 
 2 2
 Suy ra: 
 1 
 gm2 l 2 C 1 C 2 
 C1 2
 C() (3.5) 
 C 1
 2 gm l S S 
 2 2 2 1 2 
 - Phương trình động lực học của bệ Pan/Tilt-camera: 
 Dạng phương trình: 
  
 FM () t D ().()  t h (,)   C ()  (3.6) 
 47 
 F D D  h C 
 M1 11 121 1 1 (3.7) 
  
 FM 2 D 21 D 22 2 h 2 C 2 
 12 2 1 2 1 
 m l C0 m l C S   
 2 2 2  2 2 2 2 1 2 gm2 l 2 C 1 C 2 
 FM 1 3 1 3 2
 (3.8) 
 F 1 1 1
 M 2 0 m l2 2 m l 2 C S 2 gm l S S 
 322 3 22221 2 2212 
3.2. Quy chiếu tọa độ mục tiêu và camera về tọa độ tâm robot 
 Hình 3.2. Mô hình robot tự hành có gắn đế xoay Pan/Tilt 
 Gọi Cc ( xccd,, y ccd z ccd ) là điểm cuối cho một vector từ vị trí trung tâm của robot 
tự hành đến tâm của ống kính camera. khi đó vị trí này có thể được tính theo hai góc 
quay 1,  2 của đế xoay Pan/Tilt như sau: 
 xccd l2 cos( 1 )cos(  2 ),
 yccd l2 sin( 1 )cos(  2 ), (3.9) 
 zccd l0 l 1 l 2 sin( 2 ),
 Ở đây l0 là chiều cao của xe tính đến vị trí đặt đế xoay Pan/Tilt. 
 Tương tự, véc tơ trạng thái của ma trận đồng nhất đã đại diện là các góc: Roll 
(R ), Pitch (P ) và góc độ Yaw (Y ) được tính như sau: 
 48 
 0 0
 1 sin1 (A 2(12) ) c os  1 ( A 2(22) ) 1 0
 R tan(0 0 )tan()0. 
 sin1 (A 2(13) ) c os  1 ( A 2(23) ) 1
 0
 1 A2(31)
 P cot tan (0 0 ) 2 , (3.10) 
 A2(11) cos 1 A 2(21) sin  1
 0
 1A2(11) 1
 Y cot tan (0 ) cot tan (cot tan( 1 ))  1 ,
 A2(21)
 Để thực hiện bám đối tượng theo thời gian thực ta cần có thông số khoảng cách 
từ camera đến đối tượng cũng như kích thước thật của đối tượng trong không gian 3D. 
Việc này cần thiết phải có hai camera, tuy nhiên nếu dùng phương pháp này khối 
lượng tính toán sẽ rất lớn và ảnh hưởng đến tốc độ bám, do đó ở đây chỉ dùng một 
camera và khoảng cách đến đối tượng được xác định bằng sensor siêu âm và giả thiết 
kích thước của đối tượng là cố định. Vị trí của tâm đối tượng trong tọa độ ảnh là (j, k), 
khi chuyển về hệ tọa độ (j′, k′) là tâm của khung ảnh sẽ được tính theo góc quay Roll 
R và kích thước khung hình ảnh Px và Py. 
 Px 
 ' j 
 j cos(RR ) sin(  ) 2
 (3.11) 
 ' 
 k sin( ) cos(  ) P
 RR k y 
 2 
  
 Để tính vị trí thực tế (,)x0 y 0 , 0 và r0 được ước tính bằng cách sử dụng mối 
quan hệ tuyến tính giữa đối tượng thực sự trong góc nhìn và khung hình ảnh. Khi một 
điểm ảnh được chụp tại (j′, k′) trên trung tâm của khung hình ảnh, vị trí đối tượng thực 
  
sự,  0 và r0 có thể được tính toán như sau (hình 3.3): 
 Hình 3.3. Tính toán vị trí đối tượng theo tọa độ robot 
 49 
 Hình 3.4. Tính toán động học ngược cho vị trí thực camera 
  z
 r ccd (3.12) 
 0  k '
 cos( P  ry )
 2 Py
  j'
 0 rx (3.13) 
 Px
 Với rx và ry là các góc hướng nhìn x- và y- của camera CCD, tương ứng. Khi đó 
vị trí của đối tượng trong hệ tọa độ của robot (x,y) là: 
   
 x0 rccd.cos( Y ) r0 .cos(  Y  0 )
 (3.14) 
   
 y0 rccd.sin( Y ) r0 .sin(  Y  0 ),
 2 2
 Với Y là góc quay giữa robot và camera, và rccd() x ccd y ccd là khoảng cách 
từ tâm robot tới tâm ống kính camera. 
 - Động học ngược để điều khiển tâm ống kính đến vị trí mong muốn: 
 2 2 2 2 2 2 
 1 l1 l 2 l 1 l 2 ( l 1 rd )( l 1 r d ) 1 
 d cos , (3.15) 
 (l2 r 2 ) sin( )
 1 d d 
 1 yd 
 d tan , (3.16) 
 xd 
 Với d và d góc quay camera, ( xd , yd ) là vị trí mong muốn của camera, và rd là 
 2 2
 xd y d . 
3.3. Mô hình động học, động lực học robot di động 
 Một lớp rộng các hệ thống cơ non-holonomic được mô tả bởi dạng công thức 
động lực học sau dựa trên công thức Euler-Lagrange [54, 56, 57, 60]: 
 50 
 T
 MqqCqqqGq()(,)()()()    d Bq Jq  (3.17) 
 Với điều kiện ràng buộc non-holonomic là: 
 J( q ) q 0 (3.18) 
 Trong đó: 
 + q là vector n chiều ứng với n các biến khớp, 
 + M(q) là ma trận đối xứng xác định dương cỡ n x n, 
 + C(,) q q q  là thành phần moment nhớt và hướng tâm, 
 + G(q) là n vectơ moment trọng lực, 
 +  d là vectơ nhiễu tác động và nhiễu sai lệch mô hình, bị chặn và: 
 T
 d (,,)d x d y d , 
 + B(q) là ma trận chuyển đổi đầu vào cỡ n x r (r<n), 
 +  là vec tơ đầu vào r chiều, 
 +  là tích số lực ràng buộc Lagrange. 
 Một cấu trúc robot tự hành hai bánh song song chủ động phía sau được mô tả 
trên (hình 3.5) là một điển hình cơ bản của một robot dạng non-holonomic, bao gồm 2 
bánh lái được gắn trên cùng một trục, và một bánh xe tự do gắn ở giữa. Chuyển động 
và hướng được thỏa mãn bởi các cơ cấu chấp hành độc lập, chẳng hạn động cơ một 
chiều cung cấp các moment cần thiết tới các bánh phía sau. 
 Vị trí của robot trong hệ quy chiếu Cactter quán tính {O,X,Y} thì hoàn toàn được 
 T
cụ thể hóa được bởi vector q=[xc y c ] Trong đó xc y c là tọa độ của trọng tâm của robot, 
và  là phương của hệ quy chiếu {,,}CXYc c so với hệ quy chiếu quán tính. 
 Hình 3.5. Phối cảnh của robot tự hànhdạng non-honolomic cơ bản 
 51 
 Ràng buộc non-holonomic phát biểu rằng robot chỉ có thể di chuyển dọc theo 
trục vuông góc với trục nối giữa 2 bánh lái, chẳng hạn sự di động dựa trên sự thỏa mãn 
điều kiện lăn trơn và không trượt 
 
 yccos x  c sin  d  0 (3.19) 
 Mối liên hệ giữa vận tốc giữa hai tọa độ cố định và di động: 
 q S()() q v t (3.20) 
 S(q) là một ma trận Jacobian chuyển vận tốc ở hệ tọa độ di động sang vận tốc ở 
hệ tọa độ Cactter q . Do đó, các thuộc tính động lực học cơ bản vẫn được giữ trong hệ 
tọa độ mới. 
 Phương trình động học chuyển động của trọng tâm C dưới dạng vận tốc tịnh tiến 
và vận tốc góc quay là: 
 cos d sin  
 v v1 
 S(),, q v 
 sind cos  (3.21)
  v2 
 0 1 
 xc cos d sin  
 v1 
 y sind cos  (3.22) 
 c v 
  2 
  0 1 
 Trong đó v1 V max và v2 W max . Vmax và Wmax là các vận tốc tịnh tiến và góc 
quay lớn nhất của robot di động. Hệ (3.22) là phương trình động học của robot. 
 Dạng thức Lagrange được sử dụng để tìm phương trình động lực học của robot di 
động. Trong trường hợp này G(q)=0 bởi vì quỹ đạo của robot cơ bản bị giới hạn trong 
mặt phẳng ngang, vì thế nên hệ thống không thể thay đổi vị trí theo chiều đứng, nên 
thế năng của hệ, U trở thành hằng số. Năng lượng động năng KE thì được cho bởi: 
 1 1ni 1
 i T T i T 
 kE mvv i i i  i I i  i,() K E  k E qMqq 
 2 2i 1 2
 Phương trình động lực học của robot tự hành trong hình 3.5 có thể được thể hiện 
trong dạng ma trận trong đó: 
 m0 md sin md 2 cos  
  2
 M() q 0 m md sin,(,) C q q md  sin,  
 mdsin md cos  1 0 
 52 
 cos cos  sin 
 1  
 G()0,() q B q , 1 , AT ()cos, q  
 sin sin  
 r  2 (3.23) 
 RR d 
 
  m( xc cos  y  c sin  ) 
 Khi đó ta có nhận xét rằng phương trình Euler-Lagrange sẽ đơn giản hơn nếu 
chúng ta loại được bớt thành phần d trong hệ, việc loại thành phần này đơn giản chỉ là 
ta thiết kế hệ robot tự hành có trọng tâm nằm chính giữa trục nối hai bánh chủ động 
sau. Trong luận án này, NCS thiết kế robot của mình dựa trên ý tưởng này, lúc đó phối 
cảnh của hệ được vẽ lại đơn giản như trên hình 3.6. 
 Hình 3.6. Phối cảnh của robot di động 
 Vectơ vị trí của robot trên bề mặt là q (,,) x y  T 
 x và y là các tọa độ của điểm C (trung điểm của trục nối 2 bánh) và  là góc định 
hướng của robot trong hệ quy chiếu quán tính. 
 Lúc đó (3.23) được viết lại: 
 m 0 0 0 
  
 M() q 0 m 0,(,) C q q 0,()0, G q 
 0 0I 0 
 cos cos  sin 
 1  
 B( q ) , 1 , AT ( q ) cos  ,
 sin sin  
 r  2 
 RR 0 
 Khi đó có thê viết được công thức động lực học Euler-Lagrange của robot tự 
hành thông qua công thức (3.17) được viết lại: 
 m0 0  x c os c os  sin  
 1 1 
 0m 0  y sin sin  c os   (3.24) 
  
  R 2 
 0 0ILL  0 
 53 
 Đây cũng là phương trình động lực học sẽ được sử dụng để thiết kế các thuật 
toán điều khiển trong Chương 4. 
 Trong đó : 
 +1 và  2 lần lượt là moment của động cơ