2D雷达旋转方向与运动畸变去除

2D雷达的安装方向与坐标系的定义有关，range矩阵的存储顺序与去畸变有关，当倒装雷达时，会改变我们看到的雷达的旋转方向，需要使用外参对雷达的点云进行转换

1. laser to pointcloud的转换公式

angle = angle_min + (i * angle_increment)

2. range矩阵的存储顺序与去畸变

为了方便去畸变（时间插值）需要保证range矩阵中元素的存储顺序与扫描到顺序一致，即，

ranges数组中第一个元素存的scan中第一个扫描到点ranges数组中最后一个元素存的scan中最后一个扫描到的点

3. 极坐标系转换到笛卡尔右手系

对于不同旋转方向（俯视）的雷达，其角度都是从angle_min->angle_max变化的

3.1 如果雷达是顺时针旋转的（比如，正装）

旋转角度是从+180 -> -180，是递减的所以angle_min=+180，angle_max=-180，angle_increment是负值（angle_min比angle_max大有点有违常理）

3.2 如果雷达是逆时针旋转的（比如，倒装）

旋转角度是从-180 -> +180，是递增的所以angle_min=-180，angle_max=+180，angle_increment是正值（这些是经常使用的value）

4. 正放时逆时针旋转雷达

其laser scan msg如下：

angle_min =-135*(pi/180); //扫描的开始位置对应的角度，转换为弧度形式 angle_max = 135*(pi/180); //扫描的结束位置，转换为弧度形式 angle_increment = 0.25 * (pi/180); //两次扫描之间增加的角度，转换为弧度形式 time_increment = (1 / 50) / (1081); //我们假设传感器每秒扫描50次，即每20毫秒连续发出1081道光束进行一次整场的扫描，所以每两道扫描光速间隔（20ms/1081≈）0.0185ms scan_time = 0.02; //扫描时间 range_min = 0; //可以测到的最近的距离，单位米 range_max = 20; //可以测量的最远的距离 //对于这个雷达，距离就是1081个元素的数组，最大和最小测量范围这两个点的数据需要被丢弃，具体怎么丢弃我还没找到相关的解释，应该是接收方处理 ranges[0] = //角度为-135°处测量到的障碍物距离 ranges[1] = //角度为-135.75°处测量到的障碍物距离 . ranges[1080] = //角度为135°处测量到的障碍物距离 //下面这个是强度，同样是一个数组，如果传感器不能返回强度数据，就设置为空，强度的意思就是返回光的强度，代表了反射面的情况 intensities[0]= . intensities[1080]=

5. 正放时顺时针转的雷达

ydlidar雷达正放时，看到的转向是顺时针的，并且ranges数组中第一个元素存储的元素是scan中最后一次扫描到点（这样就可以方便地令angle_min=-180，angle_max=+180了） 由于range矩阵的存储顺序与扫描时间反序的，不利于去畸变，需要进行纠正： 1） 首先应保证ranges数组中第一个元素存储是scan中第一次扫描到点，即先要将ranges矩阵逆序存一遍 2） 接着根据旋转方向做如下处理：

如果雷达是正放时（顺时针转），应设置angle_increment为负值，angle_min为+180，angle_max为-180，坐标系是：X前、Y左、Z上如果雷达是倒放时（逆时针转），应设置angle_increment为正值，angle_min为-180，angle_max为+180，坐标系是：X前、Y左、Z上

6. range矩阵的存储顺序对去畸变的影响

ydlidar的原始驱动中的range矩阵存储顺序与实际的扫描时间是反序的，用carto跑出来的结果如下图左，修改后，跑出来的结果如下图右

---

@leatherwang 二零二零年九月三十日

---