CN111722641A - 一种微型无人机高机动轨迹规划方法 - Google Patents

Abstract

本发明公开了一种微型无人机高机动轨迹规划方法，属于无人机自主飞行的技术领域。具体包括以下步骤：步骤一、在障碍物杂乱环境下最优函数的设定；步骤二、计算出在障碍物杂乱的环境下最优轨迹规划；该方法不假设悬停的初始条件，因此适用于四旋翼无人机移动时的快速在线重规划。

Description

一种微型无人机高机动轨迹规划方法

技术领域

本发明属于无人机自主飞行的技术领域，特别是涉及一种微型无人机高机动 轨迹规划方法。

背景技术

微型无人机具有体积小、重量轻、机动性好、隐蔽性续航时间长等优良的特 点，在军事和民用方面具有广泛的应用价值，尤其适合在高危、未知、复杂的环 境下执行任务。军事上，微小型无人飞行器可应用于地面情报获取、战场侦察和 监视、近距离空中打击和禁飞巡逻、电子战、通信中继等方面；由于微型无人机 可局部部署，通过小范围侦察，小规模战斗部队提供所需要的信息，可使部队在 侦察过程中的伤亡率大大减小，同时大幅提高作战效率。民用上，可用于自然灾 害之后的搜索与救援，巡逻监视和目标跟踪，缉毒和反走私，高压线、大桥和地 震后关键路段的检查，航拍等。还可对生化污染环境进行探测、识别，微型无人 机可以“毫无顾忌”地飞入这类目标进行探测，不存在人员伤亡、污染等问题。

四旋翼微型无人机在上述场景中被广泛应用，但它们在自主性和机动性方面 尚未被开发利用。使四旋翼无人机更加自主且更灵活，就会带来减少操作员数量 和完成任务时间的优势，这有利于它们更好的完成任务并提高了他们的效益。为 了使四旋翼飞行器平台具有更高的机动性，可以通过减小平台尺寸的办法，但这 也使它们由于需要更快的动态特性而更难以控制；增大飞行平台的推重比，这使 得在具有高速度和高加速度的机动飞行期间，难以建模并结合到控制中的空气动 力学影响(主要是螺旋桨阻力带来的影响)与四旋翼飞行器的飞行特性紧密相关。

国内外研究机构对高机动四旋翼微型无人机的轨迹规划已经有了许多方面 的研究工作。从已有的研究成果来看具有发展潜力的高机动四旋翼微型无人机轨 迹规划方法主要集中在以下几种定位方式：在外部动态捕捉系统下进行控制和规 划；依靠地面标签或标志物进行控制和规划；依靠GPS/DGPS进行控制和规划 等。尚不能在复杂场景下仅依靠机载传感器和GPS拒绝环境下实现高机动轨迹 规划。

发明内容

本发明为解决上述背景技术存在的技术问题，提供一种微型无人机高机动轨 迹规划方法。

本发明采用以下技术方案来实现：一种微型无人机高机动轨迹规划方法，具 体包括以下步骤：

步骤一、障碍物杂乱环境下最优函数的设定；

步骤二、计算出在障碍物杂乱的环境下最优轨迹规划；

其中，所述步骤一具体包括以下流程：

设

是动力学系统的状态，R为实数集，由三维位置及其(n-1) 阶导数组成；设

表示状态空间的自由区域，除了捕获无障碍物的位置 P^free外，同时还指定系统动力学上的特定约束D^free，即每个轴的最大速度υ_max， 最大加速度α_max，以及更高阶的导数；

因此χ^free：＝P^free×D^free＝P^free×[-υ_max，υ_max]³×[-α_max，α_max]³×…

将障碍物区域表示为：χ^obs：＝χ\χ^free；

虑多项式状态轨迹：

此时，

式中， D：＝[d₀，…，d_K]∈R^3×(K+1)，为时间函数系数，d为其中的子集为‘了简化表示 法用

表示速度，用

表示加速度，为方便表示表示舍弃 下标D；

则多项式轨迹通过考虑线性时不变动力学系统

来生成，其中控 制输入是u(t)∈U：＝[-u_max，u_max]³∈R³；

其中，t为当前时间，T为轨迹持续时间，u为控制指令，U为控制空间，k 为函数的阶数，K为常数，D为时间函数的系数；

在状态空间形式中，获得一个线性时不变(LTI)系统：

式中，A矩阵相当于高阶积分器，用于生成运动基元，I₃为单位矩阵，3阶 是由于平坦输出的位置X、Y、Z三轴坐标系组成，A、B矩阵为3n阶，与控制输 入的阶数相关；

将轨迹的平滑度或效益定义为控制输入u(t)的L₂范数的平方：

式中，n决定了控制输入的阶次。

在进一步的实施例中，给定初始状态x₀∈χ^free和目标区域

找到 多项式轨迹参数化D∈R^3×(K+1)和一个时间周期T≥0，使得：

x(0)＝x₀,x(T)∈χ^goal

式中，ρ表示比例系数，区间为0-1，用于表示时间周期函数占整个最优函 数的比例；J表示平滑度；

用C^*(x₀,χ^goal)表示从初始状态x₀到目标区域χ^goal的最优成本。

在进一步的实施例中，所述步骤二具体包括以下步骤：

步骤201、构造运动基元；

步骤202、诱导状态空间离散化；

步骤203、确定性的最短轨迹；

步骤204、设计启发函数；

步骤205、碰撞检测；

步骤206、轨迹细化。

在进一步的实施例中，通过离散的栅格

其中每个控制 向量u_m∈R³将定义为短持续时间的一个运动基元；

获得离散化U_M的方法是沿着每个轴[0,u_max]选择多个样本μ∈R⁺，其定义离 散化步数

并且产生M＝(2μ+1)³个运动基元；

给定初始状态

生成一个持续时间τ＞0的运动基元，在 t∈[0,τ]时间间隔内施加恒定的控制输入u(t)≡u_m∈U_M，得到：

控制输入是常数，意味着所有涉及时间的系数必须相同为零，即：

将控制表达式u(t)＝u_m与初始条件x₀进行积分得到：

或等效地给定四旋翼无人机的初始状态x₀和控制输入u_m，等式(2)中线性 时不变系统的生成轨迹是：

由于持续时间τ和控制输入u_m都是固定的，因此运动基元的成本为

在进一步的实施例中，状态空间的离散化允许通过从x₀开始并且应用所有基 元在持续时间τ后获得M个可能状态来构建可达系统状态，将所有可能的基元 再次施加到M个状态中的每一个将在时间2τ处产生M²个可行的状态；

由于自由空间χ^free是有界的、离散的，因此可达状态S的集合是有限的；定 义图G(S,ξ)，其中S是可达系统状态的离散集合，ξ是连接图中状态的边集，每 条边由运动基元e:＝(u_m,τ)定义，设s₀为与x₀对应的状态。

在进一步的实施例中，设定控制输入u(t)在持续时间τ的间隔上是分段恒定 的，即引入另外的变量N∈R⁺，使得T＝Nτ，并且对于k＝0,...,N-1有u_k∈U_M， 则：

强制控制轨迹成为U_M中运动基元的组合。

在进一步的实施例中，给定初始状态x₀∈χ^free，目标区域

具有 持续时间τ＞0的有限运动基元U_M，选择长度为N的运动基元序列u_0:N-1，使得：

s₀＝x₀,s_N∈χ^goal

在进一步的实施例中，(a)最小时间启发函数

由于系统的最大速度受沿每个轴v_max的限制，因此达到目标区域χ^goal中的最 接近状态x_f的最小时间受

的限制，同样由于系统的最大加速度受 到a_max的限制，因此状态

不能快于：

p(0)＝p₀,v(0)＝v₀

沿着各个轴以闭合形式求解获得下限

通过

定义最小可到达时间的下限，使用易于计算但不太紧 密的边界

为了找到启发函数，通过用下边界

来替换状态约束x(t)∈χ^free和输入 约束u(t)∈U，即：

x(0)＝x₀,x(T)∈χ^goal

由于J(D)≥0，在最优成本上获得下限的直接方法是：

因此给定离散空间中的节点s₀,s_f∈S：

(b)线性二次最小时间启发函数

虽然最小时间启发式计算非常容易计算并考虑了速度约束，但它忽略了控制 输入，在去除状态约束x(t)∈χ^free和输入约束u(t)∈U之后，方程(12)的问题 是线性二次最小时间问题；

令

为固定的最终状态，并定义δ_T:＝x_f-e^ATx₀和可控格兰姆行列式

然后方程(6)中的最优时间T是下界

或下式的解：

最优控制是：

同时，最优成本是：

方程(1)中的多项式系数D∈R³×(2n)是：

当H∈R(3n)×(3n)时，有

表示最终状态和初始状态的关联；δ、W可控格兰姆行列式。

在进一步的实施例中，对于计算边e(t)＝[p(t)^T,v(t)^T,a(t)^T,...]^T，需要检测对于t∈[0,τ]时是否

检测几何空间

中的碰撞与强制执行动 力学约束

边e(t)仅在其几何形状为

和其导数

时有效，即

由于其导数v,a是多项式，计算它们在时间段[0,τ]内的极值与速度、加速度 等的最大界限进行比较；更困难的部分是检测几何空间P^free中的碰撞，将模型P 作为占据栅格地图，设P:＝{p(t_i)|t_i∈[0,τ],i＝0,...,I}是系统沿轨迹p(t)遍历的 一组位置，为了确保无碰撞轨迹只需要显示所有i∈{0,...,I}时的p(t_i)∈P^free；给 定多项式p(t),t∈[0,τ]，对位置p(t_i)进行采样，通过定义：

使得

这里R是占据网格分辨率，该条件确保两个连续样本之间的最大距离不会 超过地图分辨率。

在进一步的实施例中，细化轨迹x^*(t)是通过求解具有给定初始状态s₀、结 束状态s_g和中间航路点p_k,k∈{0,...,N-1}的无约束二次规划得到的：

s.t.x₀(0)＝s₀,x_N-1(τ_N-1)＝s_g

x_k+1(0)＝x_k(τ_k),k∈{0,...,N-2}

每个轨迹段τ_k的时间从低维空间中规划的轨迹给出。

本发明的有益效果：本发明研究了一种基于搜索的全局轨迹规划方法，用于 生成在障碍物杂乱环境中飞行的四旋翼无人机的动力学可行轨迹。通过使用一组 短时运动基元探索地图来搜索平滑的最小时间轨迹，求解最优控制问题并在状态 空间上引入离散的有限栅格来生成基元，使用图搜索算法来探索一条初始轨迹， 最后求解线性二次最小时间问题来有效地生成分辨率完备的、时间最小的、安全 的、动力学可行的轨迹。该方法不假设悬停的初始条件，因此适用于四旋翼无人 机移动时的快速在线重规划。

本方法解决了在GPS拒绝的障碍物杂乱场景下，仅依靠自身传感器和计算 机，生成一条高机动可行轨迹。

附图说明

图1为不同初始速度下的最小加加加速度轨迹规划图。

图2为使用不同启发函数生成的轨迹图。

图3基于运动基元的图搜索2D表示图。

图4基于运动基元的图搜索3D表示图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步的描述。

国内外研究机构对高机动四旋翼微型无人机的轨迹规划已经有了许多方面 的研究工作。从已有的研究成果来看具有发展潜力的高机动四旋翼微型无人机轨 迹规划方法主要集中在以下几种定位方式：在外部动态捕捉系统下进行控制和规 划；依靠地面标签或标志物进行控制和规划；依靠GPS/DGPS进行控制和规划 等。尚不能在复杂场景下仅依靠机载传感器和GPS拒绝环境下实现高机动轨迹 规划。

综上所述，在未知复杂环境下四旋翼微型无人机的高机动轨迹规划是一项极 具挑战性的问题，本方法解决了在GPS拒绝的障碍物杂乱场景下，仅依靠自身 传感器和计算机，生成一条高机动可行轨迹。

实施例1

一种微型无人机高机动轨迹规划方法，其特征在于，具体包括以下步骤：

步骤一、在障碍物杂乱环境下设定最优函数：设

是动力学系 统的状态，R为实数集；由三维位置及其(n-1)阶导数(包括速度、加速度、加 加速度等)组成。设

表示状态空间的自由区域，除了捕获无障碍物的位 置p^free(受规划的地图大小的限制)外，同时还指定系统动力学上的特定约束 D^free，即每个轴的最大速度υ_max，最大加速度α_max，以及更高阶的导数；因此 χ^free：＝P^free×D^free＝P^free×[-υ_max，υ_max]³×[-α_max，α_max]³×…

将障碍物区域表示为：χ^obs：＝χ\χ^free。

四旋翼飞行器系统的微分平坦性质允许从x，y，z三个位置轴的每一个轴中独 立指定一维时间参数化多项式轨迹来构造控制输入，因此考虑多项式状态轨迹

此时，

式中，D：＝[d₀，…，d_K]∈R^3×(K+1)，为时间函数系数，d为其中的子集。为 了简化表示法用

表示速度，用

表示加速度，为方便表 示表示舍弃下标D；

则多项式轨迹通过考虑线性时不变动力学系统

来生成，其中控 制输入是u(t)∈U：＝[-u_max，u_max]³∈R³；

其中，t为当前时间，T为轨迹持续时间，u为控制指令，U为控制空间，k 为函数的阶数，K为常数，D为时间函数的系数；

在状态空间形式中，获得一个线性时不变(LTI)系统：

式中，A矩阵相当于高阶积分器，用于生成运动基元，I₃为单位矩阵，3阶 是由于平坦输出的位置X、Y、Z三轴坐标系组成，A、B矩阵为3n阶，与控制输 入的阶数相关；

将轨迹的平滑度或效益定义为控制输入u(t)的L₂范数的平方：

式中，n决定了控制输入的阶次。

但是考虑到轨迹持续时间T对轨迹的平滑度J的影响：

定初始状态x₀∈χ^free和目标区域

找到多项式轨迹参数化 D∈R^3×(K+1)和一个时间周期T≥0，使得：

x(0)＝x₀,x(T)∈χ^goal

式中，ρ表示比例系数，区间为0-1，用于表示时间周期函数占整个最优函 数的比例；J表示平滑度；

用C^*(x₀,χ^goal)表示从初始状态x₀到目标区域χ^goal的最优成本，将轨迹持续 时间T对轨迹的平滑度J带来的问题，转化成具有3n维状态空间χ和三维空间U 的确定性最短路径问题来解决。由于控制空间始终是三维的，因此基于搜图索的 规划算法使用运动基元离散控制空间U是有效且分辨率完备的。

步骤二、计算出在障碍物杂乱的环境下最优轨迹规划：

具体包括：步骤201、构造运动基元；步骤202、诱导状态空间离散化；步 骤203、确定性的最短轨迹；步骤204、设计启发函数；步骤205、碰撞检测步 骤206、轨迹细化；步骤207、使用无约束二次规划的轨迹细化算法仿真。

下面结合数据和具体方案对步骤二进行详细的阐述。

关于构造运动基元：

根据公式(2)中的系统基元方锐构造将轨迹持续时间T对轨迹的平滑度J 带来的问题从最优控制问题转换为图搜索问题，通过离散的栅格

其中，每个控制向量u_m∈R³将定义为短持续时间的一个 运动基元。

其中，获得离散化U_M的方法是沿着每个轴[0,u_max]选择多个样本μ∈R⁺，其 定义离散化步数

并且产生M＝(2μ+1)³个运动基元。

给定初始状态

生成一个持续时间τ＞0的运动基元，在t∈[0,τ]时间间隔内施加恒定的控制输入u(t)≡u_m∈U_M，得到：

控制输入是常数，意味着所有涉及时间的系数必须相同为零，即：

将控制表达式u(t)＝u_m与初始条件x₀进行积分得到：

或等效地给定四旋翼无人机的初始状态x₀和控制输入u_m，等式(2)中线性 时不变系统的生成轨迹是：

由于持续时间τ和控制输入u_m都是固定的，因此运动基元的成本为

关于诱导状态空间离散化：状态空间的离散化允许通过从x₀开始并且应用所 有基元在持续时间τ后获得M个可能状态来构建可达系统状态，将所有可能的 基元再次施加到M个状态中的每一个将在时间2τ处产生M²个可行的状态。

由于自由空间χ^free是有界的、离散的，因此可达状态S的集合是有限的。定 义图G(S,ξ)，其中S是可达系统状态的离散集合，ξ是连接图中状态的边集，每 条边由运动基元e:＝(u_m,τ)定义，设s₀为与x₀对应的状态。

使用表1来探索自由状态空间χ^free并构建连通图：在第四行中使用完全定义 的状态s和控制输入u_m计算基元，给定恒定时间τ；第五行检查基元的可行性，； 在第六行中，评估有效基元的结束状态，并将其添加到当前节点的继承集合中同 时从相应的基元估计边缘成本。在检查有限控制输入集中的所有基元之后，将继 承集合R(s)中的节点添加到图中并且继续扩展直到到达目标区域。

表1使用运动基元探索自由状态空间并构建连通图的算法

关于确定性的最短轨迹：由于运动基元集U_M的构造和诱导状态空间离散化， 轨迹持续时间T对轨迹的平滑度J带来的问题从最优控制问题转换为图搜索问题， 通过引入额外约束来完成，固定轨迹持续时间T对轨迹的平滑度J带来的问题中 的控制输入u(t)在持续时间τ的间隔上是分段恒定的，即引入另外的变量N∈R⁺， 使得T＝Nτ，并且对于k＝0,...,N-1有u_k∈U_M，则：

强制控制轨迹成为U_M中运动基元的组合，便生成了以下确定性最短路径问 题。

即，给定初始状态x₀∈χ^free，目标区域

具有持续时间τ＞0的 有限运动基元U_M，选择长度为N的运动基元序列u_0:N-1，使得：

s₀＝x₀,s_N∈χ^goal

确定性最短路径问题(下文称问题二)的最优成本是轨迹持续时间T对轨 迹的平滑度J带来的问题(下文称问题一)的最优成的上限，因为确定性最短路 径问题只是轨迹持续时间T对轨迹的平滑度J带来的问题的约束情况，然而这种 对离散控制和状态空间的重构可以实现有效的解决方案。这些问题可以通过基于 搜索的或基于采样的运动规划算法来解决，由于只有前者保证了有限时间的最优 性或次优性，使用A^*方法并结合准确的、具有一致性的启发函数和有效的、有 保证的碰撞检测方法来实现。

启发函数的设计：通过求解问题一的条件的放宽情况来获得良好的启发函数， 替换问题一中很难满足的约束，即x(t)∈χ^free和

对时间T约束。

以下研究如何最优且有效地解决问题一的这种放宽的约束条件。

(a)最小时间启发函数

由于χ^obs和U引起的最大速度、最大加速度和最大加加速度等的约束，问题 一中的最小可到达时间得到了下限

即由于系统的最大速度受沿每个轴v_max的 限制，因此达到目标区域χ^goal中的最接近状态x_f的最小时间受

的限制，同样由于系统的最大加速度受到a_max的限制，因此状态

不 能快于：

p(0)＝p₀,v(0)＝v₀

以上是最速降线问题，可以沿着各个轴以闭合形式求解获得下限

通过

定义最小可到达时间的下限，使用易于计算但不太 紧密的边界

为了找到启发函数，通过用下边界

来替换状态约束 x(t)∈χ^free和输入约束u(t)∈U来放宽问题一，即：

x(0)＝x₀,x(T)∈χ^goal

由于J(D)≥0，在最优成本上获得下限的直接方法是：

因此给定离散空间中的节点s₀,s_f∈S，对于问题二以下是可接受的启发式函 数，并且由于三角形在距离上不等，很容易看出它也是一致的。

(b)线性二次最小时间启发函数

虽然最小时间启发式计算非常容易计算并考虑了速度约束，但它忽略了控制 输入，在去除状态约束x(t)∈χ^free和输入约束u(t)∈U之后，方程(12)的问题 是线性二次最小时间问题。令x_f∈χ^goal为固定的最终状态，并定义δ_T:＝x_f-e^ATx₀和可控格兰姆行列式

然后方程(6)中的最优时间T是下 界

或下式的解：

最优控制是：

同时，最优成本是：

方程(1)中的多项式系数D∈R³×(2n)是：

当H∈R(3n)×(3n)时，有

因此该方法中获得的最优成本h₂(x₀)对于问题二而言是比h₁更好的启发式， 因为h₂考虑了控制效益。下面分别求解速度控制和加速度控制的最优解和最优 成本。

关于碰撞检测的算法具体如下；

对于计算边e(t)＝[p(t)^T,v(t)^T,a(t)^T,...]^T，需要检测对于t∈[0,τ]时是否

检测几何空间

中的碰撞与强制执行动力学约束

边e(t)仅在其几何形状为

和其导数

时有效，即

由于其导数v,a是多项式，计算它们在时间段[0,τ]内的极值与速度、加速度 等的最大界限进行比较。更困难的部分是检测几何空间P^free中的碰撞，在本文中 将模型P作为占据栅格地图，设P:＝{p(t_i)|t_i∈[0,τ],i＝0,...,I}是系统沿轨迹 p(t)遍历的一组位置，为了确保无碰撞轨迹只需要显示所有i∈{0,...,I}时的 p(t_i)∈P^free。给定多项式p(t),t∈[0,τ]，对位置p(t_i)进行采样，通过定义：

使得

这里R是占据网格分辨率，该条件确保两个连续样本之间的最大距离不会 超过地图分辨率。

轨迹细化的具体算法如下：细化轨迹x^*(t)是通过求解具有给定初始状态s₀、 结束状态s_g和中间航路点p_k,k∈{0,...,N-1}的无约束二次规划得到的：

s.t.x₀(0)＝s₀,x_N-1(τ_N-1)＝s_g

x_k+1(0)＝x_k(τ_k),k∈{0,...,N-2}

每个轨迹段τ_k的时间从低维空间中规划的轨迹给出。

实施例2

实施例2为对实施例1中的技术方案进行实施，并通过实施例2展示其效果。

具体地，使用无约束二次规划的轨迹细化算法仿真：

图1表示从初始点s到目标点g分别以初始速度为0m/s，1m/s，2m/s， 5m/s的最小加加加速度轨迹规划，箭头为速度方向。箭头表示以正垂直向上的 初始速度开始搜索目标。

对启发函数进行选择和对比：在实施例1中提出了两种不同的启发函数：第 一种是使用最大速度约束估计最小时间的h₁启发函数；第二种是使用动力学约束 估计最小成本函数的h₂启发函数。启发函数h₁更容易计算，但是它没有考虑系统 的动力学；启发函数h₂需要求解多项式的根值，但是它考虑了系统的动力学方程， 且考虑了实际成本。通过A^*图搜索算法分别使用h₁和h₂启发函数来对比算法性 能，比较使用启发函数后扩展的节点数量。

图2中小点表示使用A^*算法使用启发函数h₁和h₂进行图搜索的扩展节点， T_p和N_p分别表示使用响应启发函数需要的规划时间和扩展节点数。比较图2(a) 和图2(b)可以看出，最小成本启发函数h₂使得搜索更快，它在不损失最优性 的情况下扩展了更少的节点。

三维环境下对运动基元的表示：

三维环境下对运动基元的表示：

图3和图4表示基于运动基元的图搜索的二维/三维表示，轨迹表示从起始 点s到达目标点g的最优轨迹，由若干个运动基元连接组成。

Claims (10)

1.一种微型无人机高机动轨迹规划方法，其特征在于，具体包括以下步骤：

步骤一、在障碍物杂乱环境下最优函数的设定；

步骤二、计算出在障碍物杂乱的环境下最优轨迹规划；

其中，所述步骤一具体包括以下流程：

设

是动力学系统的状态，R为实数集，由三维位置及其(n-1)阶导数组成；设

表示状态空间的自由区域，除了捕获无障碍物的位置P^free外，同时还指定系统动力学上的特定约束D^free，即每个轴的最大速度υ_max，最大加速度α_max，以及更高阶的导数；

因此χ^free：＝P^free×D^free＝P^free×[-υ_max，υ_max]³×[-α_max，α_max]³×…

将障碍物区域表示为：χ^obs：＝χ\χ^free；

虑多项式状态轨迹：

此时，

式中，D：＝[d₀，…，d_K]∈R^3×(K+1)，为时间函数系数，d为其中的子集；为了简化表示法用

表示速度，用

表示加速度，为方便表示表示舍弃下标D；

则多项式轨迹通过考虑线性时不变动力学系统

来生成，其中控制输入是u(t)∈U：＝[-u_max，u_max]³∈R³；

其中，t为当前时间，T为轨迹持续时间，u为控制指令，U为控制空间，k为函数的阶数，K为常数，K≥0，D为时间函数的系数；

在状态空间形式中，获得一个线性时不变(LTI)系统：

式中，A矩阵相当于高阶积分器，用于生成运动基元，I₃为单位矩阵，3阶是由于平坦输出的位置X、Y、Z三轴坐标系组成，A、B矩阵为3n阶，与控制输入的阶数相关；

将轨迹的平滑度或效益定义为控制输入u(t)的L₂范数的平方：

式中，n决定了控制输入的阶次。

2.根据权利要求1所述的一种微型无人机高机动轨迹规划方法，其特征在于，

给定初始状态x₀∈χ^free和目标区域

找到多项式轨迹参数化D∈R^3×(K+1)和一个时间周期T≥0，使得：

x(0)＝x₀，x(T)∈χ^goal

式中，ρ表示比例系数，区间为0-1，用于表示时间周期函数占整个最优函数的比例；J表示平滑度；

用C^*(x₀，χ^goal)表示从初始状态x₀到目标区域χ^goal的最优成本。

3.根据权利要求1所述的一种微型无人机高机动轨迹规划方法，其特征在于，所述步骤二具体包括以下步骤：

步骤201、构造运动基元；

步骤202、诱导状态空间离散化；

步骤203、确定性的最短轨迹；

步骤204、设计启发函数；

步骤205、碰撞检测；

步骤206、轨迹细化。

4.根据权利要求3所述的一种微型无人机高机动轨迹规划方法，其特征在于，通过离散的栅格

其中每个控制向量u_m∈R³将定义为短持续时间的一个运动基元；

获得离散化U_M的方法是沿着每个轴[0，u_max]选择多个样本μ∈R⁺，其定义离散化步数

并且产生M＝(2μ+1)³个运动基元；

给定初始状态

生成一个持续时间τ＞0的运动基元，在t∈[0，τ]时间间隔内施加恒定的控制输入u(t)≡u_m∈U_M，得到：

控制输入是常数，意味着所有涉及时间的系数必须相同为零，即：

将控制表达式u(t)＝u_m与初始条件x₀进行积分得到：

或等效地给定四旋翼无人机的初始状态x₀和控制输入u_m，等式(2)中线性时不变系统的生成轨迹是：

由于持续时间τ和控制输入u_m都是固定的，因此运动基元的成本为e_c＝(||u_m||²+ρ)τ。

5.根据权利要求3所述的一种微型无人机高机动轨迹规划方法，其特征在于，状态空间的离散化允许通过从x₀开始并且应用所有基元在持续时间τ后获得M个可能状态来构建可达系统状态，将所有可能的基元再次施加到M个状态中的每一个将在时间2τ处产生M²个可行的状态；

由于自由空间χ^free是有界的、离散的，因此可达状态S的集合是有限的；定义图G(S，ξ)，其中S是可达系统状态的离散集合，ξ是连接图中状态的边集，每条边由运动基元e：＝(u_m，τ)定义，设s₀为与x₀对应的状态。

6.根据权利要求3所述的一种微型无人机高机动轨迹规划方法，其特征在于，设定控制输入u(t)在持续时间τ的间隔上是分段恒定的，即引入另外的变量N∈R⁺，使得T＝Nτ，并且对于k＝0，...，N-1有u_k∈U_M，则：

强制控制轨迹成为U_M中运动基元的组合。

7.根据权利要求6所述的一种微型无人机高机动轨迹规划方法，其特征在于，给定初始状态x₀∈χ^free，目标区域

具有持续时间τ＞0的有限运动基元U_M，选择长度为N的运动基元序列u_0：N-1，使得：

s₀＝x₀，s_N∈χ^goal

8.根据权利要求3所述的一种微型无人机高机动轨迹规划方法，其特征在于，(a)最小时间启发函数

由于系统的最大速度受沿每个轴υ_max的限制，因此达到目标区域χ^goal中的最接近状态x_f的最小时间受

的限制，同样由于系统的最大加速度受到a_max的限制，因此状态

不能快于：

p(0)＝p₀，υ(0)＝υ₀

沿着各个轴以闭合形式求解获得下限

通过

定义最小可到达时间的下限，使用易于计算但不太紧密的边界

为了找到启发函数，通过用下边界

来替换状态约束x(t)∈χ^free和输入约束u(t)∈U，即：

x(0)＝x₀，x(T)∈χ^goal

由于J(D)≥0，在最优成本上获得下限的直接方法是：

因此给定离散空间中的节点s₀，s_f∈S：

(b)线性二次最小时间启发函数

虽然最小时间启发式计算非常容易计算并考虑了速度约束，但它忽略了控制输入，在去除状态约束x(t)∈χ^free和输入约束u(t)∈U之后，方程(12)的问题是线性二次最小时间问题；

令x_f∈χ^goal为固定的最终状态，并定义δ_T：＝x_f-e^ATx₀和可控格兰姆行列式

然后方程(6)中的最优时间T是下界

或下式的解：

最优控制是：

同时，最优成本是：

方程(1)中的多项式系数D∈R³×(2n)是：

当H∈R(3n)×(3n)时，有

表示最终状态和初始状态的关联；δ、W可控格兰姆行列式。

9.根据权利要求3所述的一种微型无人机高机动轨迹规划方法，其特征在于，

对于计算边e(t)＝[p(t)^T，v(t)^T，a(t)^T，...]^T，需要检测对于t∈[0，τ]时是否

检测几何空间

中的碰撞与强制执行动力学约束

边e(t)仅在其几何形状为

和其导数

时有效，即

由于其导数υ，a是多项式，计算它们在时间段[0，τ]内的极值与速度、加速度等的最大界限进行比较；更困难的部分是检测几何空间P^free中的碰撞，将模型P作为占据栅格地图，设P：＝{p(t_i)|t_i∈[0，τ]，i＝0，...，I}是系统沿轨迹p(t)遍历的一组位置，为了确保无碰撞轨迹只需要显示所有i∈{0，...，I}时的p(t_i)∈P^free；给定多项式p(t)，t∈[0，τ]，对位置p(t_i)进行采样，通过定义：

使得

这里R是占据网格分辨率，该条件确保两个连续样本之间的最大距离不会超过地图分辨率。

10.根据权利要求3所述的一种微型无人机高机动轨迹规划方法，其特征在于，细化轨迹x^*(t)是通过求解具有给定初始状态s₀、结束状态s_g和中间航路点p_k，k∈{0，...，N-1}的无约束二次规划得到的：

s.t.x₀(0)＝s₀，x_N-1(τ_N-1)＝s_g

x_k+1(0)＝x_k(τ_k)，k∈{0，...，N-2}

每个轨迹段τ_k的时间从低维空间中规划的轨迹给出。

Images