[发明专利]三维位姿估计方法、系统、计算机设备及存储介质

权利要求书

1.一种三维位姿估计方法，其特征在于，所述方法包括：

标定待位姿估计物上的各个传感器，并获取标定后各个所述传感器对应的传感器参数；

获取各个所述传感器按照所述传感器参数采集环境数据，并对所述环境数据进行融合处理，得到与所述待位姿估计物对应的局部位姿信息；

提取所述局部位姿信息中的激光数据，获取先验信息，并根据所述激光数据、所述先验信息生成全局位姿数据的关联信息；

根据所述环境数据、所述局部位姿信息、所述关联信息构建目标函数；

求解所述目标函数，得到与所述待位姿估计物对应的三维位姿估计。

2.根据权利要求1所述的三维位姿估计方法，其特征在于，所述标定待位姿估计物上的各个传感器，并获取标定后各个所述传感器对应的传感器参数，包括：

标定轮式里程计轮间距及轮半径参数；标定惯性传感器的内参数和测量噪声；标定磁强计的噪声参数；标定气压计的噪声参数；

标定激光雷达与载体运动中心的相对位姿关系；标定所述惯性传感器与所述激光雷达之间的相对位姿关系；标定GPS与所述激光雷达的相对位姿关系。

3.根据权利要求2所述的三维位姿估计方法，其特征在于，所述获取各个所述传感器按照所述传感器参数采集环境数据，并对所述环境数据进行融合处理，得到与所述待位姿估计物对应的局部位姿信息，包括：

对所述惯性传感器采集的数据进行积分，得到当前时刻所述待位姿估计物的运动数据；对所述惯性传感器采集的数据和所述激光数据进行时间同步，利用所述惯性传感器预积分的结果对所述激光雷达进行畸变矫正，并对所述激光雷达采集的数据进行噪点剔除，得到所述激光数据；

根据所述运动数据、所述激光数据，得到与所述待位姿估计物对应的局部位姿信息。

4.根据权利要求3所述的三维位姿估计方法，其特征在于，所述根据所述运动数据、所述激光数据，得到与所述待位姿估计物对应的局部位姿信息，包括：

根据所述运动数据，以所述惯性传感器采集的数据以及所述轮式里程计采集的数据为初值，获取所述待位姿估计物当前时间段的当前运动量；

采集所述待位姿估计物上一时刻的历史运动量，并根据所述历史运动量、所述当前运动量获取所述待位姿估计物当前时刻的初始化位姿；

根据所述初始化位姿、所述激光数据计算所述待位姿估计物的精确位姿，并根据所述精确位姿得到所述局部位姿信息。

5.根据权利要求1所述的三维位姿估计方法，其特征在于，所述根据所述激光数据、所述先验信息生成全局位姿数据的关联信息，包括：

根据所述先验信息得到与所述全局位姿数据对应的预测位姿数据；

查找所述预测位姿数据的各个附近位姿数据，分别将各个所述附近位姿数据与所述激光数据进行匹配，并分别计算出匹配度；

对各个所述匹配度进行判别，从各个所述附近位姿数据中筛选出目标位姿数据作为所述关联信息。

6.根据权利要求1所述的三维位姿估计方法，其特征在于，所述目标函数的表达式为：PP+∑||lio||²+∑||gps||²+∑||mag||²+∑||bar||²+∑||lgm||²；其中，PP表示先验信息，∑||lio||²表示相邻两帧数据之间的位置、角度、陀螺仪偏差、加速度偏差的变化量的差，∑||gps||²表示全局坐标系下与GPS的位置偏差，∑||mag||²表示全局坐标系下与磁力方向的差，∑||bar||²表示高度值与气压计测量值的差，∑||lgm||²表示局部位姿与全局位姿的误差。

7.根据权利要求1所述的三维位姿估计方法，其特征在于，所述求解所述目标函数，得到与所述待位姿估计物对应的三维位姿估计，包括：

求解所述目标函数，将所述局部位姿信息转换为全局位姿信息，并根据所述局部位姿信息对所述待位姿估计物对应的全局位姿信息进行更新，输出所述三维位姿估计。