1 input:点云数据 $C^{j}$ ，里程计数据 $p_{t}$

2 output:一帧点云扫描期间的机器人位姿变换信息矩阵 $_{c^{j - 1}}^{c^{j}} T$

3 begin:

4 do

5 $d e q_{o} \leftarrow p_{t} d e q_{l} \leftarrow C^{j}$ //将轮式里程计数据和点云数据存入对应的双端队列

6 while( $d e q_{l}$ .size<2)//防止没有对应的里程计数据，缓存两帧雷达数据程序再继续执行

7 FindCorrespondPose ( $d e q_{o}$ , $d e q_{l}$ , $p_{t_{s}}$ , $p_{t_{e}}$ ) //按每帧点云的开始和结束的时间戳 $t_{s}$ Math_140#在 $d e q_{o}$ 中寻找对应的里程计位姿，若没有对应位姿则利用附近时刻的位姿通过式(6)进行线性位姿插值。

8 $T_{t_{s}} \leftarrow p_{t_{s}} T_{t_{e}} \leftarrow p_{t_{s}}$ //位姿矩阵变换

9 ${}_{t_{s}}^{t_{e}}T = T_{t_{s}}^{- 1} T_{t_{e}}$ //计算相对位姿变换即 $t_{e}$ 时刻机器人在 $t_{s}$ 时刻机器人坐标系下的位姿

10 $b_{k} = (t_{k}, θ_{k}) = (t_{t_{s}, t_{e}}, θ_{t_{s}, t_{e}})$ //得到点云配准初值

11 if( $c_{i}^{j} . distance < d_{\min} ‖ c_{i}^{j} . distance > d_{\max}$ ) delete //数据预处理去除无效点

12 do

13 num1=VoxelFilter(C^j,l_max,l_min) //利用体素滤波对点云进行下采样num1、num2分别表示滤波后的激光点数和目标激光点数

14 mid=(l_max + l_min)/2

15 if(num1 _max = midelse l _min = mid

16 Whilenum1 1.1*num2

17 Sort(C^j) //利用选择排序将滤波后的激光点按照角度递增的关系进行排序

18 do

19 fori= 1:n

20 FindClosePoint(C^j, C^j⁻¹)

21 $E (b_{k + 1}) = \sum_{i} {(n_{i}^{T} [R (θ_{k + 1}) g_{i}^{w} + t_{k + 1} - c_{i + 1}^{j - 1}])}^{2}$

22 $C^{j} = T_{b_{k + 1}} C^{j}$ ; $_{c^{j - 1}}^{c^{j}} T = T_{b_{k + 1}}_{c^{j - 1}}^{c^{j}} T$ ; $g_{i}^{w} = c_{i}^{j}$

23 while $b_{k + 1} > ε$ //直到配准误差小于设定的值停止迭代

24 return $_{c^{j - 1}}^{c^{j}} T$

25 end