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

说明书

本发明公开了一种基于雷达和IMU的姿态动态估计方法及系统，其中方法包括：从雷达(例如毫米波雷达)扫描得到的雷达点云数据中提取地标信息；根据IMU数据估计雷达运载体的相对动作；对地标信息和雷达运载体的相对动作信息进行信息点扫描匹配，得到雷达运载体的运动姿态；对匹配得到的雷达运载体的运动姿态进行姿态回归计算，并根据姿态回归计算结果预测得到雷达运载体的六自由度运动姿态。本发明专门为雷达(例如毫米波雷达)设计了一套移动雷达自运动姿态估计算法，提高了移动雷达自运动姿态估计的准确度，且具有较高地姿态估计速度，确保了姿态估计的实时性，对提高移动雷达的室内自我定位精度具有积极作用。

技术领域

本发明涉及姿态估计技术领域，具体涉及一种基于雷达和IMU的姿态动态估计方法及系统。

背景技术

雷达(例如毫米波雷达)提供了对环境可靠的感知信息，尤其是在视觉条件退化的情况下。目前雷达，特别是毫米波雷达广泛应用于军事(空中和海上监视、导弹制导等)、民航(进场雷达、地面运动雷达)或遥感(行星观测)等领域。雷达技术和材料科学的最新进展使得毫米波雷达在尺寸、重量、能耗和成本问题方面逐渐适应更小的平台，他们体积小、成本低、精度高，特别适合便携式低功耗应用。

近些年，一些研究小组已经提出将雷达，特别是毫米波雷达应用到各种移动机器人中去解决相对应的任务，比如导航、定位、绘图等。为了将雷达，特别是毫米波雷达应用到室内定位服务中，需要专门设计适用雷达，特别是毫米波雷达的室内自运动估计方法。但目前针对毫米波雷达设计的室内自运动估计算法对雷达运载体的自运动姿态估计效果不佳，计算速度较慢，实时性较差，对自我的定位精度也不够理想。

发明内容

本发明以提高雷达运载体自运动姿态估计准确度，提升姿态估计速度，且提高室内自我定位精度为目的，提供了一种基于雷达(例如毫米波雷达)和IMU的姿态动态估计方法及系统。

为达此目的，本发明采用以下技术方案：

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

1)从雷达(例如毫米波雷达)扫描得到的雷达点云数据中提取地标信息；

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

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

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

作为本发明的一种优选方案，步骤3)中，采用NDT正态分布变换算法对所述地标信息和所述移动雷达的相对动作信息进行信息点扫描匹配。

作为本发明的一种优选方案，步骤4)中，采用UKF无迹卡尔曼滤波算法对移动雷达运动姿态进行姿态回归计算。

作为本发明的一种优选方案，步骤4)中，对移动雷达运动姿态进行姿态回归计算的过程可通过以下表达式表达：

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

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

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

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

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

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