تحية للجميع! نتيجة للانتقال إلى العمل عن بعد ، حصلت على وقت فراغ أكبر لتطوير سداسي الأرجل (+ ساعتان في اليوم بسبب التوفير على الطريق). تمكنت أخيرًا من إنشاء خوارزمية عالمية لبناء مسار الحركة في الوقت الفعلي. جعلت الرياضيات الجديدة من الممكن تنفيذ الحركات الأساسية من خلال تغيير معلمتين فقط. هذه خطوة أخرى نحو تنفيذ "الطيار الآلي". في هذه المقالة سأحاول التحدث بالتفصيل عن الرياضيات الجديدة وكيف تعمل بشكل عام. سيكون هناك العديد من الصور وصور متحركة.

مراحل التطوير:

الجزء 1 - التصميم
الجزء 2 - التجميع
الجزء 3 - علم الحركة
الجزء 4 - رياضيات المسارات والتسلسلات
الجزء 5 - الإلكترونيات
الجزء 6 - الانتقال إلى الطباعة ثلاثية الأبعاد
الجزء 7 - الإسكان الجديد وبرمجيات التطبيق وبروتوكولات الاتصال
الجزء 8 - رياضيات الحركة المتقدمة

الرجوع بالزمن

حتى هذه النقطة ، تم تعيين جميع الحركات الأساسية (للأمام ، للخلف ، للتدوير) على شكل "تحريك الساق من الموضع الحالي إلى النقطة (X ، Y ، Z) باستخدام خطي / جيبي / أي حركة". هذا يعمل بشكل جيد وموثوق ، ولكن يحد بشدة من الحركة. على سبيل المثال ، لتنفيذ الحركة على طول قوس مع نصف قطر R1 ، تحتاج إلى حساب إحداثيات بداية ونهاية الحركة لكل طرف ، وإضافة زر إضافي إلى التطبيق بحيث يمكنك تحديد هذه الحركة. وفقًا لذلك ، لإضافة حركة على طول القوس بنصف قطر مختلف R2 ، تحتاج إلى حساب الإحداثيات مرة أخرى وإضافة زر آخر. غير مريح للغاية.

الآن ، لتنفيذ الحركات الأساسية ، يتم استخدام وحدة رياضية واحدة ، والتي يمكن تكييفها مع مواقف مختلفة مع معلمتين. المكافأة الرئيسية للرياضيات الجديدة كانت دعم الحركة على طول القوس مع تغيير في نصف قطرها أثناء الحركة!

فكرة الخوارزمية

أولاً ، يجدر شرح كيفية عملها على الأصابع. ماذا يحدث إذا حددت نافذة ذات حجم ثابت على دائرة وبدأت في زيادة نصف قطرها؟ وهذا ما:

هل كل شيء صحيح؟ من خلال تغيير نصف قطر الدائرة ، يمكننا الحصول على مسارات مختلفة للحركة وبدقة ما خط مستقيم. كان من الممكن التوقف عند هذا ، ولكن ليس كل شيء وردية للغاية. لا يمكنك فقط أخذ هذا وتطبيقه على أساس نصف القطر وحده - هناك فروق دقيقة.

يمكن أن تكون المواضع الأولية لأطراف سداسي الأرجل على دوائر مختلفة ، على التوالي ، ستكون معلمات المعادلة مختلفة بالفعل. في حالتي ، يتم ترتيب الأطراف على النحو التالي (تقريبًا):

تعتمد قيمة المسافة التي يجب أن يسيرها أحد الأطراف على نصف قطر الدائرة التي يقع عليها. هذا يجبرنا على التحول إلى حساب فردي للمسار لكل طرف. لحل هذه المشكلة ، تحتاج إلى إيجاد دائرة ذات نصف قطر أقصى ، وحساب زوايا البداية والنهاية للقوس. بالإضافة إلى القوس الناتج هو أقواس للأطراف الأخرى. قمت بعمل رسوم متحركة لهذه القطعة من الخوارزمية:

هناك رسوم متحركة لتوضيح عمل كل هذا السحر (مدى سعادتي بوجود Excel في عصرنا). في البداية ، تتغير قيمة المسافة التي يجب أن يقطعها سداسي الأرجل لكل دورة (على غرار السرعة) ، ثم تتغير قيمة انحناء المسار (على غرار تدوير عجلة القيادة).

بشكل عام ، هذه هي فكرة الرياضيات الجديدة ، آمل أن تكون متاحة للشرح. يمكنك الآن تحليل الخوارزمية بمزيد من التفاصيل ومحاولة حساب كل شيء يدويًا في الممارسة.

الرياضيات

معلمات الإدخال

معلمات الإدخال المتغيرة هي المسافة (المسافة) وانحناء مسار الحركة (الانحناء). يجب أن تكون قيمة انحناء المسار في النطاقات [-1.999 ؛ -0.001] و [0.001 ؛ 1.999] ، بينما تعتمد قيمة المسافة القصوى على الخصائص الفيزيائية للسداسي الأرجل. في حالتي ، تبلغ المسافة القصوى لكل دورة 110 ملم ، مع القيم الكبيرة ، تبدأ الأطراف في التماسك. للحصول على مثال للحساب ، نأخذ قيم انحناء = 1.5 والمسافة = 20.

أيضًا ، لكي تعمل الخوارزمية ، من الضروري معرفة المواضع الأولية للأطراف. هذه هي النقاط التي تقع فيها الأطراف عندما يكون سداسي الأرجل على قدميه. على سبيل المثال ، سنستخدم النقاط التالية (يقع أصل كل طرف عند نقطة مرفق COXA):

ملاحظة: أنا أعمل في مستوى XZ ويمكنك تجاهل إحداثيات ص. يمكنك التعرف على نظام الإحداثيات سداسي الأرجل في الجزء الثالث من الدورة ،

ونتيجة لذلك لدينا ما يلي:

الصيغ والحسابات

نبدأ بحساب إحداثيات مركز حركة سداسي الأرجل اعتمادًا على قيمة الانحناء والمسافة:

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$

ونتيجة لذلك ، حصلنا على النقطة [R ؛ 0] ومسار جسم سداسي الأرجل. بالنسبة لها ، سيتم حساب مسارات كل طرف.

بعد ذلك ، من الضروري حساب نصف قطر المسارات لكل طرف بالنسبة لمركز الحركة (النقطة [R ؛ 0]) مع مراعاة الموضع الأولي للطرف [x0 ؛ Z0]. لغة مفهومة أكثر هي إيجاد طول المتجه المرسوم من النقطة [R؛ 0] إلى النقطة [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$

صورة للوضوح. يظهر الأزرق ناقلات المطلوب.

من القيم التي تم الحصول عليها نجد الحد الأقصى:

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$

بعد ذلك ، تحتاج إلى العثور على الزاوية لكل متجه جربناه في الخطوة السابقة.

α_{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°)$

الآن نجد زاوية القوس على أكبر دائرة نصف قطرها R_max (أبعد مسافة عن مركز حركة المسار) ، يجب أن يكون طولها مساوياً لقيمة المسافة. تحدد هذه الزاوية زوايا البداية والنهاية للأقواس الأخرى التي تتحرك الأطراف على طولها. أعتقد أن الصورة أدناه ستساعد على فهم ذلك.

يتم حساب الزاوية على النحو التالي:

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°)$

بعد استخدام هذه الزاوية ، يمكننا حساب زوايا البداية والنهاية للأقواس للأطراف الأخرى. شيء من هذا القبيل يجب أن يتحول:

انضغاط صغير. لتنفيذ هذه الخوارزمية ، من الضروري تقديم مفهوم الوقت الذي تكمن قيمته في النطاق [0 ؛ 1]. من الضروري أيضًا أن يعود كل طرف بقيمة 0.5 الوقت إلى نقطة البداية. هذه القاعدة هي التحقق من صحة الحسابات - يجب أن تمر جميع الدوائر من خلال نقاط البداية لكل طرف.

بعد ذلك ، يبدأ حساب النقاط بواسطة المعلمات التي تم الحصول عليها من الأقواس ، باستخدام الصيغ التالية:

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

كود مصدر الوظيفة

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

ثم قررت أن أظهر أيضًا حساب إحداثيات ص (حساب نقاط ص حسب الوقت). يعتمد الحساب على المسار المختار ، والذي تم تعيينه بشكل صارم في الكود وهو ضروري لتحقيق حركة الطرف على الأرض وفي الهواء.

هناك أيضًا قطعة لتنفيذ الاتجاه العكسي للحركة (وقت الحركة العكسية إذا لزم الأمر). من الضروري أنه أثناء حركة الأطراف الثلاثة على الأرض ، تتحرك الأطراف الثلاثة الأخرى في الاتجاه المعاكس عبر الهواء.

تطوير سداسي الأرجل من نقطة الصفر (جزء 8) - حركة رياضية محسنة