[发明专利]刚体姿态追踪方法及其装置、设备、存储介质

权利要求书

1.一种刚体姿态追踪方法，其特征在于，包括：

根据预设的函数模型分别建立卡尔曼预测四元数模型f_kQ、卡尔曼预测位移模型f_kP、滤波预测四元数模型f_kQ、滤波预测位移模型f_εP，其中，所述函数模型包括速度和加速度状态变量；

获取刚体初始帧的姿态四元数和位移，得到初始化后的模型f_kQ、f_kP、f_εQ、f_εP；

根据所述初始化后的模型f_kQ、f_kP、f_εQ、f_εP分别预测刚体当前帧的姿态四元数和位移，得到姿态四元数预测值和位移预测值；

将所述姿态四元数预测值和位移预测值以及获取的刚体当前帧姿态四元数和位移真实值引入二范数函数中，计算得到刚体当前帧的姿态信息。

2.如权利要求1所述的刚体姿态追踪方法，其特征在于，所述根据预设的函数模型分别建立卡尔曼预测四元数模型f_kQ、卡尔曼预测位移模型f_kP、滤波预测四元数模型f_kQ、滤波预测位移模型f_εP，其中，所述函数模型包括速度和加速度状态变量，包括：

将速度和加速度加入卡尔曼滤波的状态维度因子，结合滤波器原理，得到所述预设的函数模型；

根据所述预设的函数模型分别建立卡尔曼预测四元数模型f_kQ、卡尔曼预测位移模型f_kP、滤波预测四元数模型f_εQ、滤波预测位移模型f_εP。

3.如权利要求2所述的刚体姿态追踪方法，其特征在于，所述将速度和加速度加入卡尔曼滤波的状态维度因子包括：

选用kalman滤波模型，得到输出刚体姿态四元数和位移预测值的kalman滤波公式：

其中v为速度，a为加速度，Q为协方差矩阵，F为状态转移矩阵。

4.如权利要求2所述的刚体姿态追踪方法，其特征在于，所述滤波器原理包括：

令刚体姿态四元数和位移的预测值在T_i时刻为X_i，滤波后的预测值为则：

其中，α∈[0，1]代表平滑因子。

5.如权利要求1所述的刚体姿态追踪方法，其特征在于，所述卡尔曼预测四元数模型f_kQ和卡尔曼预测位移模型f_kP均包括：状态参数维度和测量变量，所述f_kQ的状态参数维度、测量变量个数分别为12、4，所述f_kP的状态参数维度、测量变量个数分别为9、3。

6.如权利要求1-5任一项所述的刚体姿态追踪方法，其特征在于，所述将所述姿态四元数预测值和位移预测值以及获取的刚体当前帧姿态四元数和位移真实值引入二范数函数中，计算得到刚体当前帧的姿态信息包括：

将所述姿态四元数预测值Qk_frame、Qε_frame和位移预测值Pk_frame、Pε_frame，以及获取的刚体当前帧姿态四元数真实值Qt_frame和位移真实值Pt_frame引入二范数函数中，以进行比较，求得刚体当前帧的姿态四元数Q_frame和位移P_frame，所述二范数函数为：

distQ＝||Qk_frame-Qt_frame||

distP＝||Pk_frame-Pt_frame||

其中ThQ、ThP分别为一固定阈值。

7.如权利要求6所述的刚体姿态追踪方法，其特征在于，所述ThQ、ThP根据调试结果得出，其取值范围为0-0.01。