[发明专利]一种四旋翼无人机栖停机动轨迹优化方法

权利要求书

1.一种四旋翼无人机栖停机动轨迹优化方法，其特征在于具体步骤如下：

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

坐标系选定：四旋翼无人机建模所需的坐标系，分别是地面坐标系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.基于时间的运动轨迹求解；

通过沿时间积分，获得无人机的位移与俯仰角随时间变化曲线，具体表达式为：

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

2.2.基于距离的运动轨迹求解；

根据当前时刻的坐标位置，采用插值方法求解当前时刻的推力，具体步骤如下：

2.2.1问题设定：给定轨迹优化的起始位置、目标位置，确定运动约束；

2.2.2推力设定：在给定飞行距离内进行划分，设定采样点数N+1，将轨迹均匀的划分为N段，采用随机取样方法设定推力序列{F₁(x_j),F₂(x_j),F₃(x_j),F₄(x_j)}，j为采样点序号且j＝1～N+1，采用样条方法生成相对于距离坐标的推力曲线；

2.2.3轨迹生成：采用时间推进方法求解飞行轨迹，当前时刻的推力由当前时刻对应的位置坐标在推力序列{F₁(x_j),F₂(x_j),F₃(x_j),F₄(x_j)}插值而来；

2.2.4截止条件：当求解轨迹的水平坐标与目标点水平坐标一致时，轨迹求解终止；

步骤三：混合约束条件下的轨迹优化；

3.1.建立约束；

四旋翼无人机在垂面栖停运动过程需要满足运动过程中最大需要推力小于电机最大推力，且着陆时满足步骤二确定的运动约束，具体表述如下：

3.1.1动力约束：轨迹优化过程中，任意时刻的推力输出均小于单轴最大推力F_max；

3.1.2俯仰角约束：无人机在与栖停表面接触时刻，四旋翼无人机的俯仰角为θ_des，确保无人机将其栖停装置朝向栖停表面；

3.1.3速度约束：无人机在与垂面接触时刻，四旋翼无人机垂直壁面速度U_des范围为U_min≤U_des≤U_max，法向速范围为W_min≤W_des≤W_max；

3.1.4位置约束：无人机在与栖停表面接触时刻，四旋翼无人机降落位置z_f与目标位置z_des的高度误差不超过某一数值，即|z_f-z_des|≤ε，ε为容许误差；

3.2.构造罚函数，包括性能函数、目标约束函数以及动力约束函数，表达式如下：

Cost＝J₁+J₂+J₃

式中，J₁为性能函数，J₂为目标约束，J₃为动力约束，各约束表达式如下：

J₁＝∑(T·v)·Δt

J₂＝(U_f-U_des)²+(W_f-W_des)²+(θ_f-θ_des)²

J₃＝∑max{F_i(t)-F_max,0}

3.3.构建优化求解器；

基于步骤二中建立的基于距离的推力序列{F₁(x_j),F₂(x_j),F₃(x_j),F₄(x_j)}j＝1～N+1，通过罚函数结合数值优化算法来获得运动约束与动力学约束的可行轨迹；轨迹优化流程步骤如下：

3.3.1初值设定：采用随机采样法生产初始推力时间序列的初始样本，该推力序列以距离为参考，记为{F₁(x_j),F₂(x_j),F₃(x_j),F₄(x_j)},j＝1～N+1，内部插值采用样条方法；

3.3.2基于步骤二的运动轨迹生成方法解算运动轨迹，并计算罚函数；

3.3.3采用数值优化算法对设计变量{F₁(x_j),F₂(x_j),F₃(x_j),F₄(x_j)},j＝1～N+1进行搜索并更新设计变量；

3.3.4当罚函数数值小于阈值时，结束优化，否则，重复进行步骤3.3.2和步骤3.3.3。