Mengembangkan hexapod dari awal (bagian 4) - lintasan dan urutan matematika

Halo semuanya! Pengembangan hexapod telah melangkah lebih jauh. Kali ini, lintasan gerakan tungkai diimplementasikan dan diuji - bagian selanjutnya dari matematika gerakan. Pada artikel ini saya akan berbicara tentang lintasan dan urutan dasar untuk gerakan ini. Saya harap ini akan menarik.

Tahapan pengembangan:
Bagian 1 - Desain
Bagian 2 - perakitan
Bagian 3 - Kinematika
Bagian 4 - lintasan dan urutan matematika
Bagian 5 - Elektronik
Bagian 6 - transisi ke pencetakan 3D
Bagian 7 - perumahan baru, perangkat lunak aplikasi, dan protokol komunikasi

Lintasan

Inti dari mekanisme ini adalah ketika Anda menetapkan dua titik, Anda dapat memilih lintasan anggota tubuh. Ketika bergerak dari satu titik ke titik lain, koordinat akan berubah sesuai dengan persamaan parametrik tertentu. Mekanisme ini ternyata sangat kuat dan memungkinkan Anda untuk mendapatkan kurva menarik untuk gerakan. Ini juga menerapkan perataan gerakan dengan mengubah langkah parameter t - semakin kecil langkahnya, semakin banyak titik antara, masing-masing, kecepatan yang lebih rendah dan kelancaran gerakan yang lebih tinggi.

Prosedur untuk mengatur parameter lintasan di beberapa tempat agak tidak bisa dimengerti dan Anda bisa bingung di dalamnya. Kesulitannya terletak pada kenyataan bahwa ketika mengatur koordinat titik awal dan akhir, koordinat titik nyata dalam ruang tidak selalu ditetapkan, mis. beberapa koordinat mengatur parameter lintasan. Saya harus menulis sebuah program yang menampilkan jalur yang diberikan dan pada saat yang sama memeriksa jangkauan setiap titik jalan.

Pengemudi mendukung jalur gerak berikut:

1. XYZ_LINAR adalah yang paling sederhana dari semua lintasan. Lintasan digunakan saat bergerak maju, mundur, naik dan turun. Semua koordinat berubah secara linear dan dihitung sebagai berikut:
   
   

   x = t * (x1 - x0) / 180.0f + x0; y = t * (y1 - y0) / 180.0f + y0; z = t * (z1 - z0) / 180.0f + z0; 

   
   Di sini kami mengerti tidak ada masalah. Koordinat menentukan sudut parallelepiped dan mereka bertepatan dengan koordinat nyata. Gerakan terjadi di sepanjang diagonal kotak.
   
   
   Visualisasi lintasan

   
   
   
   
   
   
2. YZ_ARC_Y_LINEAR - lintasan ini memungkinkan Anda untuk menerapkan gerakan di sepanjang busur. Lintasan digunakan saat berbelok, saat Anda perlu menggerakkan anggota tubuh di tanah. Koordinat dihitung sebagai berikut:
   
   

    float R = sqrt(x0 * x0 + z0 * z0); float atan0 = RAD_TO_DEG(atan2(x0, z0)); float atan1 = RAD_TO_DEG(atan2(x1, z1)); float t_mapped_rad = DEG_TO_RAD(t * (atan1 - atan0) / 180.0f + atan0); x = R * sin(t_mapped_rad); // Circle Y y = t * (y1 - y0) / 180.0f + y0; z = R * cos(t_mapped_rad); // Circle X 

   
   Di sinilah kesenangan dimulai. Koordinat menentukan arah sinar untuk membatasi busur dan mereka mungkin tidak bertepatan dengan koordinat aktual. Sinar berada di bidang yang sama, sedangkan jari-jari lingkaran sama dengan panjang vektor ke titik awal.
   
   
   Visualisasi lintasan

   
   
   
   
   
   
3. XZ_ARC_Y_SINUS - lintasan ini juga memungkinkan Anda untuk menerapkan gerakan di sepanjang busur, tetapi lebih kompleks daripada YZ_ARC_Y_LINEAR. Lintasan ini digunakan saat berbelok, saat Anda perlu menggerakkan anggota badan di udara. Koordinat dihitung sebagai berikut:
   
   

    float R = sqrt(x0 * x0 + z0 * z0); float atan0 = RAD_TO_DEG(atan2(x0, z0)); float atan1 = RAD_TO_DEG(atan2(x1, z1)); float t_mapped_rad = DEG_TO_RAD(t * (atan1 - atan0) / 180.0f + atan0); x = R * sin(t_mapped_rad); // circle Y y = (y1 - y0) * sin(DEG_TO_RAD(t)) + y0; z = R * cos(t_mapped_rad); // circle X 

   
   Kesenangan berlanjut. Koordinat juga menentukan arah sinar untuk membatasi busur, tetapi mereka TIDAK bertepatan dengan koordinat aktual. Koordinat Y dari titik target menentukan tinggi sinus.
   
   
   Visualisasi lintasan

   
   
   
   
4. XZ_ELLIPTICAL_Y_SINUS - lintasan ini memungkinkan Anda untuk menerapkan gerakan di sepanjang elips. Lintasan ini digunakan saat bergerak maju dan mundur, saat Anda perlu menggerakkan anggota tubuh di udara. Lintasan ini merupakan komplikasi dari XZ_ARC_Y_SINUS dan diperlukan hanya karena gaya berjalan yang jelek secara visual saat menggunakan XZ_ARC_Y_SINUS (kaki terlalu panjang). Koordinat dihitung sebagai berikut:
   
   

    float a = (z1 - z0) / 2.0f; float b = (x1 - x0); float c = (y1 - y0); x = b * sin(DEG_TO_RAD(180.0f - t)) + x0; // circle Y y = c * sin(DEG_TO_RAD(t)) + y0; z = a * cos(DEG_TO_RAD(180.0f - t)) + z0 + a; 

   
   Koordinat menentukan sudut parallelepiped dan mereka TIDAK bertepatan dengan koordinat nyata. Gerakan terjadi dari sudut-sudut bawah kotak, yang terletak di bidang yang sama dengan menyentuh bagian atas kotaknya. Lebih baik untuk melihat gambar di spoiler, saya tidak tahu bagaimana menggambarkannya dengan kata-kata.
   
   
   Visualisasi lintasan

   
   

Ini melengkapi matematika dasar dari pergerakan hexapod. Dalam proyek saya, ini adalah minimum yang diperlukan untuk pelaksanaan hampir semua gerakan.

Urutan

Sedikit teori

Urutan adalah tindakan dasar yang membentuk gaya berjalan. Mereka dibagi menjadi siklus dan non-siklik.

• Urutan siklik dapat dilakukan berkali-kali dan pada akhir setiap siklus harus mengembalikan anggota badan ke posisi semula (gerakan dan rotasi);
• Urutan non-siklik dilakukan hanya sekali (naik dan turun);

Setiap urutan memiliki tiga blok iterasi: blok persiapan, blok utama, blok penyelesaian.

• Blok pelatihan - berisi iterasi untuk memindahkan anggota badan ke posisi awal untuk urutan. Dalam kasus saya, bergerak maju memerlukan meletakkan kaki Anda pada posisi tertentu sebelum mulai bergerak. Ini dijalankan sekali setelah transisi ke urutan;
• Blok utama - berisi iterasi utama dari urutan. Itu dapat dilakukan secara siklis;
• Blok selesai - berisi iterasi untuk memindahkan anggota badan ke posisi dasar (posisi di mana anggota badan diatur setelah mengangkat);

Gambar di bawah ini menunjukkan urutan untuk bergerak maju.

• Titik merah menunjukkan posisi awal anggota badan sebelum mulai bergerak
• Garis biru menunjukkan lintasan anggota badan di tanah
• Garis hitam menunjukkan lintasan anggota badan di udara
• Tanda panah menunjukkan urutan

Koordinat titik dipilih berdasarkan konfigurasi perumahan. Saya memilih titik-titik sedekat mungkin dengan tubuh untuk mengurangi panjang tuas. Dalam satu siklus urutan, hexapod bergerak 18 cm (dalam 1 siklus 2 langkah diambil - 1 langkah dalam 3 tungkai). Jika Anda membuat jarak lebih jauh, maka anggota badan akan mulai menempel satu sama lain. Parameter ini hanya dibatasi oleh konfigurasi case.


Urutan ditentukan oleh dua titik (1, 2) untuk setiap ekstremitas dan dua jalur yang digunakan: XYZ_LINEAR (garis biru) dan XZ_ELLIPTICAL_Y_SINUS (garis hitam). Poin 1 digunakan oleh jalur XZ_ELLIPTICAL_Y_SINUS untuk mengatur ketinggian sinus dan, karenanya, ketinggian tempat kaki akan naik. Poin 2 dan 3 adalah poin nyata yang dicapai anggota tubuh saat bergerak.

Lokasi poin hanya tergantung pada imajinasi Anda dan kemampuan hexapod. Mungkin itu semua ternyata sedikit rumit dan ada opsi yang lebih sederhana, tetapi ternyata saya belum mencapainya.

Implementasi

Sekarang mari kita lihat implementasi dari semua kebahagiaan ini. Struktur dengan parameter urutan adalah sebagai berikut:

 typedef struct { point_3d_t point_list[SUPPORT_LIMB_COUNT]; path_type_t path_list[SUPPORT_LIMB_COUNT]; uint32_t smooth_point_count; } sequence_iteration_t; typedef struct { bool is_sequence_looped; uint32_t main_sequence_begin; uint32_t finalize_sequence_begin; uint32_t total_iteration_count; sequence_iteration_t iteration_list[15]; sequence_id_t available_sequences[SUPPORT_SEQUENCE_COUNT]; } sequence_info_t; 

sequence_iteration_t - berisi informasi tentang iterasi dari urutan:

• point_list - array poin untuk setiap ekstremitas dalam format XYZ;
• path_list - larik lintasan untuk setiap anggota gerak;
• smooth_point_count - mengatur jumlah titik jalur (parameter t langkah);

sequence_info_t - berisi informasi tentang seluruh urutan:

• is_afterence_looped - mengatur jenis urutan: siklik atau tidak;
• main_afterence_begin - menetapkan indeks awal dari blok utama dalam array iteration_list;
• finalize_afterence_begin - menetapkan indeks awal dari blok penyelesaian dalam array iteration_list;
• total_iteration_count - mengatur jumlah iterasi dalam urutan;
• iteration_list - larik iterasi;
• available_afterences - mengatur daftar urutan yang tersedia untuk transisi dari yang sekarang (misalnya, kita tidak bisa mulai berjalan tanpa terlebih dahulu bangkit dari lantai);

CATATAN: Indeks blok persiapan tidak sengaja ditunjukkan, selalu terletak di awal array iterasi.

Sayangnya, saya tidak dapat memberikan kode untuk menentukan urutan di sini, karena cukup lebar dan terlihat mengerikan setelah transfer. Saya hanya meninggalkan tautan ke file definisi di sini.

Skema pemrosesan gerak

Adalah bermanfaat untuk melihat semua lingkaran neraka yang urutannya lewati saat runtime. Skema pemrosesan adalah sebagai berikut:

1. MESIN GERAKAN - mengatur pemrosesan dan beralih antar urutan. Tidak ada perhitungan yang dilakukan di sana. Jika disederhanakan, maka modul ini menyelipkan poin berikutnya ke modul LIBMS DRIVER setelah pemrosesan saat ini selesai.
   Input modul: array koordinat titik target.
   Output modul: titik target untuk iterasi saat ini dari urutan.
2. LIBMS DRIVER adalah yang paling kompleks dari semua modul. Semua matematika gerakan memerintah di sini: kinematika terbalik, perhitungan lintasan, dan perataan gerakan. Modul ini memiliki sinkronisasi ketat dengan modul PWM. Perhitungan dilakukan dengan frekuensi 150Hz, masing-masing, pulsa kontrol ke drive juga dilengkapi dengan frekuensi 150Hz.
   Input modul: koordinat titik target.
   Output modul: sudut rotasi servo.
3. DRIVER SERVO . Tidak ada yang istimewa di dalamnya, kecuali sekelompok parameter untuk mengatur dan menyesuaikan drive.
   Input modul: sudut rotasi servo.
   Output modul: mengontrol lebar pulsa.
4. DRIVER PWM . Perangkat lunak driver PWM untuk kontrol drive. Di sini pin hanya berkedut pada waktu yang tepat. Variabel sinkronisasi sinkronisasi PWM bertambah setiap periode PWM.
   Input modul: mengontrol lebar pulsa.
   Output modul: pulsa pada pin kontrol.

Saya mencoba membuat modul-modul itu independen satu sama lain dan saya berhasil. Ini memungkinkan Anda untuk memasukkan ke dalam modul apa saja modul perantara (misalnya, modul adaptasi ke lanskap) dan tidak ada yang akan pecah pada saat yang bersamaan, sementara implementasi akan terjadi dengan perubahan minimal pada kode.

Berita terbaru dan proyek udang karang ditemukan