[发明专利]一种冗余惯性测量单元数据融合最优权重估计方法

权利要求书

1.一种冗余惯性测量单元数据融合最优权重估计方法，其特征在于将惯性测量单元先后六个面朝上放置于大理石平台上，采集各个位置陀螺仪和加速度计数据，利用加权最小二乘法对陀螺仪和加速度计数据进行数据融合将其转换为惯性测量单元坐标系下等效三轴的测量信息，并根据大理石平台天向角速率和加速度恒定的特点，构建关于加权最小二乘权重的非线性方程组，具体包括以下步骤：

(1)分别将惯性测量单元x轴朝上、x轴朝下、y轴朝上、y轴朝下、z轴朝上和z轴朝下依次放置于大理石平台上，定义为位置1～6，在每个位置每1秒采集1组陀螺仪和加速度计数据，持续采集1分钟，分别对陀螺仪和加速度计构建各位置单组数据的非线性方程如下：

其中，和分别表示第1～6个位置第j组数据关于陀螺仪和加速度计加权最小二乘权重的非线性函数，j＝1,2,...,60，W_g和W_a分别表示陀螺仪和加速度计加权最小二乘权重矩阵，H_g和H_a分别表示陀螺仪和加速度计的配置结构矩阵，ω_r1j、ω_r2j、ω_r3j、ω_r4j、ω_r5j和ω_r6j分别表示所有陀螺仪在第1～6个位置采集的第j组数据，a_r1j、a_r2j、a_r3j、a_r4j、a_r5j和a_r6j分别表示所有加速度计在第1～6个位置采集的第j组数据，|_x、|_y和|_z分别表示取惯性测量单元坐标系x、y和z轴数据，Ω表示地球自转角速率的大小，sin表示正弦函数，L表示当地纬度，G表示当地重力加速度大小；

(2)利用惯性测量单元各个位置采集的所有数据分别对陀螺仪和加速度计构建关于加权最小二乘权重的非线性方程组如下：

其中，F_g和F_a分别表示对陀螺仪和加速度计的关于加权最小二乘权重的非线性函数组，和分别表示第i个位置第j组数据构建的对陀螺仪和加速度计的非线性函数，i＝1,2,...,6；

(3)公式(3)和公式(4)的二范数表示如下：

其中，|| ||₂表示二范数；

(4)将公式(3)和公式(4)中非线性方程组的最优解转换为求其二范数最小值的优化问题，如下：

其中，min表示取最小值，s.t.表示约束条件，diag表示将向量转换为对角矩阵的构建函数，w_gn表示第n个陀螺仪的权重，n＝1,2,...,N，N为陀螺仪个数，w_am表示第m个加速度计的权重，m＝1,2,...,M，M为加速度计个数；

(5)设计约束型果蝇优化算法分别对公式(7)和公式(8)进行最优估计，获得陀螺仪和加速度计加权最小二乘的最优权重，约束型果蝇优化算法具体包括以下步骤：

(a)初始化果蝇群体位置：

其中，D_{_axis}表示果蝇群体位置K维向量，d_{k_axis}表示果蝇群体位置向量中第k维的值，k＝1,2,...,K，初始化时d_{k_axis}随机给定；

(b)以群体位置为中心，果蝇个体随机向四周飞出觅食，果蝇个体位置具体表示如下：

其中，D_l表示第l个果蝇个体位置，l＝1,2,...,L，L为群体中的果蝇总数，R_k表示果蝇个体位置向量中第k维的随机数；

(c)计算果蝇个体味道浓度和味道污染度：

其中，S_l为第l个果蝇个体的味道浓度，P_l为第l个果蝇个体的味道污染度，sFunction为味道浓度判定函数，pFunction为味道污染度判定函数；

(d)计算果蝇群体位置处的味道浓度和味道污染度，将所有果蝇个体位置和群体位置一起考虑，先按味道污染度升序排序，味道污染度相同的按味道浓度降序排序，取排序第一为最佳果蝇个体，该个体的味道污染度、味道浓度及位置保留为最优味道污染度、最优味道浓度及最优位置，所有果蝇个体向该位置聚集，该位置即更新为新的群体位置，具体表示如下：

其中，S_{_axis}为群体位置处的味道浓度，P_{_axis}为群体位置处的味道污染度，S_{_best}为最优味道浓度，P_{_best}为最优味道污染度，D_{_best}为最优位置；

(e)设置果蝇总数L和迭代次数，具体果蝇总数和迭代次数根据实际情况确定，重复步骤(a)～(d)，迭代计算，最后更新的群体位置即为最终的最优位置。