基于点云强度显著点的机器人位姿优化方法及系统

专利主权项内容

1.一种基于点云强度显著点的机器人位姿优化方法，其特征在于，包括以下过程：当某有效扫描点与前一个有效扫描点之间的强度差大于设定强度阈值时，判定此有效扫描点为强度显著候选点，对各有效扫描点依次判断，得到所有的强度显著候选点；根据强度显著候选点得到强度显著点，包括：当某强度显著候选点之前的N个点之间的强度差均小于设定平坦阈值，且此强度显著候选点之后的N个点之间的强度差均小于设定平坦阈值时，判定此强度显著候选点为强度显著点，对各强度显著候选点依次判断，得到所有的强度显著点，其中N为大于1的正整数；对得到的各强度显著点，将任一强度显著点与前一点的中间位置设为此强度显著点对应的最终强度显著点的位置；根据强度显著候选点得到强度显著点，根据相邻两帧扫描数据之间的强度显著点，进行点云匹配，得到两帧扫描数据间的位姿变换估计结果，基于位姿变换估计结果以及初始帧扫描数据，得到优化后的机器人位姿；基于位姿变换估计结果以及初始帧扫描数据，得到优化后的机器人位姿，包括：对初始帧扫描数据进行体素滤波，基于体素滤波后的扫描数据得到机器人的初始预测位姿，根据初始预测位姿以及位姿变换估计结果，得到更新后的预测位姿；结合更新后的预测位姿构建子地图，以机器人追踪位姿与子地图原点位姿在世界坐标系下的相对位姿与约束的差值作为第一残差项；以机器人追踪位姿与子地图原点位姿间在世界坐标系下的相对坐标变换，与相邻两个机器人追踪位姿在世界坐标系下的相对坐标变换的差值作为第二残差项；以机器人追踪位姿与子地图原点位姿间在世界坐标系下的相对坐标变换，与相邻两个机器人追踪位姿在强度显著点匹配后得到的相对坐标变换的差值作为第三残差项；以第一残差项、第二残差项和第三残差项的加和最小为目标，求解后得到一个位姿增量，将位姿增量与更新后的预测位姿相加后，得到优化后的位姿。