一种用于废气激光雷达的姿态估计方法与流程

本发明涉及信号处理技术领域，具体涉及一种用于废气激光雷达的姿态估计方法。

背景技术：

刚体的姿态估计在行人定位、室内导航、人体跟踪等应用中有着巨大的作用。姿态估计的问题可以描述为陀螺仪、加速计和磁力计这三种传感器的数据融合问题。通常采用由三轴陀螺仪组成的惯性测量单元来测量角速度，一个三轴加速度计来测量外部加速度和重力的总和，以及一个三轴磁传感器来测量地球的磁场，我们可以对它们的读数进行最优的融合，以提高估计的准确性。

在非道路移动污染源废气的检测中，激光雷达扮演了一个很重要的角色。由于激光雷达需要瞄准废气，所以其姿态的估计是一个关键问题。传统的技术手段在静态情况下执行得很好，但是在动态情况下会产生严重的性能下降。实际上，在这些方法中没有考虑到在动态情况下发生的外部加速度变化而导致的加速度计可靠性的变化。另外，考虑到陀螺仪比加速计消耗的能量要大得多，所以要尽可能的减少陀螺仪的工作时间。

技术实现要素：

针对现有技术的不足，本发明提供一种用于废气激光雷达的姿态估计方法，综合考虑了系统的能耗问题与外部加速度的干扰问题，能够在降低外部加速度对系统的干扰的同时，降低系统整体能耗。

为实现以上目的，本发明通过以下技术方案予以实现：

一种用于废气激光雷达的姿态估计方法，包括步骤：

步骤一、初始化参数设定，包括设定初始四元数初始误差协方差矩阵状态随机游走噪声协方差判定阈值系数α、检测窗宽度w和增益调整系数β；

步骤二、读取t时刻的陀螺仪数据yg，t、磁强计数据ym，t和加速度计数据ya，t，并将所述加速度数据ya，t存储到栈ya，t中，其中

ya，t＝{ya，t-w+1，ya，t-w+2，…，ya，t}；

步骤三、计算检测函数

和检测函数的理论方差

其中，为加速度计本体白噪声的方差，||ya，i||表示加速度数据ya，t的二范数，g为重力加速度；

步骤四、根据所述步骤三得到的ft与判断物体对象的运动状态，具体判定依据为：判断是否有如果是，则判定物体对象处于静止状态，否则判定物体对象处于运动状态；

步骤五、根据所述步骤四判断的运动状态，计算先验估计与先验估计协方差

当物体处于静止状态时，令

当物体处于运动状态时，令

其中，

φt＝exp(ωt·t)

t为采样时间间隔，rg为陀螺仪误差方差矩阵，i3表示三阶单位阵；

步骤六、计算滤波增益kt：

其中，

ht＝[h1，t；h2，t]′

ra表示加速度计噪声方差矩阵，rm表示磁力计噪声方差矩阵；g＝[0，0，g]′；m＝[mx，my，mz]′＝[||m||·cosθ，0，||m||·sinθ]′，m表示在世界坐标系下的地磁场向量，||m||表示m的二范数，θ表示磁场倾角；

步骤七、计算后验估计和后验估计协方差

进一步地，所述步骤一中判定阈值系数α的范围为α＞3。

进一步地，所述步骤一中，初始误差协方差矩阵为4×4的方阵；预设为4×4的单位向量。

进一步地，任一向量x＝[x1，x2，x3]′的斜矩阵[x^×]定义为

进一步地，任一单位范数四元数定义为四元数的积定义为

其中，i3表示三阶单位阵。

进一步地，一个四元数的逆定义为q^-1＝[q0，-q1，-q2，-q3]′

且

进一步地，地球参考系到本体参考系的四元数旋转矩阵表示为：

进一步地，重力加速度g为9.8。

与现有技术相比，本发明具有以下有益效果：

本发明充分考虑了系统的能耗问题与外部加速度的干扰问题，能够在降低外部加速度对系统的干扰的同时，降低系统整体能耗；

本发明能够对物体的运动状态进行实时的监测，并且能够有效对抗加速度计存在异常的情况，具有较强的稳定性与鲁棒性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明方法流程图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提供了一种姿态估计方法，包括预处理定义如下：

一个四元数定义为

q＝[q0，q1，q2，q3]

如果给q加上任意上下标，如上标x，下标y，则有

我们还定义

一个向量x＝[x1，x2，x3]′的斜矩阵[x^×]定义为

一个单位范数四元数定义为四元数的积定义为：

其中，i3表示三阶单位阵。

一个四元数的逆定义为：

q^-1＝[q0，-q1，-q2，-q3]′

且具有以下性质：

地球参考系到本体参考系的四元数旋转矩阵表示为：

具体步骤为：

s1、初始化：设定初始四元数初始误差协方差矩阵状态随机游走噪声协方差判定阈值系数α＞3、检测窗宽度w，增益调整系数β，误差协方差矩阵为4×4的矩阵；一般可以设置为一个4×4的单位向量；

s2、读取时刻t的陀螺仪数据yg，t、磁强计数据ym，t、加速度计数据ya，t，并将加速度数据存储到栈ya，t＝{ya，t-w+1，ya，t-w+2，…，ya，t}中；

s3、计算检测函数

和检测函数的理论方差

其中，为加速度计本体白噪声的方差，||ya，i||表示加速度数据ya，t的二范数，g为重力加速度，一般取值为9.8；

s4、根据ft与判断物体对象的运动状态，具体判定依据为：判断是否有如果是，则执行步骤s401判定物体对象处于静止状态并执行步骤s501，否则则执行步骤s402判定物体对象处于运动状态并执行步骤s502；

根据运动状态，计算先验估计与先验估计协方差包括步骤：

s501、令

s502、令

其中，

φt＝exp(ωt·t)

t为采样时间间隔，rg为陀螺仪误差方差矩阵，i3表示三阶单位阵；

s6、计算滤波增益kt：

其中，

ht＝[h1，t；h2，t]′

ra表示加速度计噪声方差矩阵，rm表示磁力计噪声方差矩阵；g＝[0，0，g]′；m＝[mx，my，mz]′＝[||m||·cosθ，0，||m||·sinθ]′，m表示在世界坐标系下的地磁场向量，||m||表示m的二范数，θ表示磁场倾角；

s7、计算后验估计和后验估计协方差

在实时本发明时，涉及到一些参数、变量的赋值，可以参考以下方案：

设置在初始时候保持激光雷达处于静止状态，检测其姿态再转换成四元数可得

设置可根据设置的准确度而设置；

设置可设置为4×4单位阵；

设置β：可设置为大于1的实数；

设置w：可设置为大于1的整数；

设置rg，ra，rm：可以根据陀螺仪、加速度计、磁力计实际情况设置。

另外，再计算ft时候如果出现i＝t-w+1≤0，则令i＝1。

本发明充分考虑了系统的能耗问题与外部加速度的干扰问题，能够在降低外部加速度对系统的干扰的同时，降低系统整体能耗；本发明能够对物体的运动状态进行实时的监测，并且能够有效对抗加速度计存在异常的情况，具有较强的稳定性与鲁棒性。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。