买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：浙江工业大学

摘要：一种基于NDT配准与轮式里程计的激光雷达运动畸变补偿方法，所述方法包括以下步骤：步骤1，轮式里程计运动估计，步骤2，帧间坐标变换，步骤3，优化运动畸变补偿。本发明建立轮式里程计传感器线性插值数学模型，根据轮式里程计测量位姿消除激光雷达的部分运动畸变；基于NDT点云配准方法，设计激光雷达运动畸变的精准补偿。本发明提供一种精准补偿激光雷达运动畸变的方法，实现降低SLAM轨迹累计误差和生成全局一致地图，最终提高SLAM系统的准确性。

主权项：1.一种基于NDT配准与轮式里程计的激光雷达运动畸变补偿方法，其特征在于，所述方法包括以下步骤：步骤1，轮式里程计运动估计，过程如下：1.1设X＝{xi},i＝1,…,n为一帧点云集合，xi表示第i个点的二维坐标向量，Xk表示第k帧点云，xk,i表示第k帧中的第i个点，记t为当前时间戳，则是第k帧的起始时间，则是第k帧的第i个点的时间，由于激光雷达连续扫描的特性，故第k帧的结束时间与k+1帧的起始时间是相等的，激光雷达坐标系{L}是一个二维坐标系，其原点位于激光雷达的几何中心，第k帧雷达坐标系{Lk}中的第i个点表示为p＝[p1p2p3]T＝[xyθ]T表示位姿向量，世界坐标系{W}是一个二维坐标系，在初始位置与{L}重合，第k帧世界坐标系{Wk}中轮式里程计时刻的位姿表示为1.2第k帧起始时间与终止时间分别为和轮式里程计在对应时间戳内的测量位姿为其中表示第k帧世界坐标系中轮式里程计位姿集合，pk,i是在第k帧时间区间内时刻的位姿，是第k帧对应的轮式里程计位姿，借助线性插值可得时刻的雷达估计位姿如下式所示： 步骤2，帧间坐标变换，过程如下：2.1由于雷达通常在移动中扫描，故Xk中每一点都有帧坐标系，运动中的雷达位姿用一个3×3的变换矩阵T表示，如式2: 其中R为2×2的旋转矩阵，t为1×2的平移向量；2.2对不同帧间坐标系的点xk,i使用匹配的轮式里程计位姿从而校正畸变的点云，激光雷达在时刻采集的点通过公式3映射到雷达的起始帧中，计算如下： 其中表示时刻从雷达帧到世界帧的变换矩阵；步骤3，优化运动畸变补偿，过程如下：3.1机器人周围的二维空间被规划地分为大小相同的单元，收集在二维空间内第k帧点云Xk，找出包含扫描点的单元，分别计算该单元中扫描点样本xk,i的平均值qk和协方差矩阵∑k： 3.2建立该单元中扫描点样本概率密度的正态分布模型为 其中，exp为指数函数，px扫描点正太分布的概率；3.3表示通过轮式里程计提供的初始值将第k+1帧扫描点xk+1,i映射到第k帧坐标系，∑k,i和qk,i分别表示点对应的第k帧正态分布模型的协方差矩阵与平均值，利用参数∑k,i和qk,i计算所有点的正态分布累加和的最大值，记总和sp的定义为： 其中，p为待估计的位姿向量参数，argmax表示p是令函数sp取得最大值的参数；3.4由于优化问题一般被描述为最小化问题，因此将函数sp表示为-sp，Newton法可以寻找令函数最优的参数p，采用Newton法迭代求解方程，方程如式7所示：HΔp＝-g7H和g分别是sp的Hessian矩阵和梯度向量，为简化公式，令梯度向量g中的元素gi为 Hessian矩阵H中的元素Hij为 3.5等式7的解是增量Δp，在每次迭代中将Δp添加到当前估计位姿p0，下一次迭代初始值为p：p＝p0+Δp103.6通过对当前帧估计位姿迭代优化，计算出精准位姿参数p，将p与p'的差值作为轮式里程计误差，使误差均匀分布到每帧，引入式1，给出如式11形式： 其中，n为第k帧中点xk,i的数量，p'为轮式里程计位姿增量。

全文数据：

权利要求：

百度查询： 浙江工业大学 一种基于NDT配准与轮式里程计的激光雷达运动畸变补偿方法