CN112947558B

CN112947558B - 一种时空同步达到协同轨迹规划方法

Info

Publication number: CN112947558B
Application number: CN202110169387.3A
Authority: CN
Inventors: 郑多; 张云飞; 林德福; 王江; 宋韬
Original assignee: Beijing Institute of Technology BIT
Current assignee: Beijing Institute of Technology BIT
Priority date: 2021-02-07
Filing date: 2021-02-07
Publication date: 2023-04-07
Anticipated expiration: 2041-02-07
Also published as: CN112947558A

Abstract

本发明公开了一种时空同步达到协同轨迹规划方法，基于贝塞尔曲线进行无人机轨迹的连续性约束，通过引入动态时间调节因子，实现分布式无人机时空同步到达的轨迹规划。本发明公开的时空同步达到协同轨迹规划方法，多无人机轨迹时间相同，能够实现时空同步打击，具有轨迹平滑、计算量小、轨迹规划速度快等诸多优点。

Description

一种时空同步达到协同轨迹规划方法

技术领域

本发明涉及一种时空同步达到协同轨迹规划方法，属于飞行器控制领域。

背景技术

在执行复杂任务时，常常具有多个任务目标，需要多个无人机采用协同作业的形式相互配合完成任务。

在协同作业过程中，涉及到不同的任务目标的分配，不同无人机的任务目标位置、运动速度等均可能不同，因此，需要对不同无人机的运行轨迹进行规划。

现有的轨迹规划多采用混合整数线性规划法进行，然而此种规划方法计算量大，不适合在线规划，故多采用中央服务器的模式进行，由中央服务器预先规划后统一对多个无人机的飞行轨迹进行控制，但是在某些情况下，例如火灾、电磁干扰情况下，无人机与中央服务器的信号传递受到较大影响，导致中央服务器轨迹规划延迟性高，甚至出现无法进行轨迹规划的情形。

现有技术中还具有基于时间约束集的发射时间规划算法框架进行轨迹规划的方法，然而此种方法每条轨迹的运算时间不同，存在时间较长的情况，不能严格实现同时达到的任务目标。

现有技术中还具有通过GA算法实现多任务分配的方法，并将时间作为约束以实现协同作业，然而其基础为速度恒定，并且只实现了任务层规划。

由于上述原因，亟待提出一种机动灵活、多无人机能够同时到达任务目标的协同轨迹规划方法。

发明内容

为了克服上述问题，本发明人进行了锐意研究，提出了一种时空同步达到协同轨迹规划方法，基于贝塞尔曲线进行无人机轨迹的连续性约束，通过引入动态时间调节因子，实现分布式无人机时空同步到达的轨迹规划。

具体地，该方法包括以下步骤：

S1、采用贝塞尔曲线描述轨迹；

S2、构建轨迹模型；

S3、获得优化后轨迹。

在步骤S1中，基于不同无人机的任务分配结果获得不同无人机初步轨迹，采用贝塞尔曲线对初步轨迹进行描述实现轨迹的规划，轨迹的贝塞尔曲线可以表示为：

其中，S′_j表示数值因子，S_j表示时间调节因子；

f_μ(t)表示贝塞尔曲线，μ表示无人机在x、y任意方向上的贝塞尔曲线，m表示贝塞尔曲线中轨迹段的总数；

t表示时间，T₀～T_m表示贝塞尔曲线不同轨迹段j对应时间。

在步骤S2中，所述构建轨迹模型包括以下子步骤：

S21、归一化轨迹模型目标函数；

S22、建立等式约束；

S23、建立不等式约束；

S24、确定轨迹模型。

进一步地，在步骤S21中，构建轨迹模型，所述轨迹模型的目标函数可以表示为：

其中，μ表示无人机在x、y任意方向上的贝塞尔曲线，T表示贝塞尔曲线总时间，

其中每一段轨迹j,其轨迹时间为[0,S_j]，轨迹j满足：

6、根据权利要求4所述的时空同步达到协同轨迹规划方法，其特征在于，

在步骤S22中，所述等式约束是指线性等式约束，其包括固定点约束，

所述固定点约束可以表示为：

其中，a为归一化贝塞尔曲线控制点，μ,j表示无人机在x、y任意方向上的第j段贝塞尔曲线，l表示导数阶数，

表示l阶导数对应的约束关系，i表示轨迹段j中不同的控制节点，n表示轨迹段j中控制节点数。

优选地，所述等式约束还包括连续性约束，所述连续性约束可以表示为：

其中，l表示导数阶数，n表示轨迹段j中控制节点数。

进一步地，在步骤S23中，所述不等式约束是指对无人机的最大速度和最大加速度进行约束。

根据本发明，在步骤S24中，结合轨迹模型目标函数与约束条件，获得轨迹模型，可以表示为：

其中，min表示取最小值，Ω_j表示优化变量，s.t.表示受限制于；

c^TQ_c为步骤S21中轨迹模型的简写；

A_eqc＝b_eq为步骤S22中等式约束的简写；

A_ieqc≤b_ieq为步骤S23中不等式约束的简写。

根据本发明，在步骤S3中，采用凸二次规划方法对轨迹模型进行求解，获得优化后轨迹。

本发明所具有的有益效果包括：

(1)根据本发明提供的一种时空同步达到协同轨迹规划方法，保证多无人机轨迹时间相同，实现时空同步打击。

(2)根据本发明提供的一种时空同步达到协同轨迹规划方法，利用Bezier曲线，并结合约束，实现了无人机轨迹的平滑。

(3)根据本发明提供的一种时空同步达到协同轨迹规划方法，选取Jerk为优化目标保证轨迹规划的平滑，提高了求解速度，提高了轨迹规划效率。

附图说明

图1示出根据本发明一种优选实施方式的时空同步达到协同轨迹规划方法示意图；

图2示出根据本发明实施例1中无人机初步轨迹图；

图3示出根据本发明实施例1中无人机轨迹图；

图4示出根据本发明实施例1中无人机速度曲线图；

图5示出根据本发明实施例1中无人机加速度曲线图。

具体实施方式

下面通过附图和实施例对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。

在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。

本发明提供的一种时空同步达到协同轨迹规划方法，基于贝塞尔曲线进行无人机轨迹的连续性约束，通过引入动态时间调节因子，实现分布式无人机时空同步到达的轨迹规划。

具体地，如图1所示，该方法包括以下步骤：

S1、采用贝塞尔曲线描述轨迹；

S2、构建轨迹模型；

S3、获得优化后轨迹。

在步骤S1中，基于不同无人机的任务分配结果获得不同无人机初步轨迹，采用贝塞尔曲线对初步轨迹进行描述实现轨迹的规划。

在本发明中，对所述初步轨迹的获取不做特别限定，可以是任意一种能够根据无人机位置及目标位置规划轨迹的方式。

所述贝塞尔曲线是一种被广泛使用的数学曲线，其由多个控制点定义，总是过初始控制点和终止控制点，且曲线形状可以通过改变控制点改变。

在本发明中，由于无人机可以解耦实现x、y两个方向的控制，因此可以独立地对x、y进行贝塞尔曲线描述。

所述贝塞尔曲线可以表示为：

其中，

为权重函数，也被叫做Bernstein多项式，j表示贝塞尔曲线中不同的轨迹段，

表示轨迹段j中不同的控制节点，i取值为0～n，n为大于3的正整数，t_b表示时间。

经典的权重函数

可以表示为：

在本发明中，时间t_b的取值范围不同于经典权重函数中的[0,1]，为满足多无人机时间约束，重新建立时间映射关系，本发明中贝塞尔曲线可以表示为：

其中，

表示轨迹中不同的控制节点，S′_j表示数值因子，S_j表示时间调节因子；

f_μ(t)表示贝塞尔曲线，μ表示无人机在x、y任意方向上的贝塞尔曲线，f_μj(t)表示不同轨迹段j对应的贝塞尔曲线，m表示贝塞尔曲线中轨迹段的总数；

t表示时间，T₀～T_m表示贝塞尔曲线不同轨迹段j对应时间，例如t∈[T₀,T₁]表示第1段轨迹段所用的时间。

T₁～T_m取值优选通过预估飞行轨迹总用时，按不同轨迹段j的长度比例分配得到；

在本发明中，通过数值因子S′_j保障了数值的稳定性，优选地，数值因子S′_j的取值等于时间调节因子S_j的值，时间因子的设定，使得多无人机轨迹时间相同，从而实现时空同步打击的效果。

优选地，S_j＝T_j-T_j-1，对应每段路径的分配时间。。

在本发明中，通过构建基于贝塞尔曲线的轨迹模型将轨迹进行参数化，简化为多项式形式，进而通过设计优化函数和约束条件进行优化，从而获得最优轨迹。

在步骤S2中，所述构建轨迹模型包括以下子步骤：

S21、归一化轨迹模型目标函数；

S22、建立等式约束；

S23、建立不等式约束；

S24、确定轨迹模型。

在步骤S21中，构建轨迹模型，所述轨迹模型的目标函数可以表示为：

其中，μ表示无人机在x、y任意方向上的贝塞尔曲线，T表示贝塞尔曲线总时间，即T＝T_m；

在本发明中，为了提升计算效率，对每一段轨迹j使用相对时间进行构造，即j段轨迹的时间为[0,S_j]，则有：

简化可得：

其中，下标j表示不同的轨迹段，i表示轨迹段j中不同的控制节点。

相较于传统的多项式形式，本发明中基于贝赛尔曲线的目标函数多项式表达相对复杂，不利于构建成二次函数的形式，因此需要通过传统多项式进行求解。

具体地，建立传统多项式系数与贝塞尔曲线系数的线性转换关系，基于贝塞尔曲线轨迹模型的目标函数转化时间归一化目标函数：

J＝a^Ts^TM^TQMsa (8)

其中，a为c₁,c₂,...,c_j,...,c_m的集合，c_j表示曲线不同段的控制点，s为每段曲线段对应的时间常数

T表示转置，

根据本发明，式(8)可以简写为J＝c^TQc形式，其中c＝Msa。

在步骤S22中，所述等式约束是指线性等式约束，通过等式约束保证了无人机轨迹是从初始位置点到终止位置点的连续平滑曲线。

具体地，所述等式约束包括固定点约束。

进一步地，所述固定点约束包括起点的位置、速度、加速度约束、终点的位置、速度、加速度约束，以及路径点的位置约束，

在本发明中，所述固定点约束可以表示为：

优选地，所述等式约束还包括连续性约束，所述连续性约束是指在每段路径的末位置和下一段路径的初始位置之间的位置、速度、加速度约束。

所述连续性约束可以表示为：

其中，a为归一化贝塞尔曲线控制点,S_j表示轨迹段j的轨迹时间，l表示导数阶数，n表示轨迹段j中控制节点数。

根据本发明，式(10)～式(12)可以简写为A_eqc＝b_eq，其中c＝[c₁,c₂,...,c_m]。

在步骤S23中，所述不等式约束是指对无人机的最大速度和最大加速度进行约束，从而保证轨迹的可行性。

进一步地，由于贝塞尔曲线具有凸包性质，只需要对每段曲线的控制点的大小进行约束，即可实现对整条曲线的速度、加速度的约束，可以表示为：

其中，

表示速度下限、

表示速度上限、

表示加速度下限、

表示加速度上限

进一步地，式(13)可以简写为A_ieqc≤b_ieq，其中c＝[c₁,c₂,...,c_m]

在步骤S24中，结合轨迹模型目标函数与约束条件，获得轨迹模型，可以表示为：

其中，min表示取最小值，Ω_j表示优化变量，通常为常数变量，s.t.表示受限制于。

在本发明中，将轨迹模型表示为式(14)的形式，使得其能够转化为二次规划问题进行求解，进而提高求解效率，降低轨迹规划用计算量，从而实现在线轨迹规划。

在步骤S3中，通过对轨迹模型求解，获得优化后轨迹。

在本发明中，优选采用凸二次规划方法对轨迹模型进行求解，其求解效率高。

凸二次规划方法是运筹学中一种常用的求解方法，在本发明中不做特别赘述，本领域技术人员可根据实际经验对轨迹模型进行求解，从而获得优化后的轨迹。

实施例

实施例1

进行实验，实验设定如下：

环境地图为10km×10km区域，其中存在5个无人机(U₁～U₅)组成的任务编队，3个任务目标(TA₁～TA₃)，，相关参数如表1所示，

表1实验参数

各无人机任务分配结果如表2所示：

表2

无人机	目标
		<![CDATA[U<sub>1</sub>]]>	<![CDATA[TA<sub>3</sub><!-- 7 -->]]>
<![CDATA[U<sub>2</sub>]]>	<![CDATA[TA<sub>2</sub>]]>
		<![CDATA[U<sub>3</sub>]]>	<![CDATA[TA<sub>1</sub>]]>
<![CDATA[U<sub>4</sub>]]>	<![CDATA[TA<sub>1</sub>]]>
		<![CDATA[U<sub>5</sub>]]>	<![CDATA[TA<sub>3</sub>]]>

无人机初步轨迹为多条直线组成的轨迹，如图2所示，每段直线表示一个曲线段j,预估在初步轨迹中最长路径无人机飞行时长为T_m＝781秒。

按照以下步骤进行轨迹规划：

S1、采用贝塞尔曲线描述轨迹；

S2、构建轨迹模型；

S3、获得优化后轨迹。

其中，步骤S1中，贝塞尔曲线表示为：

在步骤S2中，所述构建轨迹模型包括以下子步骤：

S21、归一化轨迹模型目标函数；

S22、建立等式约束；

S23、建立不等式约束；

S24、确定轨迹模型。

在步骤S21中，轨迹模型目标函数表示为：

J＝a^Ts^TM^TQMsa (8)

在步骤S22中，所述等式约束包括固定点约束和连续性约束，

所述固定点约束表示为：

所述连续性约束表示为：

在步骤S23中，

所述不等式约束表示为：

其中，无人机的最大速度：

为-11.3m/s，

为11.3m/s；

x和y方向上最大加速度：

为-2m/s²，

为2m/s²

在步骤S24中，轨迹模型表示为：

通过凸二次规划方法对其进行求解，获得最终轨迹如图3所示，无人机到达任务目标的时间如表3所示：

表3

无人机	目标	到达目标所需时间(s)	脱靶量(m)
				<![CDATA[U<sub>1</sub>]]>	<![CDATA[TA<sub>3</sub>]]>	780.78	1
<![CDATA[U<sub>2</sub>]]>	<![CDATA[TA<sub>2</sub>]]>	780.80	1
				<![CDATA[U<sub>3</sub>]]>	<![CDATA[TA<sub>1</sub>]]>	780.80	1
<![CDATA[U<sub>4</sub>]]>	<![CDATA[TA<sub>1</sub>]]>	780.80	1
				<![CDATA[U<sub>5</sub>]]>	<![CDATA[TA<sub>3</sub>]]>	780.80	1

从图上及表3可以看出，规划得到的五架无人机协同轨迹，可实现从初始位置出发，到达目标位置，并且到达时间差值在0.1s以内，即同时达到目标位置，脱靶量均不大于1m，轨迹准确性较高。

图4、5示出了五架无人机x、y方向的速度和加速度曲线，从图上可以看出，规划得到的无人机位置和速度均为平滑曲线，且加速度曲线连续，并且速度和加速度均满足无人机约束条件。

在本发明的描述中，需要说明的是，术语“上”、“下”、“内”、“外”、“前”、“后”等指示的方位或位置关系为基于本发明工作状态下的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”、“第四”仅用于描述目的，而不能理解为指示或暗示相对重要性。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”“相连”“连接”应作广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体的连接普通；可以是机械连接，也可以是电连接；可以是直接连接，也可以通过中间媒介间接连接，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

以上结合了优选的实施方式对本发明进行了说明，不过这些实施方式仅是范例性的，仅起到说明性的作用。在此基础上，可以对本发明进行多种替换和改进，这些均落入本发明的保护范围内。