机器人规划控制——工程化——心得日记-20240510

近一周一直在调试机器人过迷宫形路线，这种路线特点是障碍物之间距离较小且障碍物也比较多，基本机器人会一直发生干涉检测，请求全局路径，然后再控制机器人前进。

遇到一个特别有趣的问题，当然最后查出来原因也感觉比较纯，就是我们机器人在行进过程中，会当前要走的路径与周围障碍物发生了干涉检测即有碰撞风险，然后会请求全局规划，我们注意到计算的局部目标点一直在远处挪，突然某一贞，生了局部的路线，即全局的路线不是从起点开始的，而是从另外很远的地方开始，但终点是对的，这样全局规划的线是错误的，但是全局规划器返回的状态是success，那局部控制器就跟踪这个有问题的全局线，进而导致机器人撞到障碍物了。

上述是现象的描述，下面是解决问题过程中的尝试:
（1） 阶段一:我一开始怀疑是当前路线和全局路线合并时处理的问题，不认为是potential A规划有问题，然后我在代码中加了potential A生成的全局路径初始2个坐标打印，然后重新复现，采集数据，并分析。注意到确实是全局输出的结果不对，a规划路线的初始位置和当前机器人位置不一致。排除了路线合并时的处理问题。
（2）阶段2:我怀疑是a算法在计算过程中，由于某种原因，导致了传入的初始位置坐标错误，在在代码中添加相关打印，分析表明，传入的初始位置是正确的。依然尚未解决。
（3）阶段3:在看代码的时候，我注意到，干涉检测成功请求全局规划时，我会先使用局部的膨胀地图（10米✘10米）去用potential a*
规划路径，如果规划失败，我会调用全局膨胀地图（将局部膨胀地图合并到全局膨胀地图上）去规划，问题出现在这个全局膨胀地图和局部膨胀地图上，这是两个栅格地图，他们的原点不一样，但是在计算起点世界系下的坐标时，只考虑了局部膨胀地图，所以导致映射错误，最终计算错误。于是修复此逻辑，重新测试机器人。又注意到，机器人在请求全局规划时，使用局部膨胀地图规划成功了，但依然出现此问题。尚未解决。。。
（4）阶段4:到这里，我基本怀疑potential a算法的代码有问题了，加了很多打印，最后怀疑是回溯获取路线有问题，然后又加了很多打印，又开始实车复现，记录数据，分析数据，最终结果表明，回溯的那块确实有问题，他回溯到了一个很远的点，并不是起点，然后这俩的index索引值一致，导致回溯程序提前结束，其实并没有找到起点。到此我怀疑是这里不该用索引去判断是不是找到了初始点，而是用坐标值去判断是否一致。
朋友们，你是不是以为到此就解决了此bug，请注意，并没有！并没有！
（5）阶段5:我们进一步分析，又发现了一个问题，就是回溯到的那个起点map系下的坐标居然超过了局部膨胀地图的范围，这说明回溯这里越界了，搜索的范围不在局部地图里了。这又是为什么呢？我们又去看下计算的目标点的map系坐标果然超过了局部坐标系的范围。。。。。。。
至此，这个问题分析出来了，就是为全局规划起设定的目标点不在地图的范围，这种情况下使用potential a去计算就会生成错误的路线。。

哎，想想好蠢啊，范了这么低级错误！

=======20240511更新=
今日去公司在测试机器人的时候，发现昨天已经解决的上述问题又复现了，通过在c++代码中添加log，并实验复现抓取log，我们注意到当设置目标点在局部膨胀地图的范围内，依然会出现上述问题，我们通过对算法的分析以及理论思考，potential A算法的回溯路径的代码有问题，他在搜索回溯路径的时候未对膨胀地图的范围进行限制，由于索引映射成笛卡尔坐标不具备唯一性，如果不对搜索的地图范围进行限制，完全有几率在地图范围之外找到一个和起点一样的索引值，那么回溯函数就会提前终止，导致生成错误路径。
于是我们针对上述回溯搜索越界问题，对potential A算法回溯异常的部分添加了防止超过地图边界的限制，最终得以解决了此问题。

最后特别感谢我的同事龙哥给和我一起分析讨论，并在关键的节点给出了正确的指导，才得以顺利解决这个开源算法potential A星算法代码中潜在的严重bug。

鞠春宇
于沈阳
20240510