CN117631686B - 面向多旋翼无人机的路径优化方法以及轨迹跟踪控制方法 - Google Patents

Abstract

本发明属于无人机技术领域，具体涉及面向多旋翼无人机的路径优化方法以及轨迹跟踪控制方法，面向多旋翼无人机的路径优化方法，包括以下步骤，路径规划和轨迹优化，其中，所述路径规划和所述轨迹优化分为以下算法，路径规划算法：生成有离散点组成的安全路径，轨迹优化算法：根据各路径节点生成一条光滑连续的轨迹；一种面向多旋翼无人机的路径优化方法的轨迹跟踪控制方法，该轨迹跟踪控制方法利用如上述步骤的路径优化方法，规划无人机的路径。本发明能够基于四阶线性微分器的轨迹优化算法，将无人机位置控制***简化为二阶***，可根据二阶***阻尼比和***的固有频率直接设定控制器的参数，简化调参。

Description

面向多旋翼无人机的路径优化方法以及轨迹跟踪控制方法

技术领域

本发明属于无人机技术领域，具体涉及面向多旋翼无人机的路径优化方法以及轨迹跟踪控制方法。

背景技术

目前，由于微电子领域发展迅速，各种构型的无人机相继问世，随着控制算法的发展，实现多旋翼无人机的姿态控制，悬停控制，已经不是难事，目前，对多旋翼无人机的研究，已经不仅是对其姿态、位置等底层控制的研究，更多是结合感知、规划、控制等多种学科多种技术，实现无人机的自动飞行，自动避障飞行。而无人机的应用，也从面向广大航模爱好者的航拍飞行，fpv超视距飞行，扩展到面向工业生产领域的巡检，植保，快递运输等工业应用；

为了实现无人机的自动飞行，首先需要实现无人机轨迹生成与轨迹跟踪控制。在路径规划方面，已有许多如A*，RRT等经典路径规划算法，但这种算法输出得到的是一个个离散的路径点，由起始点经过n个中间路径节点最后到达目标运输运输点即为无人机的安全可通行路径；

由于无人机的欠驱动特性，无人机位置，速度加速度等高阶状态不可突变，仅依靠这些离散路径点，并不能让控制器控制无人机进行连贯顺畅快速地移动。因此需要先对路径点进行优化处理，利用路径节点生成一段光滑连续的参考轨迹。再利用无人机的轨迹跟踪控制算法控制无人机沿着参考轨迹飞行；

在路径优化，轨迹生成这方面，传统的做法有两大类，一类是基于多项式函数的方法，通过最优化的方法计算多项式函数的参数，使得轨迹满足一定的约束条件和代价函数；另一类是基于平滑过渡函数的方法，通过设计圆弧段，S曲线等函数，连接各个路径点，形成光滑的轨迹；

在路径优化，轨迹生成这方面，传统的做法有两大类，其一为，用n-1条多项式函数描述的轨迹平滑连接n个路径节点构建一整条连续轨迹，再根据约束条件，并设计代价函数，用最优化的方法计算对多项式轨迹中的参数，使轨迹最优，其二为，通过设计平滑连续过渡函数，通过函数连接各个路径点，在工业机械臂设计上，常用圆弧段对路径进行平滑处理，再用7段S曲线连接各个函数的两端，最终形成一条平滑连续的轨迹；

而对于上述提出的多项式轨迹生成的方法，其对算力消耗较大，对于主频较低的控制器计算耗时较久，而且随着路径点增加，生成轨迹所消耗的时间与内存也会增加；而至于基于S曲线和圆弧插补的轨迹生成方法，其生成的轨迹最大只有2次导数连续，即该轨迹的速度与加速度是连续的，而加速度是突变的，这对具有欠驱动特性的多旋翼无人机不太适用，四旋翼无人机对存在4次导数的连续轨迹才比较容易跟踪；

且由于无人机***的强耦合非线性欠驱动特性，目前控制器的设计难以处理外部的不确定扰动力，而依靠误差积分反馈可能会导致控制超调或滞后，对外部扰动力的估计问题仍需解决；

因此，提出一种面向多旋翼无人机的路径优化方法以及轨迹跟踪控制方法用于解决上述提出的问题，是有必要的。

发明内容

本发明的目的是提供面向多旋翼无人机的路径优化方法以及轨迹跟踪控制方法，能够基于四阶线性微分器的轨迹优化算法，将无人机位置控制***简化为二阶***，可根据二阶***阻尼比和***的固有频率直接设定控制器的参数，简化调参。

本发明采取的技术方案具体如下：

面向多旋翼无人机的路径优化方法，包括以下步骤：

路径规划和轨迹优化，其中，所述路径规划和所述轨迹优化分为以下算法：

路径规划算法：生成由离散点组成的安全路径；

轨迹优化算法：根据各路径节点生成一条光滑连续的轨迹。

所述路径规划算法中，设四阶线性微分器的传递函数为：

将上式写成状态空间方程形式，如下：

由此可知，四阶过渡过程输出的状态y存在四阶导数，可以作为无人机的参考轨迹。

所述轨迹优化算法中，径节点生成的具体步骤如下：

A1：假设一条由n个路径节点组成的路径，其中，第一个路径节点是无人机本身的位置，根据该路径计算预设速度V_pre；

A2：三阶微分器跟踪预设速度产生跟踪速度V_tra；

A3：令一个虚拟点从起点开始，以一定的速度沿着路径点的连线一直移动到终点，该虚拟点的位置作为四阶线性微分器的输入，其输出为轨迹参考点的位置，该虚拟点沿着路径移动，其计算公式如下：

P_img＝P_img+VECT_path·V_track

A4：四阶微分器跟踪虚拟点产生参考点，其计算公式如下：

P_ref＝G(s)·P_img

所述步骤A2中三阶微分器的传递函数为：

为使无人机在通过不同路径时提升效率并且产生的轨迹靠近原有的路径，其中，预设速度的计算由以下规则确定：

设无人机允许的最高飞行速度为V_max，最慢飞行速度为V_min，由路径转弯引起的减速为V_turn，由到达终点引起的减速为V_end，其预设速度的计算如下：

V_pre＝V_max-V_turn-V_end，(V_min≤V_pre≤V_max) (4)

V_turn的计算公式如下：

其中，R_slow表示前瞻减速范围，R_s0和K_s为设定的常数，V_tra为跟踪速度，L_i为当前节点离下一路径节点的距离，n表示离当前轨迹参考点的路径距离大于R_slow的路径节点。θ_i表示各个节点的转折角度，θ_add表示在前瞻减速范围内，路径矢量角度的改变量；

V_end的计算公式如下：

其中，L_end表示当前轨迹参考点距离路径终点的轨迹距离，L_ss表示终点减速起始距离，当L_end小于该距离时开始减速，L_se表示终点减速结束距离，当L_end小于该距离时预设速度减至最低速度。

一种面向多旋翼无人机的路径优化方法的轨迹跟踪控制方法，该轨迹跟踪控制方法利用上述的路径优化方法，规划无人机的路径，所述轨迹跟踪控制方法包括以下步骤：

S1：利用轨迹***计算目标的加速度，其中，包括以下步骤：

S11：根据轨迹的二阶导数，计算轨迹前馈加速度；

S12：利用轨迹跟踪控制算法，根据参考轨迹与无人机的状态反馈，计算反馈加速度；

S13：利用风扰估计算法，根据模型加速度与实际加速度，计算外部扰动加速度；

S14：将上述计算的加速度进行结合，计算目标加速度和目标旋转矩阵；

S2：底层控制器控制计算与控制输出，其中，利用姿态控制器和天向加速度控制器，计算出总升力和输出转矩。

所述S11中，计算轨迹前馈加速度的算法如下：

令轨迹输出的目标位置为P_Mx，P_My，P_Mz，输出的目标速度为V_Mx，V_My，V_Mz，输出的前馈加速度为A_FF＝[A_FFx A_FFy A_FFz]^T，所输出的目标量都是相对于惯性坐标系的；

设所生成的参考轨迹由4个多阶连续的变量表示，如下所示：

则，前馈加速度的计算公式如下：

所述S12中，反馈加速度的算法如下：

令：

A_FB＝[A_FBx A_FBy A_FBz]^T (8)

其中，ω_n为固有频率，固有频率越大，调节速度越快，ξ为阻尼比，取值在0.707-1之间。

所述S13中，外部扰动加速度的算法如下：

令水平方向的扰动加速度为A_Dist，定义由惯性坐标系到无人机机体坐标系的由欧拉角表示的转移矩阵得出：

其中，C表示COS()函数，S表示SIN()函数，扰动加速度的计算如下公式所示：

则，得出扰动加速度的计算，如下：

其中，a_nx，a_ny为惯性坐标系下传感器测得的加速度，其右边的项为所估计的输出加速度。

所述S14中，目标加速度和目标旋转矩阵的算法如下：

a_M＝A_FF+A_FB-A_Dist (11)

设目标旋转矩阵为则计算公式如下：

Z_B＝a_M (13)

其中，为目标偏航角度。

本发明取得的技术效果为：

本发明基于四阶线性微分器的轨迹优化算法，将无人机位置控制***简化为二阶***，可根据二阶***阻尼比和***的固有频率直接设定控制器的参数，简化调参。

本发明所需计算能力消耗十分低，可以直接在单片机上运行，减少硬件成本，与目前基于多项式的轨迹优化算法相比，其所生成的轨迹光滑且十分稳定，不会发散或偏离原路径太远，其所包含的轨迹跟踪算法能迅速估计外部扰动，相比积分反馈控制，本文的状态反馈与扰动补偿控制策略鲁棒性与抗扰能力更强。

附图说明

图1是本发明中无人机轨迹优化与轨迹跟踪算法总流程图；

图2是本发明中路径节点示意图；

图3是本发明中轨迹优化算法步骤图；

图4是本发明中轨迹优化示意图；

图5是本发明中随机轨迹优化效果图；

图6是本发明中圆形轨迹优化效果图。

具体实施方式

为了使本发明的目的及优点更加清楚明白，以下结合实施例对本发明进行具体说明。应当理解，以下文字仅仅用以描述本发明的一种或几种具体的实施方式，并不对本发明具体请求的保护范围进行严格限定。

实施例1：

如图1-6所示，面向多旋翼无人机的路径优化方法，包括以下步骤：

路径规划和轨迹优化，其中，路径规划和轨迹优化分为以下算法：

路径规划算法：生成由离散点组成的安全路径；

其中，设无人机轨迹利用四阶线性微分器的传递函数为：

将上式写成状态空间方程形式，如下：

由此可知，四阶过渡过程输出的状态y存在四阶导数，用于无人机的参考轨迹；

假设有一条由n个路径节点组成的路径，其中，第一个路径节点是无人机本身的位置，请参阅附图2所示并结合上述描述理解；

参照附图3，令一个虚拟的移动点从起点开始，以一定的速度沿着路径点的连线一直移动到终点，再用该虚拟移动点的位置作为四阶线性微分器的输入，再将微分器的输出作为轨迹参考点的位置。轨迹生成的具体步骤如下所示：

且，路径节点生成的具体步骤如下：

A1：假设一条由n个路径节点组成的路径，其中，第一个路径节点是无人机本身的位置，根据路径计算预设速度V_pre；

A2：三阶微分器跟踪预设速度产生跟踪速度V_tra；

A3：令一个虚拟点从起点开始，以一定的速度沿着路径点的连线一直移动到终点，该虚拟点的位置作为四阶线性微分器的输入，其输出为轨迹参考点的位置，该虚拟点沿着路径移动，其计算公式如下：

P_img＝P_img+VECT_path·V_track

A4：四阶微分器跟踪虚拟点产生参考点，其计算公式如下：

P_ref＝G(s)·P_img

根据上述结构，请参阅附图4-6所示，步骤A2中三阶微分器的传递函数为：

为使无人机在通过不同路径时提升效率并且产生的轨迹靠近原有的路径，预设速度的计算由以下规则确定：

当无人机允许的最高飞行速度为V_max，最慢飞行速度为V_min，由路径转弯引起的减速为V_turn，由到达终点引起的减速为V_end，其预设速度的计算如下：

V_pre＝V_max-V_turn-V_end，(V_min≤V_pre≤V_max) (4)

V_turn的计算公式如下：

其中，R_slow表示前瞻减速范围，R_s0和K_s为设定的常数，V_tra为跟踪速度，L_i为当前节点离下一路径节点的距离，n表示离当前轨迹参考点的路径距离大于R_slow的路径节点。θ_i表示各个节点的转折角度，θ_add表示在前瞻减速范围内，路径矢量角度的改变量；

V_end的计算公式如下：

其中，L_end表示当前轨迹参考点距离路径终点的轨迹距离，L_ss表示终点减速起始距离，当L_end小于该距离时开始减速，L_se表示终点减速结束距离，当L_end小于该距离时预设速度减至最低速度。

实施例2：

请参阅附图1所示：该实施例为一种面向多旋翼无人机的路径优化方法的轨迹跟踪控制方法，其利用上述的路径优化方法，来规划无人机的路径，所述轨迹跟踪控制方法包括以下步骤：

轨迹优化算法：根据各路径节点生成一条光滑连续的轨迹；

S1：利用轨迹***计算目标的加速度，其中，包括以下步骤：

S11：根据轨迹的二阶导数，计算轨迹前馈加速度；

S12：利用轨迹跟踪控制算法，根据参考轨迹与无人机的状态反馈，计算反馈加速度；

S13：利用风扰估计算法，根据模型加速度与实际加速度，计算外部扰动加速度；

S14：将上述计算的加速度进行结合，计算目标加速度和目标旋转矩阵；

进一步的，无人机轨迹跟踪控制器，通过计算轨迹的二次导数，计算前馈加速度，再通过状态反馈，计算反馈加速度，最后通过基于模型的风扰估计，估计外部扰动加速度，最后将三者叠加，计算目标加速度以及目标旋转矩阵；

S11中，计算轨迹前馈加速度的算法如下：

当轨迹输出的目标位置为P_Mx，P_My，P_Mz，输出的目标速度为V_Mx，V_My，V_Mz，输出的前馈加速度为A_FF＝[A_FFx A_FFy A_FFz]^T，并假设所输出的目标量都是相对于惯性坐标系的；

当所生成的参考轨迹由4个多阶连续的变量表示，如下所示：

则，前馈加速度的计算公式如下：

更进一步的，S12中，反馈加速度的算法如下：

令：

则，反馈加速度的计算公式如下：

A_FB＝[A_FBx A_FBy A_FBz]^T (8)

其中，ω_n为固有频率，固有频率越大，调节速度越快，ξ为阻尼比，取值在0.707-1之间，且阻尼比越小，收敛速度越快，但会略有超调。

S13中，外部扰动加速度的算法如下：

令水平方向的扰动加速度为A_Dist，定义由惯性坐标系到无人机机体坐标系的由欧拉角表示的转移矩阵得出：

其中，C表示COS()函数，S表示SIN()函数，扰动加速度的计算如下公式所示：

则，得出扰动加速度的计算，如下：

其中，a_nx，a_ny为惯性坐标系下传感器测得的加速度，其右边的项为所估计的输出加速度

S14中，目标加速度和目标旋转矩阵的算法如下：

a_M＝A_FF+A_FB-A_Dist (11)

目标旋转矩阵为则计算公式如下：

Z_B＝a_M (13)

其中，为目标偏航角度。

S2：底层控制器控制计算与控制输出，其中，利用姿态控制器和天向加速度控制器，计算出总升力和输出转矩；

这一步，根据上一步计算的目标旋转矩阵和目标天向加速度主要是对无人机的姿态与天向加速度进行闭环控制，计算输出的总升力u₁，和输出的控制转矩u₂，u₃，u₄。

基于四阶线性微分器的轨迹优化算法，其关键点在于预设速度的计算；

无人机轨迹***，其关键点在于外部扰动加速度的估计，其次，本文将无人机位置控制***简化为二阶***，可根据二阶***阻尼比和***的固有频率直接设定控制器的参数，简化调参。

且本发明所需计算能力消耗十分低，可以直接在单片机上运行，减少硬件成本，与目前基于多项式的轨迹优化算法相比，本发明所生成的轨迹光滑且十分稳定，不会发散或偏离原路径太远，其所包含的轨迹跟踪算法能迅速估计外部扰动，相比积分反馈控制，本文的状态反馈与扰动补偿控制策略鲁棒性与抗扰能力更强。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本发明中未具体描述和解释说明的结构、装置以及操作方法，如无特别说明和限定，均按照本领域的常规手段进行实施。

Claims (2)

1.面向多旋翼无人机的路径优化方法，其特征在于，包括以下步骤：

路径规划和轨迹优化，其中，所述路径规划和所述轨迹优化分为以下算法：

路径规划算法：生成由离散点组成的安全路径；

轨迹优化算法：根据各路径节点生成一条光滑连续的轨迹；

所述路径规划算法中，设四阶线性微分器的传递函数为：

将上式写成状态空间方程形式，如下：

由此可知，四阶过渡过程输出的状态y存在四阶导数，用于无人机的参考轨迹；

所述轨迹优化算法中，路径节点生成的具体步骤如下：

A1：假设一条由n个路径节点组成的路径，其中，第一个路径节点是无人机本身的位置，根据该路径计算预设速度V_pre；

A2：三阶微分器跟踪预设速度产生跟踪速度V_tra；

A3：令一个虚拟点从起点开始，以一定的速度沿着路径点的连线一直移动到终点，该虚拟点的位置作为四阶线性微分器的输入，其输出为轨迹参考点的位置，该虚拟点沿着路径移动，其计算公式如下：

P_img＝P_img+VECT_path·V_track

A4：四阶微分器跟踪虚拟点产生参考点，其计算公式如下：

P_ref＝G(s)·P_img

所述步骤A2中三阶微分器的传递函数为：

其中，预设速度的计算由以下规则确定：

设无人机允许的最高飞行速度为V_max，最慢飞行速度为V_min，由路径转弯引起的减速为V_turn，由到达终点引起的减速为V_end，其预设速度的计算如下：

V_pre＝V_max-V_turn-V_end,(V_min≤V_pre≤V_max) (4)

V_turn的计算公式如下：

其中，R_slow表示前瞻减速范围，R_s0和K_s为设定的常数，V_tra为跟踪速度，L_i为当前节点离下一路径节点的距离，n表示离当前轨迹参考点的路径距离大于R_slow的路径节点，θ_i表示各个节点的转折角度，θ_add表示在前瞻减速范围内，路径矢量角度的改变量；

V_end的计算公式如下：

其中，L_end表示当前轨迹参考点距离路径终点的轨迹距离，L_ss表示终点减速起始距离，当L_end小于该距离时开始减速，L_se表示终点减速结束距离，当L_end小于该距离时预设速度减至最低速度。

2.一种面向多旋翼无人机的路径优化方法的轨迹跟踪控制方法，其特征在于：该轨迹跟踪控制方法利用如权利要求1所述的路径优化方法规划无人机的路径，所述轨迹跟踪控制方法包括以下步骤：

S1：利用轨迹***计算目标的加速度，其中，包括以下步骤：

S11：根据轨迹的二阶导数，计算轨迹前馈加速度；

S12：利用轨迹跟踪控制算法，根据参考轨迹与无人机的状态反馈，计算反馈加速度；

S13：利用风扰估计算法，根据模型加速度与实际加速度，计算外部扰动加速度；

S14：将上述计算的加速度进行结合，计算目标加速度和目标旋转矩阵；

S2：底层控制器控制计算与控制输出，其中，利用姿态控制器和天向加速度控制器，计算出总升力和输出转矩；

所述S11中，计算轨迹前馈加速度的算法如下：

令轨迹输出的目标位置为P_Mx,P_My,P_Mz，输出的目标速度为V_Mx,V_My,V_Mz，输出的前馈加速度为A_FF＝[A_FFx A_FFy A_FFz]^T，所输出的目标量都是相对于惯性坐标系的；

设所生成的参考轨迹由4个多阶连续的变量表示，如下所示：

则，前馈加速度的计算公式如下：

所述S12中，反馈加速度的算法如下：

令：

A_FB＝[A_FBx A_FBy A_FBz]^T (8)

其中，ω_n为固有频率，固有频率越大，调节速度越快，ξ为阻尼比，取值在0.707-1之间；

所述S13中，所述外部扰动加速度的算法如下：

令水平方向的扰动加速度为A_Dist，定义由惯性坐标系到无人机机体坐标系的由欧拉角表示的转移矩阵得出：

其中，C表示COS()函数，S表示SIN()函数，扰动加速度的计算如下公式所示：

其中，a_ns，a_ny为惯性坐标系下传感器测得的加速度，其右边的项为所估计的输出加速度；

所述S14中，目标加速度和目标旋转矩阵的算法如下：

a_M＝A_FF+A_FB-A_Dist (11)

设目标旋转矩阵为则计算公式如下：

Z_B＝a_M (13)

其中，为目标偏航角度。