[发明专利]基于双雷达的路径规划方法、系统、存储介质及电子设备

权利要求书

1.一种基于双雷达的路径规划方法，其特征在于，包括：

步骤1，获取主激光雷达的第一点云以及辅助激光雷达的第二点云，通过所述第一点云以及所述第二点云生成3D点；

步骤2，基于预设条件，分别对所述主激光雷达以及所述辅助激光雷达进行建图，得到主激光雷达坐标系的全局点云图Map_t以及辅助激光雷达坐标系的全局点云图Map_s；

步骤3，通过ICP算法对所述3D点进行计算，得到粗标定的旋转矩阵R以及粗标定的平移向量t；

步骤4，基于所述主激光雷达坐标系的全局点云图Map_t以及所述辅助激光雷达坐标系的全局点云图Map_s，对所述旋转矩阵R以及所述平移向量t进行处理，得到优化后的旋转矩阵R'以及优化后的平移向量t'；

步骤5，根据优化后的旋转矩阵R'以及优化后的平移向量t'构建融合建图；

步骤6，根据所述融合建图实现路径规划。

2.根据权利要求1所述的一种基于双雷达的路径规划方法，其特征在于，步骤1具体为：

获取主激光雷达的第一点云以及辅助激光雷达的第二点云，在所述第一点云以及所述第二点云中选取共同扫描部分，并从所述共同扫描部分中选取一组3D点P_i,P_i'，其中，P_i＝{P₁,P₂…P_n}，P_i'＝{P₁',P₂'…P_n'}，所述共同扫描部分为：所述主激光雷达以及所述辅助激光雷达的共同扫描部分，所述3D点为已配对成功的3D点。

3.根据权利要求2所述的一种基于双雷达的路径规划方法，其特征在于，步骤3具体为：

通过ICP算法计算P_i以及P_i'之间的姿态变换关系，对所述姿态变换关系进行求解得到粗标定的旋转矩阵R以及粗标定的平移向量t；

其中，所述姿态变换关系为：

其中，i为1，2...n，为任意一个i。

4.根据权利要求3所述的一种基于双雷达的路径规划方法，其特征在于，步骤4具体为：

对所述辅助激光雷达坐标系的全局点云图Map_s转换为主激光雷达坐标系下的全局点云图Map'_s，转换公式为：

Map'_s＝Map_s*R+t

通过所述主激光雷达坐标系下的全局点云图Map'_s、所述主激光雷达坐标系的全局点云图Map_t、所述旋转矩阵R以及所述平移向量t构建误差优化函数，通过对所述误差优化函数的计算，得到优化后的旋转矩阵R'以及优化后的平移向量t'，

其中，所述误差优化函数E(R',t')公式为：

其中，N为主激光雷达坐标系下的全局点云图Map'_s以及所述主激光雷达坐标系的全局点云图Map_t中包含的点云坐标点数量的加和平均数，i为主激光雷达坐标系下的点云地图内部点云坐标点的序号，S_i为主激光雷达坐标系的全局点云图Map_t包含的点云坐标集合S中的第i个点云坐标，T_i为主激光雷达坐标系下的全局点云图Map'_s包含的点云坐标合集T的第i个点云坐标。

5.根据权利要求4所述的一种基于双雷达的路径规划方法，其特征在于，步骤5具体为：

通过优化后的旋转矩阵R'以及优化后的平移向量t'构建变换矩阵，实时获取辅助激光雷达输出的点云坐标，通过所述变换矩阵对所述点云坐标进行变换处理，将变换处理后的结果与主激光雷达获取的点云坐标组合成融合点云坐标，将所述融合点云坐标导入建图软件，得到融合建图。