SLAM从入门到精通（基于ros的反光柱定位）

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        我们看论文或者看书的时候，讲的最多的一般就是基于自然环境的定位。不管是激光雷达，还是视觉，其实本身还有很多的不确定性。而对企业、工厂来说，大部分情况下没有办法容忍这种不确定性的。所以很多时候，企业里面用的定位技术往往不是那么高大上的技术，比如激光就用反光柱，视觉就用二维码等等。这里反光柱定位多用于大型物流设备，也算是用的比较多的一种定位方法。

        ros上面其他slam算法很多，但是反光柱的算法没怎么提，这里面最大的原因可能是因为商业的考虑，当然也可能是不屑一顾。但反光柱本身如果做的好，其实是真的可以帮企业挣钱的。所以，我们如果想学习反光柱定位的话，可以找一些开源的博客来看下，csdn下面的这一个专栏就有很大的参考意义。

https://blog.csdn.net/weixin_48702107/category_11927168.html

        本身总共有12篇文章，我们可以简单梳理一下，挑出对自己最有价值的那个部分。当然，这些文章中的内容对于一个产品来说还是远远不够的，不过可以用它来进行反光柱入门。下面，我们一起来学习下。

1、查找反光柱最强点

        反光柱有一个很大的特点，那就是对于激光的反射强度特别高，这是一个非常明显的特点。当然这也是反光柱本身区别于其他自然物最重要的特征。一般反光柱就是一个圆柱体，圆柱体之外包着一个反光贴，直径大约7-8cm左右。所以如果环境中只有一个反光柱的话，可以通过强度比较的方法，找出反光率最大的那个点，其实就知道了反光柱的大体位置了。

2、查找多个反光柱

        前面只是讨论了一个反光柱的情形，但是如果是多个反光柱的话，简单通过最大值的方法，是没有办法找出反光柱的位置的。这个时候，就要通过适合的阈值，将雷达的离散点拆分成几个区域，每一个区域就代表着一个反光柱。找到这些区域之后，再根据中间点就可以找到反光柱的基本位置了。经过这一步，我们已经成功从识别单个反光柱，发展到识别多个反光柱。

3、反光柱rviz可视化

        rviz本身不定义反光柱，所以可以通过Marker信息将反光柱绘制出来，这样也方便调试。相关内容，之前的文章也涉及过，

https://blog.csdn.net/feixiaoxing/article/details/133973755?spm=1001.2014.3001.5501

4、确定多反光柱的位置

        和2不同的是，这里我们需要进一步确定反光柱中心的位置。假设某一个区域代表一个反光柱，它的起始角度和末尾角度的中间值就可以看成是圆心的入射角度。通过中值查找，找出它的r_laser和theta角度之后，我们就可以求解出对应的x和y，其中反光柱的半径假设为r_landmark，

x = (r_laser + r_landmark) * cos(theta)
y = (r_laser + r_landmark) * sin(theta)

          有了x和y，借助于rviz的可视化，就可以将反光柱绘制出来了。

5、三角函数法求解圆心

        相比较4求解圆心的方法，三角函数是另外一种计算圆心的方法，有兴趣的同学可以继续看下。暂时没有时间了解的同学，可以先忽略一下。

https://blog.csdn.net/weixin_48702107/article/details/125928758

6、对拟定圆心进行保存和读取

        保存和读取本身就是一个文件的保存和访问操作，这个没有什么可说的。一般来说，对于圆心的保存可以看成反光柱地图的保存，而读取则可以看成是反光柱地图的加载。不过当前保存的信息还是基于lidar坐标系，后期需要更改成全局坐标系。

        我们的机器人停在一个固定的地方，这个时候就可以通过反光柱扫描的方法生成一个反光柱地图。后期机器人移动的时候，就可以通过进一步反光柱匹配，经过计算得到当前机器人的位姿信息（x/y/theta）。

7、反光柱匹配

        如果机器人处于静止状态，本身机器人的位置就可以看成是(0,0,90)。但是一旦机器人移动开来，那么这个时候，lidar看到的反光柱数量有可能会发生了改变。不仅如此，对于lidar坐标系来说，它所看到的反光柱坐标其实也发生了改变。但是，这个里面有一点没有发生改变的，那就是不同反光柱之间的距离其实是不变的。因此这个时候，我们就要根据距离对新扫描的反光柱和之前反光柱地图中的数据，进行一个距离匹配。

        匹配之后，我们拿到了之前反光柱中圆心的位置，也就可以计算出小车的位姿信息了。

8、坐标转换

        这里的坐标系转换，主要就是将lidar坐标系转成世界坐标系。现在假设机器人的坐标是(x0/y0/theta0)，机器人lidar位于后轮法线的正前方，距离是l，那么这个时候lidar的全局位姿就是，

xa = x0 + l * cos(theta0)
ya = y0 + l * sin(theta0)

        而lidar的扫描点坐标假设为(xr/yr)，其中xr和yr可以依赖于lidar的r、angle来进行计算，那么扫描点在全局坐标下的位置为，

x_glb = xa + xr * cos(theta0) - yr * sin(theta0)
y_glb = ya + xr * sin(theta0) + yr * cos(theta0)

        将上面的xa和ya结论带入，

x_glb = x0 + l * cos(theta0) + xr * cos(theta0) - yr * sin(theta0)
y_glb = y0 + l * sin(theta0) + xr * sin(theta0) + yr * cos(theta0)

        这个时候我们假设小车最初的位置是(0/0/90)，这样就可以计算出全局坐标下的反光柱位置，有兴趣一点的话，可以把周围场景的轮廓也记录一下。

9、初步定位

        初步定位的方法主要还是基于三边测量的原理。这里初步定位只是计算了x和y，没有计算机器人的theta角。有兴趣的同学可以看一下初步定位的计算流程，

https://blog.csdn.net/weixin_48702107/article/details/126075770

        希望手动推一下计算过程的同学，可以直接看这个图尝试一下，

10、动态记录

        所谓的动态记录，就是运动过程中如果发现新的反光柱，也进行保存处理。当新反光柱加进来之后，这样新反光住就变成了旧反光柱，用这个刚加进来的旧反光柱计算位姿后，去发掘更多的新反光柱。

11、动态匹配定位

        和动态记录一样，主要是优化6的流程，将保存的反光柱地图从lidar坐标系一步升级为全局坐标系的数据。之前是先求解lidar的全局位置，而后转化成机器人的全局位置，这里做了一步到位。因此需要反光柱地图中的数据也变成了全局坐标数据。优化之后，小车也可以动态计算出位置信息来了。

https://blog.csdn.net/weixin_48702107/article/details/126129166

12、位姿发布

        在第9步过程中当时只是求解出了x和y，其实根据8中的公式，找一个反光柱，就可以分别求出cos(theta0)和sin(theta0)的数值。借助于cos(theta0)和sin(theta0)就可以知道机器人的角度。讲究一点的话，可以找三根反光柱，做一个平均值计算。这样，有了x/y/theta之后，小车本身的位姿信息其实就算被求解出来了，当然也就可以发布了。

13、总结

        至此反光柱定位的方法已经被总结出来了，但目前为止，这个流程最多算一个demo，离实际产品还有一定距离，需要我们不断迭代和完善才行。最后还是非常感谢这位实习同学的专栏，它可以帮助很多同学了解一下反光柱定位究竟是怎么做的。