[发明专利]一种阴影下的载体姿态估计方法

权利要求书

1.一种阴影下的载体姿态估计方法，该方法所采用的装置包括数据处理器(1)、设置在载体上的三轴陀螺(3)和三轴加速度计(2)，其特征在于，该方法包括以下步骤：

步骤一、建立载体坐标系：

以载体的质心为原点o，ox_b轴与所述载体的纵轴重合且指向所述载体前进方向，oy_b轴与所述载体的横轴重合且指向所述载体前进方向的右侧，oz_b轴垂直于所述ox_b轴和所述oy_b轴形成的平面ox_by_b且指向所述载体下方，建立载体坐标系ox_by_bz_b；

步骤二、载体姿态初始参数的获取：

步骤201、三轴加速度计(2)对载体初始时刻的总加速度进行检测，并将检测到的初始时刻总加速度发送至数据处理器(1)，数据处理器(1)根据公式和得到载体的初始俯仰角φ(0)和载体的初始横滚角θ(0)；g表示重力加速度，且g＝9.8m/s²；其中，f_x(0)表示三轴加速度计(2)测量到的初始时刻总加速度在ox_b轴上的加速度分量，f_y(0)表示三轴加速度计(2)测量到的初始时刻总加速度在oy_b轴上的加速度分量，f_z(0)表示三轴加速度计(2)测量到的初始时刻总加速度在oz_b轴上的加速度分量；

步骤202、采用所述数据处理器(1)根据公式得到初始的虚拟旋转矩阵

步骤203、采用所述数据处理器(1)根据公式得到退化四元数中初始的标量参数q₀(0)、退化四元数中初始的第一矢量参数q₁(0)和退化四元数中初始的第二矢量参数q₂(0)；

步骤三、基于退化四元数的观测器模型的建立并获取载体的姿态估计值：

步骤301、j为自然数，即j≥0，当j≥1时，三轴陀螺(3)对载体的角速度进行检测，并将检测到的角速度发送至数据处理器(1)，采用所述数据处理器(1)根据公式得到第j个采样时刻载体在ox_b轴上的角速度分量估计值采用所述数据处理器(1)根据公式得到第j个采样时刻载体在oy_b轴上的角速度分量估计值采用所述数据处理器(1)根据公式得到第j个采样时刻载体在oz_b轴上的角速度分量估计值其中，ω_x(j)表示三轴陀螺(3)第j个采样时刻检测到的角速度在ox_b轴上的角速度分量，ω_y(j)表示三轴陀螺(3)第j个采样时刻检测到的角速度在oy_b轴上的角速度分量，ω_z(j)表示三轴陀螺(3)第j个采样时刻检测到的角速度在oz_b轴上的角速度分量，表示第j-1个采样时刻三轴陀螺(3)在ox_b轴上的零偏估计值，表示第j-1个采样时刻三轴陀螺(3)在oy_b轴上的零偏估计值，表示第j-1个采样时刻三轴陀螺(3)在oz_b轴上的零偏估计值；

当j＝0时，三轴陀螺(3)对载体的角速度进行检测，并将检测到的角速度发送至数据处理器(1)，采用所述数据处理器(1)根据公式得到初始时刻载体在ox_b轴上的角速度分量估计值采用所述数据处理器(1)根据公式得到初始时刻载体在oy_b轴上的角速度分量估计值采用所述数据处理器(1)根据公式得到初始时刻载体在oz_b轴上的角速度分量估计值其中，ω_x(0)表示三轴陀螺(3)初始时刻检测到的角速度在ox_b轴上的角速度分量，ω_y(0)表示三轴陀螺(3)初始时刻检测到的角速度在oy_b轴上的角速度分量，ω_z(0)表示三轴陀螺(3)初始时刻检测到的角速度在oz_b轴上的角速度分量，表示三轴陀螺(3)在ox_b轴上的初始零偏估计值，表示三轴陀螺(3)在oy_b轴上的初始零偏估计值，表示三轴陀螺(3)在oz_b轴上的初始零偏估计值；

步骤302、采用所述数据处理器(1)根据公式得到第j个采样时刻载体的径向加速度判断门限τ_ω(j)；其中，ξ_ω(j)表示第j个采样时刻的载体径向加速度阈值；

采用所述数据处理器(1)根据公式得到第j个采样时刻载体的切向加速度判断门限τ_a(j)；其中，ξ_a表示单位采样时刻内俯仰角的变化量阈值，f_x(j)表示第j个采样时刻三轴加速度计(2)测量到的总加速度在ox_b轴上的加速度分量；

采用所述数据处理器(1)综合和得到第j个采样时刻的综合判断条件其中，τ(j)表示第j个采样时刻的综合判断参数；

步骤303、采用所述数据处理器(1)建立基于退化四元数的观测器模型并输入初始虚拟旋转矩阵和步骤302中第j个采样时刻的综合判断参数τ(j)，得到第j+1个采样时刻的虚拟旋转矩阵估计值其中，k_p表示观测器比例增益系数，且k_P＞0，k_I表示观测器积分增益系数，且k_I＞0，k_m表示观测量权值，k_m＞0，表示当地重力加速度归一化向量，且表示三轴陀螺(3)第j个采样时刻检测到的角速度分量向量，且表示三轴陀螺(3)第j个采样时刻的零偏估计向量，且表示三轴陀螺(3)第j个采样时刻在ox_b轴上的零偏估计值，表示三轴陀螺(3)第j个采样时刻在oy_b轴上的零偏估计值，表示三轴陀螺(3)第j个采样时刻在oz_b轴上的零偏估计值，表示第j个采样时刻的虚拟旋转矩阵估计值的转置，σ(j)表示第j个采样时刻的校正向量；S(·)表示斜对称矩阵,ΔT表示采样时间；

步骤303、采用所述数据处理器(1)建立载体第j+1个采样时刻的虚拟旋转矩阵并带入步骤304中得到的第j+1个采样时刻的虚拟旋转矩阵估计值得到第j+1个采样时刻退化的四元数矩阵其中，其中，q₀(j+1)表示退化四元数中第j+1个采样时刻的标量参数，q₁(j+1)表示退化四元数中第j+1个采样时刻的第一矢量参数，q₂(j+1)表示退化四元数中第j+1个采样时刻的第二矢量参数；

步骤304、采用所述数据处理器(1)根据公式得到第j+1个采样时刻载体的俯仰角估计值和载体的横滚角估计值