结合国际经验的主页 - 21ic电子技术开发论坛

至此，小车的寻路算法已经基本实现。

具体实现代码如下：

画面中首次出现路时，往往在画面的最边缘，这时恢复原来的算法可能导致转向速度异常。因此我们考虑重复获取 ...

对急转弯的单独讨论实现上述算法后，在急转弯时小车并不能正确运行。这是因为依据我们的算法，急转弯的尽 ...

如此一来，小车就能正常依据画面状况来控制左右轮子的速度，从而实现寻路了。 ...

......

接下来就是PD算法的主体实现部分了。设定速度的基础值为1.2，比例和微分控制常量分别为0.02和0.001，计算速 ...

如果direct_tmp为0，说明其不带有方向性，不更新实际的方向direct：

2.3 PD算法控制小车速度控制速度时，我们只考虑比例和微分部分。因为路径状况不稳定，不同的位置有不同的 ...

如此一来，就得到了当前的误差err与下次急转弯的预期转弯方向direct，并进行返回。 ...

如果当前行没有找到路，则考虑画面的上一行，对distance进行递减。但是我们不希望对太远的地方的路进行判断 ...

再或者，新路是当前第二近的路，则直接用新路的误差进行err2和direct的更新： ...

当然，最后还要对err进行更新：

否则，新的路成为最近的路，原先最近的路成为第二近的路，需要对err2和direct进行更新： ...

......

接下来依据误差值dis对err和err2进行更新。如果dis的绝对值小于err，说明新找到的路径就是离中点最近的路径 ...

......

如果当前行没有找到路，则误差err不会更新，则需要找画面上一行，因此对err的值做一个循环判断： ...

其中err表示离中点最近的路的误差，err2表示离中点第二近的路的误差。注意误差可正可负，其绝对值表示到终 ...

首先初始化以下几个变量：

结合国际经验