[发明专利]一种IMU与刚体姿态、位置对齐的方法、装置、设备以及可读存储介质

权利要求书

1.一种IMU与刚体姿态、位置对齐的方法，其特征在于，所述方法包括：

固定IMU与刚体之间的位置关系，记录刚体初始位置P₀＝{p₁，p₂，...，p_m}，其中，p_m＝(x_m，y_m，z_m)代表刚体标记点三维坐标数据，m为刚体标记点个数；

采集所述刚体和IMU的姿态数据：其中，代表第n时刻的刚体和IMU姿态的四元数集合，为第n时刻的刚体姿态的四元数，为第n时刻的IMU姿态的四元数；

根据预设的方程组模型和所述求解出旋转关系矩阵的四元数Q_ir；

根据预设第一公式得到所述和所述之间的夹角，若集合中所有的知之间的夹角均小于一设定阈值，则判断所述Q_ir求解正确，否则，求解错误；

若所述Q_ir求解正确，则进行刚体的追踪定位，获取t时刻IMU姿态的四元数Q_it和刚体的中心位置T_t；

根据预设第二公式得到t时刻刚体的姿态R_rt以及对应的四元数Q_rt；

根据预设第三公式求取t时刻刚体标记点的位置，将所述t时刻刚体标记点的位置与实际3D建模中刚体标记点的位置进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用所述t时刻刚体标记点的位置替代所述实际3D建模中刚体标记点的位置，以获得正确的刚体位置信息。

2.根据权利要求1所述的方法，其特征在于，所述根据预设的方程组模型和所述求解出旋转关系矩阵的四元数Q_ir包括：所述预设的方程组模型为：

st：||Q_ir||＝1，为四元数相乘操作，利用SVD分解求解所述方程组模型得到Q_ir或旋转关系矩阵R_ir，其中，

3.根据权利要求1所述的方法，其特征在于，所述根据预设第一公式得到所述和所述之间的夹角，若集合中所有的和之间的夹角均小于一设定阈值，则判断所述Q_ir求解正确，否则，求解错误，包括：

根据预设第一公式：算出所述和所述之间的夹角其中，为初始时刻IMU姿态的四元数的逆，为旋转关系矩阵的四元数的逆，若集合中所有的和之间的夹角dⁿ均小于一设定阈值T_d，则判断所述Q_ir求解正确，否则，求解错误。

4.根据权利要求1所述的方法，其特征在于，所述根据预设第二公式得到t时刻刚体的姿态R_rt以及对应的四元数Q_rt包括：

根据预设第二公式：得到t时刻刚体的姿态R_rt以及对应的四元数Q_rt。

5.根据权利要求1所述的方法，其特征在于，所述根据预设第三公式求取t时刻刚体标记点的位置，将所述t时刻刚体标记点的位置与实际3D建模中刚体标记点的位置进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用所述t时刻刚体标记点的位置替代所述实际3D建模中刚体标记点的位置，以获得正确的刚体位置信息，包括：

根据预设第三公式：p′_mt＝R_rt·p_m+T_t，求取t时刻刚体标记点的位置p′_mt，将所述p′_mt与实际3D建模中刚体标记点的位置p_mt进行比较，根据比较结果判断刚体追踪是否成功，若追踪成功，则采用所述P′_mt替代所述p_mt，以获得正确的刚体位置信息。