CN112947572A - 一种基于地形跟随的四旋翼飞行器自适应运动规划方法 - Google Patents

Abstract

本发明公开了一种基于地形跟随的四旋翼飞行器自适应运动规划方法，包括步骤：利用激光雷达来获取地形信息；建立四旋翼飞行器的模型和运动方程；采用高斯滤波算法进行轨迹规划，获得四旋翼飞行器的飞行轨迹；确定约束条件和目标函数，建立MPC控制器；四旋翼飞行器通过MPC控制器来跟踪飞行轨迹。本发明将单光束激光雷达附加到四旋翼飞行器的基座上，通过激光雷达将下方和前方地形的起伏信息发送回四旋翼飞行器。利用该激光反馈的信息，运动规划算法可创建一条平滑的轨迹，使四旋翼飞行控制器能够精确跟踪轨迹和跟随地形，并通过避免突然的轨迹校正而导致的能量消耗来使飞行时间最大化，进一步促进了无人机在民用方面的发展。

Description

一种基于地形跟随的四旋翼飞行器自适应运动规划方法

技术领域

本发明涉及无人机的地形跟随和运动规划的技术领域，尤其涉及一种基于地形跟随的四旋翼飞行器自适应运动规划方法。

背景技术

近年来，带传感器的四旋翼飞行器的使用允许无人机可以在复杂的环境中更加自主地飞行，这是对该行业产生的最大影响之一。计算机视觉研究者也从无人机性能的这些进步中受益，因为他们的算法现在可以在诸如跟踪或避开障碍物等活动中进行测试(R.He,A.Bachrach,M.Achtelik,A.Geramifard,D.Gurdan,S.Prentice,et al.,On the designand use of a micro air vehicle to track and avoid adversaries,”TheInternational Journal of Robotics Research,vol.29,pp.529-546,2010.)。然而，研究人员仍然面临一个挑战：如何在考虑到环境限制的情况下实现各种海拔高度的精确导航(H.Bouadi,M.Bouchoucha,and M.Tadjine,”Sliding mode control based onbackstepping approach for an UAV type-quadrotor,”World Academy of Science,Engineering and Technology,vol.26,pp.22-27,2007)。为了使这种复杂的情况成为可能，研究人员不仅需要制定精确的运动规划算法和可靠的控制方法，还需要获取更全面的环境数据。无人机固有的不稳定系统意味着迄今为止，为地面车辆自动驾驶而开发的大多数成功的运动规划技术并未在空中的飞行器上取得良好的效果(Hoffmann,Gabriel,Steven Waslander,and Claire Tomlin”Quadrotor helicopter trajectory trackingcontrol.”AIAA guidance,navigation and control conference and exhibit.2008)。但是，多年来，研究人员在无人机运动规划和轨迹跟踪方面取得了重大突破，其中许多成果已经在自主飞行、建图和监控这些方面中得到了应用。然而，在面对复杂环境的情况下，例如在应对室内飞行或偏远或复杂的地形时，现有方法仍存在缺陷。向自主飞行迈出成功的一步是：添加一种传感器，例如一个相机，到无人机上以及写出自动生成轨迹的算法。这些改进的方法虽然提高了四旋翼飞行器的独立性，但也产生了新的问题，即有效载荷限制和冗长的计算过程。

发明内容

本发明的目的在于克服现有技术的缺点和不足，提供一种基于地形跟随的四旋翼飞行器自适应运动规划方法，该方法可以通过避免突然的轨迹校正而导致的能量消耗来使飞行时间最大化，进一步促进了无人机在民用方面的发展。

为实现上述目的，本发明提供一种基于地形跟随的四旋翼飞行器自适应运动规划方法，包括以下步骤：

利用激光雷达来获取地形信息；

建立四旋翼飞行器的模型和运动方程；

采用高斯滤波算法进行轨迹规划，获得四旋翼飞行器的飞行轨迹；

确定约束条件和目标函数，建立MPC控制器；

四旋翼飞行器通过MPC控制器来跟踪飞行轨迹。

进一步地，所述利用激光雷达来获取地形信息中，所述地形信息是指地面呈现出的高低起伏的各种状态。

进一步地，所述利用激光雷达来获取地形信息中，通过激光雷达测量激光发出和收到回波的时间差，并利用激光测距原理计算来得到目标的方位和距离。

进一步地，所述建立四旋翼飞行器的模型和运动方程，包括以下步骤：

四旋翼飞行器的控制输入是通过电机产生的，机身总体的受力和三个转矩[τ_φ τ_θτ_ψ]与电机输入的关系如下所示：

式中，U为机身总体的受力，Ω₁、Ω₂、Ω₃、Ω₄分别表示四个电机的输入，b和d表示相关系数；

地轴坐标系用于描述三个轴[X Y Z]上的线性位置的参考点，四旋翼飞行器的线性位置是一个向量Γ^E，地轴坐标系还可以用作欧拉角Θ^E的局部坐标系，这些角分别是横滚角φ，俯仰角θ，偏航角ψ，机体坐标系是附着在四旋翼飞行器重心上的移动参考点，用于描述线速度V^B和角速度ω^B，四旋翼飞行器的六个自由度描述如下所示：

式中，

和v[V^B ω^B]分别表示应用在惯性坐标系和机体坐标系中的速度矢量，J_Θ是一个包含旋转和平移的矩阵，通过该矩阵可以将在机体坐标系中四旋翼飞行器受到的力和转矩在惯性坐标系中表示，并可以将在惯性坐标系中四旋翼飞行器受到的力和转矩在机体坐标系中表示；

四旋翼飞行器的动力学考虑了机体的质量m及其惯性矩阵I，采用牛顿-欧拉方程来表示四旋翼飞行器的运动方程，如下所示：

式中，[I_x I_y I_z]为每个轴的惯性矩阵，长度L是从螺旋桨中心到飞机重心的水平距离。

分别表示x方向的加速度，y方向的加速度，z方向的加速度，横滚方向的角加速度，俯仰方向的角加速度，偏航方向的角加速度，俯仰方向的角速度，偏航方向的角速度，横滚方向的角速度。

进一步地，所述采用高斯滤波算法进行轨迹规划，获得四旋翼飞行器的飞行轨迹，具体包括：

在当前目标点的附近选取多个数据点，其中也包括当前目标点；

采用二维的高斯函数计算每一个数据点的权重，然后将权重归一化，分别得到每个当前目标点的实际坐标；

根据每个当前目标点的实际坐标得到飞行轨迹。

进一步地，所述高斯函数为如下：

式中，x，y分别表示当前目标点的横坐标和纵坐标，x_i，y_i分别表示第i个点的横坐标和纵坐标，σ表示高斯滤波窗口的宽度。

进一步地，每一个数据点的权重的计算公式如下：

j＝0，1……n，n为选取的数据点数量，i是当前的目标点。

进一步地，在当前目标点的两侧选取数量相等的数据点，若当前目标点的某一侧没有足够的数据点，则将另一侧的目标点对称复制过来。

进一步地，所述确定约束条件和目标函数，建立MPC控制器，包括：

建立预测模型；

采用优化目标函数对预测模型进行优化，得到控制时域内的控制序列，所述优化目标函数如下：

式中，x表示状态矢量，N表示模型时域长度，T表示矩阵的转置，P表示终端状态权重矩阵，Q表示状态权重矩阵，R表示控制权重矩阵，u表示控制输入；x_N、U_k分别表示在N时刻的状态，在k时刻的输入；

确定约束条件为：u_min≤u≤u_max，u_min和u_max分别表示输入的下边界和上边界；

最终建立得到的MPC控制器为：

minJ(t)

x₀＝x(t)

u_min≤u≤u_max。

进一步地，所述建立预测模型中，采用状态方程作为预测模型，即

其中，

表示状态矢量x的一阶导数，x表示状态矢量，u表示控制输入，x₀＝x(t)是初始状态。

本发明与现有技术相比，能够实现的有益效果至少如下：

1、本发明将单光束激光雷达附加到四旋翼飞行器的基座上，通过激光雷达将下方和前方地形的起伏信息发送回四旋翼飞行器。利用该激光反馈的信息，运动规划算法可创建一条平滑的轨迹，使四旋翼飞行控制器能够精确跟踪轨迹和跟随地形，并通过避免突然的轨迹校正而导致的能量消耗来使飞行时间最大化，进一步促进了无人机在民用方面的发展。

2、本发明可以使四旋翼飞行器在变化的地形中仍能与地面保持一致的距离，并通过避免突然的轨迹校正而导致的能量消耗来使飞行时间最大化，进一步促进了无人机在民用方面的发展。

附图说明

图1为本发明逻辑流程示意图。

图2为四旋翼飞行器跟随地形示意图。

图3为高斯滤波算法和三次样条算法在激光束角度为45度时的轨迹图像。

图4为不同激光束角度时的轨迹图像示意图。

图5为PID、LQR、MPC在激光束角度为45度时的控制响应图像。

图6为PID、LQR、MPC控制输入的图像示意图。

具体实施方式

下面结合具体实施例对本发明作进一步说明。

本发明提供的一种基于地形跟随的四旋翼飞行器自适应运动规划方法，包括以下步骤：

步骤1：利用激光雷达来获取地形信息。

在本发明的其中一个实施例中，地形信息是指地面呈现出的高低起伏的各种状态。

在本发明的其中一个实施例中，能够通过激光雷达测量激光发出和收到回波的时间差，并利用激光测距原理计算来得到目标的方位和距离。

激光雷达的激光束角度为15度到75度之间。在本发明的其中一个实施例中，激光雷达的激光束角度为45度，设置成45度角可以使得激光雷达获取的地形信息更加全面。

根据图4不同激光束角度时的轨迹图像来看，激光束角度会影响地形跟随的误差，且激光束角度越大，地形跟随的误差越小。因此可以将激光束角度设置为比较大的值，从而减小地形跟随的误差。

步骤2：建立四旋翼飞行器的模型和运动方程。

在本发明的其中一个实施例中，本步骤具体包括以下子步骤：

步骤2.1：四旋翼飞行器的控制输入是通过电机产生的，机身总体的受力U和三个转矩[τ_φ τ_θ τ_ψ]与电机输入的关系如下所示：

式中，Ω₁、Ω₂、Ω₃、Ω₄分别表示四个电机的输入，b和d表示相关系数。

步骤2.2：地轴坐标系是用于描述三个轴[X Y Z]上的线性位置的参考点。四旋翼飞行器的线性位置是一个向量，并表示为Γ^E[m]。地轴坐标系还可以用作欧拉角Θ^E[rad]的局部坐标系，这些角分别是横滚角φ，俯仰角θ，偏航角ψ。机体坐标系是附着在四旋翼飞行器重心上的移动参考点，用于描述线速度V^B[m s^-1]和角速度ω^B[rad s^-1]。对四旋翼飞行器的六个自由度的一般描述如下所示：

式中，

和v[V^B ω^B]分别表示应用在惯性坐标系和机体坐标系中的速度矢量。J_Θ是一个包含旋转和平移的矩阵，通过该矩阵可以将在机体坐标系中四旋翼飞行器受到的力和转矩在惯性坐标系中表示；反过来，已知惯性坐标系中的速度矢量，则根据上式机体坐标系的速度矢量能够计算出来，那么也就可以将在惯性坐标系中四旋翼飞行器受到的力和转矩在机体坐标系中表示。

步骤2.3：四旋翼飞行器的动力学考虑了机体的质量m[kg]及其惯性矩阵I[N ms²]。可以用牛顿-欧拉方程来表示四旋翼飞行器的运动方程，如下所示：

陀螺仪效应对结果的影响很小，所以没包含在公式中。其他参数分别是由于重力(g)引起的加速度，以及每个轴的惯性矩阵[I_x I_y I_z]。另外，长度L[m]是从螺旋桨中心到飞机重心的水平距离。

分别表示x方向的加速度，y方向的加速度，z方向的加速度，横滚方向的角加速度，俯仰方向的角加速度，偏航方向的角加速度，俯仰方向的角速度，偏航方向的角速度，横滚方向的角速度。

步骤3：采用高斯滤波算法进行轨迹规划，获得四旋翼飞行器的平滑的飞行轨迹。

高斯滤波算法是一种用在机器人上的技术，尤其是在地形建模中。

步骤3.1：将高斯函数中的μ设为零，当u为零时，二维的高斯函数为

x，y分别表示当前点的横坐标和纵坐标；x_i，y_i分别表示第i个点的横坐标和纵坐标，σ表示高斯滤波窗口的宽度。

步骤3.2：在每个当前目标点附近选取紧邻的多个数据点。

在本发明的其中一个实施例中，在每个当前目标点附近选取紧邻的11个目标数据点，其中左边5个，右边5个，包含当前的目标点刚好11个，如果当前目标点的某一侧没有足够的数据点，则将另一侧的目标点对称复制过来。其中，以当前目标点为中点，然后在左右两侧选择数量相等的数据点，最后计算得到的实际目标点的坐标更接近选取的当前目标点，而且也能使规划的轨迹更加平滑。

在本发明的其中一个实施例中，当四旋翼飞行器上升和下降时，高斯滤波窗口的宽度分别为25和5，当然在其他实施例中也可以采用其他的宽度值。宽度值25和5是根据之前实验来得到的，选取这两个数值得到的轨迹比较平滑，而且得到的轨迹也不会发散。

步骤3.3：采用二维的高斯函数计算每一个数据点的权重，然后将权重归一化，得到当前目标点的实际坐标。

具体的，以步骤3.2中选取的数据点数量为例，有(x₀，y₀)，(x₁，y₁)……(x₁₀，y₁₀)11个点，然后运用二维的高斯函数公式计算权重

j＝0，1………10；i是11个点的中点，也就是当前目标点，此时i＝5。这样就可以算出每一个点的权重，然后将权重归一化，即

j＝0，1……10。最后就可以得到轨迹上当前目标点的实际坐标(x，y)，其中

步骤3.4：另外选取当前目标点，重复步骤3.2-3.4，得到多个当前目标点的实际坐标，根据所述多个当前目标点的实际坐标确定飞行轨迹。

在本发明的其中一个实施例中，假设需要十个当前目标点来确定飞行轨迹，那么就按上面的步骤执行10次，就能得到十个当前目标点的实际坐标，然后用这10个当前目标点的实际坐标去确定飞行轨迹。

为了验证本发明所采用的以高斯滤波算法进行轨迹规划的优越性，下面采用现有的三次样条算法来进行对比。

三次样条算法的基本思想是利用高级规划算法生成一系列可行的航点，然后基于在一定的平滑度准则下通过这些点建立轨迹规划。平滑度准则如下所示：

S_i(x)＝a_i+b_i(x-x_i)+c_i(x-x_i)²+d_i(x-x_i)³

S_i(x)表示三次多项式，a_i、b_i、c_i、d_i分别表示常数项系数、一次项系数、二次项系数、三次项系数，x表示位置节点，x_i表示i时刻的位置节点。

这些系数代表每个点上所有约束的唯一解。因此，确定这些系数是四旋翼飞行器产生平滑轨迹的关键步骤。

可以从轨迹的平滑度以及跟随地形的紧密程度这些方面来比较这两种轨迹规划算法。结果如图3所示，就地形跟随而言，高斯滤波算法的性能明显优于三次样条算法。

步骤4：确定约束条件和目标函数，建立MPC控制器。

MPC(模型预测控制)的主要目地是在考虑到约束的同时，在一定的时间间隔内最小化给定的目标函数。在每一个采样时刻通过求解一个有限时域的开环最优控制策略，是一种通过将过程的当前状态作为最优控制问题的初始状态，解得的最优控制序列只实施于下一时刻的控制方法。

在本发明其中的一个实施例中，本步骤具体包括如下子步骤：

步骤4.1：建立预测模型。

根据被控对象的历史信息和未来输入信息，预测系统的未来输出响应。该实施例中采用状态方程作为预测模型，即

其中

表示状态矢量x的一阶导数，x表示状态矢量，u表示控制输入。x₀＝x(t)是初始状态。

但对于模型预测的形式没有严格的限定，在其他的实施例中也可以采用传递函数以作为预测模型。

步骤4.2：滚动优化，通过某一性能指标的最优化求解未来有限时刻的最优控制率。实际上，系统的控制增量是未知的，只有通过设定合适的优化目标，并对其进行求解，才能得到控制时域内的控制序列。

在本发明的其中一个实施例中，优化目标函数如下所示：

式中，x表示状态矢量，N表示模型时域长度，T表示矩阵的转置，P表示终端状态权重矩阵，Q表示状态权重矩阵，R表示控制权重矩阵，u表示控制输入。x_N、U_k分别表示N时刻的状态、k时刻的控制输入。

在本发明的其中一个实施例中，优化目标函数中参数的数值如下所示：

步骤4.3：确定约束条件。

在实际控制系统中，往往需要满足系统状态量以及控制量的一些约束，本发明的其中一个实施例中的控制量约束为：u_min≤u≤u_max，u_min和u_max分别表示输入的下边界和上边界。

最终得到的MPC控制器的表达如下：

min J

x₀＝x(t)

u_min≤u≤u_max

步骤5：四旋翼飞行器通过MPC控制器来跟踪飞行轨迹。

先检验飞行器位置的实际输出，再通过实际输出对基于模型的预测输出进行修正并进行新的优化。在每个控制周期内完成对优化目标函数的求解后，得到了控制时域内的一系列控制输入增量

将该控制序列中的第一个元素作为实际的控制输入增量作用于系统，即：

系统执行这一控制量直到下一时刻。在新的时刻，系统根据状态信息重新预测下一段时域的输出，通过优化过程得到一个新的控制增量序列。如此循环，直到系统完成控制过程。

为了验证本发明所采用的MPC控制器的优越性，下面采用现有PID控制器、LQR控制器来进行对比。

将前面的高斯滤波算法应用在三种控制器上，通过观察四旋翼飞行器的响应来比较三种控制器的性能。

PID是一种经典的控制技术，已经在许多行业项目中被使用，并且已用于跟踪轨迹。PID的原理是，在识别出误差e(t)之后，PID控制器将使用一些策略，例如向系统添加或吸收能量，这些组合使系统具有可靠的响应和更好的性能。PID公式如下所示：

u(t)、K_p、K_i、K_d分别表示控制信号输出、比例系数、积分系数、微分系数。

LQR是无人机平台上使用的最佳控制技术之一。它基于最小化二次代价函数，然后将其反馈给线性系统。LQR非常适合需要将所有状态归零的系统。为了在跟踪任务中使用此控制方法，可以通过在LQR反馈回路中添加积分动作来修改LQR控制体系结构。为了设计LQR控制器，可以用下面的方程式表示四旋翼的动态系统：

式中A和B分别表示四旋翼飞行器的状态矩阵和输入矩阵，x、u、J分别表示状态矢量、控制输入、代价函数。然后，可以通过最小化控制输入来实现四旋翼飞行器的高性能。其中K[K K_i]是状态反馈增益，代价函数是一个标准，它将使控制输入最小化，Q是半正定状态控制矩阵，而R是正定输入控制矩阵。这些矩阵的重要性在于它们有助于分别权衡每个状态，设计人员可以根据权衡的结果选择哪些状态至关重要或不重要。LQR控制器中使用的加权矩阵如下所示：

如图5至图6所示，根据PID、LQR、MPC在激光束角度为45度时的控制响应图像以及它们的控制输入，可以看出，MPC控制器的控制响应明显优于现有的其他两种控制器。根据三种控制器的响应曲线观察可以得出MPC的响应是最佳的，因为它考虑了系统的约束。这三种控制器的响应在下降方面几乎相同，因为四旋翼飞行器没有负推力。根据每个控制器的控制输入值不同，可以看出所用控制器的类型会影响所消耗的能量。一方面，尽管PID在超出输入约束时也会试图完成任务，但它仍然是最简单的控制器。另一方面，虽然LQR和MPC的控制输入是在约束范围内，但是LQR具有较慢的响应和较高的超调。MPC提供最佳结果的原因是它旨在通过在系统范围内紧密合作来实现高性能。

本发明设置激光雷达来获取地形信息，可以在轨迹规划中及时关注地形的高低起伏变化，使得飞行器可以在变化的地形中仍能与地面保持一致的距离，在实际运行时当地形较高时，飞行器通过增加油门，使飞行器向上运动来维持与地面之间恒定的高度，当地形较低时，飞行器通过减少油门，使飞行器向下运动来维持与地面之间恒定的高度，而当地形平缓时，维持当前油门向前运动即可。

本发明中，当飞行器遇到障碍物时，原来的轨迹就会发生改变，而由于规划的轨迹是平滑的，所以飞行器的运动幅度不会太大，可以避免突然的轨迹校正而导致的能量消耗来使飞行时间最大化。

本发明采用激光雷达来测量无人机与地形之间的距离，根据测量的地形信息来创建最佳的无人机飞行轨迹。最后，根据输入约束，使用最优控制器来跟踪轨迹。在存在测量噪声和输入约束的情况下，通过仿真验证了该方法的有效性。

综上所述，在采用了以上方案后，本发明可以使四旋翼飞行器在变化的地形中仍能与地面保持一致的距离，并通过避免突然的轨迹校正而导致的能量消耗来使飞行时间最大化，进一步促进了无人机在民用方面的发展，具有实际推广价值，值得推广。

以上所述实施例只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (10)

1.一种基于地形跟随的四旋翼飞行器自适应运动规划方法，其特征在于，包括以下步骤：

利用激光雷达来获取地形信息；

建立四旋翼飞行器的模型和运动方程；

采用高斯滤波算法进行轨迹规划，获得四旋翼飞行器的飞行轨迹；

确定约束条件和目标函数，建立MPC控制器；

四旋翼飞行器通过MPC控制器来跟踪飞行轨迹。

2.根据权利要求1所述的一种基于地形跟随的四旋翼飞行器自适应运动规划方法，其特征在于：所述利用激光雷达来获取地形信息中，所述地形信息是指地面呈现出的高低起伏的各种状态。

3.根据权利要求1所述的一种基于地形跟随的四旋翼飞行器自适应运动规划方法，其特征在于，所述利用激光雷达来获取地形信息中，通过激光雷达测量激光发出和收到回波的时间差，并利用激光测距原理计算来得到目标的方位和距离。

4.根据权利要求1所述的一种基于地形跟随的四旋翼飞行器自适应运动规划方法，其特征在于，所述建立四旋翼飞行器的模型和运动方程，包括以下步骤：

四旋翼飞行器的控制输入是通过电机产生的，机身总体的受力和三个转矩[τ_φ τ_θ τ_ψ]与电机输入的关系如下所示：

式中，U为机身总体的受力，Ω₁、Ω₂、Ω₃、Ω₄分别表示四个电机的输入，b和d表示相关系数；

地轴坐标系用于描述三个轴[X Y Z]上的线性位置的参考点，四旋翼飞行器的线性位置是一个向量Γ^E，地轴坐标系还可以用作欧拉角Θ^E的局部坐标系，这些角分别是横滚角φ，俯仰角θ，偏航角ψ，机体坐标系是附着在四旋翼飞行器重心上的移动参考点，用于描述线速度V^B和角速度ω^B，四旋翼飞行器的六个自由度描述如下所示：

式中，

和υ[V^B ω^B]分别表示应用在惯性坐标系和机体坐标系中的速度矢量，J_Θ是一个包含旋转和平移的矩阵，通过该矩阵可以将在机体坐标系中四旋翼飞行器受到的力和转矩在惯性坐标系中表示，并可以将在惯性坐标系中四旋翼飞行器受到的力和转矩在机体坐标系中表示；

四旋翼飞行器的动力学考虑了机体的质量m及其惯性矩阵I，采用牛顿-欧拉方程来表示四旋翼飞行器的运动方程，如下所示：

式中，[I_x I_y I_z]为每个轴的惯性矩阵，长度L是从螺旋桨中心到飞机重心的水平距离，

分别表示x方向的加速度，y方向的加速度,z方向的加速度，横滚方向的角加速度，俯仰方向的角加速度，偏航方向的角加速度，俯仰方向的角速度，偏航方向的角速度，横滚方向的角速度。

5.根据权利要求1所述的一种基于地形跟随的四旋翼飞行器自适应运动规划方法，其特征在于，所述采用高斯滤波算法进行轨迹规划，获得四旋翼飞行器的飞行轨迹，具体包括：

在当前目标点的附近选取多个数据点；

采用二维的高斯函数计算当前目标点和每一个数据点的权重，然后将权重归一化，得到当前目标点的实际坐标；

重复前述步骤得到多个当前目标点的实际坐标，根据所有当前目标点的实际坐标得到飞行轨迹。

6.根据权利要求5所述的一种基于地形跟随的四旋翼飞行器自适应运动规划方法，其特征在于，所述高斯函数为如下：

式中，x,y分别表示当前目标点的横坐标和纵坐标，x_i，y_i分别表示第i个点的横坐标和纵坐标，σ表示高斯滤波窗口的宽度。

7.根据权利要求5所述的一种基于地形跟随的四旋翼飞行器自适应运动规划方法，其特征在于，每一个数据点的权重的计算公式如下：

j＝0,1……n，n为选取的数据点数量，i是当前的目标点。

8.根据权利要求5所述的一种基于地形跟随的四旋翼飞行器自适应运动规划方法，其特征在于，在当前目标点的两侧选取数量相等的数据点，若当前目标点的某一侧没有足够的数据点，则将另一侧的目标点对称复制过来。

9.根据权利要求1-8任一所述的一种基于地形跟随的四旋翼飞行器自适应运动规划方法，其特征在于，所述确定约束条件和目标函数，建立MPC控制器，包括：

建立预测模型；

采用优化目标函数对预测模型进行优化，得到控制时域内的控制序列，所述优化目标函数如下：

式中，x表示状态矢量，N表示模型时域长度，T表示矩阵的转置，P表示终端状态权重矩阵，Q表示状态权重矩阵，R表示控制权重矩阵，u表示控制输入；x_N、U_k分别表示在N时刻的状态，在k时刻的输入；

确定约束条件为：u_min≤u≤u_max，u_min和u_max分别表示输入的下边界和上边界；

最终建立得到的MPC控制器为：

minJ(t)

x₀＝x(t)

u_min≤u≤u_max。

10.根据权利要求9所述的一种基于地形跟随的四旋翼飞行器自适应运动规划方法，其特征在于，所述建立预测模型中，采用状态方程作为预测模型，即

其中，

表示状态矢量x的一阶导数，x表示状态矢量，u表示控制输入，x₀＝x(t)是初始状态。

Images