wordpress+瀑布流加载,网站排名优化软件电话,网站制作费用贵不贵,红色的网站文章目录 一、轨迹规划1.1轨迹平滑与轮迹1.2 目标距离1.3 速度限制1.4 候选速度的计算与调整1.5 路径生成 二、双轮轨迹2.1 计算梯度2.2 计算偏移轨迹2.3 返回结果 一、轨迹规划
1.1轨迹平滑与轮迹
初始时#xff0c;我们有一条由若干坐标点构成的机器人运行路径。通过对这些… 文章目录 一、轨迹规划1.1轨迹平滑与轮迹1.2 目标距离1.3 速度限制1.4 候选速度的计算与调整1.5 路径生成 二、双轮轨迹2.1 计算梯度2.2 计算偏移轨迹2.3 返回结果 一、轨迹规划
1.1轨迹平滑与轮迹
初始时我们有一条由若干坐标点构成的机器人运行路径。通过对这些坐标点进行插值我们可以得到一个更加平滑的轨迹使得机器人在移动时能够更平稳地过渡。接着我们基于这条中心轨迹计算出左轮和右轮的轨迹。这是通过向中心轨迹的左侧和右侧分别偏移一定宽度来实现的。
1.2 目标距离
对于中心轨迹以及左轮和右轮的轨迹我们分别计算出每个点到终点的距离。这个距离是为了确保机器人在接近终点时能够逐渐减速并停下来。同时我们还计算出每个点到其下一个点的距离。这个距离将用于后续的速度计算。
1.3 速度限制
对于轨迹上的每个点我们都根据当前速度、最大速度限制、最大加速度以及到下一个点和终点的距离计算出三个速度限制最大速度限制max_v、基于到下一个点距离的加速限制速度max_v_acc和基于到终点距离的减速限制速度max_v_dec。注相邻两个点之间计算max_v_acc时以前一点的速度计算下一点能达到的最大速度 (v^2 u^2 2ax)。然后我们选择这三个速度限制中的最小值作为该点的最大允许速度。
1.4 候选速度的计算与调整
在计算出中心轨迹上每个点的最大允许速度后我们得到一个候选速度。这个速度是基于中心轨迹的当前速度、最大速度限制、最大加速度以及到下一个点和终点的距离来计算的。接着我们根据左轮和右轮轨迹上对应点的距离比例将候选速度分配到左轮和右轮上得到左轮和右轮的候选速度。如果左轮或右轮的候选速度超过了它们各自的最大允许速度我们就需要对候选速度进行调整。调整的原则是确保左轮和右轮的速度都不会超过它们的最大允许速度并且尽量保持中心轨迹的速度不变。调整后的候选速度被用作当前点的实际速度并更新到结果路径中。
1.5 路径生成
经过上述计算我们得到了一个包含每个点速度和位置的结果路径。这个路径考虑了机器人的最大速度限制、最大加速度以及左轮和右轮的速度限制。最后我们在结果路径的末尾添加了一个速度为0的点表示机器人已经完全停止。
二、双轮轨迹
2.1 计算梯度
首先遍历中心轨迹上的每个点除了最后一个点计算相邻两点之间的差值即位移向量。使用atan2函数根据位移向量的y分量和x分量计算出每段轨迹的倾斜角度即梯度。这个角度表示了轨迹在该点的切线方向。为了确保最后一段轨迹也有一个梯度值将最后一个计算出的梯度值复制到梯度数组的末尾。
2.2 计算偏移轨迹
接着创建一个新的路径即偏移轨迹这个路径将包含与中心轨迹相同数量的点。对于中心轨迹上的每个点根据该点的梯度值和给定的偏移量offset计算出偏移轨迹上对应点的坐标。这个计算过程考虑了是计算左轮轨迹还是右轮轨迹通过left参数来控制。 如果是计算左轮轨迹会给梯度值加上90度即M_PI / 2.0这样计算出的偏移点就会位于中心点的左侧。如果是计算右轮轨迹会给梯度值减去90度即-M_PI / 2.0这样计算出的偏移点就会位于中心点的右侧。 使用三角函数cos和sin和偏移量计算出偏移轨迹上每个点的x坐标和y坐标。最后将计算出的偏移点添加到偏移轨迹中。
2.3 返回结果
返回计算出的偏移轨迹这个轨迹是一系列坐标点构成的路径表示了左轮或右轮的行驶轨迹。
