[发明专利]一种基于粒子群优化的物体点云位姿估算方法和系统

摘要

本发明实施例涉及一种基于粒子群优化的物体点云位姿估算方法和系统，属于空间机器人视觉感知技术领域。其中，该方法包括：初始化粒子群内各个粒子的代表的初始位姿和移动速度，根据初始位姿，对空间点云做旋转变换，根据变换后的点云数据和点云所在的几何模型，确定每个点与几何面的位置关系，进而确定变换后的点云与几何模型的最邻近距离，根据最邻近距离和算法规则确定每个粒子的适应度，根据适应度更新粒子的位姿信息，并根据更新位姿信息确定点云的位姿信息。通过本实施例提供的技术方案，实现了精准且高效的获取点云位姿信息的技术效果。

主权项

1.一种基于粒子群优化的物体点云位姿估算方法，其特征在于，所述方法包括：步骤S100：根据预先设置的预处理规则对获取到的点云中的点的初始点云数据进行预处理，得到点云数据；步骤S200：根据预先设置的初始化规则对粒子种群中的粒子进行初始化，得到粒子的位姿信息；步骤S300：根据所述位姿信息和所述点云数据对所述点云中的点进行平移和旋转，得到变换后的点云数据；步骤S400：遍历变换后的点云数据对应的点，根据变换后的点云数据和变换后的点云所在的几何模型，确定变换后的点云数据对应的点与所述几何模型中的几何面的位置关系；步骤S500：根据预先设置的计算规则和所述位置关系确定变换后的点云与所述几何模型的最邻近距离；步骤S600：根据所述最邻近距离和预先设置的算法规则确定每个所述粒子的适应度；步骤S700：根据每个所述粒子的适应度和获取到的每个所述粒子的历史适应度，确定每个所述粒子的历史最优得分和全局最优得分；步骤S800：根据预先设置的更新规则和每个所述粒子的历史最优得分和全局最优得分，更新每个所述粒子的位姿信息，得到更新位姿信息；步骤S900：当所述更新位姿信息对应的误差小于预先设置的误差阈值，或者，更新的次数等于或大于预先设置的迭代阈值时，则根据所述更新位姿信息确定所述点云的位姿信息；步骤S1000：当所述更新位姿信息对应的误差等于或大于预先设置的误差阈值，或者，更新的次数小于预先设置的迭代阈值时，则返回步骤S300。