[发明专利]一种估计无人机姿态的方法及系统

权利要求书

1.一种估计无人机姿态的方法，其特征在于，所述方法包括：

通过互补滤波器对加速度计在t时刻的姿态估计值和陀螺仪在t时刻的姿态估计值进行融合处理，得到t时刻的无人机姿态估计值0≤k_t≤1，其中，k_t是所述陀螺仪在t时刻的姿态估计值的发散速率β的加权系数，1-k_t是所述加速度计在t时刻的姿态估计值的收敛速率的加权系数；

通过所述加速度计在t时刻的误差e_a,t对所述姿态估计值的发散速率β实时调整，β＝2-||e_a,t||；

其中，采用梯度下降法获取所述加速度计在t时刻的姿态估计值梯度方向的步长为μ_t，所述互补滤波器的采样时间为Δt，

其中，所述通过互补滤波器对加速度计在t时刻的姿态估计值和陀螺仪在t时刻的姿态估计值进行融合处理，得到t时刻的无人机姿态估计值包括：

确定所述姿态估计值的收敛速率

根据所述陀螺仪在t时刻的姿态估计值的加权发散速率与所述加速度计在t时刻的姿态估计值的加权收敛速率，确定所述互补滤波器融合处理的加权系数，

2.如权利要求1所述的方法，其中，加速度计在t时刻的加速度测量值用四元数表示，其中，a_x、a_y、a_z分别表示在无人机坐标系中x轴、y轴、z轴的加速度测量值分量，所述加速度测量值归一化后的加速度向量用表示，

其中，所述采用梯度下降法获取所述加速度计在t时刻的姿态估计值包括：

基于所述加速度向量和t时刻从无人机坐标系变换到导航坐标系的归一化方向向量构建目标函数其中，为无人机坐标系中归一化向量的参考方向，为导航坐标系中重力向量g的归一化向量，为的共轭四元数

通过梯度下降法确定所述目标函数的最小值，并将最小值作为所述加速度计在t时刻的姿态估计值

3.如权利要求2所述的方法，其中，所述通过梯度下降法确定所述目标函数的最小值，包括：

根据梯度下降法，对迭代求解，确定所述加速度计在t时刻的姿态估计值

其中，为所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值，Δt为所述互补滤波器的采样时间，为所述目标函数的梯度值，J表示雅可比行列式，为所述陀螺仪在t时刻的姿态估计值的时间导数，α为比例因子。

4.如权利要求2所述的方法，其中，所述方法还包括：

基于陀螺仪在t时刻的角度漂移值对陀螺仪在t时刻的角速度测量值进行补偿，得到补偿后的角速度值

基于所述补偿后的角速度值和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值得到所述陀螺仪在t时刻的姿态估计值其中，Δt为所述互补滤波器的采样时间。

5.如权利要求4所述的方法，其中，所述角速度测量值用四元数表示，其中，ω_x、ω_y、ω_z分别表示在无人机坐标系中x轴、y轴、z轴的角速度测量值分量，

其中，基于陀螺仪在t时刻的角度漂移值对陀螺仪在t时刻的角速度测量值进行补偿，得到补偿后的角速度值基于所述补偿后的角速度值和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值得到所述陀螺仪在t时刻的姿态估计值包括：

基于所述陀螺仪在t时刻的角度漂移值对所述角速度测量值进行补偿，确定补偿后的角速度值

基于所述补偿后的角速度值和所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值确定所述陀螺仪在t时刻的姿态估计值的时间导数

基于所述陀螺仪在t时刻的姿态估计值的时间导数确定所述陀螺仪在t时刻的姿态估计值并将所述补偿后的角速度值作为观测向量。

6.如权利要求5所述的方法，其中，通过所述陀螺仪在t时刻的误差和漂移补偿参数ζ，确定所述陀螺仪在t时刻的角度漂移值

其中为t时刻的无人机姿态估计值的时间导数；

所述漂移补偿参数ζ表示消除所述陀螺仪非零平均测量误差的收敛速度：

其中，为所述陀螺仪三轴漂移的估计速率，为无人机坐标系相对于导航坐标系的方向矢量的估计值。