[发明专利]一种四旋翼无人机栖停机动轨迹规划、跟踪控制方法

权利要求书

1.一种四旋翼无人机栖停机动轨迹规划、跟踪控制方法，其特征在于具体步骤如下：

步骤一、四旋翼无人机动力学建模；

首先坐标系选定：四旋翼无人机建模所需的坐D标系，分别是地面坐标系O_gx_gy_gz_g和机体坐标系O_bx_by_bz_b；两坐标系的相对关系由滚转角φ、俯仰角θ和偏航角ψ描述；

然后建立动力学方程，根据刚体运动学方程和动力学方程建立四旋翼无人机的纵向运动的动力学方程：

其中，F_x为合外力在地面坐标系x_g轴的分量，F_z为合外力在地面坐标系z_g轴的分量，M为无人机合力矩矢量在地面坐标系y_g轴的分量；i为螺旋桨计数序号为，i＝1，2，3，4；F_i为螺旋桨i的推力，M_i为螺旋桨i的扭矩；U为无人机质心速度矢量在地面坐标系x_g轴的分量，W为无人机质心速度矢量在地面坐标系z_g轴的分量，q为无人机角速度矢量在地面坐标系y_g分量；I为四旋翼无人机关于机体坐标系y_b轴的转动惯量，m为无人机的质量；

步骤二、四旋翼无人机栖停机动轨迹规划；

2.1栖停机动时间估计；

轨迹无人机栖停机动所需时间T_est公式如下：

式中，β_max为电机最大俯仰角加速度，无人机翻转θ_des俯仰角所用时间为安全系数D，1＜D≤2；

2.2俯仰角时间序列计算；

在步骤2.1得到的四旋翼无人机栖停所需时间T_est内，四旋翼无人机俯仰角由0变化至θ_des；设定在起始和末尾两个时刻的角速度，然后，根据设定的起始和末尾两个时刻的角度和角速度要求，采用样条曲线建立俯仰角时间变化序列；

2.3运动轨迹生成；

根据步骤2.2建立的俯仰角时间变化序列，得到角速度、角加速度相对时间的变化曲线；再结合步骤一建立的动力学模型，设定栖停过程中四旋翼无人机总推力的时间变化序列；得出四旋翼无人机的纵向运动轨迹及运动信息计算公式如下：

式中，U₀为t＝0时刻的U值，W₀为为t＝0时刻的W值，q₀为为t＝0时刻的q值；x₀、z₀分别为t＝0时刻的坐标值，θ₀分别为t＝0时刻的俯仰角；

2.4起始点进行运动状态设计；

根据目标位置与位移，获得起始点位置(x₀.z₀)，表达式为：

x₀＝x_des-Δx

z₀＝z_des-Δz-W₀·T_est

其中，Δx为无人机的水平位移，Δ_z为无人机的垂直位移，(x_des.z_des)为假设目标位置坐标，W₀为无人机的垂直方向的起始点速度；

步骤三、四旋翼无人机栖停机动轨迹跟踪控制；

3.1设计与步骤一中动力学模型相对应的栖停机动轨迹跟踪控制律，并在软件平台中搭建仿真框架；

所述轨迹跟踪控制律以运动误差为输入，控制算法包括推力总和、力矩和控制分配；

(1)推力总和求解，即无人机的目标推力F为：

F＝f_des Re₃

式中，e₃的表达式为[0 0 1]^T；f_des为目标推力矢量；R表示旋转矩阵；

(2)力矩求解公式为：

M＝-k_Re_R-k_Ωe_Ω+Ω×JΩ

式中，k_R、k_Ω分别为角度误差系数和角速度误差系数，e_p为定义位置误差向量和为e_v速度误差向量，J为转动惯量矩阵；Ω表示旋转矩阵变化率；

(3)控制分配：

根据推力模型，全机总推力表示为：

式中，f为总推力，ω₁～ω₄为四轴电机的转速；c_T表示螺旋桨的推力系数；

无人机机身在坐标系下的x、y、z轴的力矩分别为：

式中，M_x、M_y、M_z分别为无人机体轴系下关于三个坐标轴的力矩表达式，d为电机中心到无人机重心的距离；

3.2将步骤二规划的四旋翼无人机栖停机动的运动轨迹数据加载至步骤3.1的软件平台中进行读取，结合步骤一建立的动力学模型获取当前时刻的无人机的运动状态信息与目标轨迹的运动误差信息，作为当前时刻的误差信息输入；

3.3基于步骤3.2得到的运动误差信息和步骤3.1轨迹跟踪控制算法得到当前时刻的总推力、力矩和，根据控制分配矩阵生成四旋翼的四个电机推力，将该推力输入到步骤一建立的无人机动力学模型，得到四旋翼无人机加速度信息表达式：

四旋翼无人机的纵向运动轨迹及运动信息计算公式如下：

式中，U₀为t＝0时刻的U值，W₀为为t＝0时刻的W值，q₀为为t＝0时刻的q值。x₀、z₀分别为t＝0时刻的坐标值，θ₀分别为t＝0时刻的俯仰角；F_i为螺旋桨i的推力，i为螺旋桨计数序号为，i＝1，2，3，4；

3.4重复进行步骤3.2至步骤3.3，对运动轨迹进行持续跟踪。