[发明专利]一种基于误差状态卡尔曼滤波的无人系统外力和外力矩在线估计方法

权利要求书

1.一种基于误差状态卡尔曼滤波的无人系统外力和外力矩在线估计方法，其特征在于该方法的步骤包括

步骤一，构建四旋翼无人机的系统动力学模型；

步骤二，构建四旋翼无人机的测量模型；

步骤三，根据步骤一构建的四旋翼无人机的系统动力学模型构建四旋翼无人机的真实状态动力学模型、标称状态动力学模型、误差状态动力学模型；

步骤四，离散步骤三中构建的标称状态动力学模型，得到标称状态离散动力学模型，离散步骤三中构建的误差状态动力学模型，得到误差状态离散动力学模型；

步骤五，根据步骤四得到的标称状态离散动力学模型、误差状态离散动力学模型以及步骤二中构建的测量模型，求取雅可比，并将求取的雅可比代入到误差状态卡尔曼滤波公式中，得到状态估计递推式，并从得到的状态估计递推式中提取出外力和外力矩两项，完成四旋翼无人机外力和外力矩的在线估计。

2.根据权利要求1所述的一种基于误差状态卡尔曼滤波的无人系统外力和外力矩在线估计方法，其特征在于：

所述的步骤一中，构建四旋翼无人机的系统动力学模型的方法为：

定义固连在四旋翼无人机质心的坐标系为x、y、z轴分别为固连在地面上的惯性系为ε，x、y、z轴分别为ε_x、ε_y、ε_z，为系相对于ε系的线速度在系下的表示，为系相对于ε系的角速度在系下的表示，R∈SO(3)为系相对于ε系的旋转，记e₃＝[0,0,1]^T，重力加速度大小为g＝9.8m/s²，无人机的质量为m，惯性张量矩阵为I，忽略惯量积I_xy,I_xz,I_yz，仅保留惯量矩I_xx,I_yy,I_zz，即I＝diag(I_xx,I_yy,I_zz)，k_f、k_τ、k_flap、k_m为螺旋桨的空气动力学参数，u_i为螺旋桨i的转速，I_i为电机i的电流，为螺旋桨i到机体质心的位置向量，D＝diag(k_dx,k_dy,k_dz)为无人机的气动阻力系数，∈_i表示螺旋桨旋转方向(顺时针为+1，逆时针为-1)；i＝1,2,3,4；

无人机受到的外力包括重力G^B、螺旋桨推力气动阻力和待估计的其他外力

其中

G^B＝mgR^Te₃

无人机会受到的外力矩为

其中

在B系下根据牛顿方程有

在B系下根据欧拉方程有

在方程中引入零均值高斯白噪声；

其中，

使用单位四元数表示旋转；

将外力和外力矩建模成随机游走过程；

其中，

定义状态向量为

系统动力学模型为