一种基于雷达和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是姿态动态估计系统实现姿态估计的原理图。

具体实施方式

下面结合附图并通过具体实施方式来进一步说明本发明的技术方案。

其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本专利的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若出现术语“上”、“下”、“左”、“右”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

在本发明的描述中，除非另有明确的规定和限定，若出现术语“连接”等指示部件之间的连接关系，该术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个部件内部的连通或两个部件的相互作用关系。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

本发明一实施例提供的基于雷达和imu的姿态动态估计方法，如图1和图3所示，具体包括如下步骤：

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

由于点云中的点可能为噪声点，噪声点会影响后续的姿态估计准确度，所以在提取地标信息之前，首先需要去除点云中的噪声点。本实施例去除点云噪声的过程包括如下步骤：

步骤1.1a)计算点云中的第i个点到其邻近(最邻近)的m个点的平均距离di；为了确保噪声点的识别精度同时兼顾噪声点的识别速度，m的数量优选为15个；

步骤1.2a)计算点云中所有点n的近邻平均距离di的均值以及标准差σ，均值通过以下公式计算而得：

标准差σ通过以下公式计算而得：

步骤1.3a)根据均值以及标准差σ，设定作为噪声判断依据的距离阈值t，距离阈值t通过以下公式设定：

n优选为1.5。

步骤1.4a)判断的点i到其邻近的m个点的平均距离di是否大于距离阈值t，

若是，则将点i判断为扫描点云中的噪声点予以去除；

若否，则保留点i。

由于毫米波雷达的雷达扫描距离较短，当目标距离雷达载体过近时，点云过于密集会影响后续的目标检测速度，所以当点云过于密集时，还需要对毫米波雷达扫描得到的点云数据进行降采样处理，本实施例采用的点云数据降采样方法包括如下步骤：

1.1b)在点云空间中设定确定大小的网格立方体；网格立方体的尺寸大小可根据点云密集程度进行合理设定，由于网格立方体尺寸大小设定的具体方法并非本发明要求权利保护的范围，所以具体设定过程在此不作阐述；

1.2b)根据网格立方体中的所有点的空间坐标，计算出网格立方体的质心坐标，以网格立方体的质心代表网格立方体内的所有点，以实现对点云数据的降采样；

网格立方体的质心坐标通过以下公式计算而得：

(xq，yq，zq)为网格立方体的质心在xyz坐标系下的坐标；

(xi，yi，zi)为点i的在xyz坐标系下的坐标；

p为网格立方体中的点的数量。

请继续参照图1，本实施例提供的姿态动态估计方法还包括：

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

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

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

步骤3)中，优选采用ndt正态分布变换算法对地标信息和移动雷达的相对动作信息进行信息点扫描匹配。

步骤4)中，优选采用ukf无迹卡尔曼滤波算法对移动雷达运动姿态进行姿态回归计算。对移动雷达运动姿态进行姿态回归计算的过程可通过以下表达式表达：

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

f(xt-1)为预测雷达运载体运动位置的第一转移方程(第一转移方程优选可使用rnn神经网络进行建模)；

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

δqt是雷达运载体在t-1时刻到当前t时刻的旋转变化量；

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

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

步骤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的姿态动态估计系统，如图2所示，该系统包括：

雷达扫描模块，用于通过毫米波雷达进行环境扫描，得到雷达点云数据；

数据关联模块，连接雷达扫描模块，用于从雷达点云数据中提取地标特征；

惯性导航模块，用于根据imu数据估计雷达运载体的相对动作；

扫描匹配模块，分别连接数据关联模块和惯性导航模块，用于对地标信息和雷达运载体的相对动作信息进行信息点扫描匹配，得到雷达运载体的运动姿态；

实时姿态估计模块，连接扫描匹配模块，用于对雷达运载体的运动姿态进行姿态回归计算，并根据姿态回归计算结果预测得到雷达运载体的六自由度运动姿态。

需要声明的是，上述具体实施方式仅仅为本发明的较佳实施例及所运用技术原理。本领域技术人员应该明白，还可以对本发明做各种修改、等同替换、变化等等。但是，这些变换只要未背离本发明的精神，都应在本发明的保护范围之内。另外，本申请说明书和权利要求书所使用的一些术语并不是限制，仅仅是为了便于描述。