Thiết kế robot di chuyển và đo lường thông số trạm biến áp tự hành

ĐẠI HỌC ĐÀ NẴNG
TRƯỜNG ĐẠI HỌC BÁCH KHOA
KHOA ĐIỆN

ĐỒ ÁN LIÊN MÔN 4
NGÀNH: KỸ THUẬT ĐIỀU KHIỂN & TỰ ĐỘNG HỐ

ĐỀ TÀI: THIẾT KẾ ROBOT DI CHUYỂN VÀ ĐO
LƯỜNG THƠNG SỐ TRẠM BIẾN ÁP TỰ HÀNH
Người hướng dẫn: TS. NGUYỄN HỒNG MAI
Nhóm thực hiện:
NHĨM 1_18NH37B
Sinh viên thực hiện:

Lê Hữu Trường
Cao Khả Tiến
Nguyễn Hồng Gia Tuấn
Bạch Hải Nam
Nguyễn Cơng Sinh
Nguyễn Hịa Hưng

Đà Nẵng, 12/2021


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

MỤC LỤC HÌNH............................................................................................................... 4

LỜI NĨI ĐẦU .................................................................................................................... 6
CHƯƠNG 1: TỔNG QUAN VỀ ĐỀ TÀI ........................................................................ 7
1.1 Tính cấp thiết của đề tài .......................................................................................... 7
1.2 Mục tiêu nghiên cứu ................................................................................................. 7
1.3 Đối tượng và phạm vi nghiên cứu ........................................................................... 8
1.4 Cách tiếp cận và phương pháp nghiên cứu ............................................................ 8
1.5 Giới thiệu về robot giám sát trạm biến áp ............................................................. 8
CHƯƠNG 2: MÔ HÌNH ĐỘNG HỌC, ĐỘNG LỰC HỌC CỦA ĐỐI TƯỢNG ...... 11
2.1 Mơ hình động học của Mobile Robot ................................................................... 12
2.1.1 Mơ hình bánh xe robot ....................................................................................... 12
2.1.2 Phương trình động học của Mobile Robot ........................................................ 13
2.2 Mơ hình động học, động lực học của cánh tay robot .......................................... 15
2.2.1 Đặc điểm kĩ thuật của cánh tay robot ................................................................ 15
2.2.2 Các phương trình động học thuận ..................................................................... 15
2.2.3 Các phương trình động học ngược .................................................................... 17
2.2.4 Các phương trình động lực học của tay máy ..................................................... 19
CHƯƠNG 3: KĨ THUẬT ĐỊNH VỊ VÀ THUẬT TOÁN ĐIỀU KHIỂN MOBILE
ROBOT ............................................................................................................................. 28
3.1 Tổng quan về kĩ thuật định vị cho Mobile Robot ................................................ 28
3.2 Phương pháp Dead - reckoning ............................................................................ 29
3.2.1 Nguyên lý ........................................................................................................... 30
3.2.2 Những lưu ý khi sử dụng phương pháp Dead-reckoning .................................. 31
3.3 Thuật toán điều khiển robot tự hành ................................................................... 33
3.3.1 Nguyên lý bộ điều khiển sử dụng thuật toán Pure Pursuit ................................ 33
3.3.2 Ảnh hưởng của LookAheadDistance đến thuật toán Pure Pursuit .................... 35
3.3.3 Hạn chế của thuật toán Pure Pursuit ................................................................ 37
3.4 Lưu đồ điều khiển robot trạm biến áp ................................................................. 37
3.4.1 Lưu đồ thuật toán tổng quát ............................................................................. 37
3.4.2 Lưu đồ thuật toán trên Arduino điều khiển vị trí Robot và vị trí tay máy ........ 39
3.4.3 Mô phỏng trên Matlab Simulink ........................................................................ 39

1


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

CHƯƠNG 4: THIẾT KẾ PHẦN CỨNG HỆ THỐNG ................................................ 42
4.1 Tổng quan thiết kế ................................................................................................. 42
4.1.1 Cơ sở thiết kế ..................................................................................................... 42
4.1.2 Chọn thiết bị điều khiển truyền động cho robot ................................................ 42
4.1.3 Chọn phần mềm lập trình điều khiển, thu thập và xử lý dữ liệu ........................ 43
4.2 Thiết kế phần cứng ................................................................................................. 43
4.2.1 Máy vi tính ......................................................................................................... 44
4.2.2 Camera............................................................................................................... 45
4.2.3 Bộ truy cập không dây WiFi (Router) ................................................................ 46
4.2.4 Module ESP8266-12E........................................................................................ 47
4.2.5 Module Arduino UNO và Modulê Micro Step Driver ....................................... 48
4.2.6 Modul CNC Shield V3........................................................................................ 50
4.2.7 Cảm biến siêu âm HY-SRF05 ............................................................................ 50
4.2.8 Cảm biến nhiệt độ ES1C-A40 ............................................................................ 51
4.2.9 Mudule la bàn HMC5883L ................................................................................ 52
4.2.10 Encoder ............................................................................................................ 53
4.2.11 Động cơ ............................................................................................................ 54
4.2.12 Pin .................................................................................................................... 55
CHƯƠNG 5: THIẾT KẾ BỘ ĐIỀU KHIỂN TAY MÁY................................................. 57
5.1 Bộ điều khiển mờ .................................................................................................... 57
5.1.1 Lịch sử phát triển ............................................................................................... 57

5.1.2 Điều khiển mờ .................................................................................................... 58
5.1.3 Thiết kế bộ điều khiển mờ .................................................................................. 59
5.1.4 Kết quả mô phỏng .............................................................................................. 61
5.1.5 Nhận xét ............................................................................................................. 64
5.2 Bộ điều khiển Neural (ANN) ................................................................................. 64
5.3 Bộ điều khiển mờ PID (FLC-PID) ........................................................................ 68
5.3.1 Sơ đồ điều khiển sử dụng Mờ PID ..................................................................... 68
5.3.2 Luật chỉnh định PID .......................................................................................... 69
5.3.3 Các bước thiết kế bộ điều khiển Mờ .................................................................. 70

2


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

5.3.4 Kết quả mô phỏng .............................................................................................. 71
5.3.5 Nhận xét ............................................................................................................. 75
5.4 Bộ điều khiển Neural PID (ANN-PID) ................................................................. 75
5.4.1 Thiết kế bộ điều khiển ........................................................................................ 75
5.4.2 Quá trình thu thập dữ liệu ................................................................................. 76
5.4.3 Kết quả mô phỏng .............................................................................................. 79
5.4.4 Nhận xét ............................................................................................................. 81
5.5 Bộ điều khiển Neural mờ PID (ANN-FLC-PID) ................................................. 82
5.5.1 Khối mờ .............................................................................................................. 82
5.5.2 Khối PID ............................................................................................................ 86
5.5.3 Khối Noron – Mạng Noron RBF ....................................................................... 87

5.5.4 Kết quả mô phỏng .............................................................................................. 92
5.6 Bộ điều khiển mờ neural PID (FLC-ANN-PID) .................................................. 96
5.6.1 Kết quả mô phỏng ............................................................................................ 103
5.7 So sánh, đánh giá chất lượng các bộ điều khiển ................................................ 108

3


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

Mục lục hình
Hình 1: Mơ tả tổng quan Robot ........................................................................................... 9
Hình 2. 1: Mơ hình bánh xe đã được lý tưởng hóa............................................................ 12
Hình 2. 2: Hệ quy chiếu của mobile robot......................................................................... 13
Hình 2. 3: Tay máy robot 3 bậc tự do ................................................................................ 15
Hình 3. 1: Sơ đồ nguyên lý của khâu định vị trong mobile robot ..................................... 28
Hình 3. 2: Sơ đồ ngun lý mơ hình NEWT ..................................................................... 30
Hình 3. 3: Thuật tốn Pure Pursuit .................................................................................... 34
Hình 3. 4: Quan hệ hình học .............................................................................................. 34
Hình 3. 5: LookAheadDistance ......................................................................................... 36
Hình 3. 6: LookAheadDistance quá nhỏ............................................................................ 36
Hình 3. 7: LookAheadDistance quá lớn ............................................................................ 36
Hình 3. 8: Lưu đồ thuật tốn tổng qt .............................................................................. 38
Hình 3. 9: Lưu đồ thuật tốn trên Arduino ........................................................................ 39
Hình 3. 10: Mơ hình mơ phỏng ......................................................................................... 40
Hình 3. 11: Kết quả mơ phỏng mobile robot ..................................................................... 40

Hình 3. 12: Sai số mơ phỏng mobile robot ........................................................................ 41
Hình 4. 1: Sơ đồ trạm biến áp ........................................................................................... 42
Hình 4. 2: Tổng quan sơ đồ thiết kế phần cứng điều khiển robot ..................................... 44
Hình 4. 3 Raspberry Pi 4 ................................................................................................... 45
Hình 4. 4: Camera Basler acA3800-10gm ........................................................................ 46
Hình 4. 5: Edup AZ800 Wifi Router ................................................................................. 47
Hình 4. 6: Modul ESP8266-12E ........................................................................................ 48
Hình 4. 7: Modul Arduino Uno ......................................................................................... 49
Hình 4. 8: Modul Micro Step Driver ................................................................................. 49
Hình 4. 9: Modul CNC Shield V3 ...................................................................................... 50
Hình 4. 10: Cảm biến siêu âm UltraSonic HY-SRF05 ...................................................... 51
Hình 4. 11: Ảnh mơ phỏng cảm diện tích và khoảng cách đặt cảm biến .......................... 52
4


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

Hình 4. 12: Mudul la bàn HMC5883L .............................................................................. 53
Hình 4. 13: Encoder .......................................................................................................... 53
Hình 4. 14: Động cơ bước ................................................................................................. 55
Hình 4. 15: Ắc quy Đồng Nai 12v/40Ah ............................................................................ 56

5


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

LỜI NĨI ĐẦU
Cùng với sự phát triển không ngừng của các ngành khoa học kỹ thuật, các ngành
công nghiệp cũng phát triển nhanh chóng. Việc áp dụng các máy móc hiện đại vào sản suất
là một yêu cầu không thể thiếu trong các nhà máy nhằm tăng năng suất, tăng chất lượng và
giảm giá thành sản phẩm.
Song song với sự phát triển đó, cơng nghệ chế tạo Robot cũng phát triển nhanh
chóng đặc biệt là ở các nước phát triển nhằm đáp các nhu cầu về sản xuất, sinh hoạt, quốc
phịng…Robot có thể thực hiện những cơng việc mà con người khó thực hiện và thậm chí
khơng thực hiện được như: làm những cơng việc địi hỏi độ chính xác cao, làm việc trong
mơi trường nguy hiểm (như lị phản ứng hạt nhân, dị phá mìn trong qn sự, thám hiểm
khơng gian vũ trụ…).
Trong các họ Robot, chúng ta không thể không nhắc đến TAY MÁY ROBOT và
ROBOT TỰ HÀNH với những đặc thù, được ứng dụng rộng rãi trên khắp các nước thay
thế con người làm những công việc ở trong môi trường độc hại hay chế tạo Robot khác với
độ chính xác mà con người khơng thể làm bằng tay được.
Sau một thời gian làm việc, nghiên cứu, tham khảo chúng em đã hoàn thành đề tài
“Robot tự hành giám sát trạm biến áp”.
Em xin chân thành cảm ơn thầy Nguyễn Hoàng Mai đã giúp em hoàn thành đồ án
này.

6


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

CHƯƠNG 1: TỔNG QUAN VỀ ĐỀ TÀI

1.1 Tính cấp thiết của đề tài
Hiện nay các nhà máy điện, trạm biến áp và lưới điện phát triển rất nhanh chóng
cả về số lượng, quy mô và chất lượng dẫn tới việc quản lý vận hành các hệ thống điện lớn
sẽ đòi hỏi phải có số lượng nhân cơng lớn làm tăng chi phí. Hơn nữa, nhiều hạng mục công
việc bắt buộc phải tự động hóa hồn tồn mới đáp ứng được các u cầu kỹ thuật. Vì vậy,
việc ứng dụng các hệ thống thơng tin, đo lường, điều khiển để tự động hóa trong hệ thống
điện là một hướng đi tất yếu.
Ở Việt Nam hiện nay, phần lớn các trạm biến áp đều vận hành theo kiểu truyền
thống, được điều khiển thông qua các tủ bảng, sử dụng khóa điều khiển. Chức năng giám
sát trạm từ các trung tâm điều độ cũng chỉ thực hiện thông qua hệ thống thu thập thông tin
xa (RTU). Tuy nhiên các màn hình HMI (Human Machine Interface) của các trung tâm
điều khiển từ xa chỉ thể hiện trạng thái logic đóng cắt của thiết bị nhưng khơng thể hiện
được trạng thái vật lý hay tình trạng thực của thiết bị, mà theo quy trình quy phạm bắt buộc
phải có, ví dụ phải thấy rõ trạng thái dao cách ly đã mở/ đóng sau thao tác, tình trạng hỏng
hóc sau bão, sau các thảm họa thiên nhiên … Một số trạm biến áp mặc dù đã đuợc tự động
hóa nhưng vẫn phải đảm bảo số luợng cơng nhân trực vận hành chỉ để theo dõi thiết bị và
trạng thái thiết bị theo nhu quy trình quy phạm quy định. Một số trạm biến áp mặc dù đã
lắp đặt camera, nhưng các camera này cũng chỉ giúp giám sát tình trạng chung của trạm,
giám sát an ninh tránh sự đột nhập từ bên ngồi, và chưa có khả năng giám sát chi tiết tình
trạng từng thiết bị.
Từ những phân tích nên trên, đưa ra yêu cầu sử dụng một loại robot tự động (Robot
giám sát trạm biến áp) để thu thập hình ảnh, thơng tin trạng thái thiết bị trong trạm biến áp.
Robot giám sát trạm biến áp có thể tự động di chuyển trong trạm theo một hệ thống dẫn
hướng đến những vị trí định trước (vị trí máy cắt, dao cách ly, máy biến áp, thanh góp, và

các vị trí khác cần theo dõi, thu thập theo yêu cầu thực tế đặt ra), thu thập hình ảnh và trạng
thái các thiết bị này đưa về trung tâm, các thiết bị trạm từ các góc độ khác nhau và truyền
về trung tâm điều khiển.
1.2 Mục tiêu nghiên cứu
7


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

Nghiên cứu chế tạo Robot có thể tự động di chuyển với tốc độ 0,4 - 1 m/s trong trạm
biến áp để thay thế con ngƣời, thu thập và xử lý hình ảnh, thông tin thông qua các cảm
biến về trạng thái các dao cách ly, máy cắt, đồng hồ nhiệt độ dầu MBA, đồng hồ nhiệt độ
cuộn cao áp MBA, đồng hồ nhiệt độ cuộn trung áp MBA, ... từ các góc độ khác nhau và
truyền về trung tâm điều khiển.
1.3 Đối tượng và phạm vi nghiên cứu
−

Nghiên cứu khảo sát trạm biến áp giả định theo mơ hình được xây dựng từ các trạm
biến áp thực tế.

−

−
−
−

Nghiên cứu thiết kế, chế tạo hệ thống điều khiển phù hợp cho robot tại trạm biến áp
.
Nghiên cứu chế tạo robot tự động mang camera giám sát,
Nghiên cứu thiết kế, chế tạo hệ thống cấp nguồn cho robot,
Nghiên cứu chế tạo hệ thống truyền dữ liệu từ robot về phòng vận hành tại trạm
biến áp.

1.4 Cách tiếp cận và phương pháp nghiên cứu
Đối tượng nghiên cứu của đề tài này là mơ hình động lực, mơ hình điều khiển của
robot, các phương thức truyền dữ liệu, xử lý hình ảnh. Ngồi ra cấu trúc trạm biến áp và
các thiết bị cần giám sát trong trạm biến áp cũng được khảo sát.
1.5 Giới thiệu về robot giám sát trạm biến áp
Robot giám sát trạm biến áp là bộ công cụ tiên tiến để kiểm tra cơ sở hạ tầng và
trạm biến áp từ xa, kết hợp với việc lập bản đồ không gian quy mô nhỏ giúp kiểm tra nhiệt
độ và phát hiện sự ăn mòn với độ nhạy cao. Với một cụm cảm biến bao gồm: cảm biến
lidar, máy ảnh nhiệt và máy ảnh quang phổ, Robot kiểm tra một cách trực quan để đánh
giá tình trạng của các thiết bị trên cơ sở định lượng. Các cảm biến được gắn trên một Robot
giám sát trạm biến áp có khả năng thực hiện việc quan sát lặp lại trong hầu hết mọi môi
trường của trạm biến áp, với nhiều loại địa hình trong mọi điều kiện thời tiết với định vị
lặp lại trong phạm vi 10 cm. Robot phát hiện hầu hết các mối nguy hiểm tiềm ẩn để đánh
giá tình trạng thiết bị.

8


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

Dữ liệu từ bộ cảm biến trong robot được sử dụng để phát hiện những hư hỏng và
các dấu hiệu xuống cấp (các vết nứt, rị điện, sự oxy hóa) và lập bản đồ thể hiện những thay
đổi vật lý đối với máy biến áp, công tắc, dây dẫn cùng các thiết bị khác
Trạm biến áp không người trực là giải pháp hợp lý cho hệ thống điện vì được quản
lý vận hành một cách tự động, nâng cao năng suất lao động, giảm tối đa nhân lực. Trong
tương lai, hàng loạt trạm biến áp không người trực, vận hành tự động, điều khiển từ xa sẽ
được ngành Điện đưa vào vận hành. Công nghệ Robot tự động sẽ xây dựng một chương
trình giám sát trạm biến áp tiên tiến nhất có thể đạt được hiện nay.
Robot giám sát trạm biến áp là thiết bị lý tưởng để khảo sát các khu vực nguy hiểm
cho con người như trạm biến áp, hầm mỏ và nhưng địa điểm chứa vật liệu nguy hiểm. Thiết
bị này vơ cùng thích hợp cho việc giám sát một khu vực để đánh giá những thay đổi của
môi trường theo thời gian.

Hình 1: Mơ tả tổng quan Robot

Thơng tin cơ bản của hệ thống:
9


Đồ án liên mơn 4
•

•

Nhóm 1

GVHD: Nguyễn Hồng Mai

Thân Robot

• Kích thước: 120x50x180 cm
•

Khối lượng: 50kg

•

Số bánh: 3

Hệ thống cảm biến
• Bộ phận xoay và nghiêng chắc chắn (PTU) được thiết kế cho các ứng
•

dụng có độ rung cao.
Hệ thống camera quan sát, camera hồng ngoại phát hiện các điểm

•

nóng và dị thường nhiệt.
Hệ thống đo tốc độ bằng encoder

•

Hệ thống điều hướng
• Hệ thống điều hướng bằng la bàn điện tử
• Khả năng tránh vật cản.
• Cho phép điều chỉnh bằng tay.

•

Trạm sạc
•

Nguồn điện: 220VAC, 50Hz.

•

Rơ bơt sau khi hoàn thành giám sát hoặc pin yếu sẽ tự động quay về
trạm sạc.

10


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

CHƯƠNG 2: MƠ HÌNH ĐỘNG HỌC, ĐỘNG LỰC HỌC CỦA ĐỐI
TƯỢNG

Động học là nghiên cứu cơ bản nhất để tìm hiểu quá trình hoạt động của một hệ
thống cơ khí, trong lĩnh vực mobile robot, chúng ta cần phải tìm hiểu đặc tính cơ của robot
để thiết kế sao cho phù hợp với các nhiệm vụ đặt ra, đồng thời việc tìm hiểu đặc tính cơ
cịn giúp ta xác định được phương pháp thiết kế phần mềm điều khiển sao cho phù hợp đối
với từng phần cứng của robot.
Trong lĩnh vực robot, mobile robot khơng phải là hệ thống cơ khí q phức tạp.
Như ta đã biết, tay máy đã và đang là lĩnh vực được chú trọng nghiên cứu trong vòng hơn
30 năm trở lại đây. Ở một vài khía cạnh nào đó, tay máy phức tạp hơn nhiều so với mobile

robot thế hệ trước đây, ví dụ: một robot hàn thiết kế theo tiêu chuẩn thường có 5 hoặc nhiều
hơn 5 khớp, trong khi các mobile robot trước đây thường chỉ là thiết bị truyền động kiểu
vi sai đơn giản.
Giữa tay máy và mobile robot có khá nhiều điểm tương đồng. Ví dụ, đối với tay
máy, khơng gian làm việc là vấn đề được quan tâm rất nhiều, nó cho phép xác định phạm
vi các vị trí khả thi của tay máy. Khơng gian làm việc của mobile robot có mức độ quan
trọng khơng kém, nó cho phép xác định phạm vi các tư thế khả thi mà mobile robot có thể
có trong mơi trường hoạt động. Tính dễ điều khiển của tay máy được định nghĩa là khả
năng điều khiển các động cơ để tay máy có thể di chuyển từ vị trí này đến vị trí kia trong
khơng gian làm việc.Mobile robot cũng bị giới hạn bởi các nguyên lý về động lực học, ví
dụ, giống như ở ô tô, khi chuyển động với tốc độ cao, nếu trọng tâm của mobile robot cao
nó sẽ là nguyên nhân giới hạn bán kính xoay thực tế. Tuy nhiên, sự khác biệt chính giữa
mobile robot và tay máy là thách thức đáng kể trong kỹ thuật ước lượng vị trí. Tay máy
thường có một đầu được gắn cố định, việc xác định vị trí của đầu hoạt động kia hồn toàn
đơn giản, vấn đề là ta phải hiểu được các nguyên lý động học của tay máy và xác định được
vị trí của các khớp trung gian. Chính vì thế, ta có thể xác định được vị trí của tay máy nhờ
dữ liệu thu được từ cảm biến. Trong khi đó, mobile robot lại là một thiết bị tự động độc
lập, nó hồn tồn có thể tự do di chuyển trong mơi trường hoạt động. Khơng có phương
pháp nào có thể giúp ta đo trực tiếp vị trí tức thời của mobile robot. Thay vào đó, thơng
thường để xác định vị trí của mobile robot, người ta phải tích hợp chuyển động của robot
theo thời gian. Ngoài ra, sự trượt của bánh xe còn là nguyên nhân khiến cho quá trình đánh
11


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

giá, ước lượng chuyển động của robot giảm bớt độ chính xác. Rõ ràng việc đo chính xác
vị trí mobile robot vẫn là lĩnh vực đầy thách thức và khá rộng và đầy thách thức cho chúng
ta,
Nhưng chúng lại có một điểm chung đó là nhiệm vụ điều khiển robot chính là điều
khiển các động cơ. Trong đề tài này sẽ tập trung vào việc nghiên cứu điều khiển mơ hình
robot tự hành cơ bản bao gồm 3 bánh. Trong đó, hai bánh sau là hai bánh chủ động được
điều khiển bởi hai động cơ độc lập, bánh phía trước là bánh lái. Coi bánh xe di chuyển trên
mặt phẳng là lăn không trượt, tốc độ của robot là tốc độ của trung điểm khoảng cách giữa
hai bánh sau. Khối lượng và qn tính của bánh xe coi là khơng đáng kể, có thể bỏ qua.
2.1 Mơ hình động học của Mobile Robot
2.1.1 Mơ hình bánh xe robot
Loại mobile robot được sử dụng trong đề tài này là Differential Drive Robot (robot
gồm 2 bánh chủ động và 1 bánh bị động).
Mơ hình bánh xe robot được lý tưởng hóa như hình 2.1. Bánh xe quay quanh trục
của nó (trục Y). Bánh xe chuyển động theo phương X (trục X ). Khi chuyển động ở tốc độ
thấp, có thể bỏ qua ảnh hưởng của sự trượt của bánh xe so với mặt đường.
Các thơng số của bánh xe :
r= bán kính bánh xe
v= vận tốc dài của bánh
w= vận tốc góc của bánh xe

Hình 2. 1: Mơ hình bánh xe đã được lý tưởng hóa.

12


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

Động học là bài tốn về chuyển động mà khơng xét tới sự tác động của lực tới
chuyển động của robot, nó bao gồm các yếu tố hình học xác định vị trí của robot. Nó bao
thể hiện mối quan hệ giữa các thông số điều khiển và các thông số trạng thái của hệ thống
trong khơng gian
2.1.2 Phương trình động học của Mobile Robot
Mơ hình robot được thể hiện ở hình dưới đây:

Hình 2. 2: Hệ quy chiếu của mobile robot
Trước tiên để xác định vị trí của robot trong mặt phẳng, ta xây dựng mối quan hệ
giữa tọa độ tham chiếu toàn cục của mặt phẳng và hệ tọa độ tham chiếu cục bộ của robot
như hinh 2.2. Các trục x, y xác định tọa độ của điểm bất kì trong hệ tọa độ tồn cục có gốc
O (xOy). Điểm P coi là tâm dịch chuyển của robot, nó được dùng để xác định vị trí của
robot. Hệ tọa độ xmPym là hệ tọa độ tham chiếu cục bộ của robot, gắn liền với robot. Như
vậy vị trí điểm P trong hệ tọa độ tham chiếu toàn cục được xác định bởi tọa độ x, y và góc
lệch θ giữa hai hệ tọa độ toàn cục và cục bộ. Các thơng số
hình học của robot bao gồm :
vr(t) - vận tốc dài của bánh phải.
vl(t) - vận tốc dài của bánh trái.
wr(t) - vận tốc góc của bánh phải.
wl(t) - vận tốc góc của bánh trái.
R - bán kính mỗi bánh của mobile robot.
13


Đồ án liên mơn 4

GVHD: Nguyễn Hồng Mai

Nhóm 1

L - khoảng cách giữa 2 bánh.
ICC - tâm vận tốc tức thời
Ta có phương trình tốn học trong khơng gian trạng thái như sau:
vx (t) = x(t) = v(t) cos

 vx (t) = y(t) = v(t)sin 

 (t) = w(t)


Trong đó vận tốc dài của robot được tính:
1

v(t) = w(t)R = (v (t) + v (t))
2

r

l

Và:

vr (t) =

2v(t) + w(t)L
2R

vl (t) =

2v(t) − w(t)L
2R

w(t)=

R

(v (t) − v (t))

L

r

l

Viết lại dưới dạng ma trận:
x

R
cos
 2
R

R

cos

2
R

 y  =  sin
sin
  2
2
 
R R

−L L





w


 wr 
l





Các thông số kĩ thuật của Mobile Robot được sử dụng trong mơ hình là: R = 0.5
(m); L = 0.15 (m).
Đây là phương trình mà sẽ được sử dụng để xây dựng mơ hình mobile robot trên
phần mềm mơ phỏng Matlab-Simulink. Trên đó mobile robot sẽ được điều khiển bởi một
bộ điều khiển sẽ được tiếp tục xây dựng trong phần sau.
14

Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

2.2 Mơ hình động học, động lực học của cánh tay robot
2.2.1 Đặc điểm kĩ thuật của cánh tay robot
Cánh tay robot 3 bậc tự do được mơ tả như trong hình, bao gồm 4 thanh (bao gồm
base) và 3 khớp với các thơng số kĩ thuật như sau:

Hình 2. 3: Tay máy robot 3 bậc tự do
L12 = L1 + L2 = 0.4 (m) là chiều dài thanh thứ nhất.
L3 = 0.4 (m) là chiều dài thanh thứ hai.
L4 = 0.2 (m) là chiều dài thanh thứ ba.
θ1 là góc quay của khớp thứ nhất.
θ2 là góc quay của khớp thứ hai.
θ3 là góc quay của khớp thứ ba.
m1 = 1.5 (kg) là khối lượng của thanh thứ nhất.
m2 = 2 (kg) là khối lượng của thanh thứ hai.
M3 = 1 (kg) là khối lượng của thanh thứ ba.

2.2.2 Các phương trình động học thuận
Động học thuận của cánh tay robot được xác định bởi một nhóm các tham số được
gọi là các tham số Denavit - Hartenberg (DH), các tham số này được sử dụng để thành lập
15

Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

các ma trận biến đổi thuần nhất giữa các hệ tọa độ khác nhau được gắn lên cấu trúc của tay
máy robot. Các tham số DH của robot 3 bậc tự do như trên hình vẽ được thành lập như sau:
Link

ai-1

αi-1

di

θi

1

0

0

L12

θ1

2

0

π/2

0

θ2

3

L3

0

0

θ3

4

L4

0

0

0

Ta các định được các ma trận biến đổi thuần nhất cho tay máy 3-DOF:
cos 1


sin 1
0T = 
1
 0

 0

−sin  1 0

cos 2

0
1

T
=
2
sin 2

 0

−sin 2 0 0
0
−1 0

cos 3

sin 3
2T = 

3
 0

−sin  3 0 L3 


1
0
3
T =
4
0

0


0

cos 1
0

0

0 0 
1 L 
12

0

0

cos 2

0

0

0

cos 3
0
0

0 0 L4 

1 0 0
0 1 0

0 0 1

0
1
0

(1)



1



0

1

0 
0 
1

(2)

(3)



(4)



Từ (1), (2), (3), (4) ta có phương trình biến đổi thuần nhất 04T :

16


Đồ án liên mơn 4

GVHD: Nguyễn Hồng Mai

Nhóm 1

cos 1.cos( 2 + 3 )
sin .cos( +  )
0

T
=
1
2
3
4
 sin( +  )
2
3

0


L3 . cos 1. cos  2 + L4 . cos 1. cos( 2 + 3)
L .sin .cos + L .sin .cos( +  )

− cos 1.sin( 2 + 3 ) sin 1
−sin .sin( +  ) −cos
1

2

cos( +  )
2

3

1

3

0

3

1

12

0

2

4

L + L .sin
3

0

1

2

+ L .sin( +  )

2

4

2

3



3

1

Vậy tọa độ vị trí của end-effector của tay máy là:
P
 x = L3.cos1.cos2 + L4.cos1.cos(2 +3)
P = L .sin .cos + L .sin .cos( +  )
 y
3
1
2
4
1
2
3
P = L + L .sin + L .sin( +  )


z

12

3

2

4

2

3

Và ma trận hướng của tay máy được xác định là ma trận 3x3 bao gồm 3 hàng và 3
cột đầu tiên của ma trận biến đổi thuần nhất 04T .
2.2.3 Các phương trình động học ngược
Bài tốn động học ngược được phát biểu là: Cho trước vị trí và hướng của endeffector của robot, tính tốn tất cả các tập hợp giá trị của các biến khớp có thể được sử
dụng để tạo ra vị trí và hướng này của end-effector. Đối với tay máy robot nối tiếp, bài toán
động học thuận thường dễ giải quyết trong khi bài toán động học ngược là một bài tốn
khó và tồn tại nhiều nghiệm.
Các phương trình tốn học được sử dụng để giải bài tốn động học ngược có thể
được xác định bằng phương pháp đại số hoặc phương pháp hình học. Trong phạm vi đề tài,
chúng em sử dụng phương pháp đại số.
Từ tọa độ vị trí của end-effector của tay máy, dễ thấy rằng:

−Py .cos1 + Px .sin1 = 0


1 = A tan 2(Py , Px) hoặc 1 = A tan 2(−Py , −Px )

17






Đồ án liên mơn 4

GVHD: Nguyễn Hồng Mai

Nhóm 1

Tiếp theo, giả sử:
 Px 1if cos  0
cos 

 = P 1
 y
if sin1  0
 cos 1
Ta có:
L3.cos2 + L4.cos(2 +3 ) = 
L3.sin2 + L4.sin(2 +3 ) = Pz − L12

Bình phương 2 vế của 2 phương trình rồi cộng lại ta được:
L 2 + L 2 + 2L L cos =  2 + ( P − L
3



4

cos =
3

3 4

1
2L3L4

3

z

)

12

2 + ( P − L ) − L 2 − L 2 
z
12
3
4



sin3 = 1− cos2 3



3 = Atan 2(sin3 , cos3 )

Mặt khác:
 L3 cos2 + L4 cos(2 +  3 ) = 
L sin + L sin( +  ) = P − L
z
12
 3
2
4
2
3

Đặt


4 cos3
a =b L=3 L+ L
sin

4
3

c =


 d = Pz − L12

18

Đồ án liên mơn 4





Nhóm 1

GVHD: Nguyễn Hồng Mai

a cos2 − b sin2 = c
.

a sin + b cos = P − L

2
2
z
12

.

2 = A tan 2 (ad − bc, ac + bd )

2.2.4 Các phương trình động lực học của tay máy
Mơ hình động lực học của tay máy liên quan đến chuyển động và các lực liên quan
đến cánh tay robot và thiết lập mối quan hệ toán học giữa vị trí của các biến khớp rơ bốt
và các tham số kích thước của rơ bốt. Có hai phương pháp để thực hiện các phương trình

động lực học của tay máy robot: Phương pháp Euler - Lagrange và phương pháp Newton
- Euler.
Trong đề tài này, chúng em sử dụng phương pháp Euler - Lagrange dựa vào việc
tính tốn các thông số về tổng động năng và thế năng để xác định tham số Lagrangian ℒ
trên toàn bộ hệ thống, rồi từ đó tính tốn được lực hoặc momen xoắn cần tác dụng lên mỗi
khớp. Với Lagrangian ℒ, ta có thể giải phương trình Euler-Lagrange dựa vào đạo hàm
riêng của các thuộc tính động năng và thế năng của hệ cơ học để tính phương trình chuyển
động, cụ thể được xác định như sau:

=

d

 L  L
−


dt   

Trong đó τ là ngoại lực hoặc momen xoắn tổng quát áp dụng lên tay máy và L là
phương trình Lagrange của chuyển động, được cho bởi phương trình:
L(, ) = K(, ) −U ( )

Để giải phương trình Lagrange, ta cần tính tốn động năng K và thế năng U, trong
đề tài, cánh tay robot gồm 3 bậc tự do với các thanh dài hình trụ, khối tâm nằm ở trọng tâm
các thanh, vì vậy momen qn tính của từng thanh là:
I =

1

mL2

1

12 1 12
1
I = m L2
2
12 1 3
1
I =
mL2
3
12 1 4

Từ đây ta tính được động năng và thế năng của hệ:
19


Đồ án liên mơn 4

GVHD: Nguyễn Hồng Mai

Nhóm 1

(

1
.m .v .T
 2 i Ci

i=1 

)

3
K  , = 

3

Ci

1

v + . i w T . Ci I . i w
i
i
i 
2


(

U ( ) =  −mi . 0 gT .0PCi +U refi

)

i=1

Trong đó:
mi là khối lượng ở khâu thứ i

VCvà i w i là vận tốc dài tại trọng tâm của khâu thứ i và vận tốc quay ở khâu thứ i
i

Ci

I i là momen quán tính tại trọng tâm của khấu thứ i

0

gT là gia tốc trọng trường

0

PCi là vị trí trọng tâm ở khâu thứ i chiếu lên hệ tọa độ (0)

Uref là mốc thế năng gốc
i

Sử dụng phương pháp Euler–Lagrange cho trường hợp tất cả các khớp là khớp xoay:
• Vận tốc góc của khâu thứ i+1 ở hệ trục (i+1):
i+1 i
i+1
w = R. w +  . i+1 Z
i

i+1

i

i+1

i+1

• Vận tốc dài của điểm gốc ở hệ trục (i+1):
v i+1 =

i+1

i+ 1

R.( iv + i w  iP )

i

i

i

i+1

Trong đó:
i+1
i

i

R là ma trận xoay của hệ trục (i) lên (i+1)

wi là vận tốc góc của khâu thứ i chiếu lên chính nó.
w i+1 là vận tốc góc của khâu thứ i+1 chiếu lên chính nó.

i+1

i+1 là vận tốc góc tại khớp thứ i+1
i+1

i+1

Z i+1 là vecto của trục z

vi+1 là vận tốc dài ở điểm gốc của (i+1)

i

vi là vận tốc dài ở điểm gốc của (i)

i

Pi+1 là vị trí của điểm gốc (i+1) chiếu lên (i)

20


Đồ án liên mơn 4

GVHD: Nguyễn Hồng Mai

Nhóm 1

Áp dụng vào bài tốn:

• Tọa độ khối tâm tại các khâu:
-

1
Khâu 1: PC = 0 0 −L12 

-

Khâu 2: 2 PC = L3 / 2 0 0 

-

Khâu 3: 3 PC = L4 / 2 0 0 

T

1

T

2

T

3

• Tại khâu 1:
-

Vị trí trọng tâm:

0

P = 0T.1P
C1

-

Vận tốc góc:
1

0

0

1

. Z1

1

Vận tốc dài tại gốc tọa độ:
1

v = 1 R( 0v + 0 w  0P )
1

-

C1

w = 1 R.0 w + 
1

-

1

0

0

0

1

Vận tốc tại trọng tâm:
V = 0 R(1v + 1 w  1P )

0

C1

1

1

1

C1

• Tại khâu 2:
-

Vị trí trọng tâm:
0

P = 0T . 2 P
C2

-

Vận tốc góc:
2

C2

w = 2 R. 1 w + 
2

-

2

1

1

2

2

.Z2

Vận tốc dài tại gốc tọa độ:
2

v = 2 R(1v + 1w  1P )
2

1

1

1

2

21


Đồ án liên mơn 4

GVHD: Nguyễn Hồng Mai

Nhóm 1

-

Vận tốc tại trọng tâm:
V = 0 R( 2v + 2 w  2 P )

0

C2

2

2

2

0

P = 0 T.3P

C2

• Tại khâu 3:
-

Vị trí trọng tâm:
C3

-

Vận tốc góc:
3

2

2

3

3

.Z3

Vận tốc dài tại gốc tọa độ:
3

v = 3 R( 2v + 2 w  2P )
3

-

C3

w = 3 R.2 w +
3

-

3

2

2

2

3

Vận tốc tại trọng tâm:
V = 0 R( 3v + 3 w  3P )

0

C3

3

3

3

C3

Sau khi đã tính được τ, chuyển phương trình vi phân của robot về dạng tổng quát:

(

)

M ( ) + C  ,   + G ( ) = 

Trong đó:

M ( ) là ma trận qn tính

(

C  ,

) là ma trận lực Coriolis và lực li tâm

G ( ) là vecto trọng trường

 =  
1


2

3

T ,  = 


1



  và  = 
T

2

3





1



 
2

3

T



Cuối cùng, ta viết chương trình tính tốn các thơng số như hình dưới:

22


Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

23

Đồ án liên mơn 4

Nhóm 1

GVHD: Nguyễn Hồng Mai

24