Giới thiệu chung
Wheeled Mobile Robot
Wheeled Mobile Robot (WMR) là một loại robot có có khả năng di chuyển bằng bánh xe trong môi trường xung quanh WMR có thể “tự động”, nghĩa là nó có khả năng tự điều chỉnh hướng di chuyển trong môi trường mà không cần đến thiết bị dẫn hướng vật lý hay cơ điện.
Thành phần chính của một WMR bao gồm bộ điều khiển, các cảm biến, cơ cấu chấp hanh (động cơ, bánh xe) và hệ thống điện.
Một số loại Wheeled Mobile Robot
Differrential Dive là một loại WMR có
2 bánh với 2 bánh được bố trí đồng trục và cả
2 đều là bánh chủ động có thể điều khiển được, tức là chúng được gắn với 2 cơ cấu chấp hanh (động cơ) riêng biệt để điều khiển riêng từng bánh. Ở loại xe này thông thường nó rất khó để có thể đứng được ở trạng thái cân bằng tự nhiên nên khá ít được sử dụng trong thực tế
Khác với Differrential Dive, Bicycle
Drive mặc dù cũng có 2 bánh, nhưng 2 bánh của nó được bố trí trên 1 đường thẳng, và thông thường chỉ có một bánh chủ động và một bánh có thể điều khiển được góc lái, cấu tạo giống như một chiếc xe đạp.
Loại robot này rất hiếm gặp do tính ứng dụng trong thực tiễn không cao.
Tricycle Drive là sự kết hợp giữa 2 loại WMR đã nêu ở trên, nó có 3 bánh xe và trong đó 2 bánh sau được bố trí đồng trục, và một bánh trước làm bánh lái; 2 trong
3 bánh xe đó sẽ được gắn với cơ cấu chấp hanh để điều khiển và bánh còn lại để tự do hoặc cũng có thể sử dụng cơ cấu chấp hành để điều khiển chỉ một bánh lái phía trước với tốc độ và góc lái mong muốn.
WMR loại này có cấu tạo tương tự như một chiếc ô tô với 2 bánh trước là bánh có thể thay đổi được góc lái.
Là một loại robot được thế kế đặc biệt với trục của các bánh đồng quy tại 1 điểm, đặc điểm này sẽ giúp cho robot có thể di chuyển theo mọi phương và tới được mọi điểm.
Ngoài những loại WMR vừa nêu ở trên, còn rất nhiều loại WMR khác nữa, trên đây chỉ là một số ví dụ điển hình về WMR.
Lựa chọn mô hình WMR 3 bánh
Như đã giới thiệu ở trên, WMR loại 3 bánh có 2 loại chính là Tricycle Drive (hay Nonholonomic) và Omni Robot, trong đó Tricyle Drive có thể được chia tiếp thành các loại nhỏ hơn tùy thuộc vào việc bánh nào được gắn cơ cấu chấp hành.
Hai bánh sau truyền động
Omnirobot Ba bánh độc lập Đối với đề tài này, nhôm lựa chọn mô hình robot Tricycle Drive với 2 bánh sau là bánh chủ động được gắn với cơ cấu chấp hanh để điều khiển và bánh trước là bánh tự do.
Mô hình hóa đối tượng Mobile Robot 3 bánh
Mô hình động học
Mô hình xe được mô tả như hình H2.1:
ICR: là tâm quay tức thời của xe R(t): bán kính tức thời của quỹ đạo chuyển động của xe : Tốc độ góc của xe quanh tâm ICR v : vận tốc dài theo phương dọc xe r : bán kính bánh xe : góc bánh lái so với trục O m X m
Mô hình xe được đặt trong một hệ trục tọa độ tổng quát ( X g ,Y g ), và hệ trục tọa độ chuyển động gắn với xe ( X m
,Y m ) Vector trạng thái của xe trong hệ tọa độ tổng quát là:
Phần 2: Mô hình hóa đối tượng Mobile Robot 3 bánh q (2.1)
Do sự chuyển động của xe là nhờ sự truyền động của 2 bánh sau từ đó điều chỉnh hướng xe di chuyển, nên vận tốc dài của xe theo phương dọc xe được xác định bởi: v
Phương trình động học ngoài của xe trên hệ tọa độ tổng quát được xác định như sau: x ( t ) = cos ( ( t ) ) v ( t ) y ( t ) = cos ( ( t ) ) v ( t )
( t ) = ( t ) Để ngắn gọn trong cách trình bày, ta tạm bỏ qua sự phụ thuộc của các đại lượng vào thời gian, hệ phương trình trên được viết lại như sau: x = cos v y = sin v
Hay ta có thể viết dưới dạng ma trận như sau: x cos y = sin
Như vậy với đầu vào điều khiển là vector vận tốc v = v T , ta có ma trận S với các cột của S là các trường vector thể hiện các hướng di chuyển khả dĩ của xe:
Phần 2: Mô hình hóa đối tượng Mobile Robot 3 bánh cos
Trong đó các phương chuyển động khả dĩ là:
Phương trình 2.5 được viết lại như sau: cos 0 s= sin ; s = 0
Tuy vậy, như đã nói xe chỉ có thể chuyển động dọc theo bánh xe, và không thể chuyển động trượt sang của bánh Do đó chuyển động của xe chịu các ràng buộc sau:
2 ) : là tọa độ của bánh trước.
( 2 Mối quan hệ của ( x 2 , y 2 ) với ( x , y ) như sau: x 2 = x + d cos y 2 = y + d sin
Từ đó suy ra: x = x − d sin
2 y 2= y + d cos Thay 2.10 vào 2.8 ta thu được:
Từ 2.11, ta có ma trận ràng buộc A như sau:
Mô hình động lực học
Mô hình xe 3 bánh đang xét là mô hình có 2 bánh sau là bánh chủ động được điều khiển bằng 2 động cơ, bánh trước là bánh tự do Hai cơ cấu chấp hành này lần lượt sẽ sinh ra các momen R và L
Mô hình động lực học chuyển động của xe được mô tả bởi công thức
: là hiệu giữa động năng và thế năng của hệ thống
P : là công suất tổn hao do ma sát k : là chỉ số của thành phần tọa độ tổng quát q k g k : trọng lực phân tích theo phương của q k d k : thành phần nhiễu theo phương của q k
7 k : nhân tử Lagrange kết hợp với mối quan hệ ràng buộc thứ j theo q k a jk : hệ số ràng buộc theo q k
Mô hình động lực học 2.13 có thể được viết lại dưới dạng ma trận như sau:
Trong đó: q : Vector tọa độ tổng quát
M q ( ) : Ma trận xác định dương của khối lượng và quán tính
V ( q, q ) : Vector lực Coriolis và lực ly tâm
G ( q ) : Vector trọng lực d : Vector nhiễu
E ( q ) : Ma trận chuyển từ không gian truyền động sang không gian tọa độ u : Vector moment đầu vào
A T ( q ) : Ma trận hệ số ràng buộc chuyển động : Vector lực ràng buộc Để đơn giản hóa 2.16, ta tạm bỏ qua sự phụ thuộc vào q
Khi đó 2.16 được viết lại thành:
Mq + V + F + G + d = E.u − A T (2.17) Mặt khác, đạo hàm 2 vế của 2.7 ta có: q = S.v + S.v Suy ra 2.17 trở thành:
Nhân cả 2 vế 2.18 với S T thu được:
Từ 2.7 và 2.21, ta có thể biết diễn không gian trạng thái x = q T v T T x q y S.v x = = =− M −1 v V v
Xét cụ thể đối với mô hình động lực học của xe 3 bánh Với m , J lần lượt là khối lượng của xe, moment quán tính của xe quanh tâm ICR đồng thời bỏ qua thành phần nhiễu d k trong công thức 2.15. Đồng thời ta xét bài toán đơn giản rằng xe chỉ chạy trên mặt phẳng, nghĩa là thế năng của nó là không đổi, hay ta có thể coi mặt đất là mốc thế năng và do đó thế năng W t của xe là 0 và thành phần g k cũng biến mất khỏi công thức 2.15. Động năng chuyển động của xe là:
Trong điều kiện lý tưởng, bỏ qua ma sát gây cản trở chuyển động của xe, tức là coi công suất tổn hao P=0.
Do đó phương trình 2.15 trở thành hệ sau: mx − 1 sin = F x my + 1 cos = F y
J = M Để 2 bánh sau của xe có thể chạy được với tốc độ moment của bánh quanh trục quay và trục lái lần lượt là R
L ực tổng hợp dùng để kéo toàn bộ xe di chuyển là này theo 2 phương chuyển động thẳng x và y ta có dài , L
Xét trên hệ trục tọa độ di động gắn trên xe, lực kéo do 2 bánh tác động lên xe có xu hướng làm xe quay quanh trục của nó với moment M xác định bởi:
Thay 2.27, 2.28 và 2.29 vào hệ phương trình 2.26 ta thu được: mx − sin − 1
Mô hình động học 2.30 được viết về dưới dạng ma trận như sau:
Mặt khác S = sin 0 suy ra S = cos
Từ đó, không gian trạng thái ở 2.22 được xác định bởi các ma trận:
Song do tính phi tuyến của hệ thống nên để thuận tiện cho việc thiết kế điều khiển ta thường tách mô hình trạng thái của hệ thống ra thành 2 thành phần Gồm:
Thành phần mô hình động học: x cos 0 q = y =sin 0
Thành phần mô hình động lực học: v = M −1 ( Eu − V )
Trong đó các ma trận M , E, V được xác định bởi 2.32
Các phương án điều khiển đối tượng Mobile Robot
Tổng quan về điều khiển
Như đã trình bày ở trên, để thuận tiện cho việc thiết kế điều khiển ta thường chia mô hình trạng thái của xe thành 2 mô hình thành phần là động học và động lực học Do đó việc xây dựng các phương án điều khiển cũng dựa trên 2 mô hình này.
Thông thường để thiết kế điều khiển cho đối tượng mobile robot dạng này, ta sẽ sử dụng tới phương án điều khiển kinh điển hay gặp trong các bài toán điều khiển, đó là sử dụng cấu trúc 2 vòng điều khiển, đây là 1 dạng sách lược điều khiển tầng Trong đó vòng ngoài là vòng điều khiển tốc độ (hay chính là vòng điều khiển động học) và vòng trong là vòng điều khiển moment (vòng điều khiển động lực học) Bộ điều khiển động học ở vòng ngoài sẽ tính toán xác định giá trị tốc độ là trở thành giá trị đặt cho bộ điều khiển động lực học ở vòng trong Sơ đồi khối của hệ thống được miêu tả như sau:
Ngoài ra, hệ thống điều khiển trên còn có thể biểu diễn theo một cách khác theo phương pháp EMR như sau:
Phần 3: Các phương án điều khiển đối tượng Mobile Robot
Các hướng tiếp cận cơ bản
3.2.1 Điều khiển hướng và điều khiển tịnh tiến
Gọi góc hướng của robot ở thời điểm t là φ(t) và góc hướng mong muốn là φref(t), sai lệch góc hướng điều khiển là:
Trong trường hợp này, biến cần điều khiển là φ(t) và để điều khiển φ(t) đạt được tới giá trị mong muốn ta cần điều khiển sai lệch ( ) về 0.
Góc hướng của robot được thể hiện qua hệ phương trình vi phân của góc hướng như sau:
Trong trường hợp này, biến điều khiển được chọn là ( t
), nó được xác định tương ứng với sai lệch góc hướng ( ) thông qua bộ điều khiển tỷ lệ như sau:
Khi đó phương trình (3.1) trở thành:
Phương trình này được xấp xỉ thành: t ( ) = t ( ) = V K t ) − d 1
Song để xe bám được theo 1 quỹ đạo cho trước hoặc 1 trạng thái cho trước, ta cần điều khiển tịnh tiến, tức là điều khiển tốc độ dài của xe Ý tưởng thiết kế điều khiển hợp lý nhất là điều khiển tỷ lệ với sai lệch khoảng cách từ vị trí hiện tại tới vị trí tham chiếu:
3.2.2 Các hướng tiếp cận cơ bản Ở phần này, bằng cách kết hợp việc điều khiển góc hướng và điều khiển tịnh tiến đã nói đến ở phần trước, ta sẽ có một số hướng tiếp cận cơ bản trong thực tế để điều khiển robot tới một trạng thái tham chiếu. b Điều khiển đến một vị trí đặt
Trong trường hợp này, robot được yêu cầu đi đến một vị trí tham chiếu, nơi mà không yêu cầu góc hướng cuối cùng Góc hướng của robot được điều khiển liên tục theo hướng của điểm tham chiếu, tức là hướng từ vị trí hiện tại của robot tới điểm tham chiếu, gọi là góc r , được xác định bởi:
Vận tốc dài và góc lái của bánh trước được điều khiển như sau:
Trong đó: v(t) và ( t ) được xác định như phần trên.
Tuy vậy luật điều khiển như trên sẽ gặp phải một số vấn đề Cụ thể, giá trị tốc độ như trên là luôn dương, do đó robot có thể vô tình đi qua điểm tham chiếu, và điều này sẽ khiến cho tốc độ lại tiếp tục tăng lên (do khoảng cách tới điểm tham
Phần 3: Các phương án điều khiển đối tượng Mobile Robot chiếu lại tiếp tục tăng) Mặt khác khi đi qua điểm tham chiếu , vô tình góc hướng tham chiếu sẽ đột ngột quay ngược lại Giải pháp được đề ra bằng cách thay đổi công thức tính vận tốc và góc lái như sau:
2 t v sign cos e t b Điều khiển tới một trạng thái tham chiếu (reference pose) thông qua điểm trung gian
Khác với phần trước, trường hợp này ngoai việc đi tới vị trí yêu cầu, robot còn cần đạt tới một góc hướng cho trước Về cơ bản việc này khá dễ thực hiện do có thể áp dụng lại các luật điều khiển ở phần trên. Ý tưởng của phương pháp này là sử một điểm trung gian ( x t , y t ) được đặt cách điểm tham chiếu một khoảng r sao cho hướng từ điểm trung gian về phía điểm tham chiếu trùng với hướng tham chiếu:
Thuật toán điều khiển robot sẽ được chia làm 2 giai đoạn: Ở giai đoạn 1, robot được điều khiển đi tới điểm trung gian Khi khoảng cách từ vị trí của robot tới điểm trung gian đủ nhỏ ( ) thì robot sẽ chuyển sang giai đoạn
2, điều khiển robot về điểm tham chiếu.
Phần 3: Các phương án điều khiển đối tượng Mobile Robot c Từ trạng thái tham chiếu tới quỹ đạo tham chiếu
Hướng tiếp cận vấn đề này khá đơn giản, nó hoàn toàn dựa trên tinh thần của điều khiển tới 1 trạng thái tham chiếu Ở đây quỹ đạo sẽ được chia thành nhiều đoạn thẳng nhỏ và đầu mút của các đoạn thẳng sẽ là các trạng thái tham chiếu Nói cách khác một quỹ đạo tham chiếu sẽ gồm một chuỗi trạng tham chiếu T i với i =1, 2,3 n Robot sẽ phải di chuyển lần lượt từ T 1 tới T 2 ,… cho tới T n
Điều khiển bám quỹ đạo
3.3.1 Bám quỹ đạo sử dụng các hướng tiếp cận cơ bản
Tương tự như phần 3.2.2.c, ta có thể hình dung quỹ đạo tham vị trí chuyển động tham chiếu Mỗi thời điểm trích mẫu, điểm tham xác định bằng điểm hiện tại của quỹ đạo tham chiếu theo thời gian ( chiếu chiếu x ref ( t ) như một lại được , y ref
Việc điều khiển tới điểm tham chiếu này được áp dụng luật điều khiển như công thức (3.2).
3.3.2 Phân tích thành phần feedforward và thành phần feedback
Cách tiếp cận nêu ở phần 3.3.1 rất dễ thực hiện nhưng nó lại có nhược điểm là rất dễ bị nhiễu trong các vòng điều khiển, do đó cần có thành phần bù nhiễu feedforward.
Trước tiên, ta xét tới một hệ thống vi phân phẳng Một hệ thống được gọi là vi phân phẳng nếu có một tập các đầu ra phẳng và tất cả các trạng thái và đầu vào hệ thống có thể được viết lại như các hàm của các đầu ra phẳng và một số hữu hạn đạo hàm theo thời gian của nó Hệ quả là, nếu một hệ thống là vi phân phẳng thì mọi biến hệ thống đều có thể tính toàn được từ các đầu ra phẳng mà không cần tích phân Nghĩa là từ quỹ đạo tham chiếu ta có thể tính toán ra các đầu vào điều khiển.
Ta có thể dễ dàng nhận thấy đối tượng mobile robot 3 bánh đang xét là một hệ thống vi phân phẳng Thật vậy: v ( t ) = x 2 t +
Như vậy công thức (3.3) chính là công thức tính toán các đầu vào tham chiếu ứng với một quỹ đạo tham chiếu Đây cũng chính là một bộ điều khiển vòng hở cho mô hình động học nhằm đảm bảo cho robot bám theo quỹ đạo đặt.
3.3.3 Tuyến tính hóa phản hồi Ý tưởng của phương pháp này là thực hiện các biến đổi nhằm tuyến tính hóa đầu vào hệ thống làm cho hệ thống giữa đầu vào mới và đầu ra tuyến tính Từ đó các việc thiết kế các bộ điều khiển tuyến tính cho hệ thống trở nên khả thi Quy trình thiết kế tuyến tính hóa phản hồi như sau:
− Chọn các đầu ra phẳng thích hợp, số lượng đầu ra phải bằng số lượng đầu vào
− Lấy vi phân các đầu ra và kiểm tra sự xuất hiện của các đầu vào, lặp lại đến khi tất cả đầu vào đều xuất hiện.
− Hệ phương trình được giải cho đạo hàm bậc cao nhất của từng đầu vào.
Trong trường hợp của mobile robot 3 bánh, như đã chứng minh từ phần trước, nó là một hệ thống vi phân phẳng Đầu ra phẳng của hệ thống là x(t) và y(t). Đạo hàm bậc nhất của các đầu vào phẳng là: x = v.cos ( ) y = v.sin ( )
Từ hệ trên ta nhận thấy mới chỉ có sự xuất hiện của đầu vào tục lấy đạo hàm bậc 2: v s
, do đó ta tiếp Đến đây hệ phương trình đã xuất hiện đủ cả 2 đầu và là được viết lại thành v s và = Hệ trên
Như vậy với phép biến đổi này đầu vào của hệ thống đã trở thành u 1 u 2 T = x y T và mô hình trạng thái z = x x y y T được mô tả như sau:
Phần 3: Các phương án điều khiển đối tượng Mobile Robot
Tiếp theo ta cần thiết kế đạo tham chiếu được cho bởi bộ điều khiển
( t ref cho hệ thống tuyến tính mới này Quỹ
) ), từ đó dễ dàng tính được trạng thái tham chiếu của hệ thống như sau: z ref
Sai lệch giữa trạng thái hệ thống thực tế với trạng thái tham chiếu là − z ref
Sử dụng phương pháp đặt điểm cực ta xác định bộ điều khiển K:
3.3.4 Phát triển mô hình sai lệch theo dõi quỹ đạo động học
Sai lệch vị trí của robot trong hệ tọa độ toàn cục (hệ tọa độ cố định) với vị trí tham chiếu là
− +Sai lệch vị trí của robot trong hệ tọa độ gắn với xe là
Phần 3: Các phương án điều khiển đối tượng Mobile Robot Đạo hàm của sai lệch:
Trong đó v ref và w ref là vận tốc tham chiếu tuyến tính vfb và wfb là các tín hiệu được xác định sau tùy thuộc vào luật điều khiển mà ta chọn Phương trình sai lệch được viết lại
3.3.5 Bộ điều khiển tuyến tính
Nhận thấy điểm zero-error ( e x = e y = e =0) là 1 điểm cân bằng khi cả 2 thành phần phản hồi v fb = fb =0 Từ mô hình sai lệch phi tuyến ở 3.3.4, ta tuyến tính hóa quanh điểm zero-error: e x e y
Nhờ có cấu trúc đặc biệt ở hệ (3.39), ta có thể sử dụng 1 ma trận phản hồi trạng thái tĩnh đơn giản, nhờ đó sai lệch trong hướng lái và góc lái sẽ đc điều chỉnh lại bằng v fb và fb :
Các hệ số điều khiển k x , k y , k được chọn sao cho các điểm cực của hệ thống nằm ở vị trí thích hợp trong miền s Hệ thống có 3 điểm cực, trong đó 1 điểm cực thực và 2 điểm cực phức liên hợp Các điểm cực được đặt ở vị trí cố định
2, 1 2 với 0, 01 Dựa vào đa thức đặc trưng của hệ kín: n n n n
Và đa thức mong muốn: ( s
Ta được các hệ số điều khiển
Trên thực tế các hệ số ở 3.41 vẫn khá khó áp dụng, bởi trong khi vận tốc thì lại nhỏ k y (t) trở nên khá lớn, Để giải quyết vấn đề này ta chọn trên, ta được các hệ số như sau: n (t ) 2
(t ), g 0 ref ref và lặp lại như
Ta rút ra 2 nhận xét quan trọng:
Phần 3: Các phương án điều khiển đối tượng Mobile Robot
Bộ điều khiển thiết kế dựa trên mô hình tuyến tính 1 mô hình tuyến tính chỉ hợp lệ ở lân cận của những điểm như điểm zero- error và hiệu suất sẽ kém hơn mong đợi khi mà sai lệch lớn
Kể cả tuyến tính nhưng hệ vẫn phụ thuộc thời gian, có thể thấy, nếu tất cả điểm cực đều nằm ở nửa mặt phẳng bờ bên trái của miền s thì hệ sẽ không ổn định
Mặc dù vậy, luật điều khiển tuyến tính vẫn thường được sử dụng bởi nó đơn giản, dễ dàng điều chỉnh trong thực tế và hiệu suất có thể chấp nhận được
Thiết kế bộ điều khiển cho đối tượng Mobile robot 3 bánh
Thiết kế bộ điều khiển động học vòng ngoài
Dựa theo những nội dung đã trình bày tại phần 3, ta thiết kế bộ điều khiển vòng ngoài cho Mobile robot dựa theo hàm ứng viên Lyapunov.
Quỹ đạo đặt của robot là
( t ) ref x ref x ref q = y ref ref ref
Mặt khác ta lại có: v = x
=x y − y x arctan ref ref ref ref ref ref 2 2 dt x x + y ref ref ref
Như vậy vận tốc tham chiếu của robot khi di chuyển trên quỹ đạo tham chiếu trên là V ref v ref ref
Sai lệch vị trí của robot trên hệ trục tọa độ gắn với xe được xác định là:
= R ( q − q ) = q = e y ref e e cos ( x − x ref ) + sin ( y − y ref )
− ref Đạo hàm của sai lệch:
= − sin ( x − x ) + cos ( x − x ) + cos ( y − ref ref e y = − cos ( x − x ref ) − sin ( x − x ref ) − sin ( y − e = − ref e = − sin x − x + cos y − y x ( ( ref ) ( ref ) )
+ ( − x sin + y cos ) + ( x sin − y cos ) ref ref e = − ref
Phần 4: Thiết kế bộ điều khiển cho đối tượng Mobile Robot 3 bánh e = e + v ( cos
) − v ( cos cos + sin sin ) x y ref ref ref e y = − e x + v (− cos sin + sin cos ) + v ref ( cos ref sin − sin ref cos ) e = − e ref x = e y
+ v − v cos e ref e y = e + v sin e x ref e = − ref
Lựa chọn bộ điều khiển: v v cos e + v c = ref
Thiết kế bộ điểu khiển động lực học vòng trong
Phần 4: Thiết kế bộ điều khiển cho đối tượng Mobile Robot 3 bánh v Đ
Mô phỏng hệ thống điều khiển trên Matlab Simulink
Mô phỏng mô hình Knematic
Sử dụng khối Matlab function trong Simulink function [x_dot, y_dot, phi_dot] = Kinematic(v, w, phi)
% S: ma tran cac phuong chuyen dong co ban
% Ma tran dau ra: q_dot = S * V;
% Dau ra: x_dot y_dot phi_dot end
Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink
Mô phỏng mô hình Dynamic
Sử dụng khối Matlab function trong Simulink function [v_dot, w_dot] = Dynamic(tau_r, tau_l, phi, v, w)
% Cac thong so cua xe
% L: khoang cach giua 2 banh xe (m)
% J: moment quan tinh cua xe (kg.m^2)
% d_G: do lech cua khoi tam so voi diem chinh giua truc banh xe
% vector momen dau vao u= [ tau_r ; tau_l];
% M: ma tran khoi luong va quan tinh he thong
% S: ma tran cac phuong chuyen dong co ban
% E: Ma tran chuyen tu khong gian truyen dong sang toa do tong quat
E = 1/r *[ cos(phi) cos(phi) ; sin(phi) sin(phi) ;
% S_dot: dao ham ma tran S
S_dot = [ -w*sin(phi) w*cos(phi) 0
% Xac dinh cac ma tran trung gian doi bien
% Dau ra: v_dot = V_dot(1); w_dot = V_dot(2); end
Bộ điều khiển vòng ngoài Kynematic Controller
Đầu vào của bộ điều khiển là tư thế tham chiếu và tư thế phản hồi vê từ đối tượng; đầu ra của bộ điều khiển là giá trị tốc độ dài và tốc độ góc điều khiển cho bộ điều khiển vòng trong.
Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink function [v,w] = fcn(x, y, phi, x_ref, y_ref, phi_ref, v_ref, w_ref) q_REF = [x_ref;y_ref;phi_ref];
% % Sai lech vi tri e = R * (q_REF-q_FB); eX = e(1); eY = e(2); ePHI = e(3);
% he so bo dieu khien zeta = 0.9; g;
Kx = 2 * zeta * sqrt(w_ref^2 + g * v_ref^2);
% bo dieu khien: v = v_ref * cos(ePHI)+ Kx * eX; w = w_ref + Ky * v_ref * eY + Kphi * sin(ePHI); end
Bộ điều khiển vòng trong Dynamic Controller
31 Đầu vào của bộ điều khiển là tín hiệu vận tốc điều khiển từ bộ điều khiển vòng ngoài và tín hiệu tốc độ phản hồi từ đối tượng; đầu ra là giá trị moment đặt vào mô hình Dynamic cảu đối tượng. function [tau_R, tau_L] = dynamic_ctr(v_REF, w_REF, v_REF_dot, w_REF_dot, v_FB, w_FB)
V_REF_dot = [v_REF_dot; w_REF_dot];
% Cac thong so cua xe
% L: khoang cach giua 2 banh xe (m)
% J: moment quan tinh cua xe (kg.m^2)
% d_G: do lech cua khoi tam so voi diem chinh giua truc banh xe (m) m = 1 ; r = 0.033; L =0.16; J = 0.002 ;
% V_dot = inv(M_) * E_ * u err = - V_REF + V_FB;
K = [ 200 0; 0 100]; u = (inv(E_)*M_) * (-K * err + V_REF_dot); tau_R = u(1); tau_L = u(2); end
Kết quả mô phỏng
5.5.1 Kết quả mô phỏng với kịch bản thứ nhất
Với kịch bản mô phỏng thứ nhất này, ta lựa các bộ điều khiển như Phần 4 đã nêu.
Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink
Lấy trạng thái ban đầu của robot là đạo tham chiếu sử dụng: x (0 ); y (0 ); (0 )
T =1,2;1,4;0 T Quỹ x = 1.1 + 0.7 sin 2 t ref 30 y ref = 0,8 + 0, 7 sin 4 t
30 Kết quả bám quỹ đạo của robot:
Song, để kiểm tra khả năng tác động tới mô hình của các bộ điều khiển ta thử thêm một số kịch bản điều khiển khác như ở các phần dưới đây.
5.5.2 Kết quả mô phỏng mới kịch bản thứ hai
Vị trí ban đầu cũng như quỹ đạo tham chiếu ta vẫn sử dụng tương tự như ở phần 5.5.1, song với kịch bản này ta sẽ thử kiểm tra tính đáp ứng của hệ thống khi quỹ đạo tham chiếu thay đổi với tốc độ nhanh hơn so với trên với tần số nhanh hơn
Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink
30 Kết quả bám quỹ đạo của robot:
So sánh với kết quả ở 5.5.1:
Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink
5.5.3 Kết quả mô phỏng với kịch bản thứ ba Ở phần này ta sẽ kiểm tra độ tác động của bộ điều khiển vòng trong tới mô hình bằng các thay đổi ma trận xác định dương trong bộ điều khiển động lực học thành: 2 0
Kết quả mô phỏng bám quỹ đạo của robot:
So sánh với quỹ đạo trong kịch bản thứ nhất:
Nhận xét: Như vậy khả năng bám quỹ đạo của robot khi thay đổi ma trận xác định dương trong bộ điều khiển vòng trong không thay đổi nhiều trong cả 2 trường hợp Điều này là do đáp ứng của bộ điêu khiển vòng trong là rất nhanh so với vòng ngoài do đó dù có thay đổi thì ảnh hưởng của nó tới hệ thống là rất nhỏ.
5.5.4 Kết quả mô phỏng với kịch bản thứ tư Đối với kịch bản này, ta sẽ xét đến khả năng tác động của bộ điều khiển vòng ngoài đến hệ thống bằng cách thay đổi kệ số tắt dần = 0.75 và hệ số g = 200
Khi đó kết quả bám quỹ đạo thu được như sau:
So với trong kịch bản thứ nhất :
Phần 5: Mô phỏng hệ thống điều khiển trên Matlab Simulink
Nhận xét: Như vậy ta có thể thấy rằng, so với khi thay đổi bộ điều khiển vòng trong, việc thay đổi bộ điều khiển vòng ngoài tác động rất nhiều tới hệ thống.
