雷达安装方向与运动畸变去除

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

二零二零年九月三十日

---