CN112015201A - 一种基于预测校正的四旋翼飞行器位置控制方法 - Google Patents

Abstract

本发明涉及一种基于预测校正的四旋翼飞行器位置控制方法，建立包含外部干扰的四旋翼飞行器位置环模型；将四旋翼飞行器的三轴直角坐标转化为虚拟经纬度坐标；利用虚拟经纬度坐标执行预测环节；利用预测信息，执行滚转角参考信号和俯仰角参考信号的校正环节；预测环节和校正环节循环执行，直到航程误差为0时，将校正环节得到的滚转角和俯仰角参考信号输出到实际系统模型进行控制；设计干扰观测器对四旋翼飞行器受到的复杂多变外部环境干扰进行估计；设计PD控制器，对高度进行控制；将观测到的干扰估计值通过前馈通道进行抵消，并与控制律进行复合。本发明具有环境适应性强、工程实用简单、抗干扰能力强等优点，适用于四旋翼飞行器高精度位置控制。

Description

一种基于预测校正的四旋翼飞行器位置控制方法

技术领域

本发明涉及一种基于预测校正的四旋翼飞行器位置控制方法，可以解决未知环境下执行复杂任务的四旋翼飞行器高精度位置控制问题，属于无人机飞行控制应用技术领域。

背景技术

四旋翼飞行器因其体积小、机动性高等优点，可以在狭小空间内完成复杂任务，在军事、民用和科学研究等诸多方面都有十分广泛的应用，如电力巡检、森林防火等任务。

四旋翼飞行器高精度位置控制在未知的环境中执行复杂任务时仍面临巨大的挑战。首先，执行任务的四旋翼飞行器处于未知环境中，仅能通过有限的传感器获得部分环境信息，亟需通过更加智能的控制算法提高系统对环境的适应性；其次，由于未知环境下的外部干扰复杂多变，会对四旋翼飞行器产生扰动，严重影响控制精度和系统稳定性。这些来自环境的不确定性和外部干扰会给四旋翼飞行器位置控制造成严重影响，使之无法完成既定任务，因此迫切需要提高四旋翼飞行器未知环境下的智能适应性和对干扰的处理能力。

目前，针对四旋翼飞行器位置控制的问题，国内外专家学者提出很多方法，其中经典PID控制方法利用位置误差结合三维方向的力进行控制，传统预测校正方法因其设计简单且对环境适应性高等优点，在实际工程中得到了广泛的应用。专利申请号201810555651.5中提出一种基于最小方差调节器的四旋翼无人机精确位置控制方法，但该专利未考虑复杂环境对四旋翼带来的影响；授权专利号201310685321.5、201410125842.X、201510611464.0所提出的预测校正方法适用于深空探测飞行器，如火星探测器等，与本方法在系统模型上、设计思路上都存在较大差异。综上所述，现有方法无法解决在未知环境下执行复杂任务的四旋翼飞行器智能适应性不足和抗干扰差的问题。

发明内容

本发明的技术解决问题是：针对四旋翼飞行器高精度位置控制在未知的环境中执行复杂任务受到外界干扰等多种因素带来的影响，提供一种基于预测校正的四旋翼飞行器位置控制方法，解决未知环境下执行复杂任务的四旋翼飞行器智能适应性不足和干扰的处理能力不强的问题，提高系统的智能适应性，具有工程实用性强、抗干扰能力强的优点。

本发明及技术解决方案为：一种基于预测校正的四旋翼飞行器位置控制方法，其实现步骤如下：第一步，建立含有外部干扰的四旋翼飞行器位置环模型；

第二步，将四旋翼飞行器的三轴直角坐标转化为虚拟经纬度坐标；

第三步，利用第二步得到的虚拟经纬度坐标和第一步中四旋翼飞行器位置环模型，执行预测校正控制器中的预测环节，得到预测航程信息，并根据待飞航程得到航程误差；

第四步，结合第三步的预测航程信息，执行预测校正控制器中滚转角参考信号φ和俯仰角参考信号θ的校正环节，得到滚转角参考信号和俯仰角参考信号；

第五步，当预测航程误差不等于0时，将第四步中滚转角参考信号φ的校正环节和俯仰角参考信号θ的校正环节得到的滚转角参考信号和俯仰角参考信号返回到预测环节，不断循环，直到航程误差为0时，将校正环节得到的滚转角参考信号和俯仰角参考信号输出到四旋翼飞行器位置环模型进行控制；

第六步，设计干扰观测器，对四旋翼飞行器受到复杂多变的外部环境干扰进行估计，得到干扰估计值，所述复杂多变外部环境干扰包括强度和频率变化的阵风、阵雨；

第七步，设计PD控制器，得到竖直方向控制律，对高度进行控制；

第八步，将第六步中得到的干扰估计值通过前馈通道进行抵消，并与第七步中设计的竖直方向控制律进行复合，得到复合控制律，实现基于预测校正的四旋翼飞行器位置控制。

具体如下：

第一步，建立含有外部干扰的四旋翼飞行器位置环模型如下：

其中，x表示四旋翼飞行器在X轴方向的位置，

表示在X轴方向的速度即v_x，y表示四旋翼飞行器在Y轴方向的位置，

表示在Y轴方向的速度即v_y，z表示四旋翼飞行器在Z轴方向的位置，

表示在Z轴方向的速度即v_z，

表示X轴方向的加速度，b₁表示四旋翼质量的倒数即

ψ表示偏航角，θ表示俯仰角参考信号，φ表示滚转角参考信号，c_ψ表示偏航角ψ的余弦函数cos(ψ)，s_θ表示俯仰角参考信号θ的正弦函数sin(θ)，c_φ表示滚转角参考信号φ的余弦函数cos(φ)，s_ψ表示偏航角ψ的正弦函数sin(ψ)，s_φ表示滚转角参考信号φ的正弦函数sin(φ)，u₁表示四旋翼飞行器的总升力，d_x表示由于外部环境引起的干扰力在X轴方向的分量，

表示Y轴方向的加速度，d_y表示由于外部环境引起的干扰力在Y轴方向的分量，

表示Z轴方向的加速度，c_θ表示俯仰角参考信号θ的余弦函数cos(θ)，d_z表示由于外部环境引起的干扰力在Z轴方向的分量，g表示地球重力加速度。沿X轴、Y轴和Z轴的三轴速度动态方程可简写为：

其中，

表示质量矩阵，

表示速度对时间的一阶导数矩阵，

表示简便定义的符号，

表示干扰矩阵，

表示简便定义的符号。

第二步，将四旋翼飞行器的三轴直角坐标转化为虚拟经纬度坐标。设四旋翼飞行器直角坐标为P(X_p,Y_p,Z_p)，得到P点距离坐标原点的距离为

则P点到Z轴的距离为

根据经纬度得到P点的虚拟纬度

且

设XOZ平面和虚拟经纬度坐标中球面相交位置为经度0度，则P点所在竖直平面和XOZ平面的夹角即为P点的虚拟经度，即

其中，当X_p＞0,Y_p＝0时,α＝0；当X_p＞0,Y_p＞0时,

当X_p＝0,Y_p＞0时,

当X_p＜0,Y_p＞0时,

当X_p＜0,Y_p＝0时,α＝π；当X_p＜0,Y_p＜0时,

当X_p＝0,Y_p＜0时,

当X_p＞0,Y_p＜0时,

得到P点的虚拟经纬度坐标(α,β)。

第三步，利用第二步得到的虚拟经纬度坐标和第一步中四旋翼飞行器位置环模型，执行预测校正控制器中的预测环节：

其中，s表示航程，是指四旋翼飞行器质心与坐标原点连线在虚拟经纬度坐标中球面上的交点走过的距离，

表示航程对应的角度，v表示四旋翼飞行器的飞行速度。预测航程通过上式积分可表示为：

待飞航程S_togo表示当前位置到目标位置的距离，可表示为：

S_togo＝arccos[sinβ_c sinβ_s+cosβ_c cosβ_s cos(α_c-α_s)]

其中，α_c表示当前位置的经度，α_s表示目标位置的经度，β_c表示当前位置的纬度，β_s表示目标位置的纬度。此时，航程误差可表示为：

ΔS＝S_togo-S_p

第四步，结合第三步的预测信息，执行预测校正控制器中滚转角参考信号φ和俯仰角参考信号θ的校正环节，其中滚转角参考信号φ的迭代公式为：

其中，φ_n+1为本次计算得到的最新滚转角参考信号，φ_n和φ_n-1分别表示前两次计算得到的滚转角参考信号，λ_k表示用于调节更新快慢的参数，ΔS(φ_n)表示滚转角参考信号为φ_n时的航程误差，ΔS(φ_n-1)表示滚转角参考信号为φ_n-1时的航程误差。俯仰角参考信号θ的迭代公式为：

其中，θ_n+1为本次计算得到的最新俯仰角参考信号，θ_n和θ_n-1分别表示前两次计算得到的俯仰角参考信号，ΔS(θ_n)表示俯仰角参考信号为θ_n时的航程误差，ΔS(θ_n-1)表示俯仰角参考信号为θ_n-1时的航程误差。

第五步，预测环节和校正环节循环执行，直到航程误差为0时，将校正环节得到的滚转角参考信号和俯仰角参考信号输出到实际系统进行控制。

第六步，设计干扰观测器对四旋翼飞行器受到的复杂多变外部环境干扰，如强度和频率未知的阵风、阵雨等，得到干扰估计值：

其中，

表示外部环境干扰的估计值，z₁为观测器辅助变量，

为辅助变量z₁的一阶导数，

为待设计变量，

为观测器增益。

第七步，设计PD控制器，得到竖直方向控制律，对高度进行控制。

其中，e_z＝z-z_b表示Z轴方向的坐标误差，z_b表示期望的Z轴坐标，

表示e_z的一阶导数，a_z表示Z轴方向加速度，K_p和K_d表示控制增益。计算可得，竖直方向控制律为：

第八步，将第六步中观测到的干扰估计值通过前馈通道进行抵消，并与第七步中设计的竖直方向控制律进行复合，得到复合控制律，实现基于预测校正的四旋翼飞行器位置控制：

其中，

表示复合控制律。

本发明与现有技术相比的优点在于：本发明针对传统预测校正单一控制量在未知环境下智能适应性差，抗干扰能力不强的特点，设计用于四旋翼飞行器的预测校正控制器，提高可控性和智能性，并利用干扰观测器对外部环境干扰进行估计和补偿，构造出一种基于预测校正的四旋翼飞行器位置控制方法，使得四旋翼飞行器具有智能实时性强、抗干扰能力强的特点，从而保证四旋翼飞行器能够在复杂多变的环境中智能可靠地完成任务。

附图说明

图1为本发明方法的设计流程图。

具体实施方式

下面结合附图及实施例对本发明进行详细说明。

如图1所示，本发明所述的一种基于预测校正的四旋翼飞行器位置控制方法设计步骤为：首先，建立含有外部干扰的四旋翼飞行器位置环模型；其次，将四旋翼飞行器的三轴直角坐标转化为虚拟经纬度坐标；再次，利用虚拟经纬度坐标和四旋翼飞行器位置环模型，执行预测校正控制器中的预测环节；然后，结合预测信息，执行预测校正控制器中滚转角参考信号φ和俯仰角参考信号θ的校正环节；然后，预测环节和校正环节循环执行，直到航程误差为0时，将校正环节得到的滚转角参考信号和俯仰角参考信号输出到实际系统进行控制；然后，设计干扰观测器对四旋翼飞行器受到的复杂多变外部环境干扰进行估计；然后，设计PD控制器，对高度进行控制；最后，将干扰估计值通过前馈通道进行抵消，并与竖直方向控制律进行复合。

具体实施步骤如下：

第一步，建立含有外部干扰的四旋翼飞行器位置环模型如下：

其中，x表示四旋翼飞行器在X轴方向的位置，初值取值为-10m，

表示在X轴方向的速度即v_x，初值取值为0m/s，y表示四旋翼飞行器在Y轴方向的位置，初值取值为-10m，

表示在Y轴方向的速度即v_y，初值取值为0m/s，z表示四旋翼飞行器在Z轴方向的位置，初值取值为10m，

表示在Z轴方向的速度即v_z，初值取值为0m/s，

表示X轴方向的加速度，b₁表示四旋翼质量的倒数即

质量m的取值为1.121kg，ψ表示偏航角，初值取值为0rad，θ表示俯仰角参考信号，初值取值为0rad，φ表示滚转角参考信号，初值取值为0rad，c_ψ表示偏航角ψ的余弦函数cos(ψ)，s_θ表示俯仰角参考信号θ的正弦函数sin(θ)，c_φ表示滚转角参考信号φ的余弦函数cos(φ)，s_ψ表示偏航角ψ的正弦函数sin(ψ)，s_φ表示滚转角参考信号φ的正弦函数sin(φ)，u₁表示四旋翼飞行器的总升力，d_x表示由于外部环境引起的干扰力在X轴方向的分量，

表示Y轴方向的加速度，d_y表示由于外部环境引起的干扰力在Y轴方向的分量，

表示Z轴方向的加速度，c_θ表示俯仰角参考信号θ的余弦函数cos(θ)，d_z表示由于外部环境引起的干扰力在Z轴方向的分量，g表示地球重力加速度。沿X轴、Y轴和Z轴的三轴速度动态方程可简写为：

其中，

表示质量矩阵，

表示速度对时间的一阶导数矩阵，

表示简便定义的符号，

表示干扰矩阵，

表示简便定义的符号。

第二步，将四旋翼飞行器的三轴直角坐标转化为虚拟经纬度坐标。设四旋翼飞行器直角坐标为P(X_p,Y_p,Z_p)，得到P点距离坐标原点的距离为

则P点到Z轴的距离为

根据经纬度得到P点的虚拟纬度

且

设XOZ平面和虚拟经纬度坐标中球面相交位置为经度0度，则P点所在竖直平面和XOZ平面的夹角即为P点的虚拟经度，即

其中，当X_p＞0,Y_p＝0时,α＝0；当X_p＞0,Y_p＞0时,

当X_p＝0,Y_p＞0时,

当X_p＜0,Y_p＞0时,

当X_p＜0,Y_p＝0时,α＝π；当X_p＜0,Y_p＜0时,

当X_p＝0,Y_p＜0时,

当X_p＞0,Y_p＜0时,

得到P点的虚拟经纬度坐标(α,β)。结合第一步中的初始坐标信息可得初始位置的虚拟经纬度坐标为(0.79,0.62)，根据目标点直角坐标(50,20,30)可得目标位置的虚拟经纬度坐标为(0.38,0.51)。

第三步，利用第二步得到的虚拟经纬度坐标和第一步中四旋翼飞行器位置环模型，执行预测校正控制器中的预测环节：

其中，航程s是指四旋翼飞行器质心与坐标原点连线在虚拟经纬度坐标中球面上的交点走过的距离，

表示航程对应的角度，v表示四旋翼飞行器的飞行速度。预测航程通过上式积分可表示为：

待飞航程S_togo表示当前位置到目标位置的距离，可表示为：

S_togo＝arccos[sinβ_c sinβ_s +cosβ_c cosβ_s cos(α_c-α_s)]

其中，α_c表示当前位置的经度，α_s表示目标位置的经度，β_c表示当前位置的纬度，β_s表示目标位置的纬度。此时，航程误差可表示为：

ΔS＝S_togo-S_p

第四步，结合第三步的预测信息，执行预测校正控制器中滚转角参考信号φ和俯仰角参考信号θ的校正环节，其中滚转角参考信号φ的迭代公式为：

其中，φ_n+1为本次计算得到的最新滚转角参考信号，φ_n和φ_n-1分别表示前两次计算得到的滚转角参考信号，λ_k表示用于调节更新快慢的参数，取值为1，ΔS(φ_n)表示滚转角参考信号为φ_n时的航程误差，ΔS(φ_n-1)表示滚转角参考信号为φ_n-1时的航程误差。俯仰角参考信号θ的迭代公式为：

其中，θ_n+1为本次计算得到的最新俯仰角参考信号，θ_n和θ_n-1分别表示前两次计算得到的俯仰角参考信号，ΔS(θ_n)表示俯仰角参考信号为θ_n时的航程误差，ΔS(θ_n-1)表示俯仰角参考信号为θ_n-1时的航程误差。

第五步，预测环节和校正环节循环执行，直到航程误差为0时，将校正环节得到的滚转角参考信号和俯仰角参考信号输出到实际系统进行控制。

第六步，设计干扰观测器对四旋翼飞行器受到的外部干扰进行估计：

其中，

表示外部环境干扰的估计值，z₁为观测器辅助变量，

为辅助变量z₁的一阶导数，

为待设计变量，

为观测器增益。令干扰估计误差为

则估计误差动态方程为：

因此，只需设计一个合适的观测器增益

使得估计误差e_d收敛。令

选取

其中c₁为调节参数，取值为0.4，则

第七步，设计PD控制器，得到竖直方向控制律，对高度进行控制。

其中，e_z＝z-z_b表示Z轴方向的坐标误差，z_b表示期望的Z轴坐标，

表示e_z的一阶导数，a_z表示Z轴方向加速度，K_p和K_d表示控制增益，取值为0.5和1.5。计算可得，竖直方向控制律为：

第八步，将第六步中观测到的干扰估计值通过前馈通道进行抵消，并与第八步中设计的竖直方向控制律进行复合，得到复合控制律，实现基于预测校正的四旋翼位置控制：

其中，

表示复合控制律。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (8)

1.一种基于预测校正的四旋翼飞行器位置控制方法，其特征在于，包括以下步骤：

第一步，建立含有外部干扰的四旋翼飞行器位置环模型；

第二步，将四旋翼飞行器的三轴直角坐标转化为虚拟经纬度坐标；

第三步，利用第二步得到的虚拟经纬度坐标和第一步中四旋翼飞行器位置环模型，执行预测校正控制器中的预测环节，得到预测航程信息，并根据待飞航程得到航程误差；

第四步，结合第三步的预测航程信息，执行预测校正控制器中滚转角参考信号φ和俯仰角参考信号θ的校正环节，得到滚转角参考信号和俯仰角参考信号；

第五步，当预测航程误差不等于0时，将第四步中滚转角参考信号φ的校正环节和俯仰角参考信号θ的校正环节得到的滚转角参考信号和俯仰角参考信号返回到预测环节，再次进行预测，预测结束后，再次对航程误差进行判断，这样不断循环，直到航程误差为0时，将校正环节得到的滚转角参考信号和俯仰角参考信号输出到四旋翼飞行器位置环模型进行控制；

第六步，设计干扰观测器，对四旋翼飞行器受到复杂多变的外部环境干扰进行估计，得到干扰估计值，所述复杂多变外部环境干扰包括强度和频率变化的阵风、阵雨；

第七步，设计PD控制器，得到竖直方向控制律，对高度进行控制；

第八步，将第六步中得到的干扰估计值通过前馈通道进行抵消，并与第七步中设计的竖直方向控制律进行复合，得到复合控制律，实现基于预测校正的四旋翼飞行器位置控制。

2.根据权利要求1所述的一种基于预测校正的四旋翼飞行器位置控制方法，其特征在于：所述第一步中，建立含有外部干扰的四旋翼飞行器位置环模型如下：

其中，x表示四旋翼飞行器在X轴方向的位置，

表示在X轴方向的速度即v_x，y表示四旋翼飞行器在Y轴方向的位置，

表示在Y轴方向的速度即v_y，z表示四旋翼飞行器在Z轴方向的位置，

表示在Z轴方向的速度即v_z，

表示X轴方向的加速度，b₁表示四旋翼质量的倒数即

ψ表示偏航角，θ表示俯仰角参考信号，φ表示滚转角参考信号，c_ψ表示偏航角ψ的余弦函数cos(ψ)，s_θ表示俯仰角参考信号θ的正弦函数sin(θ)，c_φ表示滚转角参考信号φ的余弦函数cos(φ)，s_ψ表示偏航角ψ的正弦函数sin(ψ)，s_φ表示滚转角参考信号φ的正弦函数sin(φ)，u₁表示四旋翼飞行器的总升力，d_x表示由于外部环境引起的干扰力在X轴方向的分量，

表示Y轴方向的加速度，d_y表示由于外部环境引起的干扰力在Y轴方向的分量，

表示Z轴方向的加速度，c_θ表示俯仰角参考信号θ的余弦函数cos(θ)，d_z表示由于外部环境引起的干扰力在Z轴方向的分量，g表示地球重力加速度；沿X轴、Y轴和Z轴的三轴速度动态方程简写为：

其中，

表示质量矩阵，

表示速度对时间的一阶导数矩阵，

表示简便定义的符号，

表示干扰矩阵，

表示简便定义的符号。

3.根据权利要求1所述的一种基于预测校正的四旋翼飞行器位置控制方法，其特征在于：所述第二步中，将四旋翼飞行器的三轴直角坐标转化为虚拟经纬度坐标，具体为：

设四旋翼飞行器直角坐标为P(X_p,Y_p,Z_p)，X_p，Y_p和Z_p表示P点的三轴坐标，得到P点距离坐标原点的距离为

则P点到Z轴的距离为

根据经纬度得到P点的虚拟纬度

且

设XOZ平面和虚拟经纬度坐标中球面相交位置为经度0度，则P点所在竖直平面和XOZ平面的夹角即为P点的虚拟经度，即

其中，当X_p＞0,Y_p＝0时,α＝0；当X_p＞0,Y_p＞0时,

当X_p＝0,Y_p＞0时,

当X_p＜0,Y_p＞0时,

当X_p＜0,Y_p＝0时,α＝π；当X_p＜0,Y_p＜0时,

当X_p＝0,Y_p＜0时,

当X_p＞0,Y_p＜0时,

得到P点的虚拟经纬度坐标(α,β)。

4.根据权利要求1所述的一种基于预测校正的四旋翼飞行器位置控制方法，其特征在于：所述第三步中，执行预测校正控制器中的预测环节，得到航程的动态方程为：

其中，

s表示航程，是指四旋翼飞行器质心与坐标原点连线在虚拟经纬度坐标中球面上的交点走过的距离，

表示航程对时间的一阶导数，v表示四旋翼飞行器的飞行速度，预测航程通过积分表示为：

待飞航程S_togo表示当前位置到目标位置的距离，表示为：

S_togo＝arccos[sinβ_c sinβ_s+cosβ_ccosβ_s cos(α_c-α_s)]

其中，α_c表示当前位置的经度，α_s表示目标位置的经度，β_c表示当前位置的纬度，β_s表示目标位置的纬度，此时航程误差为：

ΔS＝S_togo-S_p。

5.根据权利要求1所述的一种基于预测校正的四旋翼飞行器位置控制方法，其特征在于：所述第四步中，预测校正控制器中滚转角参考信号φ和俯仰角参考信号θ的校正环节，其中滚转角参考信号φ的校正公式为：

其中，φ_n+1为本次计算得到的最新滚转角参考信号，φ_n和φ_n-1分别表示前两次计算得到的滚转角参考信号，λ_k表示用于调节更新快慢的参数，ΔS(φ_n)表示滚转角参考信号为φ_n时的航程误差，ΔS(φ_n-1)表示滚转角参考信号为φ_n-1时的航程误差；俯仰角参考信号θ的校正公式为：

其中，θ_n+1为本次计算得到的最新俯仰角参考信号，θ_n和θ_n-1分别表示前两次计算得到的俯仰角参考信号，ΔS(θ_n)表示俯仰角参考信号为θ_n时的航程误差，ΔS(θ_n-1)表示俯仰角参考信号为θ_n-1时的航程误差，n表示的是计算的次数。

6.根据权利要求1所述的一种基于预测校正的四旋翼飞行器位置控制方法，其特征在于：所述第六步中，干扰观测器为：

其中，

表示外部环境干扰的估计值，z₁为观测器辅助变量，

为辅助变量z₁的一阶导数，

为待设计变量，

为观测器增益。

7.根据权利要求1所述的一种基于预测校正的四旋翼飞行器位置控制方法，其特征在于：所述第七步中，PD控制器为：

其中，e_z＝z-z_b表示Z轴方向的坐标误差，z_b表示期望的Z轴坐标，

表示e_z的一阶导数，a_z表示Z轴方向加速度，K_p和K_d表示控制增益，计算得到竖直方向控制律为：

8.根据权利要求1所述的一种基于预测校正的四旋翼飞行器位置控制方法，其特征在于：所述第八步中，复合控制律为：

其中，

表示复合控制律。

Images