[发明专利]一种基于雷达和IMU的姿态动态估计方法及系统

权利要求书

1.一种基于雷达和IMU的姿态动态估计方法，其特征在于，具体步骤包括：

1)从雷达扫描得到的雷达点云数据中提取地标信息；

2)根据IMU数据估计雷达运载体的相对动作；

3)对所述地标信息和所述雷达运载体的相对动作信息进行信息点扫描匹配，得到所述雷达运载体的运动姿态；

4)对步骤3)匹配得到的所述雷达运载体的运动姿态进行姿态回归计算，并根据姿态回归计算结果预测得到所述雷达运载体的六自由度运动姿态。

2.根据权利要求1所述的一种基于雷达和IMU的姿态动态估计方法，其特征在于，步骤3)中，采用NDT正态分布变换算法对所述地标信息和所述移动雷达的相对动作信息进行信息点扫描匹配。

3.根据权利要求1所述的一种基于雷达和IMU的姿态动态估计方法，其特征在于，步骤4)中，采用UKF无迹卡尔曼滤波算法对移动雷达运动姿态进行姿态回归计算。

4.根据权利要求1所述的一种基于雷达和IMU的姿态动态估计方法，其特征在于，步骤4)中，对移动雷达运动姿态进行姿态回归计算的过程可通过以下表达式表达：

上式中，p_t-1为所述雷达运载体在当前t时刻的上一时刻的所处位置；

f(X_t-1)为预测所述雷达运载体运动位置的第一转移方程；

q_t-1为表征所述雷达运载体在当前t时刻的上一时刻的旋转状态的旋转四元数；

Δq_t是所述雷达运载体在t-1时刻到当前t时刻的旋转变化量；

v_t-1表示所述雷达运载体在当前t时刻的上一时刻的运动速度；

是所述雷达运载体在当前t时刻的上一时刻的角速度的偏置。

5.根据权利要求4所述的一种基于雷达和IMU的姿态动态估计方法，其特征在于，所述第一转移方程使用RNN神经网络进行建模。

6.根据权利要求4所述的一种基于雷达和IMU的姿态动态估计方法，其特征在于，步骤4)中，预测所述雷达运载体的六自由度运动姿态的第二转移方程使用LSTM长短期记忆人工神经网络进行建模。

7.根据权利要求6所述的一种基于雷达和IMU的姿态动态估计方法，其特征在于，用于对所述第二转移方程进行建模的LSTM神经网络的内部预测过程可通过以下公式(1)和公式(2)表达：

C_k＝f_k⊙C_k-1+i_k⊙g_k 公式(1)

公式(1)中，C_k用于表示记忆细胞是通过遗忘门和输入门来控制信息的流动；

f_k用于表示遗忘门；

C_k-1用于表示上一个时刻的记忆细胞；

i_k用于表示输入门；

g_k用于表示候选记忆细胞；

h_k＝o_K⊙tanh(C_k) 公式(2)

公式(2)中，h_k表示当前时刻的隐藏状态；

o_K表示输出门；

tanh是激活函数，值域在[-1,1]。