一种使用卡尔曼滤波器辨识多旋翼动力学模型参数的方法与流程

本发明涉及了一种使用卡尔曼滤波器辨识多旋翼动力学模型参数的方法，属于多旋翼飞行器导航、制导与控制领域。

背景技术：

多旋翼飞行器具有垂直起降、操控性好、成本较低等特性，对不同环境具有良好的适应性，是诸多应用的理想平台，例如其在农业播种、航空拍摄、气象监测、城市反恐、灾区救援、军事侦察等方面已展现出广阔的应用前景。多旋翼飞行器动力学模型是多旋翼控制系统设计与实现的基础，因此对动力学模型参数的准确辨识对飞行器性能的提高有重大意义。

目前多旋翼飞行器动力学模型参数的获取方式主要分为机理推导法和风洞试验法两种。前者基于流体力学与叶素理论计算其动力学模型参数，计算复杂且存在近似计算导致的误差；后者利用风洞进行参数辨识试验，对设备条件的要求较高，试验周期较长。而且这两种方法得到的参数并不能完全反映多旋翼飞行器作为一个整体飞行时的真实状态。

技术实现要素：

为了解决上述背景技术提出的技术问题，本发明提出了一种使用卡尔曼滤波器辨识多旋翼动力学模型参数的方法，解决了传统参数辨识方法未将飞行器作为整体考虑，实现困难的问题。

本发明为解决其技术问题采用如下技术方案：

一种使用卡尔曼滤波器辨识多旋翼动力学模型参数的方法，包括以下步骤：

(1)采集机载传感器信息，包括gps信息、三轴加速度计的输出、三轴陀螺仪的输出和各旋翼转速信息；

(2)根据多旋翼动力学模型特性与牛顿力学原理建立辨识动力学模型参数的卡尔曼滤波器状态方程与量测方程；

(3)对所建立的卡尔曼滤波器进行离散化，更新卡尔曼滤波器，对动力学参数进行辨识与输出。

在步骤(2)中，所述建立的卡尔曼滤波状态方程如下：

上式中为机体系相对于导航系的角速度在机体系x轴上的投影对时间的一阶微分，为机体系相对于导航系的角速度在机体系y轴上的投影对时间的一阶微分，为机体系相对于导航系的角速度在机体系z轴上的投影对时间的一阶微分；为飞行器机体系相对于导航系的速度在机体系x轴上的投影对时间的一阶微分，为飞行器机体系相对于导航系的速度在机体系y轴上的投影对时间的一阶微分，为飞行器机体系相对于导航系的速度在机体系z轴上的投影对时间的一阶微分；为机体系x轴阻力参数对时间的一阶微分，为机体系y轴阻力参数对时间的一阶微分，为升力参数对时间的一阶微分，为绕机体系x轴旋转的角加速度参数对时间的一阶微分，为绕机体系y轴旋转的角加速度参数对时间的一阶微分，为绕机体系z轴旋转的角加速度参数对时间的一阶微分，机体系y轴上气动阻力引起的绕机体系x轴旋转的角加速度参数对时间的一阶微分，为机体系x轴上气动阻力引起的绕机体系y轴旋转的角加速度参数对时间的一阶微分；kx为机体系x轴阻力参数，ky为机体系y轴阻力参数，kt为升力参数,kp为绕机体系x轴旋转的角加速度参数，kq为绕机体系y轴旋转的角加速度参数，kr为绕机体系z轴旋转的角加速度参数，kpy为机体系y轴上气动阻力引起的绕机体系x轴旋转的角加速度参数，kqx为机体系x轴上气动阻力引起的绕机体系y轴旋转的角加速度参数；g为当地重力加速度；m为飞行器质量；φ为飞行器的横滚角，θ为飞行器的俯仰角；ωi为飞行器第i个旋翼的转速，ω1为飞行器第1个旋翼的转速，ω2为飞行器第2个旋翼的转速，ω3为飞行器第3个旋翼的转速，ω4为飞行器第4个旋翼的转速；为飞行器机体系相对于导航系的速度在机体系x轴上的投影，为飞行器机体系相对于导航系的速度在机体系y轴上的投影。

在步骤(2)中，所述建立的卡尔曼滤波量测方程如下：

式中，为陀螺仪x轴的输出，为陀螺仪y轴的输出，为陀螺仪z轴的输出；为gps输出的飞行器机体系相对于导航系的速度在机体系x轴上的投影，为gps输出的飞行器机体系相对于导航系的速度在机体系y轴上的投影，为gps输出的飞行器机体系相对于导航系的速度在机体系z轴上的投影；fbx为加计x轴的输出，fby为加计y轴的输出，fbz为加计z轴的输出；p为机体系相对于导航系的角速度在机体系x轴上的投影，q为机体系相对于导航系的角速度在机体系y轴上的投影，r为机体系相对于导航系的角速度在机体系z轴上的投影；为飞行器机体系相对于导航系的速度在机体系z轴上的投影。

在步骤(3)中，对所建立的卡尔曼滤波器进行离散化，得到状态方程：

上式中，为x对时间的一阶微分，状态量

，控制量u＝g，w为系统噪声向量，是均值为0的高斯白噪声，其中：

在步骤(3)中，对卡尔曼滤波器状态进行更新：

xk+1＝φk+1,kxk+bkuk+wk

zk+1＝hk+1xk+1+vk+1

上式中，xk+1为k+1时刻的状态向量，φk+1,k为k到k+1时刻的系统一步转移矩阵，xk为k时刻的状态向量，bk为uk的系数矩阵，uk为k时刻的控制向量，wk为k时刻的系统噪声向量；zk+1为k+1时刻的状态向量，hk+1为k+1时刻的量测矩阵，vk+1为k+1时刻的量测噪声向量。

本发明的有益效果如下：

(1)本发明相较于传统的动力学模型参数辨识方法，不依赖专业的设备，且更易于实现。

(2)本发明采用卡尔曼滤波器进行参数辨识，实现对参数的最优估计。

附图说明

图1是本发明的流程框架示意图。

具体实施方式

下面将结合附图对本发明创造做进一步详细说明。

一种使用卡尔曼滤波器辨识多旋翼动力学模型参数的方法，如图1所示，步骤如下：

步骤1：

采集机载传感器信息，包括gps输出的飞行器机体系相对于导航系的速度信息在机体系x、y、z轴上的投影三轴加速度计的输出[fbxfbxfbx]，三轴陀螺仪的输出和多旋翼飞行器各旋翼的转速ωi。

步骤2：

以x型四旋翼为例，建立如下动力学特性方程：

上式中，t是由旋翼旋转产生的升力之和，方向沿机体系z轴负向；fbx为飞行器在机体系x轴方向上受到的阻力，方向与的方向相反；fby为飞行器在机体系y轴方向上受到的阻力，方向与的方向相反；p为旋翼升力引起的横滚力矩，q为旋翼升力引起的俯仰力矩；r为旋翼旋转时产生的反扭矩；kx为机体系x轴阻力参数，ky为机体系y轴阻力参数，kt为升力参数,kp为绕机体系x轴旋转的角加速度参数，kq为绕机体系y轴旋转的角加速度参数，kr为绕机体系z轴旋转的角加速度参数；ixx为绕机体系x轴的转动惯量，iyy为绕机体系y轴的转动惯量，izz为绕机体系z轴的转动惯量；ω1为飞行器第1个旋翼的转速，ω2为飞行器第2个旋翼的转速，ω3为飞行器第3个旋翼的转速，ω4为飞行器第4个旋翼的转速；为飞行器机体系相对于导航系的速度在机体系x轴上的投影，为飞行器机体系相对于导航系的速度在机体系y轴上的投影。

在本发明中，机体系：原点与载体质心重合，x轴沿载体纵轴向前，y轴沿载体横轴向右，z轴沿载体竖轴向下，即“前右下”坐标系，用b表示；导航坐标系：原点与载体质心重合，x轴和y轴在当地水平面内分别沿当地经线和纬线的切线方向，z轴沿当地地理垂线方向指向地面，即“北东地”坐标系，用n表示。

步骤3：

根据牛顿力学原理，得到如下力与力矩方程：

上式中g为当地重力加速度；m为飞行器的质量；φ为飞行器的横滚角，θ为飞行器的俯仰角；fbylx为由于质心不在机体系水平面上从而由气动阻力引起的横滚力矩，lx为质心到机体系x轴的距离；fbxly为由于质心不在机体系水平面上从而由气动阻力引起的俯仰力矩，ly为质心到机体系y轴的距离；为机体系相对于导航系的角速度在机体系x轴上的投影对时间的一阶微分，为机体系相对于导航系的角速度在机体系y轴上的投影对时间的一阶微分，为机体系相对于导航系的角速度在机体系z轴上的投影对时间的一阶微分；为飞行器机体系相对于导航系的速度在机体系x轴上的投影对时间的一阶微分，为飞行器机体系相对于导航系的速度在机体系y轴上的投影对时间的一阶微分，为飞行器机体系相对于导航系的速度在机体系z轴上的投影对时间的一阶微分。

步骤4：

根据以上动力学特性与牛顿力学原理，构建卡尔曼滤波状态方程如下：

式中，kpy为机体系y轴上气动阻力引起的绕机体系x轴旋转的角加速度参数；kqx为机体系x轴上气动阻力引起的绕机体系y轴旋转的角加速度参数，为机体系x轴阻力参数对时间的一阶微分，为机体系y轴阻力参数对时间的一阶微分，为升力参数对时间的一阶微分，为绕机体系x轴旋转的角加速度参数对时间的一阶微分，为绕机体系y轴旋转的角加速度参数对时间的一阶微分，为绕机体系z轴旋转的角加速度参数对时间的一阶微分，机体系y轴上气动阻力引起的绕机体系x轴旋转的角加速度参数对时间的一阶微分，为机体系x轴上气动阻力引起的绕机体系y轴旋转的角加速度参数对时间的一阶微分，ωi为飞行器第i个旋翼的转速。

由加速度计敏感比力，可得如下量测方程：

式中，为gps输出的飞行器机体系相对于导航系的速度在机体系x轴上的投影，为gps输出的飞行器机体系相对于导航系的速度在机体系y轴上的投影，为gps输出的飞行器机体系相对于导航系的速度在机体系z轴上的投影；fbx为加计x轴的输出，fby为加计y轴的输出，fbz为加计z轴的输出；为陀螺仪x轴的输出，为陀螺仪y轴的输出，为陀螺仪z轴的输出；p为机体系相对于导航系的角速度在机体系x轴上的投影，q为机体系相对于导航系的角速度在机体系y轴上的投影，r为机体系相对于导航系的角速度在机体系z轴上的投影；为飞行器机体系相对于导航系的速度在机体系z轴上的投影；ωi为飞行器第i个旋翼的转速。

步骤5：

对所建立的卡尔曼滤波器进行离散化，得到状态方程：

上式中，为x对时间的一阶微分，状态量

，控制量u＝g，w为系统噪声向量，是均值为0的高斯白噪声，其中：

所建立的卡尔曼滤波器的量测方程：

z＝hx+v

上式中，量测量v为量测噪声向量，是均值为0的高斯白噪声，其中：

式中i6×6为6阶单位矩阵。

步骤6：

对卡尔曼滤波器状态进行更新：

xk+1＝φk+1,kxk+bkuk+wk

zk+1＝hk+1xk+1+vk+1

上式中，xk+1为k+1时刻的状态向量，φk+1,k为k到k+1时刻的系统一步转移矩阵，xk为k时刻的状态向量，bk为uk的系数矩阵，uk为k时刻的控制向量，wk为k时刻的系统噪声向量；zk+1为k+1时刻的状态向量，hk+1为k+1时刻的量测矩阵，vk+1为k+1时刻的量测噪声向量。

首先进行状态一步预测：

上式中，为k时刻状态向量xk的卡尔曼滤波估值，是利用计算得到的对k+1时刻状态向量xk+1的预测值。

其次求解一步预测的均方差误差：

上式中，pk为k时刻估计均方差误差矩阵，qk为wk的方差矩阵，pk+1|k为的均方差误差矩阵，φ^tk+1,k为φk+1,k的转置矩阵。然后求解滤波增益：

上式中，kk+1为k+1时刻滤波增益矩阵，rk+1为vk+1的方差矩阵，h^tk+1为hk+1的转置矩阵。

最后对状态估值，得到滤波结果：

其中，为k+1时刻状态向量xk+1的卡尔曼滤波估值。

同时更新估计均方差误差：

pk+1＝[i-kk+1hk+1]pk+1|k

上式中，pk+1为k+1时刻估计均方差误差矩阵，i为单位矩阵。带入旋翼飞行器飞行时记录的数据，按照上述步骤进行循环更新，卡尔曼滤波器输出对参数[kxkyktkpkqkrkpykqx]^t的估计。

实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。