Halo semuanya! Sebagai hasil dari transisi ke pekerjaan jarak jauh, saya mendapat lebih banyak waktu luang untuk mengembangkan hexapod (+2 jam sehari karena penghematan di jalan). Saya akhirnya bisa membuat algoritma universal untuk membangun jalur gerak secara real time. Matematika baru memungkinkan untuk menerapkan gerakan dasar dengan mengubah hanya dua parameter. Ini adalah langkah lain menuju implementasi "autopilot". Pada artikel ini saya akan mencoba untuk berbicara secara rinci tentang matematika baru dan cara kerjanya secara umum. Akan ada banyak gambar dan gif.

Tahap pengembangan:

Bagian 1 - desain
Bagian 2 - perakitan
Bagian 3 - kinematika
Bagian 4 - matematika lintasan dan urutan
Bagian 5 - elektronik
Bagian 6 - transisi ke pencetakan 3D
Bagian 7 - perumahan baru, perangkat lunak aplikasi, dan protokol komunikasi
Bagian 8 - matematika pergerakan lanjutan

Kembali ke waktu itu

Hingga titik ini, semua gerakan dasar (maju, mundur, rotasi) diatur dalam bentuk "pindahkan kaki dari posisi saat ini ke titik (X, Y, Z) menggunakan linear / sinusoidal / gerakan apa saja". Ini bekerja dengan sangat baik dan andal, tetapi sangat membatasi pergerakan. Misalnya, untuk menerapkan gerakan di sepanjang busur dengan jari-jari R1, Anda harus menghitung terlebih dahulu koordinat awal dan akhir gerakan untuk setiap anggota tubuh, tambahkan tombol tambahan ke aplikasi sehingga Anda dapat memilih gerakan ini. Dengan demikian, untuk menambahkan gerakan di sepanjang busur dengan jari-jari R2 yang berbeda, Anda perlu menghitung koordinat lagi dan menambahkan tombol lain. Sangat tidak nyaman.

Sekarang, untuk penerapan gerakan dasar, satu unit matematika digunakan, yang dapat disesuaikan dengan berbagai situasi dengan dua parameter. Bonus utama dari matematika baru adalah dukungan untuk gerakan sepanjang busur dengan perubahan jari-jarinya tepat selama gerakan!

Ide algoritma

Pertama, ada baiknya menjelaskan cara kerjanya pada jari. Apa yang terjadi jika Anda memilih jendela ukuran tetap pada lingkaran dan mulai meningkatkan radiusnya? Itu yang:

Apakah semuanya benar? Dengan mengubah jari-jari lingkaran, kita bisa mendapatkan lintasan pergerakan yang berbeda dan dengan akurasi satu garis lurus. Itu mungkin untuk berhenti pada ini, tetapi tidak semuanya begitu cerah. Anda tidak bisa hanya mengambil dan menerapkan ini berdasarkan radius saja - ada nuansa.

Posisi awal anggota badan hexapod dapat berada di lingkaran yang berbeda, masing-masing, parameter dari persamaan sudah akan berbeda. Dalam kasus saya, anggota badan diatur sebagai berikut (kurang-lebih):

Nilai jarak yang harus dilalui anggota gerak tergantung pada jari-jari lingkaran tempat ia berada. Ini memaksa kita untuk beralih ke perhitungan individual lintasan untuk setiap anggota tubuh. Untuk mengatasi masalah ini, Anda perlu menemukan lingkaran dengan jari-jari maksimum, menghitung sudut awal dan akhir busur. Lebih lanjut relatif terhadap busur yang dihasilkan adalah busur untuk anggota tubuh lainnya. Saya membuat animasi dari algoritme ini:

Ada animasi untuk menggambarkan karya semua keajaiban ini (betapa senangnya saya bahwa di zaman kita ada Excel). Pada awalnya, nilai jarak yang harus dilalui hexapod per siklus (mirip dengan kecepatan) berubah, kemudian nilai kelengkungan lintasan (mirip dengan memutar setir) berubah.

Secara umum, ini adalah ide matematika baru, saya harap ini bisa diakses untuk menjelaskan. Sekarang Anda dapat menganalisis algoritme lebih detail dan mencoba menghitung semuanya dengan tangan.

Matematika

Parameter input

Parameter input variabel adalah jarak (jarak) dan kelengkungan jalur gerak (kelengkungan). Nilai kelengkungan lintasan harus dalam kisaran [-1,999; -0,001] dan [0,001; 1,999], sedangkan nilai jarak maksimum tergantung pada karakteristik fisik hexapod. Dalam kasus saya, jarak maksimum per siklus adalah 110 mm, dengan nilai besar, anggota badan mulai berbatasan satu sama lain. Sebagai contoh perhitungan, kita mengambil nilai kelengkungan = 1.5 dan jarak = 20.

Juga, agar algoritma berfungsi, perlu mengetahui posisi awal anggota tubuh. Ini adalah titik di mana anggota badan berada ketika hexapod berada di kakinya. Sebagai contoh, kita akan menggunakan titik-titik berikut (asal masing-masing anggota badan terletak di titik lampiran COXA):

Catatan: Saya bekerja di pesawat XZ dan Anda dapat mengabaikan koordinat Y. Anda dapat berkenalan dengan sistem koordinat hexapod di bagian ketiga siklus.

Sebagai hasilnya, kami memiliki yang berikut ini:

Rumus dan perhitungan

Kita mulai dengan menghitung koordinat pusat pergerakan hexapod tergantung pada nilai kelengkungan dan jarak:

R = t g (\frac{(2 - c u r v a t u r e) * Π}{4}) * d i s t a n c e

$R = tg(\frac{(2 - curvature) * \Pi}{4}) * distance$

R = t g (\frac{(2 - 1.5) * Π}{4}) * 20 = 8.28

$R = tg(\frac{(2 - 1.5) * \Pi}{4}) * 20 = 8.28$

Sebagai hasilnya, kami mendapatkan poin [R; 0] dan lintasan tubuh hexapod. Sehubungan dengan itu, lintasan untuk setiap ekstremitas akan dihitung.

Selanjutnya, perlu untuk menghitung jari-jari lintasan untuk setiap ekstremitas relatif terhadap pusat gerak (titik [R; 0]) dengan memperhitungkan posisi awal ekstremitas [x0; z0]. Bahasa yang lebih mudah dimengerti adalah menemukan panjang vektor yang diambil dari titik [R; 0] ke titik [x0; z0]:

R_{i} = \sqrt{(R - x_{0 i})^{2} + z_{0 i}^{2}}

$R_i = \sqrt{(R - x_{0i})^2 + z_{0i}^2}$

R_{0} = \sqrt{(8.28 - (- 20))^{2} + 20^{2}} = 34.64

$R_0 = \sqrt{(8.28 - (-20))^2 + 20^2} = 34.64$

R_{1} = \sqrt{(8.28 - (- 35))^{2} + 0^{2}} = 43.28

$R_1 = \sqrt{(8.28 - (-35))^2 + 0^2} = 43.28$

R_{2} = \sqrt{(8.28 - (- 20))^{2} + (- 20)^{2}} = 34.64

$R_2 = \sqrt{(8.28 - (-20))^2 + (-20)^2} = 34.64$

R_{3} = \sqrt{(8.28 - 20)^{2} + (- 20)^{2}} = 23.17

$R_3 = \sqrt{(8.28 - 20)^2 + (-20)^2} = 23.17$

R_{4} = \sqrt{(8.28 - 35)^{2} + 0^{2}} = 26.71

$R_4 = \sqrt{(8.28 - 35)^2 + 0^2} = 26.71$

R_{5} = \sqrt{(8.28 - 20)^{2} + 20^{2}} = 23.17

$R_5 = \sqrt{(8.28 - 20)^2 + 20^2} = 23.17$

Gambar untuk kejelasan. Biru menunjukkan vektor yang diinginkan.

Dari nilai yang diperoleh kami menemukan maksimum:

R_{m a x} = m a x i m u m (R_{0 - 5})

$R_{max} = maximum(R_{0-5})$

R_{m a x} = 43.28

$R_{max} = 43.28$

Selanjutnya, Anda perlu menemukan sudut untuk setiap vektor yang kami coba pada langkah sebelumnya.

α_{0 i} = a t a n 2 (z_{0 i}; - (R - x_{0 i}))

$\alpha_{0i} = atan2(z_{0i}; -(R - x_{0i}))$

α_{00} = a t a n 2 (20; - (8.28 - (- 20))) = 2.52 (144.7 °)

$\alpha_{00} = atan2(20; -(8.28 - (-20))) = 2.52 (144.7°)$

α_{01} = a t a n 2 (0; - (8.28 - (- 35))) = 3.14 (180 °)

$\alpha_{01} = atan2(0; -(8.28 - (-35))) = 3.14 (180°)$

α_{02} = a t a n 2 (- 20; - (8.28 - (- 20))) = - 2.52 (- 144.7 °)

$\alpha_{02} = atan2(-20; -(8.28 - (-20))) = -2.52 (-144.7°)$

α_{03} = a t a n 2 (- 20; - (8.28 - 20)) = - 1.04 (- 59.6 °)

$\alpha_{03} = atan2(-20; -(8.28 - 20)) = -1.04 (-59.6°)$

α_{04} = a t a n 2 (0; - (8.28 - 35)) = 0 (0 °)

$\alpha_{04} = atan2(0; -(8.28 - 35)) = 0 (0°)$

α_{05} = a t a n 2 (20; - (8.28 - 20)) = 1.04 (59.6 °)

$\alpha_{05} = atan2(20; -(8.28 -20)) = 1.04 (59.6°)$

Sekarang kita menemukan sudut busur pada lingkaran terbesar jari-jari R_max (yang paling jauh dari pusat gerakan jalan), yang panjangnya harus sama dengan nilai jarak. Sudut ini menentukan sudut awal dan akhir dari busur lain di sepanjang mana anggota badan akan bergerak. Saya pikir gambar di bawah ini akan membantu untuk memahami hal ini.

Sudut dihitung sebagai berikut:

a r c M a x = s i g n (R) * \frac{d i s t a n c e}{R_{m a x}}

$arcMax = sign(R) * \frac{distance}{R_{max}}$

a r c M a x = \frac{20}{43.28} = 0.462 (26 °)

$arcMax = \frac{20}{43.28} = 0.462 (26°)$

Lebih lanjut menggunakan sudut ini, kita dapat menghitung sudut awal dan akhir dari busur untuk anggota tubuh lainnya. Sesuatu seperti ini akan berubah:

Penyimpangan kecil. Untuk mengimplementasikan algoritma ini, perlu untuk memperkenalkan konsep waktu, yang nilainya terletak pada kisaran [0; 1]. Juga perlu bahwa setiap anggota badan dengan nilai waktu 0,5 kembali ke titik awal. Aturan ini adalah pemeriksaan kebenaran perhitungan - semua lingkaran harus melewati titik awal setiap anggota badan.

Selanjutnya, perhitungan poin dengan parameter yang diperoleh dari busur dimulai, menggunakan rumus berikut:

a r c A n g l e_{i} = (t i m e - 0.5) * α_{0 i} + a r c M a x

$arcAngle_i = (time - 0.5) * \alpha_{0i} + arcMax$

x_{i} = R + R_{i} * c o s (a r c A n g l e_{i})

$x_i = R + R_i * cos(arcAngle_i)$

z_{i} = R_{i} * s i n (a r c A n g l e_{i})

$z_i = R_i * sin(arcAngle_i)$

Kode sumber fungsi

static bool process_advanced_trajectory(float motion_time) {

    // Check curvature value
    float curvature = (float)g_current_trajectory_config.curvature / 1000.0f;
    if (g_current_trajectory_config.curvature == 0)    curvature = +0.001f;
    if (g_current_trajectory_config.curvature > 1999)  curvature = +1.999f;
    if (g_current_trajectory_config.curvature < -1999) curvature = -1.999f;
    
    //
    // Calculate XZ
    //
    float distance = (float)g_current_trajectory_config.distance;

    // Calculation radius of curvature
    float curvature_radius = tanf((2.0f - curvature) * M_PI / 4.0f) * distance;

    // Common calculations
    float trajectory_radius[SUPPORT_LIMBS_COUNT] = {0};
    float start_angle_rad[SUPPORT_LIMBS_COUNT] = {0};
    float max_trajectory_radius = 0;
    for (uint32_t i = 0; i < SUPPORT_LIMBS_COUNT; ++i) {
        
        float x0 = g_motion_config.start_positions[i].x;
        float z0 = g_motion_config.start_positions[i].z;

        // Calculation trajectory radius
        trajectory_radius[i] = sqrtf((curvature_radius - x0) * (curvature_radius - x0) + z0 * z0);

        // Search max trajectory radius
        if (trajectory_radius[i] > max_trajectory_radius) {
            max_trajectory_radius = trajectory_radius[i];
        }

        // Calculation limb start angle
        start_angle_rad[i] = atan2f(z0, -(curvature_radius - x0));
    }
    if (max_trajectory_radius == 0) {
        return false; // Avoid division by zero
    }

    // Calculation max angle of arc
    int32_t curvature_radius_sign = (curvature_radius >= 0) ? 1 : -1;
    float max_arc_angle = curvature_radius_sign * distance / max_trajectory_radius;

    // Calculation points by time
    for (uint32_t i = 0; i < SUPPORT_LIMBS_COUNT; ++i) {
        
        // Inversion motion time if need
        float relative_motion_time = motion_time;
        if (g_motion_config.time_directions[i] == TIME_DIR_REVERSE) {
            relative_motion_time = 1.0f - relative_motion_time;
        }

        // Calculation arc angle for current time
        float arc_angle_rad = (relative_motion_time - 0.5f) * max_arc_angle + start_angle_rad[i];

        // Calculation XZ points by time
        g_limbs_list[i].position.x = curvature_radius + trajectory_radius[i] * cosf(arc_angle_rad);
        g_limbs_list[i].position.z = trajectory_radius[i] * sinf(arc_angle_rad);
        
        // Calculation Y points by time
        if (g_motion_config.trajectories[i] == TRAJECTORY_XZ_ADV_Y_CONST) {
            g_limbs_list[i].position.y = g_motion_config.start_positions[i].y;
        }
        else if (g_motion_config.trajectories[i] == TRAJECTORY_XZ_ADV_Y_SINUS) {
            g_limbs_list[i].position.y = g_motion_config.start_positions[i].y;
            g_limbs_list[i].position.y += LIMB_STEP_HEIGHT * sinf(relative_motion_time * M_PI);  
        }
    }
    
    return true;
}

Kemudian saya memutuskan untuk menunjukkan juga perhitungan koordinat Y (Perhitungan Y poin berdasarkan waktu). Perhitungan tergantung pada lintasan yang dipilih, yang ditetapkan secara kaku dalam kode dan diperlukan untuk mewujudkan pergerakan anggota tubuh di darat dan di udara.

Ada juga bagian untuk menerapkan arah gerakan terbalik (waktu gerak Inversi jika perlu). Adalah penting bahwa selama pergerakan tiga anggota badan di tanah, tiga anggota tubuh lainnya bergerak ke arah yang berlawanan melalui udara.

Mengembangkan hexapod dari awal (bagian 8) - peningkatan pergerakan matematika