CN110928322A - 四旋翼飞行器运送悬挂负载的非线性运动控制方法 - Google Patents

Abstract

本发明属于四旋翼飞行器技术领域，四旋翼飞行器运送悬挂负载的非线性运动控制方法。本方法建立考虑空气阻力和载荷惯性矩的新型动力学模型，使动力学模型更接近于真实的飞行器和飞行环境。所设计的控制器能够对四旋翼飞行器悬挂负载的速度进行主动控制，从而实现负载跟踪指定轨迹。在运输贵重、易碎物品时，我们必须考虑被运输物品的准确性和稳定性。本发明设计的非线性串级控制器可以主动地去控制被运输负载的运动，并且能够稳定、准确地控制负载轨迹。被运送负载的大小和质量通常与四旋翼飞行器大小质量相当，本发明设计的非线性串级控制器考虑了负载的转动惯量和空气阻力的影响，从而提高四旋翼飞行器在接近真实的环境中运送大型货物的稳定性。

Description

四旋翼飞行器运送悬挂负载的非线性运动控制方法

技术领域

本发明涉及四旋翼飞行器技术领域，具体地说设计了一种非线性串级控制器对四旋翼飞行器悬挂负载进行精确和稳定的运动控制。

背景技术

四旋翼飞行器(Quadrotor)是一种不需要飞行员驾驶，本身带有控制程序，操作者通过无线电遥控来操作的飞行器。四旋翼飞行器主要框架由两个相互交叉并且垂直的支架组成，位于支架四端的四个电机带动旋翼来产生飞行的动力。通过改变四个电机的转速就可以实现四旋翼飞行器起飞、降落、滚转、俯仰、偏航运动的控制。

四旋翼飞行器由于体型小巧、成本低廉、机动灵活的特点，在许多领域得到了广泛的应用，如航拍、植保、电力巡检等。然而，在一些特定的场景中，例如救援、易碎物品运输和送餐，四旋翼必须能够在复杂的环境中实现稳定、准确地运输。在上述场景中，一个四旋翼通过绳索与负载连接是常见的应用，这种运输负载的方式有许多优点，例如节省时间和精力、减轻重量、装载或者卸载货物不用起飞或者降落，同时没有额外的惯性会降低四旋翼的灵活性。

但是通过四旋翼飞行器运送悬挂负载也存在着诸多难点。首先，四旋翼飞行器运送悬挂负载***是一个非线性、强耦合、欠驱动的***。其次，被运送的悬挂负载大小、质量通常与四旋翼飞行器大小、质量相当，这就导致在运输的过程中易受到强风等恶劣天气的影响。所以在设计四旋翼飞行器运送悬挂负载控制器的时候，我们必须考虑负载的转动惯量和空气阻力的影响。最后，四旋翼飞行器的升力沿着机体固定支架的Z轴方向，因此被运送负载的平移运动只能通过将四旋翼飞行器倾斜到所需要的方向来实现，而绳子与负载构成了一个不受控制的摆动***。但是，四旋翼飞行器运送悬挂负载的环境非常复杂，因此四旋翼飞行器运送负载必须能够精确、稳定地控制负载的运动轨迹，使负载能够准确地避开大楼、路灯、绿化设施等。因此，设计高性能的控制器通过对四旋翼飞行器悬挂负载的速度进行主动控制，从而实现负载轨迹跟踪已成为业内热门研究的问题。

发明内容

本发明的目的是设计非线性串级控制器对四旋翼飞行器悬挂负载的运动能够准确、稳定地控制。

本发明的技术方案：

四旋翼飞行器运送悬挂负载的非线性运动控制方法，步骤如下：

(1)建立四旋翼飞行器运送悬索负载的模型

(1.1)四旋翼飞行器运送悬索负载的模型参数

q＝[ξ^T η^T σ^T]^T：四旋翼飞行器相对广义的坐标系；

ξ＝[x y z]^T：四旋翼飞行器重心在惯性坐标系中的坐标，x、y、z表示分别沿X、Y、Z轴的位置；

η＝[φ θ ψ]^T：四旋翼飞行器在欧拉坐标系中的姿态角；φ：四旋翼飞行器的滚转角，θ：四旋翼飞行器的俯仰角，ψ：四旋翼飞行器的偏航角；

σ＝[α β]^T：负载在欧拉坐标系中的摆动角度，α：绳子的滚转角，β：绳子的俯仰角；

δ＝[x_p y_p z_p]^T：负载的重心在惯性坐标系中的位置；

m_q：四旋翼飞行器的质量；

m_p：负载的质量；

l_r：四旋翼飞行器质心到电机的距离；

l：悬挂负载绳子的长度；

I_pxx：四旋翼飞行器的转动惯量；

I_pyy：四旋翼飞行器的俯仰惯量；

I_pzz：四旋翼飞行器的偏航惯量；

I_pxx：负载的转动惯量；

I_pyy：负载的俯仰惯量；

I_pzz：负载的偏航惯量；

(1.2)四旋翼飞行器运送悬索负载的动态模型：

式中，P_η为四旋翼飞行器旋转运动的动量；

P_σ为负载旋转运动的动量；

P_ξ为四旋翼飞行器的平移运动的动量，

为四旋翼飞行器的平移运动的动量的导数，F_t为绳索作用在负载上的拉力，R为从机体坐标系到惯性坐标系的旋转矩阵，F_l为四旋翼飞行器的升力，D_ξ为四旋翼飞行器平移运动的阻力系数，F_qg为四旋翼飞行器的重力；

D_δ为负载旋转运动的阻力系数；

D_η为四旋翼飞行器旋转运动的阻力系数；

F_pg为负载的重力；

为四旋翼飞行器的转动惯量矩阵；

J_q11＝I_qxx

J_q13＝J_q31＝-I_qxxsinθ

J_q22＝I_qyycos²φ+I_qzzsin²φ

J_q23＝J_q32＝(I_qyy-I_qzz)sinφcosφcosθ

J_q33＝I_qxxsin²θ+I_qyysin²φcos²θ+I_qzzcos²φcos²θ

τ_η为四旋翼飞行器电机推力的不平衡产生的扭矩；

τ_σ为由空气阻力产生的作用于负载上的阻力扭矩；

A＝[A_φ A_θ 0]

C＝[C_α C_β]^T

m₇₇＝m_pl²+I_pxx

m₈₈＝m_pl²cos²α+I_pyycos²α+I_pzzsin²α

m₇₁＝m_plsinαsinβ

m₇₂＝m_plcosα

m₇₃＝m_plsinαcosβ

m₈₁＝-m_plcosαcosβ

m₈₃＝m_plcosαsinβ

V_dσ＝[V_α V_β]^T

V_α＝m_pglsinαcosβ

V_β＝m_pglcosαcosβ

(2)设计非线性串级控制器

四旋翼飞行器运送悬挂负载非线性串级控制器主要由内环无人机姿态控制器、中环摆动角控制器、外环速度控制器构成；内环无人机姿态控制器通过设计扭矩τ_η控制四旋翼飞行器的欧拉角η跟随期望的角度η_d；中环摆动角控制器获得四旋翼飞行器的升力F_l和期望的滚转φ_d、期望的俯仰角θ_d，来控制绳子的张力F_t跟随期望的张力F_td，同时也控制绳子张力的滚转角α、俯仰角β跟随期望的滚转角α_d、俯仰角β_d；外环速度控制器获得期望的绳子的张力F_td和期望的滚转角α_d、俯仰角β_d控制负载的速度

跟随期望的速度

这样对四旋翼飞行器悬挂的负载的速度进行主动控制，从而实现四旋翼飞行器悬挂的负载跟踪指定的轨迹；

(2.1)跟踪误差

四旋翼飞行器的姿态误差、角速度误差定义如下：

e_η,pη＝[e_η ^T e_pη ^T]^T

其中，e_η＝[e_φ e_θ e_ψ]^T＝η_d-η，

η_d是四旋翼飞行器期望的姿态角，

是期望的欧拉角速度，K_η＝diag(k₁,k₂,k₃)是正定的；

负载的姿态误差、旋转动量误差定义如下：

e_σ,pσ＝[e_σ ^T e_pσ ^T]^T

其中，e_σ＝[e_α e_β]^T＝σ_d-σ，

σ_d是期望的摆动角，

是期望的摆动角速度，K_σ＝diag(k₇,k₈)；

负载的速度误差、加速度误差定义如下：

其中，

负载的期望速度，

是负载的期望加速度；

(2.2)内环无人机姿态控制器

内环无人机姿态控制器通过转子推力的不平衡产生的扭矩控制四旋翼飞行器的姿态角η和稳定四旋翼飞行器的姿态避免强烈的震动和碰撞；

扭矩τ_η设计如下：

式中，K_η＝diag(k₁,k₂,k₃)和K_pη＝diag(k₄,k₅,k₆)是正定的；式(2.1)的扭矩，

是用来抵消四旋翼飞行器转动过程受到的空气阻力，

是用来抵消四旋翼飞行器动力模型的耦合部分，J_p(e_η+K_ηe_pη-K² _ηe_η+K_pηe_pη)是通过K_η、K_pη调节作用使四旋翼飞行器的姿态误差趋于0和角速度误差达到指数稳定，从而保证四旋翼飞行器的滚转角φ、俯仰角θ跟随期望的滚转角φ_d、俯仰角θ_d；

(2.3)中环摆动角控制器

中环摆动角控制器用来控制负载的摆角σ和绳子上的张力F_t，包含解耦器和摆角控制器；

(2.3.1)摆角控制器

摆角控制器用来控制绳子的摆角σ跟随期望的角度σ_d；对于期望的摆角σ_d，输入的扭矩τ_η由式(2.1)给定，摆角加速度

设计为：

对于式(1.2)、(1.3)定义的四旋翼飞行器运送悬索负载的动态模型，负载的摆角加速度

设计成式(2.2)，那么姿态角跟踪误差和角加速度跟踪误差是指数稳定的，即绳子的摆角σ能稳定跟踪期望的角度σ_d；

(2.3.2)解耦器

解耦器通过解耦外环速度控制器产生的张力F_td和摆角控制器产生的摆动角加速度

获得四旋翼飞行器的升力F_l和期望的滚转角φ_d、俯仰角θ_d；

步骤如下：

首先，使用F_td、α_d和β_d解耦出

由于四旋翼飞行器与负载通过绳索连接，所以它们有相同的平移加速度，即

上式右边是四旋翼飞行器沿着绳子方向的加速度，左边k₀是负载的加速度，

是惯性坐标系到体坐标系的旋转矩阵，表示为：

对负载受力分析得：

通过式(1.3)，得到如下的方程

式中，k₁＝C_α-V_α+τ_α，k₂＝C_β-V_β+τ_β

由方程(2.3)、(2.4)、(2.5)、(2.6)解出三个未知数

得：

然后，使用

解耦出F_ld；通过对四旋翼飞行器的动态分析，期望的升力从以下的方程中解出：

期望的升力在惯性坐标系中表示成F_ld＝[F_lxd F_lyd F_lzd]^T的形式；

最后，使用F_ld解耦出F_l和四旋翼飞行器期望的滚转角φ_d、俯仰角θ_d；

F_lxd为四旋翼飞行器期望的升力F_ld在惯性坐标中沿着X_i轴的分力，F_lyd为四旋翼飞行器期望的升力F_ld在惯性坐标中沿着Y_i轴的分力，F_lzd为四旋翼飞行器期望的升力F_ld在惯性坐标中沿着Z_i轴的分力；

通过解上式得到：

因此，通过解耦

和F_td得到总升力F_l和期望的滚转角φ_d、俯仰角θ_d；

(2.4)外环速度控制器

外环速度控制器通过绳子上的张力F_td来控制负载的速度

跟随期望的速度

绳子期望的张力F_td的大小表示为：

F_tzd为绳子的张力F_td在惯性坐标中沿着Z_i轴的分力；

负载的摆动角α_d、β_d也是绳子期望的张力F_td的欧拉角，表示为：

F_txd为绳子的张力F_td在惯性坐标中沿着X_i轴的分力，F_tyd为绳子的张力F_td在惯性坐标中沿着Y_i轴的分力；

绳子的张力F_td：

式中，

是正定的；

计算式(2.17)并转换F_td＝[F_txd F_tyd F_tzd]^T，代入到式(2.14)、式(2.15)、式(2.16)中计算绳子的张力F_td的大小和期望的滚转角α_d、俯仰角β_d，然后把绳子的张力和期望的摆动角作为中环摆动角控制器的输入；

对于负载的动态模型，通过控制参数矩阵

和

把绳子上的张力大小F_td设计成式(2.14)，把期望的摆动角α_d、β_d设计成式(2.15)、(2.16)，那么负载的速度

能够稳定地跟踪期望的速度

本发明的有益效果：

(1)建立了考虑空气阻力和载荷惯性矩的新型动力学模型，使动力学模型更接近于真实的飞行器和飞行环境。

(2)提出了一种具有指数稳定性的非线性级联控制器。所设计的控制器能够对四旋翼飞行器悬挂负载的速度进行主动控制，从而实现负载跟踪指定轨迹。

(3)在运输贵重、易碎物品时，我们必须考虑被运输物品的准确性和稳定性。本发明设计的非线性串级控制器可以主动地去控制被运输负载的运动，并且能够稳定、准确地控制负载轨迹。

(4)被运送负载的大小和质量通常与四旋翼飞行器大小质量相当，本发明设计的非线性串级控制器考虑了负载的转动惯量和空气阻力的影响，从而提高了四旋翼飞行器在接近真实的环境中运送大型货物的稳定性。

附图说明

图1为本发明设计的四旋翼飞行器运送悬挂负载的结构图。

图2为本发明设计的四旋翼飞行器运送悬挂负载控制器的原理图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

S1、给定负载期望的速度

通过绳子的张力F_td计算公式：

得到期望的张力并转换为F_td＝[F_txd F_tyd F_tzd]^T。F_txd为绳子的张力F_td在惯性坐标中沿着X_i轴的分力，F_tyd为绳子的张力F_td在惯性坐标中沿着Y_i轴的分力，F_tzd为绳子的张力F_td在惯性坐标中沿着Z_i轴的分力。

把F_tzd、F_txd、F_tyd代入外环控制器中绳子期望张力F_td的大小计算公式和摆角计算公式：

得到达到负载期望的速度

所需要的绳子张力F_td的大小和期望的滚转角α_d、俯仰角β_d。

S2、外环负载速度控制器得到的期望的滚转角α_d、俯仰角β_d作为中环摆动角控制器的输入，通过中环控制器中摆动角加速度的计算公式：

得到达到期望的滚转角α_d、俯仰角β_d所需要的摆动角加速度

S3、中环摆角控制器得到的摆动角加速度

和外环控制器的得到的绳子期望的张力F_td，通过以下的解耦得到四旋翼飞行器的升力F_l和期望的滚转角φ_d、俯仰角θ_d。

解耦的步骤如下：

首先，使用F_td、α_d和β_d解耦出

由于四旋翼飞行器与负载通过绳索连接，所以它们有相同的平移加速度，即

上式右边是四旋翼飞行器沿着绳子方向的加速度，左边k₀是负载的加速度。

是惯性坐标系到体坐标系的旋转矩阵表示为：

对负载受力分析得：

通过式(1.3)，可以得到如下的方程

式中，k₁＝C_α-V_α+τ_α，k₂＝C_β-V_β+τ_β

由以上四个方程解出三个未知数

得：

然后，使用

解耦出F_ld。通过对四旋翼飞行器的动态分析，期望的升力可以从以下的方程中解出：

期望的升力在惯性坐标系中可以表示成F_ld＝[F_lxd F_lyd F_lzd]^T的形式。

最后，使用F_ld解耦出F_l和期望的滚转角φ_d、俯仰角θ_d。

F_lxd为四旋翼飞行器期望的升力F_ld在惯性坐标中沿着X_i轴的分力，F_lyd为四旋翼飞行器期望的升力F_ld在惯性坐标中沿着Y_i轴的分力，F_lzd为四旋翼飞行器期望的升力F_ld在惯性坐标中沿着Z_i轴的分力。

通过解上式我们可以得到：

因此，通过解耦

和F_td可以得到四旋翼飞行器总升力F_l和期望的滚转角φ_d、俯仰角θ_d。

S4、解耦器得到的期望的滚转角φ_d、俯仰角θ_d和设定偏航角ψ_d通过内环四旋翼姿态控制器扭矩τ_η的计算公式：

式中，K_η＝diag(k₁,k₂,k₃)和K_pη＝diag(k₄,k₅,k₆)是正定的。

得到达到四旋翼飞行器期望姿态η_d所需要的扭矩τ_η。

S4、旋翼总升力F_l和扭矩τ_η作为四旋翼飞行器悬挂负载动力***的输入，输出四旋翼飞行器的实际姿态角和位置、负载的实际摆动角。

四旋翼飞行器运送悬挂负载非线性控制器的原理图如图2所示，该非线性串级控制器可以实现对负载速度的主动控制。非线性串级控制器由三个环组成：内环四旋翼飞行器姿态控制器、中环绳索摆角控制器和解耦器、外环负载速度控制器。

非线性控制器的主要目的是使负载的实际速度

跟随期望的速度

内环无人机姿态控制器通过设计扭矩τ_η控制四旋翼飞行器的欧拉角η跟随期望的角度η_d；中环摆动角控制器获得四旋翼飞行器的升力F_l和期望的滚转φ_d、期望的俯仰角θ_d，来控制绳子的张力F_t跟随期望的张力F_td，同时也控制绳子张力的滚转角α、俯仰角β跟随期望的滚转角α_d、俯仰角β_d；外环速度控制器获得期望的绳子的张力F_td和期望的滚转角α_d、俯仰角β_d控制负载的速度

跟随期望的速度

这样对四旋翼飞行器悬挂的负载的速度进行主动控制，从而实现四旋翼飞行器悬挂的负载跟踪指定的轨迹。

Claims (1)

1.一种四旋翼飞行器运送悬挂负载的非线性运动控制方法，其特征在于，步骤如下：

(1)建立四旋翼飞行器运送悬索负载的模型

(1.1)四旋翼飞行器运送悬索负载的模型参数

q＝[ξ^T η^T σ^T]^T：四旋翼飞行器相对广义的坐标系；

ξ＝[x y z]^T：四旋翼飞行器重心在惯性坐标系中的坐标，x、y、z表示分别沿X、Y、Z轴的位置；

η＝[φ θ ψ]^T：四旋翼飞行器在欧拉坐标系中的姿态角；φ：四旋翼飞行器的滚转角，θ：四旋翼飞行器的俯仰角，ψ：四旋翼飞行器的偏航角；

σ＝[α β]^T：负载在欧拉坐标系中的摆动角度，α：绳子的滚转角，β：绳子的俯仰角；

δ＝[x_p y_p z_p]^T：负载的重心在惯性坐标系中的位置；

m_q：四旋翼飞行器的质量；

m_p：负载的质量；

l_r：四旋翼飞行器质心到电机的距离；

l：悬挂负载绳子的长度；

I_pxx：四旋翼飞行器的转动惯量；

I_pyy：四旋翼飞行器的俯仰惯量；

I_pzz：四旋翼飞行器的偏航惯量；

I_pxx：负载的转动惯量；

I_pyy：负载的俯仰惯量；

I_pzz：负载的偏航惯量；

(1.2)四旋翼飞行器运送悬索负载的动态模型：

式中，P_η为四旋翼飞行器旋转运动的动量；

P_σ为负载旋转运动的动量；

P_ξ为四旋翼飞行器的平移运动的动量，

为四旋翼飞行器的平移运动的动量的导数，F_t为绳索作用在负载上的拉力，R为从机体坐标系到惯性坐标系的旋转矩阵，F_l为四旋翼飞行器的升力，D_ξ为四旋翼飞行器平移运动的阻力系数，F_qg为四旋翼飞行器的重力；

D_δ为负载旋转运动的阻力系数；

D_η为四旋翼飞行器旋转运动的阻力系数；

F_pg为负载的重力；

为四旋翼飞行器的转动惯量矩阵；

J_q11＝I_qxx

J_q13＝J_q31＝-I_qxxsinθ

J_q22＝I_qyycos²φ+I_qzzsin²φ

J_q23＝J_q32＝(I_qyy-I_qzz)sinφcosφcosθ

J_q33＝I_qxxsin²θ+I_qyysin²φcos²θ+I_qzzcos²φcos²θ

τ_η为四旋翼飞行器电机推力的不平衡产生的扭矩；

τ_σ为由空气阻力产生的作用于负载上的阻力扭矩；

A＝[A_φ A_θ 0]

C＝[C_α C_β]^T

m₇₇＝m_pl²+I_pxx

m₈₈＝m_pl²cos²α+I_pyycos²α+I_pzzsin²α

m₇₁＝m_plsinαsinβ

m₇₂＝m_plcosα

m₇₃＝m_plsinαcosβ

m₈₁＝-m_plcosαcosβ

m₈₃＝m_plcosαsinβ

V_dσ＝[V_α V_β]^T

V_α＝m_pglsinαcosβ

V_β＝m_pglcosαcosβ

(2)设计非线性串级控制器

四旋翼飞行器运送悬挂负载非线性串级控制器主要由内环无人机姿态控制器、中环摆动角控制器、外环速度控制器构成；内环无人机姿态控制器通过设计扭矩τ_η控制四旋翼飞行器的欧拉角η跟随期望的角度η_d；中环摆动角控制器获得四旋翼飞行器的升力F_l和期望的滚转φ_d、期望的俯仰角θ_d，来控制绳子的张力F_t跟随期望的张力F_td，同时也控制绳子张力的滚转角α、俯仰角β跟随期望的滚转角α_d、俯仰角β_d；外环速度控制器获得期望的绳子的张力F_td和期望的滚转角α_d、俯仰角β_d控制负载的速度

跟随期望的速度

这样对四旋翼飞行器悬挂的负载的速度进行主动控制，从而实现四旋翼飞行器悬挂的负载跟踪指定的轨迹；

(2.1)跟踪误差

四旋翼飞行器的姿态误差、角速度误差定义如下：

e_η,pη＝[e_η ^T e_pη ^T]^T

其中，e_η＝[e_φ e_θ e_ψ]^T＝η_d-η，

η_d是四旋翼飞行器期望的姿态角，

是期望的欧拉角速度，K_η＝diag(k₁,k₂,k₃)是正定的；

负载的姿态误差、旋转动量误差定义如下：

e_σ,pσ＝[e_σ ^T e_pσ ^T]^T

其中，e_σ＝[e_α e_β]^T＝σ_d-σ，

σ_d是期望的摆动角，

是期望的摆动角速度，K_σ＝diag(k₇,k₈)；

负载的速度误差、加速度误差定义如下：

其中，

负载的期望速度，

是负载的期望加速度；

(2.2)内环无人机姿态控制器

内环无人机姿态控制器通过转子推力的不平衡产生的扭矩控制四旋翼飞行器的姿态角η和稳定四旋翼飞行器的姿态避免强烈的震动和碰撞；

扭矩τ_η设计如下：

式中，K_η＝diag(k₁,k₂,k₃)和K_pη＝diag(k₄,k₅,k₆)是正定的；式(2.1)的扭矩，

是用来抵消四旋翼飞行器转动过程受到的空气阻力，

是用来抵消四旋翼飞行器动力模型的耦合部分，J_p(e_η+K_ηe_pη-K² _ηe_η+K_pηe_pη)是通过K_η、K_pη调节作用使四旋翼飞行器的姿态误差趋于0和角速度误差达到指数稳定，从而保证四旋翼飞行器的滚转角φ、俯仰角θ跟随期望的滚转角φ_d、俯仰角θ_d；

(2.3)中环摆动角控制器

中环摆动角控制器用来控制负载的摆角σ和绳子上的张力F_t，包含解耦器和摆角控制器；

(2.3.1)摆角控制器

摆角控制器用来控制绳子的摆角σ跟随期望的角度σ_d；对于期望的摆角σ_d，输入的扭矩τ_η由式(2.1)给定，摆角加速度

设计为：

对于式(1.2)、(1.3)定义的四旋翼飞行器运送悬索负载的动态模型，负载的摆角加速度

设计成式(2.2)，那么姿态角跟踪误差和角加速度跟踪误差是指数稳定的，即绳子的摆角σ能稳定跟踪期望的角度σ_d；

(2.3.2)解耦器

解耦器通过解耦外环速度控制器产生的张力F_td和摆角控制器产生的摆动角加速度

获得四旋翼飞行器的升力F_l和期望的滚转角φ_d、俯仰角θ_d；

步骤如下：

首先，使用F_td、α_d和β_d解耦出

由于四旋翼飞行器与负载通过绳索连接，所以它们有相同的平移加速度，即

上式右边是四旋翼飞行器沿着绳子方向的加速度，左边k₀是负载的加速度，

是惯性坐标系到体坐标系的旋转矩阵，表示为：

对负载受力分析得：

通过式(1.3)，得到如下的方程

式中，k₁＝C_α-V_α+τ_α，k₂＝C_β-V_β+τ_β

由方程(2.3)、(2.4)、(2.5)、(2.6)解出三个未知数

得：

然后，使用

解耦出F_ld；通过对四旋翼飞行器的动态分析，期望的升力从以下的方程中解出：

期望的升力在惯性坐标系中表示成F_ld＝[F_lxd F_lyd F_lzd]^T的形式；

最后，使用F_ld解耦出F_l和四旋翼飞行器期望的滚转角φ_d、俯仰角θ_d；

F_lxd为四旋翼飞行器期望的升力F_ld在惯性坐标中沿着X_i轴的分力，F_lyd为四旋翼飞行器期望的升力F_ld在惯性坐标中沿着Y_i轴的分力，F_lzd为四旋翼飞行器期望的升力F_ld在惯性坐标中沿着Z_i轴的分力；

通过解上式得到：

因此，通过解耦

和F_td得到总升力F_l和期望的滚转角φ_d、俯仰角θ_d；

(2.4)外环速度控制器

外环速度控制器通过绳子上的张力F_td来控制负载的速度

跟随期望的速度

绳子期望的张力F_td的大小表示为：

F_tzd为绳子的张力F_td在惯性坐标中沿着Z_i轴的分力；

负载的摆动角α_d、β_d也是绳子期望的张力F_td的欧拉角，表示为：

F_txd为绳子的张力F_td在惯性坐标中沿着X_i轴的分力，F_tyd为绳子的张力F_td在惯性坐标中沿着Y_i轴的分力；

绳子的张力F_td：

式中，

是正定的；

计算式(2.17)并转换F_td＝[F_txd F_tyd F_tzd]^T，代入到式(2.14)、式(2.15)、式(2.16)中计算绳子的张力F_td的大小和期望的滚转角α_d、俯仰角β_d，然后把绳子的张力和期望的摆动角作为中环摆动角控制器的输入；

对于负载的动态模型，通过控制参数矩阵

和

把绳子上的张力大小F_td设计成式(2.14)，把期望的摆动角α_d、β_d设计成式(2.15)、(2.16)，那么负载的速度

能够稳定地跟踪期望的速度

Images