从零开始开发六脚架（第4部分）-数学轨迹和序列

大家好！ 六足动物的发展又向前迈了一步。 这次，肢体运动的轨迹得以实现和测试-运动数学的下一部分。 在本文中，我将讨论这些轨迹和运动的基本顺序。 我希望这会很有趣。

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

弹道

该机制的本质是，当您设置两个点时，可以选择肢体的轨迹。 从一个点移动到另一个点时，坐标将根据某些参数方程式发生变化。 该机制非常强大，可以让您获得有趣的运动曲线。 它还可以通过更改参数t的步长来实现运动的平滑-步长越小，中间点越多，分别具有较低的速度和较高的运动平滑度。

在某些地方设置轨迹参数的过程有些令人费解，您可能会感到困惑。 困难在于以下事实：当设置起点和终点的坐标时，并不总是设置空间中实点的坐标，即 一些坐标设置轨迹参数。 我必须编写一个显示给定路径的程序，同时检查该路径各点的可达性。

驱动程序支持以下运动路径：

1. XYZ_LINAR是所有轨迹中最简单的。 在向前，向后，向上和向下移动时使用该轨迹。 所有坐标都是线性变化的，计算如下：
   
   

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

   
   在这里我们知道没有问题。 坐标指定平行六面体角度，并且它们与真实坐标重合。 移动沿着盒子的对角线进行。
   
   
   轨迹可视化

   
   
   
   
   
   
2. YZ_ARC_Y_LINEAR-此轨迹允许您实现沿弧的移动。 当您需要在地面上移动肢体时，会使用该轨迹。 坐标计算如下：
   
   

    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 

   
   这就是乐趣的开始。 坐标指定了射线的方向以限制电弧，并且它们可能与实际坐标不一致。 光线在同一平面上，而圆的半径等于到起点的向量的长度。
   
   
   轨迹可视化

   
   
   
   
   
   
3. XZ_ARC_Y_SINUS-此轨迹还允许您实现沿弧的移动，但比YZ_ARC_Y_LINEAR复杂。 当需要在空中移动肢体时，会使用该轨迹。 坐标计算如下：
   
   

    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 

   
   乐趣继续。 坐标还指定了射线的方向以限制圆弧，但它们与实际坐标不一致。 目标点的Y坐标设置正弦高度。
   
   
   轨迹可视化

   
   
   
   
4. XZ_ELLIPTICAL_Y_SINUS-此轨迹允许您实现沿椭圆的运动。 当需要在空中移动肢体时，可使用轨迹进行前后移动。 该轨迹是XZ_ARC_Y_SINUS的复杂部分，仅由于使用XZ_ARC_Y_SINUS时视觉上步态难看（步长太大）而需要使用。 坐标计算如下：
   
   

    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; 

   
   坐标指定平行六面体角度，它们与真实坐标不重合。 移动发生在盒子的下角，位于与触摸部件顶部相同的平面上。 最好看一下扰流板中的图片，我不知道该如何简单地用文字来描述。
   
   
   轨迹可视化

   
   

这样就完成了六脚架运动的基本数学运算。 在我的项目中，这是几乎所有动作实施的必要最低要求。

顺序

一点理论

顺序是构成步态的基本动作。 它们分为周期性和非周期性。

• 循环序列可以执行多次，并且在每个循环结束时应使肢体返回其原始位置（运动和旋转）；
• 非循环序列仅执行一次（上升和下降）。

每个序列具有三个迭代块：准备块，主块，完成块。

• 训练块-包含将肢体移动到序列的起始位置的迭代。 就我而言，向前移动需要在开始移动之前将双脚置于一定位置。 转换到序列后执行一次；
• 主块-包含序列的主迭代。 它可以循环执行；
• 块完成-包含将肢体移动到基本位置（举起后肢体放置的位置）的迭代；

下图显示了前进的顺序。

• 红点表示四肢开始运动之前的初始位置
• 蓝线表示地面上肢体的轨迹
• 黑线表示空中肢体的运动轨迹
• 箭头指示顺序

点的坐标是根据外壳的配置选择的。 我选择了尽可能靠近身体的点以减少杠杆的长度。 在该序列的一个周期中，六脚架移动了18厘米（在1个周期中，执行2步-3个肢体中执行1步）。 如果使距离更大，则四肢将开始相互紧贴。 此参数仅受案例配置限制。


该序列由每个肢体的两个点（1、2）定义，并使用两个路径：XYZ_LINEAR（蓝线）和XZ_ELLIPTICAL_Y_SINUS（黑线）。 XZ_ELLIPTICAL_Y_SINUS轨迹使用点1来设置正弦高度，并相应地设置脚的上升高度。 点2和3是肢体在移动时到达的真实点。

点的位置仅取决于您的想象力和六足动物的能力。 也许这一切都变得有点复杂，并且有一个更简单的选择，但是显然我还没有达到目标。

实作

现在，让我们看看所有这些幸福的实现。 具有序列参数的结构如下：

 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-包含有关序列迭代的信息：

• point_list-XYZ格式的每个肢体的点数组；
• path_list-每个肢体的轨迹数组；
• smooth_point_count-设置路径点的数量（参数步骤t）；

sequence_info_t-包含有关整个序列的信息：

• is_sequence_looped-设置序列类型：循环或非循环；
• main_sequence_begin-设置迭代列表数组中主块的起始索引；
• finalize_sequence_begin-设置eration_list数组中完成块的起始索引；
• total_iteration_count-设置序列中的迭代次数；
• eration_list-迭代数组；
• available_sequences-设置可用于从当前序列过渡的序列列表（例如，我们必须先从地板上站起来才能开始行走）；

注意：没有有意指示准备块的索引；它始终位于迭代数组的开头。

不幸的是，我在这里无法想象序列定义代码，因为 它很宽，传输后看起来很糟糕。 我只在此处保留指向定义文件的链接。

运动处理方案

找出序列在运行时经历的所有地狱是值得的。 处理方案如下：

1. 运动引擎 -组织处理和序列之间的切换。 在那里不执行任何计算。 如果得到简化，则在当前处理完成后，该模块会将下一个点滑至LIBMS DRIVER模块。
   模块输入：目标点的坐标数组。
   模块输出：序列当前迭代的目标点。
2. LIBMS DRIVER是所有模块中最复杂的模块。 运动的所有数学都在这里占主导地位：逆运动学，轨迹计算和运动平滑。 该模块与PWM模块具有严格的同步。 计算分别以150Hz的频率进行，驱动器的控制脉冲也以150Hz的频率提供。
   模块输入：目标点的坐标。
   模块输出：伺服旋转角度。
3. 伺服驱动器 。 除了一堆用于设置和调整驱动器的参数外，它没有什么特别的。
   模块输入：伺服旋转角度。
   模块输出：控制脉冲宽度。
4. PWM驱动器 。 用于驱动器控制的PWM驱动器软件。 在这里，针脚仅在适当的时间抽搐。 PWM同步同步变量在每个PWM周期增加。
   模块输入：控制脉冲宽度。
   模块输出：控制引脚上的脉冲。

我试图使模块彼此独立，我成功了。 这样，您就可以在该电路中插入任何中间模块（例如，景观的适配模块），并且不会有任何东西同时中断，而实现只需很少的代码更改即可完成。

发现最新消息和项目小龙虾