a) Cơ sở lý thuyết
Định nghĩa về đường cong Spline :
Cho hàm số f xác định trên [a,b] và (n+1) điểm nút a = x 0 < x1 <...< xn = b;
S là hàm nội suy spline bậc 3 của hàm số f thỏa mãn những điều kiện sau:
a) S(x) là đa thức bậc 3, kí hiệu Sj(x) là hàm S(x) trên đoạn [xj,xj+1] với j = 0,1,..,n-1
b) Sj(xj) = f(xj) và Sj(xj+1 ) = f(xj+1 ) với j = 0,1,..,n-1
c) Sj+1 (xj+1 )
d) S’
e) S’’
Các điều kiện cần thiết
a) Điều kiện đảm bảo cho sự liên tục và trơn của đường cong
Spline
+ Sj+1(xj+1 ) = Sj(xj+1 )
+ S’j+1(xj+1 ) = S’j(xj+1)
+ S’’j+1 (xj+1 ) = S’’j(xj+1 )
b) Điều kiện biên
với j = 0,1,...,n-2 với j = 0,1,...,n-2 với j = 0,1,...,n-2
Thông thường đường cong Spline cần thỏa mãn điều kiện biên tự nhiên
S’’ (x0) = S’’ (xn) = 0 Các mối liên hệ thiết lập được:
Hàm số f là đa thức bậc 3 có dạng
f(x) = ai(x – xi)3 + bi(x – xi)2 + ci(x – xi) + di Trong đó: ai , bi , ci , di là hệ số của đa thức nội suy
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
ĩa
Theo định ngh
hệ số ai , b , a
Trong đó: hi = xi +1 – xi ; yi = f(xi )
Thiết lập mối liên hệ giữa h,y và D, kết hợp với điều kiện biên tự nhiên ta sẽ lập được hệ phương trình tuyến tính dạng ma trận để giải ra Di từ đó xác định được hệ số của đa thức nội suy Sj.
Hình 3-4: Xấp xỉ hoá đa thức nội suy trên mỗi đoạn giữa 2 điểm nút
Tính di: Từ phương trình (4.7) ta có : fi (xi ) yi Hay a i(xi xi )3 bi ( xi xi)2 c i( xi xi ) di yi di yi Tính các hệ số còn lại: Ta có : f ( x) 3a i fi (x) 6a Đặt Mi fi ( x)
LUẬN VĂN THẠC S Ĩ Từ phương trình (4.9) ta có:
Hay:
i
Cũng từ phương trình (4.9) suy ra: f ( x Và f ( x ) 6a i i 1 Theo phương trình (4.3) thì: f Hay: 6aih 2b i a 2b i 1 2b i M i i Từ phương trình (4.1) : f ( x Mặt khác : fi ( xi 1 ) di 1 Và f ( x ) a i i 1 a d i 1 a h i i c di 1 d i i h i c yi 1 y i i hi
Các hệ số của đa thức nội suy được tính theo công thức sau :
a i b Mi i c yi 1 y i ( Mi 1 2 M i )h i yi d i
(4.12)
(4.13)
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
LUẬN VĂN THẠC S Ĩ Từ phương trình (4.2) ta có : Với i 0,2,...,n 2 c c i 1 i 1
Nên c 3a i 1 y y i 2 i 1 h i 1 3 M i 1 6hi 3 M i 1 6 M i 2 M i)hi y y i 1 i hi yi Kết hợp với các điề Khi ệ số đượ bằng H. M Với ma trận H( H = ⎣ iện biên: M0 = Mn = 0 (k = 2,..,n-1 ) là các hệ số của M giải phương tr -2)), ve 2(ℎ ℎ … 0 0 … 0 …2(ℎ ℎ
( (
⎢
(
⎣
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
Hình 3-5: Sơ đồ thuật toán nội suy Spline
Kết luận:
Trong chương này, tác giả đã xây dựng hệ toạ độ cho toàn hệ Robot – Đồ gá – Thiết bị di trượt, lược giản phương pháp biến đổi thuần nhất của toàn hệ để đưa việc khảo sát bài toán hệ thống Robot – Đồ gá – Thiết bị di trượt về bài toán
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
LUẬN VĂN THẠC S Ĩ
khảo sát động học của Robot 6 bậc tự do chuỗi động hở có quỹ đạo và hướng khâu chấp hành cuối. Từ kết quả xác lập phương pháp giải bài toán động học ngược, tác giả đưa vào giải thuật nội để từ đó làm cơ sở nội suy quỹ đạo Robot trước khi thực hiện giải bài toán động học ngược.
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
CHƯƠNG 4. CHƯƠNG TR ÌNH TÍNH ĐỘNG HỌC VÀ MÔ PHỎNG Chương trình tính toán động học được lập trình bằng ngôn ngữ C# trong bộ Visual Studio 2010, trên nền kiến trúc dotNET 4.5, quy mô của chương trình là 1758 dòng lệnh. Chương trình mô phỏng được lập trình bằng ngôn ngữ C#, kết hợp thư viện đồ hoạ XNA Studio 4.0 trên nền kiến trúc dotNet, quy mô của chương trình là 2040 dòng lệnh.
Trong chương trình tác giả đã sử dụng thuật giải Pieper kết hợp với việc xử lý tính toán trực tiếp trên các thanh ghi của máy tính, giúp cho quá trình giải bài toán động học có tốc độ rất cao. Cụ thể là giải động học ngược với 801 điểm nội suy mất 0.9 giây. Ngoài ra, việc sử dụng các điều kiện chặn biến khớp và phương pháp khử suy biến đã giúp cho kết quả giải từ chương trình là hoàn toàn chính xác.
Chương trình mô phỏng sử dụng phương pháp lưu bắt điểm tại từng vị trí khi dắt Robot qua nhằm tăng tính tương tác thực tế tương tự như dạy Robot ở ngoài thế giới vật lý.
Hình 4-1: Giải bài toán động học thuận
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
LUẬN VĂN THẠC S Ĩ
Hình 4-2: Giải bài toán động học ngược tìm ra các biến khớp
Hình 4-3: Kiểm tra kết quả tính toán động học
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
Hình 4-4: Đối sánh kết quả tìm đư ợc
Bằng việc truyền tham chiếu kết quả tính toán động học từ chương trình tính toán sang chương trình mô phỏng để mô phỏng lại toàn bộ quá trình dạy học cũng như vận hành Robot. Chương trình đư ợc xây dựng phù hợp với quá trình vận hành Robot trong thực tế và mang giao diện Tiếng Việt nên hoàn toàn dễ sử dụng.
Trong chương trình mô phỏng, tác giả xây dựng mô hình dựa trên nguyên mẫu là Robot ABB IRB1520ID và đồ gá quay kết hợp thiết bị di trượt nhằm nâng cao tầm hoạt động của Robot. Mô hình vật mẫu là hệ ống nhiều đầu nối dạng hàn góc, biên dạng hàn là đường cong bậc 4 tại vị trí tiếp giáp giữa các ống vuông góc theo chiều trục.
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
LUẬN VĂN THẠC S Ĩ
Hình 4-5: Giao diện chương tr ình mô ph ỏng
Hình 4-6: Giao diện tính toán động học trước mô phỏng
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
Hình 4-7: Kết quả so sánh phương vị khi dạy với kết quả giải động học ngược
Kết luận:
Trong chương này, tác giả đã giới thiệu phần mềm tính toán và mô phỏng hệ thống Robot – Đồ gá – Thiết bị di trượt, bằng việc sử dụng thư viện đồ hoạ XNA kết hợp với chương trình tính toán, mô phỏng viết bằng ngôn nhữ C#, tác giả đưa người dụng hướng đến vận hành phần mềm do tác giả viết tương tự như cách vận hành Robot trong thế giới thực.
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
LUẬN VĂN THẠC S Ĩ KẾT LUẬN
- Trong luận văn tác giả đã đạt được những thành công nhất định. Cụ thể:
+ Tác giả đã nêu được tổng quan và ý nghĩa và các thông số cần thiết của
Robot hàn.
+ Tác giả đã nêu lên được những lý thuyết quan trọng, và cơ bản về
Robot để phục vụ cho quá trình tính toán các bước về sau.
+ Tác giả đã xây d ựng thành công cấu hình Robot, gắn các hệ toạ độ
trung gian và thiết lập bảng Denavit – Hartenberg và tìm ra phương trình đ ộng họccủa Robot. Điểm đạt được có ý nghĩa l ớn nhất đó là xây dựng thuật giải bài toán động học thuận và động học ngược cho Robot có cấu hình mà tác giả xây dựng.
+ Tác giả đã nêu được nguyên lý và giải thuật của thuật toán nội suy áp
dụng cho việc xác lập quỹ đạo khâu chấp hành cuối của Robot.
+ Tác giả đã hoàn thiện được phần mềm tính toán động học, giải bài toán
động học ngược Robot và phần mềm mô phỏng quá trình hoạt động của Robot kết hợp đồ gá quay và thiết bị di trượt.
- Luận văn đã xây dựng thành công thuật giải bài toán động học Robot 6 bậc
tự do chuỗi động hở và hoàn thiện mô phỏng quá trình hoạt động và điều khiển Robot kết hợp đồ gá quay và thiết bị di trượt, được ứng dụng đặc biệt trong Robot hàn hồ quang.
- Kết quả xuất ra của phần mềm là sáu góc quay tại từng khớp của Robot,
góc quay của đồ gá và vị trí của thiết bị di trượt, có độ chính xác cao, thuận lợi cho việc điều khiển Robot thực tế.
- Luận văn có ý nghĩa quan trọng trong việc xây dựng nguyên lý điều khiển
chính xác giúp Robot bám theo biên dạng mép hàn.
- Với sự ưu việt của phần mềm là tốc độ giải bài toán động học rất nhanh với thời gian tính toán là 0.9 giây cho gần 1000 điểm trên quỹ đạo. Tác giả tiến tới
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
tối ưu thuật giải và sử dụng máy tính chuyên dụng để xây dựng mô hình và điều khiển trong thực tế sản xuất.
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
LUẬN VĂN THẠC S Ĩ TÀI LIỆU THAM KHẢO
[1] Nguyễn Văn Khang - Cơ sở cơ học kỹ thuật– NXB Đại học Quốc gia
HN - 2005
[2] Bộ môn Công nghệ Chế tạo máy - Cơ sở công nghệ chế tạo máy -
NXB Khoa học kỹ thuật - 2008
[3] TS. Phạm Đăng Phước - Giáo trình Robot công nghiệp - NXBĐHBK
Đà Nẵng
[4] Catalogue phụ kiện: https://sdp-si.com/
[5] Catalogue robot:http://www.daihen-usa.com/roboticsystems.php
[6] John J.Craig - Introduction to RoboticsMechanic and Control
ISBN:0-13-123629-6
[7] Wesley L.Stone - Robotics and Automation Handbook
Western Carolina University - 2003
[8] Prof. Oussama Khatib - Lectures Introduction to Robotics
Standford University - 2007
[9] John Sharp - MS Visual C# Step by Step 2008
Microsoft Corporation - 2008
[10] Sean James - 3D Graphics with XNA Game Studio
4.0 ISBN:978-1-849690-04-1
[11] Department of Mathematics – Numerical methods –HongKong
Baptist University.
[12] Prof. John H. Mathews - Cubic Splines Module - California State
University Fullerton.
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
Phụ lục 1: Mã nguồn thuật giải động học ngược Robot public void pieper_solve()
{
npoint = (dgr_inv_Tdes.RowCount - 1) / 4; dgr_theta_inv.Rows.Clear();
for (i = 0; i < npoint; i++)
{
inv_theta[i] = new double[6];
//Tinh pw[i][]
pw[i] = new double[3];
for (j = 0; j < 3; j++)
{
pw[i][j] = Tdes[i][j][3] - d[5] * Tdes[i][j][2];//Bao gom pw[i][0]=pwx || pw[i][1]=pwy || pw[i][2]=pwz
}
//--- //TINH THETA1
//Theta 1 giới hạn bằng kết cấu cơ học của Robot, không sử dụng giới hạn toán học.
if ((pw[i][0] == 0) && (pw[i][1] == 1)) inv_theta[i][0] = M_PI /
2;
else {
if ((pw[i][0] == 0) && (pw[i][1] == -1)) inv_theta[i][0] = -
M_PI / 2;
else {
inv_theta[i][0] = Math.Atan2(pw[i][1], pw[i][0]); }
}
//--- // TINH THETA3
tmp_pw[i] = new double[3];
tmp_pw[i][0] = Math.Sqrt(pw[i][0] * pw[i][0] + pw[i][1] * pw[i][1]);// tinh pwx1 = sqrt(pwx^2 + pwy^2)
tmp_pw[i][2] = pw[i][2] - d[0];
// tinh pwz1 = pwz - d1
tmp_pw[i][0] -= a[1]; // tinh pwx1' = pwx1 - a1
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
LUẬN VĂN THẠC S Ĩ
//---Tinh cac bien trung gian
//Tinh K = [pwx1'^2 +pwz1'^2 -(a3^2 + d4^2 + a2^2)]/(2*a2)
K[i] = (tmp_pw[i][0] * tmp_pw[i][0] + tmp_pw[i][2] * tmp_pw[i][2]
-(a[3] * a[3] + d[3] * d[3] + a[2] * a[2])) / (2 * a[2]); double ro, phi;
ro = Math.Sqrt(d[3] * d[3] + a[3] * a[3]);
phi = Math.Atan2(a[3], d[3]);
inv_theta[i][2] = Math.Atan2((double)(K[i] / ro), Math.Sqrt(1 - ((double)(K[i] / ro)) * ((double)(K[i] / ro)))) - Math.Atan2(a[3], d[3]);
//--- //TINH THETA2
//---Khai bao cac bien trung gian
double ro2, mu;
ro2 = Math.Sqrt((a[3] * Math.Cos(inv_theta[i][2]) + d[3] *
Math.Sin(inv_theta[i][2]) + a[2]) * (a[3] * Math.Cos(inv_theta[i][2]) + d[3] *
Math.Sin(inv_theta[i][2]) + a[2]) + (a[3] * Math.Sin(inv_theta[i][2]) - d[3] *
Math.Cos(inv_theta[i][2])) * (a[3] * Math.Sin(inv_theta[i][2]) - d[3] *
Math.Cos(inv_theta[i][2])));
mu = Math.Atan2((a[3] * Math.Cos(inv_theta[i][2]) + d[3] *
Math.Sin(inv_theta[i][2]) + a[2]), (a[3] * Math.Sin(inv_theta[i][2]) - d[3] *
Math.Cos(inv_theta[i][2])));
Q[i] = pw[i][2] - d[0];
inv_theta[i][1] = mu - Math.Atan2(Math.Sqrt(1 - (Q[i] / ro2) * (Q[i] / ro2)), Q[i] / ro2);
//Hien thi ra DatagridView
dgr_theta_inv.Rows.Add();
for (j = 0; j < 3; j++)
{
dgr_theta_inv.Rows[i].Cells[j + 1].Value =
Math.Round(double.Parse(inv_theta[i][j].ToString()) * 180 / M_PI, 6);
}
dgr_theta_inv.Rows[i].Cells[0].Value = int.Parse((i + 1).ToString());
}
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
//---TINH THETA 4 - 5 - 6 --- ---//
//Su dung goc quay Euler de tinh góc 4,5,6
R03_Trans = R03_Transpose(inv_theta, npoint); //Tinh ma tran chuyen vi cua ma tran R03, tinh theo cac goc theta1, theta2, theta3
//Tach ma tran R06 tu ma tran T06
double[][][] R06 = new double[2000000][][];
for (i = 0; i < rows; i++)
{
R06[i] = new double[3][];
for (j = 0; j < 3; j++)
{
R06[i][j] = new double[3];
for (k = 0; k < 3; k++) { R06[i][j][k] = Tdes[i][j][k]; } } } //Tinh ma tran R36
MatrixProducts mp2 = new MatrixProducts();
R36 = mp2.MatrixProduct_Jagged(R03_Trans, R06, rows, 3);
for (i = 0; i < rows; i++)
{
double tmp_theta51, tmp_theta52, tmp_theta41, tmp_theta42, tmp_theta61, tmp_theta62;
if ((R36[i][0][2] == 0) && (R36[i][2][2] == 0))
{
inv_theta[i][4] = 0;
inv_theta[i][3] = theta_thuan[i][3];
inv_theta[i][5] = Math.Atan2(R36[i][2][0], R36[i][0][0]) - inv_theta[i][3];
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
LUẬN VĂN THẠC S Ĩ }
else {
tmp_theta41 = Math.Atan2(R36[i][2][2], R36[i][0][2]); //0 < theta5 < 180
tmp_theta42 = Math.Atan2(-R36[i][2][2], -R36[i][0][2]);// -180 < theta5 < 0
tmp_theta51 = Math.Atan2(Math.Sqrt(R36[i][0][2] * R36[i][0][2] + R36[i][2][2] * R36[i][2][2]), -R36[i][1][2]);// 0 < theta5 < 180
tmp_theta52 = Math.Atan2(-Math.Sqrt(R36[i][0][2] * R36[i][0][2] + R36[i][2][2] * R36[i][2][2]), R36[i][1][2]); // -180 < theta5 < 0
tmp_theta61 = inv_theta[i][5] = Math.Atan2(-R36[i][1][1], R36[i][1][0]); //0 < theta5 < 180
tmp_theta62 = inv_theta[i][5] = Math.Atan2(R36[i][1][1], - R36[i][1][0]); // -180 < theta5 < 0 inv_theta[i][3] = tmp_theta41; inv_theta[i][4] = tmp_theta51; inv_theta[i][5] = tmp_theta61; } for (j = 3; j < 6; j++) { dgr_theta_inv.Rows[i].Cells[j + 1].Value =
Math.Round(double.Parse(inv_theta[i][j].ToString()) * 180.0 / M_PI, 6);
} }
//Bo sung them 2 goc quay q_A, q_B
for (i = 0; i < dgr_ns_import.RowCount - 1; i++)
{ for (j = 6; j < 8; j++) { dgr_theta_inv.Rows[i].Cells[j + 1].Value = dgr_ns_import.Rows[i].Cells[j + 1].Value; } } }
HV: Nguyễn Tuấn Hưng - Mã HV: CB130326
Phụ lục 2: Mã nguồn chương tr ình n ạp mô hình và biến đổi thuần nhất hệ toạ độ Robot – Đồ gá và thiết bị di trượt
using System; using System.Collections.Generic; using System.Linq; using Microsoft.Xna.Framework; using Microsoft.Xna.Framework.Audio; using Microsoft.Xna.Framework.Content; using Microsoft.Xna.Framework.GamerServices; using Microsoft.Xna.Framework.Graphics; using Microsoft.Xna.Framework.Input; using Microsoft.Xna.Framework.Media; using System.IO; using System.Windows.Forms; using System.Drawing; //using RBSim; namespace RBSim { / <summary>
/ This is the main type for your game
/ </summary>
public partial class RBSim : Microsoft.Xna.Framework.Game
{ GraphicsDeviceManager graphics; SpriteBatch spriteBatch; RasterizerState rasterizerState; BasicEffect basicEffect; VertexPositionColor[] vertices; VertexPositionColor[] pointList; VertexDeclaration vertexDeclaration; VertexBuffer vertexBuffer; int points_no; short[] lineListIndices;
//Tao model cho Robot 6 DOF
Model model0, model1, model2, model3, model4, model5, model6, points, vat_han,
object2;
//Tao model cho do ga 2 DOF so1
Model jig_base, jig_comp_01, jig_comp_02; //Tao model cho do ga 2 DOF so 2
Model next_jig_base, next_jig_comp_01, next_jig_comp_02; //Tao model cho ray truot
Model rail;
Model[] red_points;
public Texture2D otherTexture;
MouseState lastMouseState;
//Tao ma tran chuyen vi cho tung khau cua Robot
Matrix[] transforms0, transforms1, transforms2, transforms3, transforms4, transforms5, transforms6, transforms7, transforms8, transforms9;
//Tao ma tran chuyen vi cho tung khau cua Do ga 2 DOF so 1
Matrix[] transforms_base, transforms_comp_01, transforms_comp_02; //tao ma tran chuyen vi cho ray truot
Matrix[] transforms_rail;
//Tao ma tran huong nhin va huong chieu cua camera
Matrix view, projection; //Khai bao bien dem ve quy dao
public float q1 = 0.0f; public float q2 = 90.0f; public float q3 = 0.0f; public float q4 = 0.0f; public float q5 = 0.0f; public float q6 = 0.0f;
//Khai bao bien goc quay do ga so 1
public float q_A = 0.0f;
public float q_B = 0.0f;
//Khai bao bien goc quay do ga so 2
public float next_q_A = 0.0f;
public float next_q_B = 0.0f;
//Khai bao bien vi tri tren ray truot
public float rail_position = 0.0f;
public float t = 0.0f;
public double[][] theta_ns_import = new double[200000][];
public double[][] theta_teach = new double[200000][];
public int i, sodiem, k, record_i = 0;
public int speed = 490;
public int delta_speed = 5;
private float RotX = 0.0f;
private float RotY = 0.0f;
private float RotZ = 0.0f;
//Khoi tao ma tran cua tung mo hinh
private Matrix WorldMatrix_0, WorldMatrix_1, WorldMatrix_2, WorldMatrix_3,
WorldMatrix_4, WorldMatrix_5, WorldMatrix_6, Common_matrix, WorldMatrix_7, WorldMatrix_8;
private Matrix WorldMatrix_base, WorldMatrix_comp_01; //Ma tran cum do ga so
private Matrix WorldMatrix_rail;
//Dieu chinh trang thai nut bam dieu khien truc quay do ga