ĐẠ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

1


Đà Nẵng, 12/2021

2


DANH MỤC HÌNH

3


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.

4


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 yê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
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.

5


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ị.
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.

6


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:
• 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
7


•

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 hồn thành giám sát hoặc pin yếu sẽ tự động quay về
trạm sạc.


8


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 q 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í quá 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
hoà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. Ngồi ra, sự trượt của bánh xe còn là nguyên nhân
khiến cho quá trình đánh 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,
9


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à quán 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.
Độ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:

10


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 độ tồ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.
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:

11


1
2

v(t ) = w(t ) R = (vr (t ) + vl (t ))
Và:

vr (t ) =

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

vl (t ) =

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

w(t)=

R

(vr (t ) − vl (t ))
L

Viết lại dưới dạng ma trận:
R
 2 cos θ
 x& 
 y& =  R sin θ
  2
θ& 
R

−
L


R

cos θ 
2

w 
R
sin θ   l 
  wr 
2

R



L

Các thông số kĩ thuật của Mobile Robot được sử dụng trong mơ hình là: R = 0.1
(m); L = 0.5 (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.
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:

12


Hình 2.3: Mơ hình tay máy robot

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 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

13


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
Bảng 1: Bảng tham số D-H

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
 sin θ
cos θ 1
1
0

1T =
 0
0

0
 0

cos θ 2
 0
1

2T =
 sin θ 2

 0

0 0
0 0 
1 L12 

0 1

− sin θ 2 0

0
−1
cos θ 2 0
0
0

0
0
0

1

14

(1)

(2)


cos θ 3
 sin θ
3
2

T
=
3
 0

 0


1
0
3

4T =
0

0

0
1
0
0

− sin θ 3
cos θ 3
0
0

0 L3 
0 0 
1 0

0 1

(3)

0 L4 
0 0 

1 0

0 1

(4)

Từ (1), (2), (3), (4) ta có phương trình biến đổi thuần nhất
cos θ1.cos(θ 2 + θ 3 ) − cos θ1.sin(θ 2 + θ 3 ) sin θ1

0
 sin θ1.cos(θ 2 + θ 3 ) − sin θ1.sin(θ 2 + θ 3 ) − cosθ1
4T =
 sin(θ 2 + θ 3 )
cos(θ 2 + θ 3 )
0

0
0
0


T

0
4

:

L3 . cosθ1. cos θ 2 + L4 . cos θ1. cos(θ 2 + θ3 ) 
L3 .sin θ1. cosθ 2 + L4 .sin θ1. cos(θ 2 + θ3 ) 


L12 + L3 .sin θ 2 + L4 .sin(θ 2 + θ 3 )

1


Vậy tọa độ vị trí của end-effector của tay máy là:

 Px = L3 .cos θ1.cos θ 2 + L4 .cos θ1.cos(θ 2 + θ 3 )

 Py = L3 .sin θ1.cos θ 2 + L4 .sin θ1.cos(θ 2 + θ 3 )

Pz = L12 + L3 .sin θ 2 + L4 .sin(θ 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
0
4T
cột đầu tiên của ma trận biến đổi thuần nhất
.
15


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 tố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 )

Tiếp theo, giả sử:
 Px
 cos θ if cosθ1 ≠ 0

1
α =
 Py
if sinθ1 ≠ 0
 cos θ1

16


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:

L32 + L4 2 + 2 L3 L4 cos θ 3 = α 2 + ( Pz − L12 )
cos θ3 =


1
α 2 + ( Pz − L12 ) − L32 − L4 2 
2 L3 L4

sin θ3 = 1 − cos2 θ 3




θ3 = A tan 2(sin θ 3 , cos θ3 )

Mặt khác:

 L3 cos θ 2 + L4 cos(θ 2 + θ3 ) = α

 L3 sin θ 2 + L4 sin(θ 2 + θ3 ) = Pz − L12

17


Đặt




.

 a = L3 + L4 cos θ3
 b = L sin θ

4
3

c =α

 d = Pz − L12

 a cos θ 2 − b sin θ 2 = c

a sin θ 2 + b cos θ 2 = Pz − L12

.

θ 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ệ tố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:
18


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 quán tính của từng thanh là:
1
m1 L12 2
12
1
I 2 = m1 L3 2
12
1
I 3 = m1 L4 2
12

I1 =

Từ đây ta tính được động năng và thế năng của hệ:
3
1
1

K θ ,θ& = ∑  .mi .vCi .T vCi + . i w iT . Ci I i . i w i ÷
2

i =1  2

(

)

3

(

U ( θ ) = ∑ −mi . 0 g T . 0 PCi + U refi
i =1

)

Trong đó:

mi là khối lượng ở khâu thứ i
VCi
Ci


0

0

i

và

Ii

wi

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

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

gT

PCi

U refi

là gia tốc trọng trường
là vị trí trọng tâm ở khâu thứ i chiếu lên hệ tọa độ (0)
là mốc thế năng gốc
19


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

•

w i +1 =

i +1
i

i +1
R. i w i + θ&
Z¶i +1
i +1 .

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

vi +1 =

i +1
i

R.( i vi + i w i × i Pi +1 )

Trong đó:


i +1
i

i

R

wi

i +1

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

w i +1

θ&
i +1
i +1

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

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

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

Zµ i +1

là vecto của trục z


i +1

vi +1

i

i

vi
Pi +1

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

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


Á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 1 = [ 0 0 − L12 ]

2

PC 2 = [ L3 / 2 0 0]

T

3

PC 3 = [ L4 / 2 0 0]

T

Khâu 2:

Khâu 3:

T

Tại khâu 1:

0


-

Vị trí trọng tâm:

1

-

Vận tốc góc:

PC1 = 01T .1PC1

1
µ
w1 = 01 R . 0 w 0 + θ&
1. Z 1

v1 = 01 R( 0 v0 + 0 w 0 × 0 P1 )

1

-

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

21


VC1 = 01 R( 1v1 + 1 w1 × 1PC1 )


0

-

•

Vận tốc tại trọng tâm:

Tại khâu 2:

0

-

Vị trí trọng tâm:

2

-

Vận tốc góc:

PC2 = 02T . 2 PC2

2
µ
w 2 = 21 R .1 w1 + θ&
2. Z 2

2


-

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

v2 = 21 R( 1v1 + 1 w1 × 1P2 )

VC2 = 20 R( 2 v2 + 2 w 2 ì 2 PC2 )

0

-

ã

Vn tc ti trng tõm:

Ti khâu 3:

-

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

-

Vận tốc góc:
22

PC3 = 03T . 3 PC3



3

-

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

-

3
µ
w 3 = 23 R . 2 w 2 + θ&
.
3 Z3

v3 = 23 R( 2 v2 + 2 w 2 × 2 P3 )

Vận tốc tại trọng tâm:
VC3 = 03 R( 3v3 + 3 w 3 × 3 PC3 )

0

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

(

)


&+ C θ ,θ&θ&+ G ( θ ) = τ
M ( θ ) θ&

Trong đó:

M (θ )

(

là ma trận quán tính

)

C θ ,θ&

G(θ )

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

là vecto trọng trường

θ = [ θ1 θ 2 θ3 ]

T

,

T

θ&= θ&

θ&
θ&
1
2
3

và

& θ&
& θ&
&T
θ = θ&
1
2
3

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:

23


24


25