🤾🏻 👩🏼‍✈️ 👩🏿‍🤝‍👩🏼 Kinematika Robot Delta 🚘 🤲🏼 ↖️

Kembali pada tahun 2009, saya bersemangat membangun robot industri saya sendiri yang dapat melakukan sesuatu yang berguna (yaitu, mengurutkan bagian-bagian kecil pada ban berjalan). Saya harus segera mengatakan bahwa saya membuat robot (hasil yang Anda lihat di foto judul), dan pada saat yang sama, sebagai produk sampingan, saya menulis sebuah artikel pendek tentang kinematika robot delta di forum TrossenRobotics - seorang penjual kit suku cadang robot Amerika. Mereka baru saja mengadakan kompetisi untuk penulis pada waktu itu. Tentu saja, saya tidak memenangkan kompetisi, tetapi artikel dalam bahasa Inggris tetap ada. Beberapa kali saya mencoba menerjemahkannya ke dalam bahasa ibu saya, tetapi saya berhasil menyelesaikan apa yang baru saya mulai sekarang.

Jika Anda ingin membuat model robot delta, atau hanya untuk mengetahui bagaimana Anda bisa mendapatkan rumus kinematik untuk jenis robot ini (tanpa melampaui kurikulum sekolah dalam aljabar dan geometri) - selamat datang di Cat. Bagi mereka yang tidak terlalu suka teori, di akhir artikel adalah contoh kode yang sudah jadi di C.

Apa itu robot delta

Ilmuwan Swiss Raymond Clavel menemukan robot delta ( wiki ) pada awal 1980-an, berikut ini adalah ilustrasi dari paten US4976582 asli untuk "Perangkat untuk memindahkan dan memposisikan elemen di luar angkasa":

Robot terdiri dari dua platform: basis atas tetap (1) dan platform bergerak kecil (8) dihubungkan oleh tiga tuas. Setiap tuas terdiri dari dua bagian: pundak atas (4) terhubung dengan kaku ke mesin (3) yang terletak di pangkalan atas, dan yang lebih rendah adalah jajaran genjang (5), di sudut-sudut yang disebut sambungan universal (6, 7) ( wiki) yang memungkinkan sudut berubah. Setiap jajar genjang dihubungkan ke lengan atas oleh engsel (16) sehingga sisi atasnya selalu tetap tegak lurus dengan lengannya dan sejajar dengan bidang alas atas. Berkat ini, platform robot yang dapat bergerak yang terpasang pada sisi bawah dari jajaran genjang juga akan selalu sejajar dengan bagian atas. Kita dapat mengontrol posisi platform dengan mengubah sudut rotasi tuas atas relatif terhadap pangkalan robot menggunakan motor.

Di tengah platform yang lebih rendah (8), yang disebut badan yang bekerja (dalam bahasa Inggris istilah end effector digunakan) dari robot (9). Ini bisa berupa manipulator, perangkat yang mencengkeram, atau, misalnya, ekstruder untuk printer 3D. Selain itu, mesin lain (11) dapat digunakan, yang menyediakan rotasi benda kerja melalui batang (14).

Keuntungan utama dari robot delta adalah kecepatan: mesin berat ditempatkan pada pangkalan tetap, hanya tuas dan gerakan platform yang lebih rendah, yang mereka coba buat dari bahan komposit ringan, sehingga mengurangi kelembamannya. Di sini, misalnya, sebuah artikel tentang Geektimes dengan beberapa video yang sangat efektif.

Pernyataan tugas

Untuk membangun robot delta Anda sendiri, Anda perlu belajar bagaimana menyelesaikan dua masalah. Dalam situasi pertama, kita tahu posisi di mana kita ingin menggerakkan lengan robot dari robot kita (misalnya, kita ingin mengambil cookie yang ada di conveyor pada titik dengan koordinat (x, y, z) . Untuk melakukan ini, kita perlu menentukan sudut di mana kita mereka harus memutar motor yang terhubung dengan tuas robot untuk mengaturnya pada posisi yang benar untuk mencengkeram. Prosedur untuk menentukan sudut ini disebut terbalik (dalam beberapa sumber berbahasa Rusia kata "terbalik" digunakan) masalah kinematik.
Dalam situasi kedua, kita tahu sudut bahwa motor kontrol robot dihidupkan (jika kita menggunakan servomotor, kita dapat dengan mudah menemukan sudut dengan membaca bacaan dari sensor sudut), dan kami ingin menemukan posisi platform robot di ruang angkasa (misalnya, untuk menyesuaikan posisinya) . Ini adalah tugas kinematik langsung.

Kami memformalkan kedua tugas. Baik dasar tetap dari robot dan platform yang bergerak dapat direpresentasikan sebagai segitiga sama sisi: dalam diagram di bawah, mereka dicat masing-masing dalam warna hijau dan pink. Sudut rotasi lengan robot relatif terhadap bidang dasar (mereka juga sudut rotasi motor) ditetapkan sebagai Ѳ ₁ , Ѳ ₂ dan Ѳ ₃ , dan koordinat titik adalah E ₀terletak di tengah platform seluler dan di mana lengan robot robot kami akan diperbaiki dalam kehidupan nyata - seperti (x ₀ , y ₀ , z ₀ ) .

Ternyata kita harus memunculkan dua fungsi:

f _terbalik (x ₀ , y ₀ , z ₀ ) → (Ѳ ₁ , Ѳ ₂ , Ѳ ₃ ) untuk menyelesaikan masalah kinematik terbalik dan
f _maju (Ѳ ₁ , Ѳ ₂ , Ѳ ₃ ) → (x ₀ , y ₀ , z ₀ ) untuk menyelesaikan masalah kinematik langsung.

Kinematika terbalik

Mari kita tentukan beberapa parameter kunci yang ditentukan oleh dimensi geometrik robot kami:

Mendenotasikan atas sisi basis panjang f , sisi bawah platform e , atas tuas lengan panjang r _f dan lebih rendah lengan panjang (sisi panjang genjang) r _e . Untuk perhitungan, kami memilih sistem koordinat dengan titik referensi yang bertepatan dengan pusat geometris dari segitiga atas. Sumbu Z diarahkan ke atas, sehingga koordinat z platform bergerak akan selalu negatif.

desain robot mengasumsikan bahwa tuas F ₁ J ₁ (lihat. Gambar di bawah) dapat memutar hanya di pesawat YZ , menggambarkan sebuah lingkaran dengan jari-jari r _f berpusat diF ₁ (di tempat ini terpasang ke mesin). Berbeda dengan F ₁ , bening J ₁ dan E ₁ menggunakan sambungan universal, di mana bahu E _1, J ₁ dapat berputar bebas relatif terhadap E ₁ , menggambarkan lingkup radius r _e berpusat pada titik E ₁ .

Perpotongan bola ini dan bidang YZ adalah lingkaran yang berpusat di titik E ' _{1 dari} jari _- jari E' ₁ J ₁ , di mana titik E ' ₁ terletak sebagai proyeksi titik E ₁ pada bidang YZ . Maka titik J ₁ akan berlokasi di persimpangan dua lingkaran dengan pusat pada titik-titik E ' ₁ dan F ₁ , dengan jari-jari lingkaran, kita dapat menentukan. Ada sedikit kehalusan: lingkaran berpotongan di dua titik, tetapi kami hanya tertarik pada salah satu dari mereka - dengan nilai yang lebih kecil dari koordinat y, karena kami ingin lengan robot selalu mencuat "siku" keluar. Dengan demikian menentukan koordinat titik J ₁ , kita dapat dengan mudah menemukan sudut _{1 yang} menarik bagi kita .

Untuk memudahkan persepsi, proyeksi gambar tiga dimensi kami ke bidang YZ ditunjukkan di bawah ini :

Platform yang lebih rendah adalah segitiga sama sisi, yang tengahnya adalah titik E ₀ (x ₀ , y ₀ , z ₀ ) . Jarak yang berarti

$E_0E_1 = {e \ over2} \ tan (30 ^ o) = {e \ over2 \ sqrt3},$

yang memberi kita koordinat titik E _{1 berikut} dan proyeksi E ' ₁ pada pesawat YZ :

$E_1(x_0, y_0-{e\over2\sqrt3}, z_0) \Rightarrow E'_1(0, y_0-{e\over2\sqrt3}, z_0)$

Jarak E ₁ E ' ₁ = x ₀ , kemudian, menurut teorema Pythagoras,

$E'_1J_1=\sqrt{E_1J_1^2-E_1E'_1^2}=\sqrt{r_e^2-x_0^2}$

Karena platform atas juga merupakan segitiga sama sisi, koordinat titik F ₁ akan menjadi

$F_1(0, {-f\over{2\sqrt3}}, 0)$

Untuk menemukan koordinat titik J ₁ , yang merupakan persimpangan dua lingkaran, perlu untuk menyelesaikan sistem persamaan:

$\begin{equation*} \begin{cases} (y_{J_1}-y_{F_1})^2+(z_{J_1}-z_{F_1})^2 = r_f^2 \\ (y_{J_1}-y_{E'_1})^2+(z_{J_1}-z_{E'_1})^2 = r_e^2-x_0 \end{cases} \end{equation*}$

Kita tahu koordinat pusat lingkaran, jika kita menggantinya, kita mendapatkan ungkapan berikut:

$\begin{equation*} \begin{cases} (y_{J_1}+{f\over{2\sqrt3}})^2+z_{J_1}^2 = r_f^2 \\ (y_{J_1}-y_0+{e\over{2\sqrt3}})^2+(z_{J_1}-z_0)^2 = r_e^2-x_0 \end{cases} \end{equation*}$

Jika kita membuka tanda kurung dan mengurangi satu persamaan dari yang lain, kita dapat secara linear mengekspresikan koordinat z dari titik J1 _dalam hal koordinat y, setelah itu, menggantikannya dalam persamaan kedua, kita mendapatkan persamaan kuadratik biasa untuk y, dari mana dua solusi kita memilih yang terkecil (lebih lanjut tentang ini kami berbicara di atas). Dan dengan demikian mendapatkan koordinat titik J ₁ , kita menemukan sudutnya

$\theta_1=\arctan\left({z_{J_1}\over{y_{F_1}-y_{J_1}}}\right)$

Semua ekspresi ternyata cukup sederhana karena pilihan yang sukses dari sistem koordinat: lengan tuas F ₁ J ₁ selalu bergerak di bidang YZ , jadi kita bisa mengabaikan koordinat x . Untuk mempertahankan keunggulan ini ketika menemukan dua sudut yang tersisa Ѳ ₂ dan Ѳ ₃ , kami menggunakan simetri konstruksi robot delta. Pertama, putar sistem koordinat kami 120 ° berlawanan arah jarum jam pada bidang XY di sekitar sumbu Z :

Kami mendapatkan sistem koordinat baru X'Y'Z ' , dan dalam sistem baru ini kami dapat menggunakan rumus siap pakai kami untuk menemukan sudut Ѳ ₂ . Satu-satunya kehalusan adalah bahwa pertama-tama kita harus menghitung ulang koordinat titik E ₀ dalam kerangka referensi baru. Ini mudah dilakukan dengan menggunakan rumus terkenal (transformasi ketika sistem berputar di sekitar asal), ditunjukkan pada gambar di atas. Untuk menemukan sudut Ѳ ₃Anda juga perlu memutar sistem referensi asli, tetapi sekarang searah jarum jam. Teknik ini sangat mudah diterapkan dalam bentuk program: cukup untuk menulis fungsi untuk menghitung sudut Ѳ pada bidang YZ, dan kemudian memanggilnya tiga kali untuk masing-masing sudut dan sistem referensi.

Kinematika langsung

Mari kita coba untuk memecahkan masalah invers: sekarang kita tahu sudut Ѳ ₁ , Ѳ ₂ dan Ѳ ₃ , dan kami ingin menemukan koordinat (x ₀ , y ₀ , z ₀ ) dari titik E _{0 yang} terletak di tengah platform bergerak robot kami. Mengetahui sudutnya, kita dapat dengan mudah menemukan koordinat titik J ₁ , J ₂ dan J ₃ (lihat gambar di bawah). Bahu tuas J ₁ E ₁ , J ₂ E ₂ dan J ₃ E₃ dapat dengan bebas memutar tentang poin J ₁ , J ₂ dan J ₃ masing-masing, membentuk ruang dalam tiga bidang dengan jari-jari r _e .

Kami menggunakan teknik rumit: kami memindahkan pusat dari masing-masing bidang ini dari titik J ₁ , J ₂ , J ₃ dalam bidang XY ke arah sumbu Z , menggunakan vektor perpindahan masing-masing E ₁ E ₀ , E ₂ E ₀ dan E ₃ E _0, pada gambar ditampilkan sebagai panah merah). Setelah transformasi ini, ternyata ketiga bola berpotongan di titik E ₀ , seperti yang ditunjukkan pada gambar di bawah ini:

Ternyata untuk menentukan koordinat (x ₀ , y ₀ , z ₀ ) dari titik E _0, kita harus menemukan titik persimpangan tiga bola, jari-jari dan koordinat pusat yang diketahui oleh kita. Dengan kata lain, kita perlu menyelesaikan sistem tiga persamaan yang menggambarkan bola tiga dimensi:

$(x-x_i)^2 + (y-y_i)^2 + (z-z_i)^2 = r_e^2,$

di mana (x _i , y _i , z _i ) adalah koordinat pusat bola J ' ₁ , J' ₂ dan J ' ₃ , yang dapat ditemukan sebagai berikut:

Di bawah, untuk mempersingkat notasi, saya akan menggunakan notasi (x ₁ , y ₁ , z ₁ ) , (x ₂ , y ₂ , z ₂ ) dan (x ₃ , y sebagai koordinat titik J ' ₁ , J' ₂ dan J ' ₃₃ , z ₃ ), masing-masing. Saya juga ingin mencatat bahwa x ₁ = 0 (karena titik J ' ₁ ada di bidang YZ ). Kami mendapatkan sistem persamaan berikut:

$\begin{equation*} \begin{cases} x^2+(y-y_1)^2+(z-z_1)^2 = r_e^2 \\ (x-x_2)^2+(y-y_2)^2+(z-z_2)^2 = r_e^2 \\ (x-x_3)^2+(y-y_3)^2+(z-z_3)^2 = r_e^2 \end{cases} \end{equation*} \Rightarrow \begin{equation*} \begin{cases} x^2+y^2+z^2-2y_1y-2z_1z=r_e^2-y_1^2-z_1^2 \\ x^2+y^2+z^2-2x_2x-2y_2y-2z_2z=r_e^2-x_2^2-y_2^2-z_2^2 \\ x^2+y^2+z^2-2x_3x-2y_3y-2z_3z=r_e^2-x_3^2-y_3^2-z_3^2 \end{cases} \end{equation*}$

Kami memperkenalkan notasi

$w_i=x_i^2+y_i^2+z_i^2$

dan, dengan mengurangkan persamaan kedua dan ketiga dari persamaan atas, serta yang ketiga dari persamaan kedua, kita dapatkan:

$\begin{equation*} \begin{cases} x_2x+(y_1-y_2)y+(z_1-z_2)z=(w_1-w_2)/2 \\ x_3x+(y_1-y_3)y+(z_1-z_3)z=(w_1-w_3)/2 \\ (x_2-x_3)x+(y_2-y_3)y+(z_2-z_3)z=(w_2-w_3)/2 \end{cases} \end{equation*}$

Mengurangkan yang kedua dari persamaan pertama (dengan demikian mengurangi y ) dan yang ketiga dari yang kedua (mengurangi x ), kita dapat mengekspresikan x dan y dalam hal z :

$x=a_1z+b_1\qquad y=a_2z+b_2$

$a_1={1\over{d}}\left[(z_2-z_1)(y_3-y_1)-(z_3-z_1)(y_2-y_1)\right] \qquad a_2=-{1\over{d}}\left[(z_2-z_1)x_3-(z_3-z_1)x_2\right]$

$b_1 = - {1 \ lebih {2d}} \ kiri [(w_2-w_1) (y_3-y_1) - (w_3-w_1) (y_2-y_1) \ kanan] \ qquad b_2 = {1 \ lebih {2d}} \ kiri [(w_2-w_1) x_3- (w_3-w_1) x_2 \ kanan]$

$d = (y_2-y_1) x_3- (y_3-y_1) x_2$

Sekarang, menggantikan x dan y , diekspresikan melalui z , dalam persamaan untuk lingkaran pertama (berpusat pada titik J ' ₁ ), kita memperoleh:

$(a_1 ^ 2 + a_2 ^ 2 + 1) z ^ 2 + 2 (a_1 + a_2 (b_2-y_1) -z_1) z + (b_1 ^ 2 + (b_2-y_1) ^ 2 + z_1 ^ 2-r_e ^ 2) = 0$

Masih untuk menyelesaikan persamaan kuadratik ini (dengan cara standar, melalui diskriminan) untuk menemukan z (kita ingat bahwa kita harus memilih yang terkecil dari dua z !), Dan melaluinya x dan y .

Contoh kode sumber

Di bawah ini adalah contoh fungsi untuk menghitung kinematika robot delta dalam C. Nama-nama variabel sesuai dengan notasi yang digunakan dalam artikel, sudut theta1, theta2dan theta3ditunjukkan dalam derajat.

//  
// ( .  )
const float e = 115.0;     //   
const float f = 457.3;     //   
const float re = 232.0;
const float rf = 112.0;

//  
const float sqrt3 = sqrt(3.0);
const float pi = 3.141592653;    // PI
const float sin120 = sqrt3/2.0;
const float cos120 = -0.5;
const float tan60 = sqrt3;
const float sin30 = 0.5;
const float tan30 = 1/sqrt3;

//  : (theta1, theta2, theta3) -> (x0, y0, z0)
//  : 0=OK, -1= 
int delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) {
    float t = (f-e)*tan30/2;
    float dtr = pi/(float)180.0;

    theta1 *= dtr;
    theta2 *= dtr;
    theta3 *= dtr;

    float y1 = -(t + rf*cos(theta1));
    float z1 = -rf*sin(theta1);

    float y2 = (t + rf*cos(theta2))*sin30;
    float x2 = y2*tan60;
    float z2 = -rf*sin(theta2);

    float y3 = (t + rf*cos(theta3))*sin30;
    float x3 = -y3*tan60;
    float z3 = -rf*sin(theta3);

    float dnm = (y2-y1)*x3-(y3-y1)*x2;

    float w1 = y1*y1 + z1*z1;
    float w2 = x2*x2 + y2*y2 + z2*z2;
    float w3 = x3*x3 + y3*y3 + z3*z3;

    // x = (a1*z + b1)/dnm
    float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
    float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;

    // y = (a2*z + b2)/dnm;
    float a2 = -(z2-z1)*x3+(z3-z1)*x2;
    float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;

    // a*z^2 + b*z + c = 0
    float a = a1*a1 + a2*a2 + dnm*dnm;
    float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
    float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);

    // 
    float d = b*b - (float)4.0*a*c;
    if (d < 0) return -1; //  

    z0 = -(float)0.5*(b+sqrt(d))/a;
    x0 = (a1*z0 + b1)/dnm;
    y0 = (a2*z0 + b2)/dnm;
    return 0;
}

//  
//  ,   theta1 (  YZ)
int delta_calcAngleYZ(float x0, float y0, float z0, float &theta) {
    float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
    y0 -= 0.5 * 0.57735 * e;       //    
    // z = a + b*y
    float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
    float b = (y1-y0)/z0;
    // 
    float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
    if (d < 0) return -1; //  
    float yj = (y1 - a*b - sqrt(d))/(b*b + 1); //   
    float zj = a + b*yj;
    theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0);
    return 0;
}

//  : (x0, y0, z0) -> (theta1, theta2, theta3)
//  : 0=OK, -1= 
int delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) {
    theta1 = theta2 = theta3 = 0;
    int status = delta_calcAngleYZ(x0, y0, z0, theta1);
    if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2);  // rotate coords to +120 deg
    if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3);  // rotate coords to -120 deg
    return status;
}

Literatur bekas

Saya mengambil semua ide kunci tentang kinematika robot delta dari karya prof. Paul Zsombor-Murray " Analisis Kinematika Geometris Deskriptif Robot" Delta "Clavel ." Jujur saya akui bahwa pelatihan matematika saya tidak cukup untuk memahaminya sampai akhir, jadi banyak yang harus disimpulkan sendiri.

Kesimpulan

Terima kasih kepada semua orang yang membaca artikel ini sampai akhir. Saya berharap bagi seseorang itu akan berguna dan menginspirasi untuk membuat versi robot delta mereka sendiri.

Kinematika Robot Delta