[发明专利]六旋翼无人机的自动容错姿态控制方法

权利要求书

1.一种六旋翼无人机的自动容错姿态控制方法，所述六旋翼无人机的每一电机具有相对于水平面沿一方倾斜安装角α，沿另一方向的倾斜安装角β，其特征在于，包括如下步骤：

步骤S1：采集六旋翼无人机上多个电机的电流和/或电压，根据所述电流和/或电压判断每一所述电机是否存在故障；

步骤S2：当判断出一所述电机为故障电机时，关闭另一与所述故障电机相邻且转动方向相反的电机；

步骤S3：根据所述故障电机所驱动的桨翼i的序号选择相对应的映射矩阵R_i，并根据所述映射矩阵R_i将输入姿态角转换为控制姿态角，i为大于等于0小于等于6的自然数；

步骤S4：将所述控制姿态角输入所述六旋翼无人机的飞行控制模块以控制所述六旋翼无人机飞行；

在所述步骤S1之前还包括如下步骤：

步骤M1：建立机体坐标系F_B，F_B＝{O_B，(x_B，y_B，z_B)}，其中，O_B为机体的中心，z_B、x_B、y_B分别为六旋翼无人机的Z轴、X轴、Y轴；z_B表示六旋翼无人机在平衡状态时，垂直于地面的方向，x_B表示穿过所述机体的中心沿所述机体的机头延伸的方向，y_B表示垂直于z_B和x_B的方向；

在电机i水平安装时，建立初始电机坐标系，在所述初始电机坐标系中z_C为Z轴，表示电机i的轴线方向且垂直于地面的方向，x_C为X轴，表示穿过所述电机i的中心沿机臂延伸的方向，y_C为Y轴，表示垂直于z_C和x_C形成平面的方向；

步骤M2：对于每一电机i，当该电机i驱动浆翼i顺时针转动时，将该电机i绕x_C轴逆时针旋转α_i角度以及绕y_C轴逆时针旋转β_i角度；当该电机i驱动浆翼i逆时针转动时，将该电机i绕x_C轴顺时针旋转α_i角度以及绕y_C轴逆时针旋转β_i角度；在本实施例，所述逆时针旋转和所述顺时针旋转的观测位置位于所述六旋翼无人机的外侧；

步骤M3：对于电机i，建立旋转后电机坐标系其中，i表示电机的序号，i为大于0小于等于6的自然数，表示电机i的中心，为电机i的Z轴、X轴、Y轴；为Z轴，表示电机i的轴线方向，为X轴，表示穿过所述电机i的中心与机臂延伸的方向呈β_i角度的方向，为Y轴，表示垂直于和的方向；

步骤M4：将每一轴与z_B轴之间的夹角通过倾斜安装角α、β来表示，由于第i桨翼的z_pi轴与该翼的旋转轴的轴向同方向，第i桨翼的z_pi轴与机体坐标系z_B轴之间的旋转关系为，

z_pi＝R_y(β_i)R_x(α_i)z_B

其中，R_y(β_i)表示y_B方向的旋转矩阵，R_x(α_i)表示x_B方向的旋转矩，分别表示如下：

当z_B＝[0 0 1]的时候，能够得到：

矩阵的第三个元素；

步骤M5：第i桨翼会产生沿z_pi方向的推力f_i和转矩τ_i，在些标系中可以表示为，

其中，k_fi、k_τi为与桨翼物理特性相关的常系数，kfi为推力与转速的关系参数，k_τi为力矩与转速的关系参数，ω_i为每个桨翼的转速，+表示该桨翼的旋转方向为顺时针，-表示该桨翼的旋转方向为逆时针；

六旋翼无人机的所有能够转动的螺旋桨所产生的总推力和总扭矩τ_c在地面坐标系下可以表示为

其中，p_i表示原点在机体坐标系F_B中的位置，u＝[ω₁|ω₁|，ω₂|ω₂|…，ω₆|ω₆|]，F₁为控制输入u到施加到无人机的推力的转换矩阵，F₂为控制输入u到施加到无人机的转矩的转换矩阵，n为能够转动的电机的总数，n小于等于i，R为地面坐标系与旋转后电机坐标系之间的转换矩阵，表示3×n的有理数矩阵；

根据牛顿欧拉公式，带有倾角的六旋翼无人机的动力学模型如下，

其中，六旋翼无人机的质量为m，g为重力加速度，ω为六旋翼无人机的姿态角，为六旋翼无人机的角速度，J为转动惯量，为加速度，e₃为地面坐标系Z轴(0，0，1)。