一种基于雷达和IMU的姿态动态估计方法及系统技术方案

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

A dynamic attitude estimation method and system based on radar and IMU

【技术实现步骤摘要】
一种基于雷达和IMU的姿态动态估计方法及系统
本专利技术涉及姿态估计
，具体涉及一种基于雷达和IMU的姿态动态估计方法及系统。
技术介绍
雷达(例如毫米波雷达)提供了对环境可靠的感知信息，尤其是在视觉条件退化的情况下。目前雷达，特别是毫米波雷达广泛应用于军事(空中和海上监视、导弹制导等)、民航(进场雷达、地面运动雷达)或遥感(行星观测)等领域。雷达技术和材料科学的最新进展使得毫米波雷达在尺寸、重量、能耗和成本问题方面逐渐适应更小的平台，他们体积小、成本低、精度高，特别适合便携式低功耗应用。近些年，一些研究小组已经提出将雷达，特别是毫米波雷达应用到各种移动机器人中去解决相对应的任务，比如导航、定位、绘图等。为了将雷达，特别是毫米波雷达应用到室内定位服务中，需要专门设计适用雷达，特别是毫米波雷达的室内自运动估计方法。但目前针对毫米波雷达设计的室内自运动估计算法对雷达运载体的自运动姿态估计效果不佳，计算速度较慢，实时性较差，对自我的定位精度也不够理想。
技术实现思路
本专利技术以提高雷达运载体自运动姿态估计准确度，提升姿态估计速度，且提高室内自我定位精度为目的，提供了一种基于雷达(例如毫米波雷达)和IMU的姿态动态估计方法及系统。为达此目的，本专利技术采用以下技术方案：一种基于雷达和IMU的姿态动态估计方法，具体步骤包括：1)从雷达(例如毫米波雷达)扫描得到的雷达点云数据中提取地标信息；2)根据IMU数据估计雷达运载体的相对动作；3)对所述地标信息和所述雷达运载体的相对动作信息进行信息点扫描匹配，得到所述雷达运载体的运动姿态；4)对步骤3)匹配得到的所述雷达运载体的运动姿态进行姿态回归计算，并根据姿态回归计算结果预测得到所述雷达运载体的六自由度运动姿态。作为本专利技术的一种优选方案，步骤3)中，采用NDT正态分布变换算法对所述地标信息和所述移动雷达的相对动作信息进行信息点扫描匹配。作为本专利技术的一种优选方案，步骤4)中，采用UKF无迹卡尔曼滤波算法对移动雷达运动姿态进行姿态回归计算。作为本专利技术的一种优选方案，步骤4)中，对移动雷达运动姿态进行姿态回归计算的过程可通过以下表达式表达：上式中，pt-1为所述雷达运载体在当前t时刻的上一时刻的所处位置；f(Xt-1)为预测所述雷达运载体运动位置的第一转移方程；qt-1为表征所述雷达运载体在当前t时刻的上一时刻的旋转状态的旋转四元数；Δqt是所述雷达运载体在t-1时刻到当前t时刻的旋转变化量；vt-1表示所述雷达运载体在当前t时刻的上一时刻的运动速度；是所述雷达运载体在当前t时刻的上一时刻的角速度的偏置。作为本专利技术的一种优选方案，所述第一转移方程使用RNN神经网络进行建模。作为本专利技术的一种优选方案，步骤4)中，预测所述雷达运载体的六自由度运动姿态的第二转移方程使用LSTM长短期记忆人工神经网络进行建模。作为本专利技术的一种优选方案，用于对所述第二转移方程进行建模的LSTM神经网络的内部预测过程可通过以下公式(1)和公式(2)表达：Ck＝fk⊙Ck-1+ik⊙gk公式(1)公式(1)中，Ck用于表示记忆细胞是通过遗忘门和输入门来控制信息的流动；fk用于表示遗忘门；Ck-1用于表示上一个时刻的记忆细胞；ik用于表示输入门；gk用于表示候选记忆细胞；hk＝oK⊙tanh(Ck)公式(2)公式(2)中，hk表示当前时刻的隐藏状态；oK表示输出门；tanh是激活函数，值域在[-1，1]。作为本专利技术的一种优选方案，公式(1)中，fk通过以下公式(11)计算而得：fk＝σ(Wf·[xk，hk-1]+bf)公式(11)公式(11)中，σ表示sigmoid激活函数；W表示权重参数；xk表示LSTM模块的输入；hk-1表示上一个时间步的隐藏状态；bf表示遗忘门的偏差参数；公式(1)中，ik通过以下公式(12)计算而得：ik＝σ(Wi·[xk，hk-1]+bi)公式(12)公式(12)中，bi表示输入门的偏差参数。公式(1)中，gk通过以下公式(13)计算而得：gk＝tanh(Wg·[xk，hk-1]+bg)公式(13)公式(13)中，bg表示候选记忆细胞的偏差参数；tanh表示激活函数，值域是[-1，1]。作为本专利技术的一种优选方案，公式(2)中，oK通过以下公式(21)表达：oK＝σ(Wo·[xk，hk-1]+bo)公式(21)xk表示LSTM模块的输入；hk-1表示上一个时刻的隐藏状态；Wo表示输出门的权重参数；bo表示输出门的偏差参数；σ表示sigmoid激活函数，其值域在[0，1]，因此可以用来表示遗忘门输入门等的开关状态。本专利技术还提供了一种基于雷达和IMU的姿态动态估计系统，包括：雷达扫描模块，用于通过毫米波雷达进行环境扫描，得到雷达点云数据；数据关联模块，连接所述雷达扫描模块，用于从所述雷达点云数据中提取地标特征；惯性导航模块，用于根据IMU数据估计雷达运载体的相对动作；扫描匹配模块，分别连接所述数据关联模块和所述惯性导航模块，用于对所述地标信息和所述雷达运载体的相对动作信息进行信息点扫描匹配，得到所述雷达运载体的运动姿态；实时姿态估计模块，连接所述扫描匹配模块，用于对所述雷达运载体的运动姿态进行姿态回归计算，并根据姿态回归计算结果预测得到所述雷达运载体的六自由度运动姿态。本专利技术的有益效果：1、本专利技术通过从雷达扫描的雷达点云数据中提取地标信息，并根据IMU数据估计雷达运载体的相对动作信息，并将地标信息与相对动作信息进行信息点扫描匹配，得到的雷达运载体自运动姿态更加准确；2、使用UKF算法对匹配得到的运动姿态进行姿态回归计算，进一步确保了雷动运载体的姿态估计精度；3、系统的核心部件采用尺寸较小的毫米波雷达也可以是其他类型的雷达，较小的硬件尺寸有利于使用该系统的产品的小型化设计。附图说明为了更清楚地说明本专利技术实施例的技术方案，下面将对本专利技术实施例中所需要使用的附图作简单地介绍。显而易见地，下面所描述的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。图1是本专利技术一实施例提供的基于雷达和IMU的姿态动态估计方法的实现步骤图；图2是本专利技术一实施例提供的基雷达和IMU的姿态动态估计系统的结构示意图；图3是姿态动态估计系统实现姿态估计的原理图。具体实施方式下面结合附图并通过具体实施方式来进一步说明本专利技术的技术方案。其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本专利的限制；为了更好本文档来自技高网...

【技术保护点】
1.一种基于雷达和IMU的姿态动态估计方法，其特征在于，具体步骤包括：/n1)从雷达扫描得到的雷达点云数据中提取地标信息；/n2)根据IMU数据估计雷达运载体的相对动作；/n3)对所述地标信息和所述雷达运载体的相对动作信息进行信息点扫描匹配，得到所述雷达运载体的运动姿态；/n4)对步骤3)匹配得到的所述雷达运载体的运动姿态进行姿态回归计算，并根据姿态回归计算结果预测得到所述雷达运载体的六自由度运动姿态。/n

【技术特征摘要】
1.一种基于雷达和IMU的姿态动态估计方法，其特征在于，具体步骤包括：
1)从雷达扫描得到的雷达点云数据中提取地标信息；
2)根据IMU数据估计雷达运载体的相对动作；
3)对所述地标信息和所述雷达运载体的相对动作信息进行信息点扫描匹配，得到所述雷达运载体的运动姿态；
4)对步骤3)匹配得到的所述雷达运载体的运动姿态进行姿态回归计算，并根据姿态回归计算结果预测得到所述雷达运载体的六自由度运动姿态。


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


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


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



上式中，pt-1为所述雷达运载体在当前t时刻的上一时刻的所处位置；
f(Xt-1)为预测所述雷达运载体运动位置的第一转移方程；
qt-1为表征所述雷达运载体在当前t时刻的上一时刻的旋转状态的旋转四元数；
Δqt是所述雷达运载体在t-1时刻到当前t时刻的旋转变化量；
vt-1表示所述雷达运载体在当前t时刻的上一时刻的运动速度；

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


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


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


7.根据权利要求6所述的一种基于雷达和IMU的姿态动态估计方法，其特征在于，用于对所述第二转移方程进行建模的LSTM神经网络的内部预测过程可通过以下公式(1)和公式(2)表达：
Ck＝fk⊙Ck-1+ik⊙gk公式(1)
公式(1)中，Ck用于表示记忆细胞是通过遗忘门和输入门来控制信息的流动；
fk用于表示遗忘门；
Ck-...

【专利技术属性】
技术研发人员：不公告发明人，
申请(专利权)人：北京中科深智科技有限公司，
类型：发明
国别省市：北京;11