一种基于四旋翼动力学模型的时间最优轨迹规划方法及装置制造方法及图纸

本发明专利技术公开了一种基于四旋翼动力学模型的时间最优轨迹规划方法及装置，首先建立四旋翼的动力学模型，同时考虑空气阻力模型，在给定航路点的飞行过程中，引入了过程度量，并采用航路点和障碍物的接近度对其进行了补充。制定了三个过程变量，第一个变量是用于约束经过航路点的顺序，第二个变量是用于约束靠近并通过各个所给定的航路点；第三个变量是用于约束避开规则障碍物，然后将轨迹的时间最优控制问题转换成非线性优化问题，从而允许同时优化状态和输入轨迹以及航路点时间分配。本发明专利技术同时规划了合理的时间分配和时间最优平滑轨迹，并且避开了规则障碍物，适用于不同四旋翼模型的物理差别，提高了四旋翼的飞行效率，实现时间最优。最优。最优。

【技术实现步骤摘要】
一种基于四旋翼动力学模型的时间最优轨迹规划方法及装置


[0001]本专利技术属于轨迹规划领域，尤其涉及一种基于四旋翼动力学模型的时间最优轨迹规划方法及四旋翼。

技术介绍

[0002]目前，自主无人机正在被广泛应用，由于受电池影响导致其续航有限，因此在搜索、营救等场景中如何尽可能快地通过航路点是目前亟需解决的问题。现阶段的四旋翼时间最优轨迹规划方法主要分为两大类：
[0003]1、连续时间多项式方法，充分利用四旋翼无人机微分平坦性的输出状态，能够获得较高的计算效率。但由于该方法规划出的多项式轨迹本质上是平滑的，因此只能在无限短的持续时间内达到输入极限，或者是在整个轨迹飞行时间内不断达到输入极限，而不能以合理的顺序来表示无人机状态以及输入的快速变化，难以处理四旋翼单转轴上的推力约束。故该方法不能完全发挥四旋翼无人机的执行器功能，使得规划出的轨迹达不到最优。
[0004]2、离散时间分解轨迹方法，最早采用的是基于图搜索的方法来求解，其主要用于低维度空间上的轨迹规划问题，尽管具有较好的完备性，但是需要对环境进行完整的建模工作，难以考虑四旋翼无人机的非完整约束限制，同时存在搜索速度慢、效率低下等问题。为了解决这些问题，学者们又提出了基于随机采样的方法。该方法可以解决四旋翼质点模型等较简单动力学模型的轨迹规划问题，这类算法适用于较高的维度空间，它们以概率完备性来代替完备性，即当时间接近无限时一定有解，从而使得轨迹规划的效率较高。但是由于需要在离散时间步骤上针对四旋翼复杂的状态空间方程进行采样，很容易导致计算复杂度过高，耗费时间长。用基于优化的方法规划四旋翼轨迹是近期为数不多可行的解决方案，规划出时间细分的轨迹，其将四旋翼系统的动力学模型和电机输入的上下界作为约束条件，保证轨迹能够满足这些强制约束条件。而且针对时间最优轨迹规划，该方法将轨迹总时长作为优化变量的一部分，根据场景需求建立相应的优化模型，并允许优化在每个离散时间步长范围内选择的任何输入，可以解决采样复杂度过高的问题，但基于优化的方法在处理三维航路点约束的问题时其性能往往不佳，无法考虑真实的四旋翼模型进行各项动力学约束，且难以同时规划合理的时间分配和飞行轨迹。

技术实现思路

[0005]本专利技术的目的在于针对目前时间最优轨迹规划方法的不足，提出一种基于四旋翼动力学模型的时间最优轨迹规划方法及装置，融合了过程变量与非线性优化模型，考虑到真实的四旋翼模型进行各项动力学约束，且同时规划合理的时间分配和飞行轨迹。
[0006]本专利技术的目的是通过以下技术方案实现的：
[0007]根据本说明书的第一方面，提供一种基于四旋翼动力学模型的时间最优轨迹规划方法，包括以下步骤：
[0008]S1，基于四旋翼物理模型，对实际应用中的四旋翼动态性能进行建模，构建系统输
入输出上下界约束不同的四旋翼动力学模型，所述四旋翼动力学模型计算公式如下：
[0009][0010][0011][0012][0013]其中，是p的导数，而p是四旋翼的位置，是q的导数，而q是四旋翼空间旋转的四元数，是四元数乘法的符号，ω是四旋翼的角速度，是v的导数，v是四旋翼的速度，g是当前位置下的重力加速度，m是四旋翼的质量，R(q)是世界坐标系转换到机体坐标系下的旋转矩阵，T是电机产生的推力，D是近似的线性空气阻力效应，R
T
(q)是R(q)矩阵的转置，是ω的导数，J
‑1是J矩阵的求逆，J是四旋翼的转动惯量，τ是四旋翼产生的力矩；
[0014][0015]0≤T
min
≤T
i
≤T
max
[0016]其中，T
i
分别是四旋翼四个桨叶产生的四个推力，T
min
是单个桨叶产生推力的最小值，T
max
是单个桨叶产生推力的最大值，l是四旋翼的轴距长度，c
τ
是电机转子的转矩常数；
[0017]S2，将所述步骤S1中的四旋翼动力学模型的各项动力学约束先进行时间离散，然后采用数值积分算法转换成四旋翼动力学模型的动态等式约束；
[0018]S3，设定多个航路点，将所有的航路点使用所述步骤S2中四旋翼动力学模型的动态等式约束并以二次型的距离代价进行表现，将二次型的距离代价的表现形式转换成约束形式，构建航路点约束；
[0019]S4，建立按顺序通过航路点和避开规则障碍物的过程变量约束；
[0020]S5，对四旋翼轨迹采用四旋翼动力学模型的动态等式约束、航路点约束和过程变量约束，构建时间最优轨迹规划的非线性优化模型；
[0021]S6，基于所述步骤S5中的非线性优化问题模型，采用大规模非线性求解器Casadi对各时刻四旋翼状态进行求解，得到时间最优的轨迹。
[0022]根据本说明书的第二方面，提供一种基于四旋翼动力学模型的时间最优轨迹规划装置，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述处理器执行所述可执行代码时，用于实现如第一方面所述的基于四旋翼动力学模型的时间最优轨迹规划方法。
[0023]本专利技术的有益效果如下：
[0024]1.引入了三个过程变量，用于度量四旋翼无人机的飞行轨迹过程，同时把过程变量分别与航路点的接近度和躲避规则障碍物进行互补松弛约束，确保轨迹能够按顺序通过设定的航路点，并且避开可能发生碰撞的规则障碍物；
[0025]2.构建四旋翼无人机在实际应用中的动力学模型，同时考虑精度与计算复杂度，
采用改进欧拉法进行动力学方程离散，解决了无法完全考虑各项动力学约束的问题；
[0026]3.采用多重打靶法，将轨迹的最优控制问题转换成非线性优化问题，构建了一个由无人机状态、控制量、过程变量等组成的优化变量，允许同时规划合理的时间分配和飞行轨迹；
[0027]4.针对无人机的时间最优规划任务，建立了相应的优化模型，并采用Casadi求解器进行求解，解决了无法充分发挥无人机执行器功能的问题。
附图说明
[0028]图1为本专利技术一实施例提供的一种基于四旋翼动力学模型的时间最优轨迹规划方法流程图；
[0029]图2为本专利技术一实施例提供的竞速四旋翼时间最优轨迹规划图；
[0030]图3为本专利技术一实施例提供的时间最优轨迹规划的四旋翼位置状态信息图；
[0031]图4为本专利技术一实施例提供的时间最优轨迹规划的四旋翼速度状态信息图；
[0032]图5为本专利技术一实施例提供的四旋翼姿态状态信息图；
[0033]图6为本专利技术一实施例提供的四旋翼角速度状态信息图；
[0034]图7为本专利技术一实施例提供的四旋翼推力状态信息图；
[0035]图8为本专利技术一实施例提供的衡量四旋翼飞行过程的过程变量图；
[0036]图9为本专利技术一实施例提供的带真实物理引擎的Gazebo仿真中的四旋翼X轴上位置时间的跟踪曲线图；
[0037]图10为本专利技术一实施例提供的Gazebo仿真中的四旋翼本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于四旋翼动力学模型的时间最优轨迹规划方法，其特征在于，包括以下步骤：S1，基于四旋翼物理模型，对实际应用中的四旋翼动态性能进行建模，构建系统输入输出上下界约束不同的四旋翼动力学模型，所述四旋翼动力学模型计算公式如下：出上下界约束不同的四旋翼动力学模型，所述四旋翼动力学模型计算公式如下：出上下界约束不同的四旋翼动力学模型，所述四旋翼动力学模型计算公式如下：出上下界约束不同的四旋翼动力学模型，所述四旋翼动力学模型计算公式如下：其中，是p的导数，而p是四旋翼的位置，是q的导数，而q是四旋翼空间旋转的四元数，是四元数乘法的符号，ω是四旋翼的角速度，是v的导数，v是四旋翼的速度，g是当前位置下的重力加速度，m是四旋翼的质量，R(q)是世界坐标系转换到机体坐标系下的旋转矩阵，T是电机产生的推力，D是近似的线性空气阻力效应，R
T
(q)是R(q)矩阵的转置，是ω的导数，J
‑1是J矩阵的求逆，J是四旋翼的转动惯量，τ是四旋翼产生的力矩；0≤T
min
≤T
i
≤T
max
其中，T
i
分别是四旋翼四个桨叶产生的四个推力，T
min
是单个桨叶产生推力的最小值，T
max
是单个桨叶产生推力的最大值，l是四旋翼的轴距长度，c
τ
是电机转子的转矩常数；S2，将所述步骤S1中的四旋翼动力学模型的各项动力学约束先进行时间离散，然后采用数值积分算法转换成四旋翼动力学模型的动态等式约束；S3，设定多个航路点，将所有的航路点使用所述步骤S2中四旋翼动力学模型的动态等式约束并以二次型的距离代价进行表现，将二次型的距离代价的表现形式转换成约束形式，构建航路点约束；S4，建立按顺序通过航路点和避开规则障碍物的过程变量约束；S5，对四旋翼轨迹采用四旋翼动力学模型的动态等式约束、航路点约束和过程变量约束，构建时间最优轨迹规划的非线性优化模型；S6，基于所述步骤S5中的非线性优化问题模型，采用大规模非线性求解器Casadi对各时刻四旋翼状态进行求解，得到时间最优的轨迹。2.根据权利1要求所述的一种基于四旋翼动力学模型的时间最优轨迹规划方法，其特征在于，所述步骤S1具体包括以下步骤：采用四元数表示四旋翼姿态旋转，计算公式如下：
其中，q
a
是表示一个姿态的四元数，q
b
是表示另一个姿态的四元数，是四元数乘法的运算符号表示，ω
a
、ω
b
分别是两个四元数的第一个参数，x
a
、x
b
分别是两个四元数的第二个参数，y
a
、y
b
分别是两个四元数的第三个参数，z
a
、z
b
分别是两个四元数的第四个参数。3.根据权利要求2所述的一种基于四旋翼动力学模型的时间最优轨迹规划方法，其特征在于，所述步骤S1具体包括以下步骤：对四旋翼采用近似的线性空气阻力效应计算，计算公式如下：其中，D是近似线性空气阻力效应的矩阵表示，d
x
、d
y
、d
z
分别是线性空气阻力在x、y、z三轴上的具体分量。4.根据权利要求1所述的一种基于四旋翼动力学模型的时间最优轨迹规划方法，其特征在于，所述步骤S2具体包括以下步骤：S21，采用多重打靶法将所述步骤S1中的四旋翼动力学模型的各项动力学约束进时间离散；S22，采用多重打靶法的数值积分算法求解S21中得到的离散后的四旋翼动力学模型的各项动力学约束，得到四旋翼动力学模型的动态等式约束；通过将四旋翼动力学模型的各项动力学约束转换成动态等式约束，使得四旋翼轨迹规划问题从原本的最优控制问题转换成初级非线性优化问题，并建立初级非线性优化模型，其中，多重打靶法的数值积分算法计算公式如下：k1＝f(x
n
,u
n
)，k2＝f(x
n
+hk1,u
n
+h)其中，x
n+1
是在n+1时刻下四旋翼的状态，x
n
是在n时刻下四旋翼的状态，h是离散的时间步长，k1和f(x
n
,u
n
)均是在n时刻下数值积分所用方法的函数值，u
n
是在n时刻下四旋翼动力学模型的系统控制量，k2和f(x
n
+hk1,u
n
+h)均是在n+h时刻下数值积分所用方法的函数值；而四旋翼动力学模型的动态等式约束计算公式如下：x
k+1
‑
x
k
‑
f
Num
(x
k
,u
k
)
·
dt＝0其中，x
k+1
是四旋翼动力学模型在k+1时刻下的状态，x
k
和x
dyn,k
是四旋翼动力学模型在k时刻下的状态，f
Num
(x
k
,u
k
)是数值积分所用方法的函数，u
k
是在k时刻下四旋翼动力学模型
的系统控制量，t是运行的时间，p
k
是在k时刻下四旋翼的位置，q
k
是在k时刻下四旋翼空间旋转的四元数，v
k
是在k时刻下四旋翼的速度，ω
k
是在k时刻下四旋翼的角速度。5.根据权利4要求的一种基于四旋翼动力学模型的时间最优轨迹规划方法，其特征在于，所述S22中使得四旋翼轨迹规划问题从原本的最优控制问题转换成初级非线性优化问题，并建立初级非线性优化模型，具体包括以下步骤：S221，基于四旋翼轨迹规划原本的最优控制问题中的时间最优条件，构建优化指标函数，使得轨迹规划的时间最优，优化指标函数具体为；J＝t
N
其中，J是优化指标函数，t
N
是时间离散后的最后一个时刻点；S222，基于四旋翼轨迹规划中存在的时间最优条件和四旋翼动力学模型的动态等式约束条件，构建合理的优化变量，使得四旋翼轨迹规划问题从原本的最优控制问题转换成初级非线性优化问题，并得到初级非线性优化模型，其中，不断调整优化变量的值，能够使得优化指标函数达到最优。6.根据权利1要求的一种基于四旋翼动力学模型的时间最优轨迹规划方法，其特征在于，所述步骤S3具体包括以下步骤：S31，设定M个航路点p
wj
，将所有航路点分配到所述步骤S2中四旋翼动力学模型的动态等式约束并以二次型的距离代价进行表现，二次型的距离代价公式如下：L
d,j
＝(p
k
‑
p
wj
)
T
(p
k
‑
p
wj
)，j∈[0,M)其中，L
d,j
是四旋翼当前位置与第j个航路点之间的距离，p
k
是四旋翼在k时刻下的位置，p
wj
是第j个航路点的位置；S32，将二次型的距离代价的表现形式转换成约束形式，具体如下：(p
k
‑
p
wj
)
T
(p
k
‑
p
wj
)≤τ
j2
其中，τ
j
是第j个航路点的容忍区间；由此，构建航路点约束。7.根据权利1要求的一种基于四旋翼动力学模型的时间最优轨迹规划方法，其特征在于，所述步骤S4具体包括以下步骤：S41，引入三个过程变量，两个过程变量用来度量四旋翼按顺序通过所设定的各个航路点的飞行过程，一个过程变量用来度量四旋翼躲避规则障碍物的过程；S42，建立用于描述四旋翼时间最优轨迹规划的三个过程变量约束，首先引入μ来度量四旋翼轨迹能否通过所设定的航路点，其中每个航路点都具有向量μ
k
是用于描述第k个时间节点下能否通过航路点的过程变化，所有的过程变量应该满足非负条件，因此，令μ
kj
≥0，由于μ
k
起着标志位的作用，因此，制定μ
k
变化的规则，μ
kj
只有当四旋翼在某个时刻下的位置与所设定航路点之间的距离在容忍区间内才能发生变化，制定μ
k
变化的规则可以表示为：其中，μ
kj
是在k时刻下第j个航路点的过程变量μ，μ是度量四旋翼轨迹能否通过所设定
的航路点的过程变量，p
k
是在k时刻下四旋翼的位置，p
wj
是第j个航路点的位置，||p
k
‑
p
wj
||
22
是在k时刻下四旋翼的位置与第j个航路点之间的距离，τ
j
是第j个航路点的容忍区间，k是离散的时间节点，N是离散的最后一个时间节点，M是所设定航路点的总数量；由于所规划的四旋翼轨迹需要避开规则障碍物，且必须满足四旋翼动力学模型的动态等式约束，故要求四旋翼轨迹必须精确通过每个航路点是不实际的，为尽可能使轨迹平滑且安全，引入了一个松弛变量δ来提高所规划轨迹的鲁棒性，针对四旋翼位置与航路点之间的每段距离都给予一定的容忍距离但容忍度又会对轨迹的收敛性和时间最优产生不良影响，因此，需要选择一个适中的容忍距离来进行约束，容忍度可以表示为：影响，因此，需要选择一个适中的容忍距离来进行约束，容忍度可以表示为：其中，p
k
是在k时刻下四旋翼的位置，p
wj
是第j个航路点的位置，||p
k
‑
p
wj
||
22
...

【专利技术属性】
技术研发人员：沈雨阳，刘泽民，赵一先，闫昌智，徐金明，孟文超，
申请(专利权)人：浙江大学，
类型：发明
国别省市：