CN114924579A - 一种可倾转四旋翼无人机的运动轨迹生成方法及系统 - Google Patents

Abstract

一种可倾转四旋翼无人机的运动轨迹生成方法及系统，涉及无人机轨迹规划技术领域，用以解决现有技术不能在可倾转四旋翼无人机穿越不规则障碍物时对其位置及姿态进行有效单独规划的问题。本发明技术要点包括：对可倾转四旋翼无人机在含有障碍物的环境中飞行的运动轨迹进行规划，包括：搜索并生成无碰撞的初始轨迹；对初始轨迹进行优化，直至优化后轨迹中每个控制点到最近障碍物的距离均不小于预设最短距离阈值；对可倾转四旋翼无人机穿越障碍物的运动姿态进行规划，包括：将姿态对应到无人机运动轨迹中每个时间点，生成包含时间的目标姿态序列；对目标姿态序列进行优化。本发明使得可倾转四旋翼无人机可安全高效地通过飞行环境中的不规则障碍物。

Description

一种可倾转四旋翼无人机的运动轨迹生成方法及系统

技术领域

本发明涉及无人机轨迹规划技术领域，具体涉及一种可倾转四旋翼无人机的运动轨迹生成方法及系统。

背景技术

常规的四旋翼飞行器属于欠驱动系统，由于驱动力和驱动力矩具有强耦合性，导致在轨迹规划问题中，无法分别规划控制旋翼无人机的位置与姿态。为解决这个问题，在四旋翼无人机的机臂引入可倾转的舵机，使得固定垂直向上的旋翼可以绕机臂轴倾转，从而让四旋翼飞行器变为过驱动系统，实现位置与姿态的独立规划控制。根据微分平坦性质，常规四旋翼的轨迹规划问题中的变量只有三轴位置与偏航，然而，对于倾转四旋翼无人机，滚转与俯仰也作为单独的规划变量考虑，对于一些限制通过姿态的特殊障碍物，如椭圆环、狭缝等，三轴姿态均需要同时规划，在含有这些障碍物的场景下，倾转四旋翼明显更加适合执行穿越障碍物等飞行任务。因此，如何设计一种轨迹规划方法，从而保证倾转四旋翼无人机符合穿越不规则障碍物的位置姿态限制的工作是一个挑战性的问题。

近年来，在倾转四旋翼无人机的轨迹规划领域已有一定研究的成果。前端的路径搜索算法主要有人工势场法和基于随机采样的搜索树方法等。人工势场法应用灵活，可以在保证安全的情况下获得一条平滑路径，但是在障碍物多的环境下，人工势场法容易使规划陷入局部极小值点，从而使无人机停止运动。基于随机采样的搜索树方法可以在复杂约束环境中获得可行解，但是其求解速度较慢，很难应用于含有众多障碍物的大型复杂环境。后端的轨迹优化主要有Minimum-Snap多项式轨迹优化和微分动态规划法。Minimum-Snap多项式轨迹优化可以提升初始轨迹的平滑性，调整轨迹的时间分配使其更加高效，但是无法严格保证轨迹的安全。微分动态规划法当代价函数在最优轨迹附近趋近于零时可以求出最优轨迹，但是由于求解涉及的Hessian矩阵维数往往较高，计算比较耗时。在姿态规划方面，目前的算法多为姿态与位置规划的一种权衡，即为了保持特定姿态而牺牲部分速度或轨迹安全性。

发明内容

鉴于以上问题，本发明提出一种可倾转四旋翼无人机的运动轨迹生成方法及系统，用以解决现有技术不能在可倾转四旋翼无人机穿越不规则障碍物时对其位置及姿态进行有效单独规划的问题。

根据本发明的一方面，提供一种可倾转四旋翼无人机的运动轨迹生成方法，该方法包括以下步骤：

步骤一、对可倾转四旋翼无人机在含有障碍物的环境中飞行的运动轨迹进行规划；包括：

搜索并生成一条无碰撞的初始轨迹；

对初始轨迹进行优化，直至优化后轨迹中每个控制点到最近障碍物的距离均不小于预设最短距离阈值，获得优化后的无人机运动轨迹；

步骤二、对可倾转四旋翼无人机穿越障碍物的运动姿态进行规划；包括：

将姿态对应到无人机运动轨迹中每个时间点，生成包含时间的目标姿态序列；

对所述目标姿态序列进行优化，获得优化后对应时间点的无人机姿态序列。

进一步地，步骤一中利用A-star算法搜索并生成一条无碰撞的初始轨迹；其中，A-star算法中惩罚函数设计为：

g_i＝(‖u‖²+λ)T

其中，u为控制量，λ为时间耗费的权重，T为选择的前向积分步长，i为当前搜索节点的序号；

启发式函数设计为：

h_i＝||m_goal-m_i||

其中，m_goal为目标搜索节点，m_i为当前搜索节点；

则A-star算法中目标优化函数f_cci为：

f_cci＝g_i+h_i。

进一步地，步骤一中在利用A-star算法获得初始轨迹后，进一步通过B样条插值方法对初始轨迹进行插值，获得插值后的初始轨迹，其表达形式为：

其中，

s(t)＝(t-t_m)/Δt，表示时间占比，Δt表示时间间隔，t表示当前时间节点，t_m表示第m个时间节点；M_p+1表示由B样条曲线阶次p定义的恒定方阵；q_m表示参与该段B样条轨迹的控制点序列。

进一步地，步骤一中对初始轨迹进行优化的具体过程包括：

设计位置优化函数形式如下：

f_total＝λ₁f_s+λ₂f_c+λ₃(f_v+f_a)

其中，f_s表示平滑项，f_c表示碰撞项，f_v与f_a分别表示速度与加速度的动力学可行项；λ₁，λ₂，λ₃分别表示平滑项、碰撞项和动力学可行项的权重；

平滑项f_s设计如下：

其中，Q_i+1-Q_i与Q_i-1-Q_i分别表示轨迹在控制点Q_i两侧的另外两个控制点对Q_i的弹性力；N+1表示控制点总数；

碰撞项f_c设计如下：

其中，d(Q_i)表示当前控制点Q_i到最近障碍物的距离；当d(Q_i)大于等于预设最短距离阈值d₀时，F_c(d(Q_i))＝(d(Q_i)-d₀)²；

速度的动力学可行项f_v设计如下：

其中，v_μ表示无人机在三维空间μ∈(x,y,z)的飞行速度，当v_μ大于等于μ维度下允许无人机飞行的最大速度v_max时，F_v(v_μ)＝(v_μ-v_max)²；

加速度的动力学可行项f_a设计如下：

其中，a_μ表示无人机在μ维度的飞行加速度(μ∈(x,y,z))，当a_μ大于等于μ维度下允许无人机飞行的最大加速度a_max时，F_a(a_μ)＝(a_μ-a_max)²；

通过对设计的位置优化函数的非线性无约束最小值求解，获得优化后的控制点序列，并重新生成B样条曲线作为优化后的无人机运动轨迹。

进一步地，步骤一中对初始轨迹进行优化的过程中，若优化后轨迹中每个控制点到最近障碍物的距离小于预设最短距离阈值，则增加位置优化函数中碰撞项的权重，以使位置优化函数的迭代优化能快速得到满足碰撞约束的轨迹。

进一步地，步骤二中生成包含时间的目标姿态序列的具体过程为：以穿越障碍物后的三轴姿态角最小值与最大值的平均值作为终点姿态；以穿越障碍物的时间起点减去规划时间得到的时间点所对应的无人机三轴姿态角作为起点姿态；穿越障碍物的过程中其余时刻的姿态通过对起点姿态和终点姿态进行均匀插值获得；将相应姿态对应到步骤一优化后的无人机运动轨迹的时间点，生成包含时间的目标姿态序列；其中，所述规划时间为无人机在穿越障碍物的时间起点向前的一段时间。

进一步地，步骤二中对所述目标姿态序列进行优化的具体过程包括：

设计姿态优化函数f_atti形式如下：

f_atti＝λ_trf_track+λ_smf_smooth

其中，f_track表示姿态跟踪项；f_smooth表示姿态平滑项；λ_tr与λ_sm分别表示姿态跟踪项和姿态平滑项的权重；

姿态跟踪项设计如下：

其中，h表示规划步长；α(i)表示待优化姿态序列；α(i)表示目标姿态序列；φ,θ,ψ表示三轴姿态角；t_tilt表示规划时间；

姿态平滑项设计如下：

通过对设计的姿态优化函数的非线性无约束最小值求解，获得优化后的无人机姿态序列。

根据本发明的另一方面，提供一种可倾转四旋翼无人机的运动轨迹生成系统，该系统包括：

轨迹规划模块，其配置成对可倾转四旋翼无人机在含有障碍物的环境中飞行的运动轨迹进行规划；包括初始轨迹生成子模块和轨迹优化子模块，所述初始轨迹生成子模块配置成搜索并生成一条无碰撞的初始轨迹；所述轨迹优化子模块配置成对初始轨迹进行优化，直至优化后轨迹中每个控制点到最近障碍物的距离均不小于预设最短距离阈值，获得优化后的无人机运动轨迹；

姿态规划模块，其配置成对可倾转四旋翼无人机穿越障碍物的运动姿态进行规划；包括目标姿态生成子模块和姿态优化子模块，所述目标姿态生成子模块配置成将姿态对应到无人机运动轨迹中每个时间点，生成包含时间的目标姿态序列；所述姿态优化子模块配置成对所述目标姿态序列进行优化，获得优化后对应时间点的无人机姿态序列。

进一步地，所述初始轨迹生成子模块中利用A-star算法搜索并生成一条无碰撞的初始轨迹；其中，A-star算法中惩罚函数设计为：

g_i＝(‖u‖²+λ)T

其中，u为控制量，λ为时间耗费的权重，T为选择的前向积分步长，i为当前搜索节点的序号；

启发式函数设计为：

h_i＝||m_goal-m_i||

其中，m_goal为目标搜索节点，m_i为当前搜索节点；

则A-star算法中目标优化函数f_cci为：

f_cci＝g_i+h_i

在利用A-star算法获得初始轨迹后，进一步通过B样条插值方法对初始轨迹进行插值，获得插值后的初始轨迹，其表达形式为：

其中，

s(t)＝(t-t_m)/Δt，表示时间占比，Δt表示时间间隔，t表示当前时间节点，t_m表示第m个时间节点；M_p+1表示由B样条曲线阶次p定义的恒定方阵；q_m表示参与该段B样条轨迹的控制点序列；

所述轨迹优化子模块中对初始轨迹进行优化的具体过程包括：

设计位置优化函数形式如下：

f_total＝λ₁f_s+λ₂f_c+λ₃(f_v+f_a)

其中，f_s表示平滑项，f_c表示碰撞项，f_v与f_a分别表示速度与加速度的动力学可行项；λ₁，λ₂，λ₃分别表示平滑项、碰撞项和动力学可行项的权重；

平滑项f_s设计如下：

其中，Q_i+1-Q_i与Q_i-1-Q_i分别表示轨迹在控制点Q_i两侧的另外两个控制点对Q_i的弹性力；N+1表示控制点总数；

碰撞项f_c设计如下：

其中，d(Q_i)表示当前控制点Q_i到最近障碍物的距离；当d(Q_i)大于等于预设最短距离阈值d₀时，F_c(d(Q_i))＝(d(Q_i)-d₀)²；

速度的动力学可行项f_v设计如下：

其中，v_μ表示无人机在三维空间μ∈(x,y,z)的飞行速度，当v_μ大于等于μ维度下允许无人机飞行的最大速度v_max时，F_v(v_μ)＝(v_μ-v_max)²；

加速度的动力学可行项f_a设计如下：

其中，a_μ表示无人机在μ维度的飞行加速度(μ∈(x,y,z))，当a_μ大于等于μ维度下允许无人机飞行的最大加速度a_max时，F_a(a_μ)＝(a_μ-a_max)²；

通过对设计的位置优化函数的非线性无约束最小值求解，获得优化后的控制点序列，并重新生成B样条曲线作为优化后的无人机运动轨迹；对初始轨迹进行优化的过程中，若优化后轨迹中每个控制点到最近障碍物的距离小于预设最短距离阈值，则增加位置优化函数中碰撞项的权重，以使位置优化函数的迭代优化能快速得到满足碰撞约束的轨迹。

进一步地，所述目标姿态生成子模块中生成包含时间的目标姿态序列的具体过程为：以穿越障碍物后的三轴姿态角最小值与最大值的平均值作为终点姿态；以穿越障碍物的时间起点减去规划时间得到的时间点所对应的无人机三轴姿态角作为起点姿态；穿越障碍物的过程中其余时刻的姿态通过对起点姿态和终点姿态进行均匀插值获得；将相应姿态对应到步骤一优化后的无人机运动轨迹的时间点，生成包含时间的目标姿态序列；其中，所述规划时间为无人机在穿越障碍物的时间起点向前的一段时间；

所述姿态优化子模块中对所述目标姿态序列进行优化的具体过程包括：

设计姿态优化函数f_atti形式如下：

f_atti＝λ_trf_track+λ_smf_smooth

其中，f_track表示姿态跟踪项；f_smooth表示姿态平滑项；λ_tr与λ_sm分别表示姿态跟踪项和姿态平滑项的权重；

姿态跟踪项设计如下：

其中，h表示规划步长；α(i)表示待优化姿态序列；α^*(i)表示目标姿态序列；φ,θ,ψ表示三轴姿态角；t_tilt表示规划时间；

姿态平滑项设计如下：

通过对设计的姿态优化函数的非线性无约束最小值求解，获得优化后的无人机姿态序列。

本发明的有益技术效果是：

由于可倾转四旋翼无人机的位置与姿态可单独规划，基于软约束优化理论，本发明提出一种可倾转四旋翼无人机的轨迹规划方法及系统，包括对可倾转四旋翼无人机在含有障碍物的环境中飞行的运动轨迹进行规划和对其穿越障碍物的运动姿态进行规划；其中，在轨迹优化部分增加了迭代优化，在迭代优化过程中调整权重，可以更快的得到更安全的轨迹。大量实验数据表明，本发明适用于可倾转四旋翼无人机的轨迹规划与生成，且能够让可倾转四旋翼无人机安全高效快速地通过飞行环境中的不规则障碍物。

附图说明

本发明可以通过参考下文中结合附图所给出的描述而得到更好的理解，所述附图连同下面的详细说明一起包含在本说明书中并且形成本说明书的一部分，而且用来进一步举例说明本发明的优选实施例和解释本发明的原理和优点。

图1是可倾转四旋翼无人机的结构示意图；

图2是本发明实施例一种可倾转四旋翼无人机的轨迹规划方法的流程示意图；

图3是本发明实施例中轨迹生成及优化示例图；

图4是本发明实施例中可倾转四旋翼无人机位置跟踪示例图；

图5是本发明实施例中可倾转四旋翼无人机姿态跟踪示例图；

图6是本发明实施例中可倾转四旋翼无人机电机转速示意图；

图7是本发明实施例中可倾转四旋翼无人机舵机倾转角示意图。

图8是本发明实施例一种可倾转四旋翼无人机的轨迹规划系统的结构示意图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，在下文中将结合附图对本发明的示范性实施方式或实施例进行描述。显然，所描述的实施方式或实施例仅仅是本发明一部分的实施方式或实施例，而不是全部的。基于本发明中的实施方式或实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式或实施例，都应当属于本发明保护的范围。

如图1所示，可倾转四旋翼无人机包括机身1、飞行控制器2、GPS定位模块3、四个无刷电机4-1、4-2、4-3、4-4和四个倾转舵机5-1、5-2、5-3、5-4，倾转舵机设置在四旋翼无人机的机臂上，当收到电机转速指令和舵机偏转指令后，无刷电机的转子以指定速度转动，倾转舵机产生不同角度的偏转角，进而带动无刷电机围绕机臂整体转动。

建立地球固连坐标系

机体坐标系

和旋翼坐标系

旋翼坐标系的原点固定于i号电机质心，

轴过i号电机所在机械臂的轴心且正方向朝外，

轴正方向垂直向上，

轴符合右手螺旋定则；i号旋翼绕倾转轴

的倾转角为ρ_i，1、2号旋翼逆时针方向旋转，3、4号旋翼顺时针旋转。

根据牛顿-欧拉方程可建立可倾转四旋翼无人机的动力学模型如下：

其中，m表示飞行器的质量，J表示飞行器惯性矩阵，

表示地球固连坐标系下飞行器线速度，

表示机体坐标系下飞行器角加速度，Ω^B是机体坐标系下飞行器角速度；F^E表示地球固连坐标系下飞行器所受合力，M^B是机体坐标系下飞行器所受力矩。

由可倾转四旋翼无人机的动力学模型可知：由于F^E与M^B可以解耦合，因此确定位置与姿态可单独规划。

本发明实施例提供一种可倾转四旋翼无人机的运动轨迹生成方法，该方法包括以下步骤：

步骤一、对可倾转四旋翼无人机在含有障碍物的环境中飞行的运动轨迹进行规划；包括：

搜索并生成一条无碰撞的初始轨迹；

对初始轨迹进行优化，直至优化后轨迹中每个控制点到最近障碍物的距离均不小于预设最短距离阈值，获得优化后的无人机运动轨迹；

步骤二、对可倾转四旋翼无人机穿越障碍物的运动姿态进行规划；包括：

将姿态对应到无人机运动轨迹中每个时间点，生成包含时间的目标姿态序列；

对目标姿态序列进行优化，获得优化后对应时间点的无人机姿态序列。

本实施例中，可选地，步骤一中利用A-star算法搜索并生成一条无碰撞的初始轨迹；其中，A-star算法中惩罚函数设计为：

g_i＝(‖u‖²+λ)T

其中，u为控制量，λ为时间耗费的权重，T为选择的前向积分步长，i为当前搜索节点的序号；

启发式函数设计为：

h_i＝||m_goal-m_i||

其中，m_goal为目标搜索节点，m_i为当前搜索节点；

则A-star算法中目标优化函数f_cci为：

f_cci＝g_i+h_i。

本实施例中，可选地，步骤一中在利用A-star算法获得初始轨迹后，进一步通过B样条插值方法对初始轨迹进行插值，获得插值后的初始轨迹，其表达形式为：

其中，

s(t)＝(t-t_m)/Δt，表示时间占比，Δt表示时间间隔，t表示当前时间节点，t_m表示第m个时间节点；M_p+1表示由B样条曲线阶次p定义的恒定方阵；q_m表示参与该段B样条轨迹的控制点序列。

本实施例中，可选地，步骤一中对初始轨迹进行优化的具体过程包括：

设计位置优化函数形式如下：

f_total＝λ₁f_s+λ₂f_c+λ₃(f_v+f_a)

其中，f_s表示平滑项，f_c表示碰撞项，f_v与f_a分别表示速度与加速度的动力学可行项；λ₁，λ₂，λ₃分别表示平滑项、碰撞项和动力学可行项的权重；

平滑项f_s设计如下：

其中，Q_i+1-Q_i与Q_i-1-Q_i分别表示轨迹在控制点Q_i两侧的另外两个控制点对Q_i的弹性力；N+1表示控制点总数；

碰撞项f_c设计如下：

其中，d(Q_i)表示当前控制点Q_i到最近障碍物的距离；当d(Q_i)大于等于预设最短距离阈值d₀时，F_c(d(Q_i))＝(d(Q_i)-d₀)²；

速度的动力学可行项f_v设计如下：

其中，v_μ表示无人机在三维空间μ∈(x,y,z)的飞行速度，当v_μ大于等于μ维度下允许无人机飞行的最大速度v_max时，F_v(v_μ)＝(v_μ-v_max)²；

加速度的动力学可行项f_a设计如下：

其中，a_μ表示无人机在μ维度的飞行加速度(μ∈(x,y,z))，当a_μ大于等于μ维度下允许无人机飞行的最大加速度a_max时，F_a(a_μ)＝(a_μ-a_max)²；

通过对设计的位置优化函数的非线性无约束最小值求解，获得优化后的控制点序列，并重新生成B样条曲线作为优化后的无人机运动轨迹。

本实施例中，可选地，步骤一中对初始轨迹进行优化的过程中，若优化后轨迹中每个控制点到最近障碍物的距离小于预设最短距离阈值，则增加位置优化函数中碰撞项的权重，以使位置优化函数的迭代优化能快速得到满足碰撞约束的轨迹。

本实施例中，可选地，步骤二中生成包含时间的目标姿态序列的具体过程为：以穿越障碍物后的三轴姿态角最小值与最大值的平均值作为终点姿态；以穿越障碍物的时间起点减去规划时间得到的时间点所对应的无人机三轴姿态角作为起点姿态；穿越障碍物的过程中其余时刻的姿态通过对起点姿态和终点姿态进行均匀插值获得；将相应姿态对应到步骤一优化后的无人机运动轨迹的时间点，生成包含时间的目标姿态序列；其中，规划时间为无人机在穿越障碍物的时间起点向前的一段时间。

本实施例中，可选地，步骤二中对目标姿态序列进行优化的具体过程包括：

设计姿态优化函数f_atti形式如下：

f_atti＝λ_trf_track+λ_smf_smooth

其中，f_track表示姿态跟踪项；f_smooth表示姿态平滑项；λ_tr与λ_sm分别表示姿态跟踪项和姿态平滑项的权重；

姿态跟踪项设计如下：

其中，h表示规划步长；α(i)表示待优化姿态序列；α^*(i)表示目标姿态序列；φ,θ,ψ表示三轴姿态角；t_tilt表示规划时间；

姿态平滑项设计如下：

通过对设计的姿态优化函数的非线性无约束最小值求解，获得优化后的无人机姿态序列。

本发明另一实施例基于软约束优化理论，提供一种可倾转四旋翼无人机的轨迹规划方法，如图2所示，该方法包括以下步骤：

步骤一、选用A*(即A-star)算法对含有障碍物的环境进行搜索并生成一条无碰撞的初始路径。

根据本发明实施例，根据无人机动力学约束的要求，选用前向积分的方式离散控制空间，设计A*算法的相关函数，则A*算法的惩罚函数设计为：

g_i＝(‖u‖²+λ)T

其中，u为控制量，λ为时间耗费的权重，T为选择的前向积分步长，i为当前扩展节点的序号。

选用当前节点距终点的欧式距离评估当前节点代价，则A*算法的启发式函数设计为：

h_i＝||m_goal-m_i||

其中，m_goal为目标节点，m_i为当前节点。

最终，A*算法的目标优化函数为：

f_cci＝g_i+h_i

通过A*算法获得一系列的搜索节点，相邻节点连线构成初始路径。选用B样条曲线对初始路径进行插值，得到初始轨迹。B样条曲线由曲线阶次p，N+1个控制点Q_i组成的控制点序列{Q₀,Q₁,…,Q_n}，M+1维的节点向量{t₀,t₁,…,t_M}定义，M、N、p满足如下关系：M＝N+p+1。

定义每个时间间隔Δt＝t_m+1-t_m为定值，定义s(t)＝(t-t_m)/Δt，得到轨迹的具体表达形式P：

其中，M_p+1是由p定义的恒定方阵。通过比较，选用四阶B样条曲线插值初始路径，得到初始轨迹。

B样条曲线由于具有强凸包性质，可以通过移动控制点来改变并控制轨迹形状；B样条每个区间的轨迹形状由相应不超过p+1个控制点Q控制；并且B样条曲线的导数为低一阶次的B样条曲线，天然保证导数连续性，无需对轨迹施加速度与加速度的连续性约束；B样条曲线的任一轨迹点的距离障碍物的距离均大于相应控制点距障碍物的距离，速度，加速度均小于相应轨迹的Q控制点的速度与加速度。根据凸包性质，对控制点施加碰撞约束，动力学约束，即可保证整个轨迹安全且动力学可行。

步骤二、由于初始路径不够安全，部分轨迹点距离障碍物较近且不满足动力学约束，并且不够平滑，需要对初始路径进行轨迹优化满足轨迹安全高效，因此，设计位置优化函数形式如下：

f_total＝λ₁f_s+λ₂f_c+λ₃(f_v+f_a)

其中，f_s为平滑项，f_c为碰撞项，f_v与f_a分别对应速度与加速度的动力学可行项。λ₁，λ₂，λ₃分别为对应优化项的权重。

设计平滑项，碰撞项和动力学可行项。基于弹性带理论，平滑项f_s设计如下：

其中，Q_i+1-Q_i与Q_i-1-Q_i分别代表轨迹在Q_i两侧的控制点对Q_i的弹性力。

碰撞项f_c设计如下：

F_c(d(Q_i))＝(d(Q_i)-d₀)² if d(Q_i)≥d₀

其中，d(Q_i)为当前控制点Q_i到最近障碍物的距离，d₀为轨迹上任意一点到最近障碍物的最短距离，d₀＝0.2m。

关于速度的动力学可行项设计，首先定义F_v如下：

F_v(v_μ)＝(v_μ-v_max)² if v_μ≥v_max

其中，v_μ为无人机在μ维度的飞行速度(μ∈(x,y,z))，v_max为μ维度下允许无人机飞行的最大速度，三个维度的v_max＝2m/s。

然后通过三个维度各个控制点F_v的累加求和得到速度的动力学可行项：

关于加速度的动力学可行项如下：

其中F_a(a_μ)的构造与F_v(v_μ)类似，a_μ为无人机在μ维度的飞行速度(μ∈(x,y,z))，a_max为μ维度下允许无人机飞行的最大加速度，三个维度的a_max＝2m/s。

选择λ₁，λ₂，λ₃为4，2，0.01，至此，目标优化函数构造全部完成。通过对目标优化函数的非线性无约束最小值求解，得到优化后的控制点序列，重新定义B样条曲线，得到优化后的空间轨迹。

步骤三、将优化后空间轨迹中每个控制点到最近障碍物的距离与规定的最短距离进行比较，若不符合要求，则代入目标优化函数继续优化，此时为更快得到满足碰撞约束的轨迹，目标函数中碰撞项权重会随着迭代次数的增多而增加，每次增加0.5。反复优化直至满足碰撞约束。其中，软约束碰撞项的迭代优化伪代码如下所示。

步骤四、当飞行环境中存在特殊障碍物(椭圆环、狭缝等)，往往需要倾转四旋翼以特定姿态穿过，因此需要对无人机的姿态进行规划与优化。为使得倾转四旋翼无人机飞行平稳，初始化姿态序列时，将三轴姿态都设置为0，使得飞机以悬停姿态沿空间轨迹飞行。

当相机检测到不规则障碍物后，利用现有方法解算出穿越该障碍物的三轴姿态角最小值和最大值，取中值作为目标姿态。以倾转旋翼无人机舵机倾转180°的时间作为规划时间，规划起点为穿越障碍物时间减去规划时间t_tilt所对应的无人机状态，规划过程中其余时刻的目标姿态为始末两点均匀插值结果。将相应姿态对应到空间位置轨迹的时间戳，生成包含时间的目标姿态序列。

步骤五、姿态优化函数f_atti包括姿态跟踪项和姿态跟踪项。具体形式如下：

f_atti＝λ_trf_track+λ_smf_smooth

其中f_track为姿态跟踪项，f_smooth为姿态平滑项，λ_tr与λ_sm分别为对应优化项的权重。

姿态跟踪项表征优化姿态与目标姿态的差距，定义倾转四旋翼无人机倾转180°所需时间为t_tilt，整个优化过程限定在穿越障碍物的起点向前t_tilt时间段内，姿态跟踪项设计如下：

其中，h为规划步长，α(i)为优化姿态，α^*(i)为目标姿态，φ,θ,ψ为三轴姿态角。

姿态平滑项的设计如下：

选定λ_tr与λ_sm分别为0.5、0.1。至此姿态优化函数构造完成，通过求解该函数非线性无约束最小值，来获得优化后的姿态序列。

进一步通过下述实验验证本发明的技术效果。

在matlab仿真环境中搭建障碍物地图，在仿真环境中进行路径搜索，空间轨迹优化与姿态序列优化。预设参数如表1所示。

表1仿真实例相关参数

参数名称	参数取值
		threshold	0.31
p	4
		h	0.4
vmax	2
		amax	2
ms	4
		md	2
mindist	0.2
		mas	0.1
mat	0.5

本发明空间轨迹规划与跟踪效果如图3，4所示，姿态规划与跟踪效果如图5，6，7所示。由图3可知，为保证轨迹初末状态不变，前四个控制点与最后四个控制点保持不变，将中间六个控制点进行优化，故图中插值轨迹与优化轨迹初末两部分重合。从两条轨迹中间段可以看出，优化后的控制点更倾向于均匀排布在一条直线上，对应的B样条曲线平滑性更好。插值轨迹相较初始路径轨迹长度增加较多，软约束优化后轨迹长度明显缩减；插值轨迹部分速度与加速度超出动力学限制，重新调整时间分配，优化后轨迹飞行时间稍长，轨迹速度与加速度均符合动力学约束；初始路径与插值路径距障碍物的平均距离不够理想，极限距离非常近，经优化后轨迹的极限避障距离与平均避障距离均有所提高，轨迹避障性更好。

从图中可以看出，穿越灰色区域(特殊障碍物)时，倾转旋翼无人机达到了规定姿态与规定位置，可以稳定的穿过障碍物；电机转速与舵机倾转角变化稳定。

表2仿真效果分析

根据表2分析可得，轨迹的长度减少，对无人机的能耗有所降低，避障性更好，更加安全，且满足无人机的动力学约束。规划出的空间轨迹与姿态序列较为平滑，跟踪效果良好，可以满足可倾转四旋翼无人机按特定姿态穿越障碍物，使得无人机在复杂环境中安全、高效地飞行。

本发明另一实施例提供一种可倾转四旋翼无人机的运动轨迹生成系统，如图8所示，该系统包括：

轨迹规划模块10，其配置成对可倾转四旋翼无人机在含有障碍物的环境中飞行的运动轨迹进行规划；包括初始轨迹生成子模块110和轨迹优化子模块120，初始轨迹生成子模块110配置成搜索并生成一条无碰撞的初始轨迹；轨迹优化子模块120配置成对初始轨迹进行优化，直至优化后轨迹中每个控制点到最近障碍物的距离均不小于预设最短距离阈值，获得优化后的无人机运动轨迹；

姿态规划模块20，其配置成对可倾转四旋翼无人机穿越障碍物的运动姿态进行规划；包括目标姿态生成子模块210和姿态优化子模块220，目标姿态生成子模块210配置成将姿态对应到无人机运动轨迹中每个时间点，生成包含时间的目标姿态序列；姿态优化子模块220配置成对目标姿态序列进行优化，获得优化后对应时间点的无人机姿态序列。

本实施例中，可选地，初始轨迹生成子模块110中利用A-star算法搜索并生成一条无碰撞的初始轨迹；其中，A-star算法中惩罚函数设计为：

g_i＝(‖u‖²+λ)T

其中，u为控制量，λ为时间耗费的权重，T为选择的前向积分步长，i为当前搜索节点的序号；

启发式函数设计为：

h_i＝||m_goal-m_i||

其中，m_goal为目标搜索节点，m_i为当前搜索节点；

则A-star算法中目标优化函数f_cci为：

f_cci＝g_i+h_i

在利用A-star算法获得初始轨迹后，进一步通过B样条插值方法对初始轨迹进行插值，获得插值后的初始轨迹，其表达形式为：

其中，

s(t)＝(t-t_m)/Δt，表示时间占比，Δt表示时间间隔，t表示当前时间节点，t_m表示第m个时间节点；M_p+1表示由B样条曲线阶次p定义的恒定方阵；q_m表示参与该段B样条轨迹的控制点序列。

本实施例中，可选地，轨迹优化子模块120中对初始轨迹进行优化的具体过程包括：

设计位置优化函数形式如下：

f_total＝λ₁f_s+λ₂f_c+λ₃(f_v+f_a)

其中，f_s表示平滑项，f_c表示碰撞项，f_v与f_a分别表示速度与加速度的动力学可行项；λ₁，λ₂，λ₃分别表示平滑项、碰撞项和动力学可行项的权重；

平滑项f_s设计如下：

其中，Q_i+1-Q_i与Q_i-1-Q_i分别表示轨迹在控制点Q_i两侧的另外两个控制点对Q_i的弹性力；N+1表示控制点总数；

碰撞项f_c设计如下：

其中，d(Q_i)表示当前控制点Q_i到最近障碍物的距离；当d(Q_i)大于等于预设最短距离阈值d₀时，F_c(d(Q_i))＝(d(Q_i)-d₀)²；

速度的动力学可行项f_v设计如下：

其中，v_μ表示无人机在三维空间μ∈(x,y,z)的飞行速度，当v_μ大于等于μ维度下允许无人机飞行的最大速度v_max时，F_v(v_μ)＝(v_μ-v_max)²；

加速度的动力学可行项f_a设计如下：

其中，a_μ表示无人机在μ维度的飞行加速度(μ∈(x,y,z))，当a_μ大于等于μ维度下允许无人机飞行的最大加速度a_max时，F_a(a_μ)＝(a_μ-a_max)²；

通过对设计的位置优化函数的非线性无约束最小值求解，获得优化后的控制点序列，并重新生成B样条曲线作为优化后的无人机运动轨迹；对初始轨迹进行优化的过程中，若优化后轨迹中每个控制点到最近障碍物的距离小于预设最短距离阈值，则增加位置优化函数中碰撞项的权重，以使位置优化函数的迭代优化能快速得到满足碰撞约束的轨迹。

本实施例中，可选地，目标姿态生成子模块210中生成包含时间的目标姿态序列的具体过程为：以穿越障碍物后的三轴姿态角最小值与最大值的平均值作为终点姿态；以穿越障碍物的时间起点减去规划时间得到的时间点所对应的无人机三轴姿态角作为起点姿态；穿越障碍物的过程中其余时刻的姿态通过对起点姿态和终点姿态进行均匀插值获得；将相应姿态对应到步骤一优化后的无人机运动轨迹的时间点，生成包含时间的目标姿态序列；其中，规划时间为无人机在穿越障碍物的时间起点向前的一段时间；

本实施例中，可选地，姿态优化子模块220中对目标姿态序列进行优化的具体过程包括：

设计姿态优化函数f_atti形式如下：

f_atti＝λ_trf_track+λ_smf_smooth

其中，f_track表示姿态跟踪项；f_smooth表示姿态平滑项；λ_tr与λ_sm分别表示姿态跟踪项和姿态平滑项的权重；

姿态跟踪项设计如下：

其中，h表示规划步长；α(i)表示待优化姿态序列；α^*(i)表示目标姿态序列；φ,θ,ψ表示三轴姿态角；t_tilt表示规划时间；

姿态平滑项设计如下：

通过对设计的姿态优化函数的非线性无约束最小值求解，获得优化后的无人机姿态序列。

本实施例所述一种可倾转四旋翼无人机的运动轨迹生成系统的功能可以由前述一种可倾转四旋翼无人机的运动轨迹生成方法说明，因此本实施例未详述部分，可参见以上方法实施例，在此不再赘述。

尽管根据有限数量的实施例描述了本发明，但是受益于上面的描述，本技术领域内的技术人员明白，在由此描述的本发明的范围内，可以设想其它实施例。对于本发明的范围，对本发明所做的公开是说明性的，而非限制性的，本发明的范围由所附权利要求书限定。

Claims (10)

1.一种可倾转四旋翼无人机的运动轨迹生成方法，其特征在于，包括以下步骤：

步骤一、对可倾转四旋翼无人机在含有障碍物的环境中飞行的运动轨迹进行规划；包括：

搜索并生成一条无碰撞的初始轨迹；

对初始轨迹进行优化，直至优化后轨迹中每个控制点到最近障碍物的距离均不小于预设最短距离阈值，获得优化后的无人机运动轨迹；

步骤二、对可倾转四旋翼无人机穿越障碍物的运动姿态进行规划；包括：

将姿态对应到无人机运动轨迹中每个时间点，生成包含时间的目标姿态序列；

对所述目标姿态序列进行优化，获得优化后对应时间点的无人机姿态序列。

2.根据权利要求1所述的一种可倾转四旋翼无人机的运动轨迹生成方法，其特征在于，步骤一中利用A-star算法搜索并生成一条无碰撞的初始轨迹；其中，A-star算法中惩罚函数设计为：

g_i＝(||u||²+λ)T

其中，u为控制量，λ为时间耗费的权重，T为选择的前向积分步长，i为当前搜索节点的序号；

启发式函数设计为：

h_i＝||m_goal-m_i||

其中，m_goal为目标搜索节点，m_i为当前搜索节点；

则A-star算法中目标优化函数f_cci为：

f_cci＝g_i+h_i。

3.根据权利要求2所述的一种可倾转四旋翼无人机的运动轨迹生成方法，其特征在于，步骤一中在利用A-star算法获得初始轨迹后，进一步通过B样条插值方法对初始轨迹进行插值，获得插值后的初始轨迹，其表达形式为：

其中，

s(t)＝(t-t_m)/Δt，表示时间占比，Δt表示时间间隔，t表示当前时间节点，t_m表示第m个时间节点；M_p+1表示由B样条曲线阶次p定义的恒定方阵；q_m表示参与该段B样条轨迹的控制点序列。

4.根据权利要求3所述的一种可倾转四旋翼无人机的运动轨迹生成方法，其特征在于，步骤一中对初始轨迹进行优化的具体过程包括：

设计位置优化函数形式如下：

f_total＝λ₁f_s+λ₂f_c+λ₃(f_v+f_a)

其中，f_s表示平滑项，f_c表示碰撞项，f_v与f_a分别表示速度与加速度的动力学可行项；λ₁，λ₂，λ₃分别表示平滑项、碰撞项和动力学可行项的权重；

平滑项f_s设计如下：

其中，Q_i+1-Q_i与Q_i-1-Q_i分别表示轨迹在控制点Q_i两侧的另外两个控制点对Q_i的弹性力；N+1表示控制点总数；

碰撞项f_c设计如下：

其中，d(Q_i)表示当前控制点Q_i到最近障碍物的距离；当d(Q_i)大于等于预设最短距离阈值d₀时，F_c(d(Q_i))＝(d(Q_i)-d₀)²；

速度的动力学可行项f_v设计如下：

其中，v_μ表示无人机在三维空间μ∈(x，y，z)的飞行速度，当v_μ大于等于μ维度下允许无人机飞行的最大速度v_max时，F_v(v_μ)＝(v_μ-v_max)²；

加速度的动力学可行项f_a设计如下：

其中，a_μ表示无人机在μ维度的飞行加速度(μ∈(x，y，z))，当a_μ大于等于μ维度下允许无人机飞行的最大加速度a_max时，F_a(a_μ)＝(a_μ-a_max)²；

通过对设计的位置优化函数的非线性无约束最小值求解，获得优化后的控制点序列，并重新生成B样条曲线作为优化后的无人机运动轨迹。

5.根据权利要求4所述的一种可倾转四旋翼无人机的运动轨迹生成方法，其特征在于，步骤一中对初始轨迹进行优化的过程中，若优化后轨迹中每个控制点到最近障碍物的距离小于预设最短距离阈值，则增加位置优化函数中碰撞项的权重，以使位置优化函数的迭代优化能快速得到满足碰撞约束的轨迹。

6.根据权利要求5所述的一种可倾转四旋翼无人机的运动轨迹生成方法，其特征在于，步骤二中生成包含时间的目标姿态序列的具体过程为：以穿越障碍物后的三轴姿态角最小值与最大值的平均值作为终点姿态；以穿越障碍物的时间起点减去规划时间得到的时间点所对应的无人机三轴姿态角作为起点姿态；穿越障碍物的过程中其余时刻的姿态通过对起点姿态和终点姿态进行均匀插值获得；将相应姿态对应到步骤一优化后的无人机运动轨迹的时间点，生成包含时间的目标姿态序列；其中，所述规划时间为无人机在穿越障碍物的时间起点向前的一段时间。

7.根据权利要求6所述的一种可倾转四旋翼无人机的运动轨迹生成方法，其特征在于，步骤二中对所述目标姿态序列进行优化的具体过程包括：

设计姿态优化函数f_atti形式如下：

f_atti＝λ_trf_track+λ_smf_smooth

其中，f_track表示姿态跟踪项；f_smooth表示姿态平滑项；λ_tr与λ_sm分别表示姿态跟踪项和姿态平滑项的权重；

姿态跟踪项设计如下：

其中，h表示规划步长；α(i)表示待优化姿态序列；α^*(i)表示目标姿态序列；φ，θ，ψ表示三轴姿态角；t_tilt表示规划时间；

姿态平滑项设计如下：

通过对设计的姿态优化函数的非线性无约束最小值求解，获得优化后的无人机姿态序列。

8.一种可倾转四旋翼无人机的运动轨迹生成系统，其特征在于，包括：

轨迹规划模块，其配置成对可倾转四旋翼无人机在含有障碍物的环境中飞行的运动轨迹进行规划；包括初始轨迹生成子模块和轨迹优化子模块，所述初始轨迹生成子模块配置成搜索并生成一条无碰撞的初始轨迹；所述轨迹优化子模块配置成对初始轨迹进行优化，直至优化后轨迹中每个控制点到最近障碍物的距离均不小于预设最短距离阈值，获得优化后的无人机运动轨迹；

姿态规划模块，其配置成对可倾转四旋翼无人机穿越障碍物的运动姿态进行规划；包括目标姿态生成子模块和姿态优化子模块，所述目标姿态生成子模块配置成将姿态对应到无人机运动轨迹中每个时间点，生成包含时间的目标姿态序列；所述姿态优化子模块配置成对所述目标姿态序列进行优化，获得优化后对应时间点的无人机姿态序列。

9.根据权利要求8所述的一种可倾转四旋翼无人机的运动轨迹生成系统，其特征在于，所述初始轨迹生成子模块中利用A-star算法搜索并生成一条无碰撞的初始轨迹；其中，A-star算法中惩罚函数设计为：

g_i＝(||u||²+λ)T

其中，u为控制量，λ为时间耗费的权重，T为选择的前向积分步长，i为当前搜索节点的序号；

启发式函数设计为：

h_i＝||m_goal-m_i||

其中，m_goal为目标搜索节点，m_i为当前搜索节点；

则A-star算法中目标优化函数f_cci为：

f_cci＝g_i+h_i

在利用A-star算法获得初始轨迹后，进一步通过B样条插值方法对初始轨迹进行插值，获得插值后的初始轨迹，其表达形式为：

其中，

s(t)＝(t-t_m)/Δt，表示时间占比，Δt表示时间间隔，t表示当前时间节点，t_m表示第m个时间节点；M_p+1表示由B样条曲线阶次p定义的恒定方阵；q_m表示参与该段B样条轨迹的控制点序列；

所述轨迹优化子模块中对初始轨迹进行优化的具体过程包括：

设计位置优化函数形式如下：

f_total＝λ₁f_s+λ₂f_c+λ₃(f_v+f_a)

其中，f_s表示平滑项，f_c表示碰撞项，f_v与f_a分别表示速度与加速度的动力学可行项；λ₁，λ₂，λ₃分别表示平滑项、碰撞项和动力学可行项的权重；

平滑项f_s设计如下：

其中，Q_i+1-Q_i与Q_i-1-Q_i分别表示轨迹在控制点Q_i两侧的另外两个控制点对Q_i的弹性力；N+1表示控制点总数；

碰撞项f_c设计如下：

其中，d(Q_i)表示当前控制点Q_i到最近障碍物的距离；当d(Q_i)大于等于预设最短距离阈值d₀时，F_c(d(Q_i))＝(d(Q_i)-d₀)²；

速度的动力学可行项f_v设计如下：

其中，v_μ表示无人机在三维空间μ∈(x，y，z)的飞行速度，当v_μ大于等于μ维度下允许无人机飞行的最大速度v_max时，F_v(v_μ)＝(v_μ-v_max)²；

加速度的动力学可行项f_a设计如下：

其中，a_μ表示无人机在μ维度的飞行加速度(μ∈(x，y，z))，当a_μ大于等于μ维度下允许无人机飞行的最大加速度a_max时，F_a(a_μ)＝(a_μ-a_max)²；

通过对设计的位置优化函数的非线性无约束最小值求解，获得优化后的控制点序列，并重新生成B样条曲线作为优化后的无人机运动轨迹；对初始轨迹进行优化的过程中，若优化后轨迹中每个控制点到最近障碍物的距离小于预设最短距离阈值，则增加位置优化函数中碰撞项的权重，以使位置优化函数的迭代优化能快速得到满足碰撞约束的轨迹。

10.根据权利要求9所述的一种可倾转四旋翼无人机的运动轨迹生成系统，其特征在于，所述目标姿态生成子模块中生成包含时间的目标姿态序列的具体过程为：以穿越障碍物后的三轴姿态角最小值与最大值的平均值作为终点姿态；以穿越障碍物的时间起点减去规划时间得到的时间点所对应的无人机三轴姿态角作为起点姿态；穿越障碍物的过程中其余时刻的姿态通过对起点姿态和终点姿态进行均匀插值获得；将相应姿态对应到步骤一优化后的无人机运动轨迹的时间点，生成包含时间的目标姿态序列；其中，所述规划时间为无人机在穿越障碍物的时间起点向前的一段时间；

所述姿态优化子模块中对所述目标姿态序列进行优化的具体过程包括：

设计姿态优化函数f_atti形式如下：

f_atti＝λ_trf_track+λ_smf_smooth

其中，f_track表示姿态跟踪项；f_smooth表示姿态平滑项；λ_tr与λ_sm分别表示姿态跟踪项和姿态平滑项的权重；

姿态跟踪项设计如下：

其中，h表示规划步长；α(i)表示待优化姿态序列；α^*(i)表示目标姿态序列；φ，θ，ψ表示三轴姿态角；t_tilt表示规划时间；

姿态平滑项设计如下：

通过对设计的姿态优化函数的非线性无约束最小值求解，获得优化后的无人机姿态序列。

Images