买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：深圳市瑞立视多媒体科技有限公司

摘要：本发明涉及测量领域，公开了一种IMU与刚体的位姿融合方法、装置、设备及存储介质，用于解决在测量刚体的位姿数据时，刚体的测量位姿数据与真实位姿数据之间误差大的问题。IMU与刚体的位姿融合方法包括：确定惯性测量单元IMU与刚体之间的位置关系；当刚体携带IMU运动时，获取IMU的测量姿态数据，并通过IMU的测量姿态数据计算得到IMU的待融合姿态数据：在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据；利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。

主权项：1.一种IMU与刚体的位姿融合方法，其特征在于，所述IMU与刚体的位姿融合方法包括：确定惯性测量单元IMU与刚体之间的位置关系；当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据；获取刚体的测量姿态数据及所述IMU的测量姿态数据，在所述IMU的测量姿态数据中提取第0帧IMU测量姿态数据以及第n帧IMU测量姿态数据，在所述刚体的测量姿态数据中提取第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据；在预置的世界坐标系中，根据预设关系等式，计算第0帧IMU到世界的测量姿态数据，以及所述IMU从IMU坐标到刚体坐标的旋转矩阵，根据预设姿态转换公式将IMU的姿态数据转化为刚体的姿态数据，得到刚体的候选姿态数据；提取所述刚体的候选姿态数据中的姿态四元数，并分别获取刚体的候选位置数据以及候选位移数据、IMU的姿态四元数及IMU从IMU坐标到刚体坐标的旋转矩阵；根据融合算法中的位置融合公式、IMU的测量姿态数据、所述IMU的待融合姿态数据计算刚体的融合位置；根据融合算法中的姿态融合公式、IMU的测量姿态数据、待融合姿态数据计算刚体的融合姿态；将所述刚体的融合位置与所述刚体的融合姿态合并，确定刚体的融合位姿数据；所述位置融合公式为： 其中，表示在k+1时刻时刚体位于预置的世界坐标系中的候选位置，表示在k时刻时刚体位于预置的世界坐标系中的候选位置，α为位置融合系数，表示IMU从IMU坐标到刚体坐标的旋转矩阵，表示在k时刻时刚体的候选位移，Δtk表示时间间隔，Δt＝tk+1-tk，at表示测量加速度，表示k时刻IMU的速度，表示t时刻的标准加速度偏置，na为标准加速度噪声，表示t时刻IMU姿态下重力方向分量，表示将IMU的速度和待融合加速度投影到预置的世界坐标系下时，[tk,tk+1]时间段中待融合加速度产生的位移量；所述姿态融合公式为： 其中，表示在k+1时刻时刚体位于预置的世界坐标系中的姿态四元数，表示在k时刻时刚体位于预置的世界坐标系中的姿态四元数，表示四元数的乘法计算符号，β表示姿态融合系数，表示IMU从IMU坐标到刚体坐标的旋转矩阵，为k时刻IMU的姿态四元数，wt表示测量角速度，表示t时刻的标准角速度偏置，nw表示标准加速度噪声，表示[tk,tk+1]时段中IMU产生的旋转变化。

全文数据：

权利要求：

百度查询： 深圳市瑞立视多媒体科技有限公司 IMU与刚体的位姿融合方法、装置、设备及存储介质