大家好！过渡到远程工作的结果是，我有更多的空闲时间来开发六脚架（由于节省了道路，因此每天增加2个小时）。我终于能够制作出一种通用算法，用于实时构建运动路径。新的数学仅通过更改两个参数即可实现基本运动。这是实现“自动驾驶”的又一步。在本文中，我将尝试详细讨论新数学及其一般的工作原理。将有许多图片和GIF。

开发阶段：

第1部分-设计
第2部分-组装
第3部分-运动学
第4部分-轨迹和序列的数学
第5部分-电子学
第6部分-过渡到3D打印
第7部分-新外壳，应用软件和通信协议
第8部分-高级运动数学

时光倒流

到此为止，所有基本运动（向前，向后，旋转）都以“使用线性/正弦/任何运动将腿从当前位置移动到点（X，Y，Z）”的形式设置。这工作得很好并且可靠，但是严重限制了运动。例如，要实现沿半径为R1的圆弧的运动，您需要预先计算每个肢体运动开始和结束的坐标，请在应用程序中添加一个附加按钮，以便您可以选择该运动。因此，要增加沿半径R2不同的圆弧的移动，需要再次计算坐标并添加另一个按钮。极不舒服。

现在，为了实现基本运动，使用了一个数学单元，该数学单元可以通过两个参数适应各种情况。新数学的主要优点是支持在运动过程中沿圆弧移动并改变半径！

算法的想法

首先，值得解释一下它如何在手指上起作用。如果在圆上选择固定大小的窗口并开始增加其半径会发生什么情况？那是什么：


一切都是真的吗？通过改变圆的半径，我们可以获得不同的运动轨迹，并以一定的精度获得一条直线。可以停下来，但并非一切都那么乐观。您不能仅根据半径来采用和实现此功能-会有细微差别。

六脚架的四肢的初始位置可以分别在不同的圆上，等式的参数将已经不同。在我的情况下，四肢的安排如下（大约）：


肢体必须行进的距离值取决于其所在圆的半径。这迫使我们切换到每个肢体的轨迹的单独计算。要解决此问题，您需要找到一个最大半径的圆，并计算圆弧的起点和终点角度。相对于生成的弧，还有其他肢的弧。我制作了这段算法的动画：


有动画来说明所有这些魔术的工作（我很高兴在我们这个时代有了Excel）。刚开始时，六脚架每个周期必须行进的距离值（类似于速度），然后是轨迹曲率的值（类似于转动方向盘）。


总的来说，这是新数学的想法，我希望它可以被解释。现在，您可以更详细地分析算法，并在实践中尝试手动计算所有内容。

数学

输入参数

可变的输入参数是距离（距离）和运动路径的曲率（曲率）。轨迹的曲率值应在[-1.999; -0.001]和[0.001; 1.999]，而最大距离值则取决于六脚架的物理特性。在我的情况下，每个循环的最大距离为110毫米，数值较大时，四肢开始彼此邻接。作为计算示例，我们采用曲率= 1.5和距离= 20的值。

此外，为了使算法起作用，有必要知道四肢的初始位置。这些是将六脚架放在脚上时四肢所在的位置。例如，我们将使用以下点（每个肢体的原点位于COXA附着点）：


注意：我在XZ平面上工作，您可以忽略Y坐标。您可以在周期的第三部分熟悉六足坐标系，

因此，我们具有以下内容：

公式和计算

我们首先根据曲率和距离的值来计算六脚架运动中心的坐标：

$$

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

$$

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

结果，我们得到了[R; 0]和六足动物的身体轨迹。相对于此，将计算每个肢体的轨迹。


接下来，有必要考虑到肢体的初始位置[x0; 0]，计算每个肢体相对于运动中心（点[R; 0]）的轨迹半径。z0]。一种更易理解的语言是找到从点[R; 0]到点[x0; z0]：

$$

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

$$

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

$$

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

$$

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

$$

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

$$

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

$$

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

图片清晰。蓝色显示所需的向量。


从获得的值中我们找到最大值：

$$

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

$$

$$R_{max} = 43.28$$

接下来，您需要找到我们在上一步中尝试过的每个矢量的角度。

$$

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

$$

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

$$

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

$$

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

$$

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

$$

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

$$

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

现在，我们在半径R_max（距运动中心最远的轨迹）的最大圆上找到圆弧的角度，该圆的长度应等于距离值。该角度确定肢体将沿其移动的其他弧的起始和终止角度。我认为下面的图片将有助于理解这一点。


角度计算如下：

$$

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

$$

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

进一步使用该角度，我们可以计算其他肢体的圆弧的起始和终止角度。应该是这样的：


一个小题外话。为了实现该算法，有必要引入时间的概念，时间的值在[0; 1]。每个时间值为0.5的肢体也必须返回其起点。此规则是对计算正确性的检查-所有圆都必须穿过每个肢体的起点。

接下来，使用以下公式开始通过获得的圆弧参数计算点：

$$

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

$$

$$x_i = R + R_i * cos(arcAngle_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;
}

然后，我决定还显示Y坐标的计算（按时间计算Y点）。计算取决于所选的轨迹，该轨迹在代码中严格设置，对于实现肢体在地面和空中的移动是必不可少的。

还有一个用于实现反向运动的部分（如果需要，可以反向运动时间）。在地面上的三个分支运动期间，其他三个分支必须通过空气以相反的方向运动。

结果