cartographer landmark问题 方法

关注
cartographer中增加landmarks
创作声明：内容包含虚构创作
1 年前
cartographer在跑纯定位的时候，判断定位丢失的标准是一个比较棘手的事情，目前我们采取的措施是，scan_tomap的方式，根据PCL库中的函数，得出一个类似于匹配度的值，然后测试一些实际场景，根据实际场景来设定该值的阈值。但是这在环境经常变化的场景中，效果会很差，当然cartographer自己维护的PoseExtrapolator,虽然环境变化比较大，但是计算出来的定位坐标是正确的。但是在环境比较相似的情况下，这种还是会出现坐标的跳变，从一个位置跳到了另外一个位置，这种情况也会发生在环境变化不大的场景中。目前我们的做法是，对定位的坐标点做了一个类似于cartographer中的MotionFilter的一个逆过程。如果前后两个坐标的欧式距离大于车体的速度（这里需要根据自己获取坐标的频率做一下处理），那么认为定位坐标是不对的。目前，又加入了一些新的功能，其实在cartographer的官方实例中已经有demo了，我们所做的只不过是把实时看到的landmark按照相应ros格式发布。

我的数据发布的格式是：landmark_list_.header.frame_id = “laser”，位置权重和旋转权重设置的都是10，这个还没有进行大量数据的测试，而且在官方文档中也找不到具体数据所代表的含义。因为发布的一个优点是，只需要计算landmark相对与自己雷达的坐标系，比较适用与LaserScan这样的数据格式。这里边比较难的一点是landmark的id识别问题，测距和反光柱的距离计算是比较简单的（因为我们的反光柱定位已经做的很成熟了）。但是如何识别id呢？我现在是有两个思路：1、按照反光柱的方式，进行三角定位识别反光柱的id;2、依靠cartographer的map中的坐标来识别。

方法一中的计算可能比较难一些，需要自己去建立一个最小二乘的数学模型，来迭代计算，而且对反光柱的布置也有要求（感兴趣的同学可以看一些sick nv350）。但是在cartogarpher中，反光柱（也就是landmark）尽量稀疏一些，因为就是一个辅助吗，给优化增加一些元素而已。所以这种方法不是很实用，不过也跟场景有关，如果场景允许的话，这种方式还是比较鲁棒的，而且在此基础上可以做一个融合。

方法二呢，有点很明显，直接用欧式距离来判断就ok了，但是一个致命的缺点就是，如果cartographer定位失败了，那么会产生很严重的后果，就是landmark的识别就全错了，导致更严重的错误。这其实可以认为是一个鸡生蛋和蛋生鸡的问题。

目前我采用的是第二种方案。为什么呢？首先计算简单，而且我们的客户不允许布置拉么多的反光柱，所以就想办法去解决。首先，初始定位。我采用的是一个全排列的方式，将实时看到的反光柱跟地图中的反光柱的id进行排列，AMN;然后，挨个尝试。地图中的发光柱的获取，可以看cartographer中的源码，用cartographer::io::ProtoStreamReader stream和
cartographer::io::ProtoStreamDeserializer两个类，就可以获取pbstream中的数据。但是呢，cartographer呢，对landmark的优化，又有一个时间的需求，所以这一块我还是没有做的很好，这个以后再详细研究。笨办法是，自己的robot原地转圈找定位。定位成功之后，我采取了两种方式来锁定反光柱的id，landmark_in_local,滑动窗口的方式，类似与MotionFilter，如果这种方式失败了，会进行landmark_in_global的识别。由于公司的原因这里不方便写出了，不过可以私下交流。