CHƯƠNG 5: MÔ HÌNH HÓA VÀ MÔ PHỎNG

Chương này bao gồm việc giới thiệu mô hình động học của robot, cách xác định vị
trí robot so với line, bộ điều khiển tracking sử dụng cho robot và mô phỏng bám sa bàn
cho robot.

5.1. Mô hình động học của robot

Mô hình động học của xe dò line (mobile platform) đã được giới thiệu nhiều trong
các nghiên cứu. Mô hình này bao gồm các điểm quan trọng: Điểm R: điểm tham chiếu
cho robot; Điểm M: trung điểm của hai bánh chủ động; Điểm C: Điểm tracking của robot.
Mô hình này được thể hiện trên Hình 5.1

e3

R
YR
e2

YC
C e1
YM M

fc XR XM XC X

Hình 5.1 Mô hình động học của robot dò line

- M (xM, yM): Trung điểm đoạn nối tâm hai bánh xe.

- C (xC, yC): Tọa độ điểm bám line (Tracking Point).

- R (xR, yR): Tọa độ điểm mong muốn của điểm C trên đường line.

- v: Vận tốc dài của xe

- ω: Vận tốc góc của xe.

 Phương trình động học tại điểm M:

[ ] [ ][ ]
ẋ M cosφ 0
= v
ẏ M sinφ 0 . (5.1)
ω
φ̇ M 0 1

Trong đó v là vận tốc dài và ω là vận tốc góc của xe

Ngoài ra ta tìm được mối quan hệ của vận tốc xe và vận tốc bánh trái/bánh phải:

{
r . ω r r . ωl
v= +
2 2
r . ω r−r . ωl
ω=
b

[ ][ ]
1 b

[ ]
ωr
ωl
=
r
1
r
2r

2r
v
. ( 5.2)
−b ω

Với r: bán kính bánh xe trái và phải

ωl: vận tốc góc bánh xe trái

ωr: vận tốc góc bánh xe phải

b: khoảng cách giữa hai bánh xe

Vậy phương trình động học của robot tại điểm M là:

[ ]
cosφ cosφ

[ ] [ ][ ]
2 2
ẋ M cosφ 0
ẏ M = sinφ 0 .
φ̇ M 0 1
v = sinφ
ω 2
1
2
−1
.
[ ]
sinφ r . ωr
r .ω l
(5.3)

b b

Với φM = φC = φ
 Ta xét tọa độ điểm C:

{ xC =x M +d . cosφ
y C =x M +d . sinφ

Phương trình động học tại điểm C:

{
ẋ C = ẋ M −d φ̇ sinφ
ẏ C = ẏ M + d φ̇ cosφ (5.4)
φ̇C =φ̇

Với d là khoảng cách từ M đến C.

Từ công thức (5.3) và (5.4) ta có:

[ ]
cosφ cosφ

[ ][ ]
ẋ C 2 2
−d . φ̇ sinφ
ẏ C = d . φ
φ̇˙C
˙ cosφ +
0
sinφ
2
1
2
−1
.
[ ]
sinφ r . ω r
r . ωl
(5.5)

b b

Phương trình động học tại điểm R:

{
ẋ R=v R . cosφ
ẏ R=v R . sinφ (5.6)
φ̇ R=ωR

Trong đó là vR vận tốc mong muốn của xe tại điểm tham chiếu

5.2. Cách xác định các sai số bám đường của robot:

Bộ điều khiển được thiết kế cho điểm bám đường C đến vị trí mong muốn R và vận
tốc mong muốn vR. Để làm được điều đó, ta xác định được các sai số bám đường e 1, e2, e3
như sau:
 [][ ][ ]
e1 cosφ sinφ 0 x R −xC
e 2 = −sinφ cosφ 0 . y R − y C (5.7)
e3 0 0 1 φ R −φC

Hay:

{ e1=cosφ ( x R −x C )+ sinφ ( y ¿ ¿ R− y C ) ¿ e 2=−sinφ ( x R −x C ) +cosφ ( y ¿ ¿ R− y C )¿ e3=φR −φ C

Suy ra đạo hàm của các sai số:

{
ė1=v R . cos ( φR −φ C )−v+ e2 . ω
ė 2=v R .sin ( φR −φC )−(d+ e 1) .ω
ė 3=ω R −ω

[ ][ ][ ][ ]
ė 1 v R . cos e 3 −1 e2
v
ė 2 = v R . sin e 3 + 0 −d−e1 . (5.8)
ω
ė 3 ωR 0 −1

Đây là một hệ thống phi tuyến, để thiết kế điều khiển cần có bộ điều khiển cho hệ
thống phi tuyến. Một hệ phi tuyến (có thể hiểu là có những phần tử phi tuyến, ở đây nó là
cose3 và sine3)

5.3. Bộ điều khiển tracking và mô phỏng bám sa bàn:

5.3.1. Tiêu chuẩn kiểm định Lyapunov và bộ điều khiển v, ω (Tracking flowing):

Ta sử dụng tiêu chuẩn kiểm tra độ ổn định Lyapunov:

Đây là tiêu chuẩn đánh giá sự ổn định của một hệ thống . Tuy là phương pháp đánh
giá ổn định, nhưng nó cũng dùng để thiết kế bộ điều khiển. Ý tưởng là xác định hàm vô
hướng V luôn dương theo các biến hệ thống, sau đó chọn ra quy luật giữa chúng để cho V
luôn giảm khi hệ thoát khỏi vị trí cân bằng (tức là tại vị trí cân bằng thì V nhỏ nhất, khi
lệch ra thì V dương nhưng luôn có xu hướng giảm theo thời gian, giảm tới 0)

Để rõ hơn ta có thể tham khảo định lý Lyapunov về ổn định tiệm cận trong tiêu
chuẩn kiểm tra độ ổn định Lyapunov:
 Nếu tìm được một hàm vô hướng liên tục V(y) với mọi biến trạng thái y 1, …, yn có
các đạo hàm bậc nhất liên tục và thỏa mãn:

1. V ( y )> 0 , ∀ y ≠0

2. V̇ ( y )< 0

3. V ( y ) → ∞ khi| y|→ ∞

thì trạng thái cân bằng ye thỏa mãn f(ye ,0) = 0 sẽ ổn định tiệm cận (hay nói cách khác là
chuyển động không bị nhiễu sẽ ổn định tiệm cận).

Vậy trở lại vấn đề bài toán đầu tiên ta cần xác định hàm V (không có quy luật nhất
định):

1 2 1 2 1−cos e 3
V = e1 + e2 − ≥ 0(5.9)
2 2 k2

Nhận xét ta thấy hàm này liên tục và có đạo hàm bậc nhất liên tục

Xác định quan hệ biến trạng thái:

Quan hệ này chính là bộ điều khiển, máy tính cần tính toán v, ω liên hệ với e1, e2, e3
như thế nào để cái hàm V bên trên luôn giảm (tức là đạo hàm V luôn âm khi e1, e2, e3 ≠0)

Vậy ta xác định bộ điều khiển bám line như sau (không có quy luật nhất định):

{ v=v R cos e 3 +k 1 e 1
ω=k 2 v R e2 +ω R + k 3 sin e3
(5.10)

Tiến hành đạo hàm hàm V:

dV sin e 3
=e 1 . e˙1+ e2 . e˙2+ e˙ (5.11)
dt k2 3

sin e3
¿> V̇ =e 1 ( v R cos e3−v +e 2 . ω ) +e 2 ( v R sin e 3−d . ω+e 1 . ω ) + (ω R−ω)
k2

Từ công thức (5.10) và (5.11) ta được:

 2 k3 2
V̇ =−k 1 e1 −d e 2 ( k 2 v R e2 +ω R + k 3 sin e 3 )− sin e3 (5.12)
k2

Trong đó k1, k2, k3 mang giá trị dương

Như vậy ta có đạo hàm của V luôn âm dưới tác động của bộ điều khiển (hàm V luôn
giảm). Điều đó có nghĩa là hệ thống ổn định với bộ điều khiển đã chọn theo tiêu chuẩn
Lyapunov.

5.3.2. Xác định sai số trong mô phỏng

Do hệ thống phototransistor chỉ xác định được sai số theo phương pháp tuyến với
phương chuyển động của xe, mô hình động học của xe cần được giới thiệu lại với điểm C
là tâm của dãy sensor, M là trung điểm hai bánh chủ động và là điểm tracking của xe. Khi
đó, ta có thể set e1 = 0. Mô hình này được thể hiện trên Hình 5.2. Như vậy, để xác định
được đầy đủ thông tin về vị trí của điểm tracking so với tham chiếu, sai số e 2 và e3 cần
được xác định.

R
YR
e2

YC C
d
YM M

fc XR XM XC X

Hình 5.2 Mô hình sau khi được xây dựng lại để tìm sai số e2, e3
 Trên thực tế, e2 được xác định trực tiếp từ hệ thống sensor. Đối với e 3, phương án
xác định được đề xuất là cho robot di chuyển theo phương trước đó một đoạn d S đủ nhỏ
để khi nối 2 điểm RR’ tạo thành tiếp tuyến của đường cong (Hình 5.3). Khi đó, sai số e 3
được xác định theo công thức:

e 3=arctan
e '2−e 2
ds ( )
(5.13)

e3
R
R '
e2
'
e2
C
C
'
M C
M ' ds

Hình 5.3 Mô hình để xác định sai số e3

5.4. Kết quả mô phỏng:

Ta chọn vận tốc khởi động ban đầu v = 100 mm/s để chạy và xe sẽ tham chiếu theo
vận tốc VR = 1000 mm/s để bám theo.

Khi bắt đầu vào những đoạn cua do phát sinh thêm vận tốc góc ôm cua ω R = vR /R
(vR là vận tốc tham chiếu trên đoạn line) nên sẽ làm vận tốc góc của bánh xe tăng nhanh
theo công thức đánh giá độ ổn định Lyapunov của bộ điều khiển Tracking flowing:

ω=k 2 v R e 2+ ωR + k 3 sin e3
 Như vậy xe sẽ kém ổn định hơn khi bắt đầu chuyển tiếp từ đoạn cong sang thẳng
hay ngược lại.

Nhóm chọn thời gian lấy mẫu là 30ms (chương 6) và với các hệ số của bộ điều
khiển k1 = 0; k2 = 1000; k1 = 7; thì kết quả mô phỏng được thể hiện như sau:

Hình 5.4 Đoạn cong thứ 1

Hình 5.5 Đoạn thẳng thứ 1

Hình 5.6 Đoạn cong thứ 2

Hình 5.7 Đoạn cong thứ 3

 Hình 5.8 Đoạn thẳng thứ 2

Hình 5.9 Đoạn cong thứ 4

Kết quả cho thấy xe bám line tốt (không bị lệch khỏi đường line) .
 Hình 5.10 Hình ảnh thể hiện khả năng bám line qua các đoạn chuyển tiếp trên sa bàn

Các đồ thị thể hiện sai số pháp tuyến, sai số góc lệch, vận tốc xe và vận tốc 2 bánh xe:

+ Sai số pháp tuyến (e2):

 Hình 5.11 Sai số pháp tuyến e2 (mm)
Sai số pháp tuyến vẫn nằm trong đoạn sai số cho phép (e 2 cp=± 5 mm).

Ở đây, những đoạn “chuyển góc” (từ đoạn cong sang thẳng) là có sự biến thiên sai
số lớn nhất và độ lệch lớn nhất giữa tâm sensor và đường line là e 2 max = -4 mm (khi xe ở
đoạn cong) - vẫn duy trì cho đến khi vào lại đoạn thẳng. Ở đoạn đường thẳng, sai số e 2 là
nhỏ nhất (khoảng -2mm).

+ Sai lệch góc giữa xe so với line (e3):

Thể hiện khả năng chuyển đổi hướng của xe, được thể hiện qua đồ thị sau:
 Hình 5.12 Sai số góc lệch giữa xe và đường line (độ)
Bỏ qua sai số ban đầu khi cho xe bắt đầu chạy, độ lệch góc lớn nhất khoảng (e 3 max
= 60), theo đồ thị thì khi xe bắt đầu chuyển tiếp từ đoạn đường cong sang đoạn đường
thẳng (và ngược lại) thì sẽ xảy ra sự biến thiên về sai lệch góc. Sai lệch góc nhỏ nhất là
khoảng 0.20 khi ở đoạn đường thẳng.

+ Vận tốc dài của xe (v):

 Hình 5.13 Vận tốc dài của xe (mm/s)
Sau khi đánh giá được sai số pháp tuyến, sai số góc lệch ban đầu thì tăng vận tốc của
xe tại điểm tham chiếu bằng vận tốc mà ta mong muốn vR = 1000 mm/s.

+ Vận tốc góc của 2 bánh xe:

Trên đồ thị ta thấy lúc có sự chuyển tiếp từ đường cong sang thẳng hay thẳng sang
cong, 2 bánh xe sẽ có vận tốc đối xứng (1 bánh xe sẽ quay nhanh, bánh còn lại sẽ quay
chậm để rẽ) nhau qua 1 đường thẳng ổn định, đường thẳng ổn định đó là khi vận tốc góc
của 2 bánh xe bằng nhau, xe chạy trên đoạn đường thẳng.
 Hình 5.14 Vận tốc góc 2 bánh xe (rpm)
Và ta phải nhận xét 1 điều rằng đối với bánh xe quay nhanh, ω max = 390 vòng/phút <
ωmax động cơ = 620 vòng/phút nên kết quả mô phỏng phù hợp với phần tính toán cơ khí.

Kết luận: Bước đầu mô phỏng với thời gian lấy mẫu là 30ms v thì sẽ phù hợp với
tiêu chí yêu cầu đề đặt ra là để đua, vận tốc xe đạt được max là 1m/s, thời gian chạy hết
vòng sa bàn là khoảng 7s và bám line tốt xe không bị trật khỏi đường line.

Tuy nhiên đây mới chỉ là bước đầu mô phỏng, ta chỉ xem xét khả năng bám sa bàn
thiết kế bộ điều khiển Tracking theo tiêu chuẩn kiểm định Lyapunov có khả thi hay
không, các điều kiện vẫn còn lý tưởng.

Kết quả mô phỏng sẽ còn chính xác hơn gần với điều kiện thực tế hơn sau khi xem
xét khả năng đáp ứng của 2 động cơ bằng việc thiết kế bộ điều khiển vòng trong và xét
luôn sự nhiễu của thanh cảm biến đã thực nghiệm ở chương 4. Đây chính là bước cuối
cùng của mô phỏng, kiểm tra đánh giá lại với bước đầu và so sánh tiêu chí đề bài – bước
cuối này sẽ được trình bày ở chương 7: Đánh giá kết quả mô phỏng.
Tài liệu tham khảo:

T.S. Đỗ Quang Thông – Th.S. Vũ Đức Tường (2010). Lý thuyết điều khiển tự động 2.
Nhà xuất bản Quân đội Nhân dân Hà Nội.

Nguyễn Tấn Tiến, Trần Thanh Tùng và Kim Sang Bong (2016). Giảng Dạy Thiết Kế Hệ
Thống Cơ Điện Tử qua Đồ Án (Mechatronics System Design throgh Project). Hội nghị
toàn quốc lần thứ 8 về Cơ Điện tử - VCM-2016, pp. 416 - 422