[发明专利]标定IMU与刚体姿态的方法、设备以及可读存储介质

权利要求书

1.一种标定IMU与刚体姿态的方法，其特征在于，所述方法包括：

固定IMU与刚体之间的位置关系；

获取所述IMU对应的原始姿态数据以及所述刚体对应的原始姿态数据；

对所述IMU对应的原始姿态数据以及所述刚体对应的原始姿态数据进行预处理，所述预处理用于剔除无效数据，所述无效数据为所述IMU以及所述刚体未处于运动状态时的姿态数据；

根据所述预处理后的姿态数据计算IMU坐标到刚体坐标的旋转矩阵以及所述IMU到世界的姿态数据；

根据计算数据将所述IMU的姿态转换为所述刚体的姿态，并得出所述刚体的位姿；

所述根据所述预处理后的姿态数据计算IMU坐标到刚体坐标的旋转矩阵以及所述IMU到世界的姿态数据包括：

从所述预处理后的IMU姿态数据中提取第0帧IMU姿态数据以及第n帧IMU姿态数据，并从所述预处理后的刚体姿态数据中提取第0帧刚体到世界的姿态数据以及第n帧刚体到世界的姿态数据；

根据关系等式，计算所述IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及计算第0帧IMU到世界的姿态数据，所述关系等式为：

其中，所述用于指示所述第n帧刚体到世界的姿态数据，所述用于指示所述IMU姿态数据从所述IMU坐标到所述刚体坐标的旋转矩阵，所述[R_i]_n用于指示所述第n帧IMU姿态数据，所述用于指示所述第0帧刚体到世界的姿态数据，所述[R_i]₀用于指示所述第0帧IMU姿态数据，所述用于指示所述第0帧IMU到世界的姿态数据，所述用于指示所述第n帧IMU姿态数据相对于所述第0帧IMU姿态数据的逆矩阵，所述E用于指示单位矩阵。

2.根据权利要求1所述的方法，其特征在于，所述根据所述关系等式，计算所述IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及计算第0帧IMU到世界的姿态数据的方法包括：

将所述第0帧IMU姿态数据、所述第n帧IMU姿态数据、所述第0帧刚体到世界的姿态数据以及所述第n帧刚体到世界的姿态数据代入所述关系等式，并通过最小二乘法，当所述关系等式的等号两边数据之差的平方和为最小时求得和所述最小二乘法公式Q包括：

3.根据权利要求2所述的方法，其特征在于，所述根据所述计算数据将所述IMU的姿态转换为所述刚体的姿态包括：

根据姿态转换公式将所述IMU的姿态转换为所述刚体的姿态，所述姿态转换公式为：

其中，所述为IMU姿态数据从所述IMU坐标到所述刚体坐标的旋转矩阵的逆变换，所述为经过转换后的所述刚体的姿态数据。

4.根据权利要求1-3任一权利要求所述的方法，其特征在于，当所述IMU对应的原始姿态数据以及所述刚体对应的原始姿态数据分别用四元数表示时，至少需采集8x组数据，所述x为正整数。

5.根据权利要求4所述的方法，其特征在于，所述采集的8x组数据分别在俯仰角、航向角以及偏航角都具有至少90度的变化。

6.根据权利要求1所述的方法，其特征在于，所述对所述IMU对应的原始姿态数据以及所述刚体对应的原始姿态数据进行预处理包括：

通过距离计算公式，计算所述IMU对应的原始姿态数据以及所述刚体对应的原始姿态数据中处于相邻帧和/或间隔帧的2个帧数据之间的距离，所述距离计算公式为：

其中，所述x_m、y_m以及z_m用于表示第m帧IMU或刚体姿态数据的三轴坐标，所述x_m、y_m以及z_m用于表示第n帧IMU或刚体姿态数据的三轴坐标，所述d用于表示所述第m帧IMU或刚体姿态数据与所述第n帧IMU或刚体姿态数据之间的距离；

当所述d小于距离阈值时，判定所述IMU或所述刚体未处于运动状态，并剔除所述未处于运动状态下的姿态数据，完成所述预处理。