CN106647792A - 用于无人机悬挂负载***的抗扰控制方法 - Google Patents

Abstract

本发明属于多旋翼无人机负载运输控制研究领域，为提出一套多旋翼无人机悬挂负载***的非线性鲁棒控制算法，提高***在强交变风扰下的控制鲁棒性以及控制精度。本发明采取的技术方案是，用于无人机悬挂负载***的抗扰控制方法，包括如下步骤：多旋翼无人机悬挂负载***的运动学模型搭建步骤；扰动观测器设计步骤；基于所提出的多旋翼无人机悬挂负载***的运动学方程，进行数学推导得到扰动观测器的观测模型。本发明主要应用于多旋翼无人机负载运输控制场合。

Description

用于无人机悬挂负载***的抗扰控制方法

技术领域

本发明属于多旋翼无人机负载运输控制研究领域。针对多旋翼无人机悬挂负载***的鲁棒控制需求，提出了一套基于扰动观测器的旋翼无人机-悬挂负载***控制方法，实现了***在强交变外界风扰下的无人机位置镇定控制与悬挂负载摆角抑制控制。具体讲,涉及用于无人机悬挂负载***的抗扰控制方法。

背景技术

随着小型无人机导航传感器的小型化、成本低廉化趋势，多旋翼无人机在军用、民用等领域的应用发展迅速。多旋翼无人机机动飞行能力强，可实现包括垂直起降、悬停和侧飞等多种机动飞行任务。

多旋翼无人机悬挂负载***是一种较为理想的负载运输工具，适用于在危险且难以抵达的区域进行运输作业，如山顶、建筑楼顶等。针对多旋翼无人机的悬挂负载控制与应用研究引起了国外高校等研究机构的广泛关注。

多旋翼无人机悬挂负载***的控制问题涉及以下两个方面：无人机的位置控制以及悬挂负载摆角控制。NASA(美国国家航空航天局)的好奇号火星车登陆***采用了一个类似于四旋翼无人机的空中起重机，以悬挂负载形式实现了好奇号火星车在火星近地表面的减速和登陆(网页：https://www.nasa.gov/mission-pages/msl/index.html)。美国宾夕法尼亚大学对四旋翼无人机悬挂负载***的控制问题进行了研究，基于平面微分***的分析方法，实现了悬挂负载的减摆控制与悬挂负载的位置跟踪控制(会议：the 2013 IEEEinternational Conference on Robotics and Automation；著者：Steenath K,MichaelNM,Kumar VJ；出版年月：2013年；文章题目：Trajectory Generation and Control of aQuadrotor with a Cable-Suspended Load--A Differentially-Flat Hybrid System；页码：4888-4895)。美国新墨西哥大学对四旋翼无人机悬挂负载***的轨迹生成与跟踪问题进行了研究，基于强化学习算法设计了四旋翼无人机的轨迹生成算法，实现了悬挂负载的摆角抑制控制以及无人机的位置控制(会议：the 2012IEEE International Conferenceon Robotics and Automation；著者：Palunko I,Fierro R,Cruz P；出版年月：2012年；文章题目：Trajectory Generation for Swing-Free Maneuvers of a Quadrotor withSuspended Payload:A Dynamic Programming Approach；页码：2691-2697)。美国乔治华盛顿大学将四旋翼无人机悬挂负载***的连接绳索定性为多段直杆串联的链状结构，在此基础上设计了几何非线性控制算法，实现了无人机的位置渐近收敛以及悬挂负载摆角抑制控制(会议：American Control Conference；著者：Farhad A,Goodarzi,Lee D,Lee TY；出版年月：2014年；文章题目：Geometric Stabilization of a Quadrotor UAV with aPayload Connected by Flexible Cable；页码：4925-4930)。

以上述方法为代表的多旋翼无人机悬挂负载***的控制方法取得了一定理论成果，但仍存在着一点主要的不足：未对多旋翼无人机悬挂负载***的鲁棒控制问题进行研究。以上控制算法的实验验证均在无扰动的情况下进行，并未考虑***在外界扰动情况下的控制精度及鲁棒性问题，无法保证***在常见外界扰动(如强风扰)下的控制效果。

发明内容

为克服现有技术的不足，本发明旨在提出一套多旋翼无人机悬挂负载***的非线性鲁棒控制算法，提高***在强交变风扰下的控制鲁棒性以及控制精度。本发明采取的技术方案是，用于无人机悬挂负载***的抗扰控制方法，包括如下步骤：

多旋翼无人机悬挂负载***的运动学模型搭建步骤：

多以I表示惯性坐标系，以B表示无人机体坐标系，以p_Q＝(x_Q,y_Q,z_Q)^T表示多旋翼无人机相对于坐标系I的位置，其中x_Q，y_Q，z_Q分别为多旋翼无人机在坐标系I下的x，y，z方向位置，以p_L＝(x_L,y_L,z_L)^T表示悬挂负载相对于坐标系I的位置，其中x_L，y_L，z_L分别为悬挂负载在坐标系I下的x，y，z方向位置，以q＝(q_x,q_y,q_z)^T表示由无人机重心指向悬挂负载重心的单位向量，

以T表示悬挂绳索的拉力，以m_Q，m_L，l分别表示无人机的质量，悬挂负载的质量以及悬挂绳索的长度，以F＝(F_x,F_y,F_z)^T表示无人机的平动推力控制输入，以D＝(D_x,D_y,D_z)^T表示作用下无人机重心的外界未知平动扰动力，以G＝(0,0,g)^T表示重力加速度向量，以符号表示变量·的一阶时间导数，以符号表示变量·的二阶时间导数，则多旋翼无人机悬挂负载***的运动学模型表示如下：

p_L＝p_Q+lq

扰动观测器设计步骤：

基于所提出的多旋翼无人机悬挂负载***的运动学方程，进行数学推导可得扰动观测器的观测模型如下式所示：

为方便扰动观测器的设计，将观测模型简写成式其中 以p_d表示无人机的期望控制位置，定义悬挂负载对应的虚拟负载位置为p_V＝p_Q+ρq，ρ为负常数，定义虚拟负载位置控制偏差ε＝p_V-(p_d+ρr₃)，r₃＝(0,0,1)^T，所设计的扰动观测器如下式所示

其中K₁＝diag{k_1i}∈R^3×3,K₂＝diag{k_2i}∈R^3×3表示对角增益矩阵，表示估计扰动项d的估计值，表示的估计值，表示的估计偏差，符号sign(·)表示变量·的符号函数，符号diag(·)表示由变量·生成对角矩阵，α，p,λ分别表示大于零的常数，对角增益矩阵K₁，K₂要满足以下不等式条件

其中δ₂为扰动力D的一阶时间导数上界；

带有扰动补偿机制的非线性鲁棒控制器设计步骤：

针对所提出的多旋翼无人机悬挂负载运动学模型，设计非线性鲁棒控制器如下所示

其中K_p1＝diag{k_p1i}∈R^3×3，K_d1＝diag{k_d1i}∈R^3×3，K_d2＝diag{k_d2i}∈R^3×3均表示对角增益矩阵，K_p2＝{k_p2i}∈R^1×3表示增益向量，增益均大于零，该控制器实现多旋翼无人机的位置控制误差收敛，即p_Q→p_d，以及悬挂负载摆角的消除，即q→(0,0,1)^T。

还包括如下验证步骤：利用多旋翼无人机悬挂负载***实现，所述***由两部分组成，其一为四旋翼无人机，其二为质量为悬挂负载；利用运动捕捉***实时获取悬挂负载以及四旋翼无人机的三维位置信息，并通过Xbee模块将位置信息实时传输至无人机机载微处理器，按照权利要求1所述的步骤设计的扰动观测器、带有扰动补偿机制的非线性鲁棒控制器运行于无人机机载微处理器单元，采用两台工业风扇模拟强交变风扰。两台工业风扇分别位于四旋翼无人机的两侧，与四旋翼无人机期望控制位置水平相距约2.5m，在扰动模拟开始后，每台工业风扇前各自设置一块挡风板，每台挡风板均以3s为周期轮流遮挡/移开风扇出风口，以此形成方向周期***替，强度随时间变化的时变风扰，

本发明的特点及有益效果是：

本发明提出方法可对作用在无人机重心上的外界平动扰动力进行实时估计，并在所设计的非线性鲁棒控制器中对扰动进行补偿。与现有的多旋翼无人机悬挂负载***控制算法相比，本发明所设计的方法能提高在***在外界扰动下的无人机位置控制精度以及悬挂负载摆角抑制控制的效果。多组控制器在强交变风扰下的对比实验证明，相比现有的LQR、PID控制算法，本发明在强交变风扰下有着更为优越的无人机位置控制精度以及悬挂负载摆角抑制效果。

附图说明：

图1是本发明中多旋翼无人机悬挂负载***示意图；

图2是本发明中强交变风扰实验的无人机位置响应曲线图；

图3是本发明中强交变风扰实验的悬挂负载摆角响应曲线图；

图4是本发明中强交变风扰实验的控制输入曲线图；

图5是本发明中强交变风扰实验的无人机姿态角曲线图；

图6是本发明中强交变风扰实验的扰动观测器曲线图；

图7是本发明实验验证所采用的多旋翼无人机悬挂负载***实验平台。

具体实施方式

为了克服现有技术的不足，发明旨在提出一套多旋翼无人机悬挂负载***的非线性鲁棒控制算法，提高***在强交变风扰下的控制鲁棒性以及控制精度。本发明采取的技术方案是，设计扰动观测器对作用在旋翼无人机机体上的平动扰动力进行实时估计，设计带有扰动补偿机制的非线性鲁棒控制器，方法包含以下内容：

多旋翼无人机悬挂负载***的运动学模型搭建步骤：

多旋翼无人机(以四旋翼无人机为例)悬挂负载***示意图如图1所示。以I表示惯性坐标系，以B表示无人机体坐标系，以p_Q＝(x_Q,y_Q,z_Q)^T表示多旋翼无人机相对于坐标系I的位置，其中x_Q，y_Q，z_Q分别为多旋翼无人机在坐标系I下的x，y，z方向位置，以p_L＝(x_L,y_L,z_L)^T表示悬挂负载相对于坐标系I的位置，其中x_L，y_L，z_L分别为悬挂负载在坐标系I下的x，y，z方向位置，以q＝(q_x,q_y,q_z)^T表示由无人机重心指向悬挂负载重心的单位向量，以T表示悬挂绳索的拉力，以m_Q，m_L，l分别表示无人机的质量，悬挂负载的质量以及悬挂绳索的长度，以F＝(F_x,F_y,F_z)^T表示无人机的平动推力控制输入，以D＝(D_x,D_y,D_z)^T表示作用下无人机重心的外界未知平动扰动力(该扰动力被视为有界，且一阶时间导数有界的变量)，以G＝(0,0,g)^T表示重力加速度向量，以符号表示变量·的一阶时间导数，以符号表示变量·的二阶时间导数，则多旋翼无人机悬挂负载***的运动学模型可表示如下：

p_L＝p_Q+lq

扰动观测器设计步骤：

所设计的扰动观测器的作用为：基于***的控制输入F，***物理参数信息m_Q，m_L，l及q对作用在无人机重心上的外界扰动力D进行实时精确估计。

基于所提出的多旋翼无人机悬挂负载***的运动学方程，进行数学推导可得扰动观测器的观测模型如下式所示：

为方便扰动观测器的设计，将观测模型简写成式其中 以p_d表示无人机的期望控制位置，定义悬挂负载对应的虚拟负载位置为p_V＝p_Q+ρq，ρ为负常数，定义虚拟负载位置控制偏差ε＝p_V-(p_d+ρr₃)，r₃＝(0,0,1)^T，所设计的扰动观测器如下式所示

其中K₁＝diag{k_1i}∈R^3×3,K₂＝diag{k_2i}∈R^3×3表示对角增益矩阵，表示估计扰动项d的估计值，表示的估计值，表示的估计偏差，符号sign(·)表示变量·的符号函数，符号diag(·)表示由变量·生成对角矩阵，α，p,λ分别表示大于零的常数。对角增益矩阵K₁，K₂要满足以下不等式条件

其中δ₂为扰动力D的一阶时间导数上界。

带有扰动补偿机制的非线性鲁棒控制器设计步骤：

针对所提出的多旋翼无人机悬挂负载运动学模型，设计非线性鲁棒控制器如下所示

其中K_p1＝diag{k_p1i}∈R^3×3，K_d1＝diag{k_d1i}∈R^3×3，K_d2＝diag{k_d2i}∈R^3×3均表示对角增益矩阵，K_p2＝{k_p2i}∈R^1×3表示增益向量，增益均大于零。该控制器可实现多旋翼无人机的位置控制误差e＝p_d-p_Q收敛，即p_Q→p_d，以及悬挂负载摆角的消除，即q→(0,0,1)^T。

为验证本发明的有效性，利用多旋翼无人机悬挂负载***实验平台进行飞行实验验证。

一、***硬件连接及配置

实验平台如图7所示。多旋翼无人机悬挂负载***由两部分组成，其一为四旋翼无人机，质量为1.12kg，其二为质量为60g的悬挂负载。利用运动捕捉***实时获取悬挂负载以及无人机的三维位置信息，并通过美国DIGI公司生产的Xbee无线数据传输模块将位置信息实时传输至无人机机载微处理器。本发明设计的算法运行于无人机机载微处理器单元。对于发明中提及的强交变风扰，实验平台采用两台320W/750mm规格的工业风扇进行模拟。两台风扇分别位于四旋翼无人机的两侧，与无人机期望控制位置水平相距约2.5m。两台风扇均工作在最大功率。在扰动模拟开始后，每台工业风扇前各自设置一块挡风板，每台挡风板均以3s为周期轮流遮挡/移开风扇出风口，以此形成方向周期***替，强度随时间变化的时变风扰。

二、实验结果

本实施例对上述实验平台进行了强交变风扰下的多旋翼无人机悬挂负载***镇定实验。

无人机的位置控制误差如图2所示。本发明方法的实验结果对应DOB曲线。在扰动下，基于本发明方法的无人机的x/y/z位置控制精度达到±1.6cm/±1.6cm/±1.4cm，基于PID(对应Loadfree曲线)算法以及基于LQR(对应LQR曲线)算法的无人机的x/y/z位置控制误差均大于本发明方法。

悬挂负载的摆角如图3所示。本发明方法的实验结果对应DOB曲线。在扰动下，悬挂负载摆角控制精度达到±3.01°/±3.26°，基于PID算法以及基于LQR算法的悬挂负载摆角抑制效果均低于本发明方法。

控制输入如图4所示，其均保持在合理范围内。

无人机的姿态角如图5所示，其均保持在合理范围内。

扰动观测器如图6所示。从图中可知，扰动观测器实现了对外界扰动的迅速跟踪估计，其变化周期也与所设置的强交变风扰的周期一致。

以上实验结果说明了本发明的有效性。

Claims (2)

1.一种用于无人机悬挂负载***的抗扰控制方法，其特征是，包括如下步骤：

多旋翼无人机悬挂负载***的运动学模型搭建步骤：

多以I表示惯性坐标系，以B表示无人机体坐标系，以p_Q＝(x_Q,y_Q,z_Q)^T表示多旋翼无人机相对于坐标系I的位置，其中x_Q，y_Q，z_Q分别为多旋翼无人机在坐标系I下的x，y，z方向位置，以p_L＝(x_L,y_L,z_L)^T表示悬挂负载相对于坐标系I的位置，其中x_L，y_L，z_L分别为悬挂负载在坐标系I下的x，y，z方向位置，以q＝(q_x,q_y,q_z)^T表示由无人机重心指向悬挂负载重心的单位向量，

以T表示悬挂绳索的拉力，以m_Q，m_L，l分别表示无人机的质量，悬挂负载的质量以及悬挂绳索的长度，以F＝(F_x,F_y,F_z)^T表示无人机的平动推力控制输入，以D＝(D_x,D_y,D_z)^T表示作用下无人机重心的外界未知平动扰动力，以G＝(0,0,g)^T表示重力加速度向量，以符号表示变量·的一阶时间导数，以符号表示变量·的二阶时间导数，则多旋翼无人机悬挂负载***的运动学模型表示如下：

${\stackrel{\·\·}{p}}_Q=\frac{1}{m_Q}(F+D)+G+\frac{1}{m_Q}Tq$

$\stackrel{\·\·}{q}=-\frac{m_L+m_Q}{{\mathrm{lm}}_L{m}_Q}Tq-\frac{1}{{\mathrm{lm}}_Q}(F+D)$

$T=\frac{m_Q{m}_L}{m_Q+m_L}\[-\frac{1}{m_Q}{q}^T(F+D)+l{\stackrel{\·}{q}}^T\stackrel{\·}{q}\]$

p_L＝p_Q+lq

扰动观测器设计步骤：

基于所提出的多旋翼无人机悬挂负载***的运动学方程，进行数学推导可得扰动观测器的观测模型如下式所示：

$\stackrel{\·\·}{q}=-\frac{m_L+m_Q}{{\mathrm{lm}}_L}({\stackrel{\·\·}{p}}_Q-G)+\frac{1}{{\mathrm{lm}}_L}(F+D)$

为方便扰动观测器的设计，将观测模型简写成式其中 以p_d表示无人机的期望控制位置，定义悬挂负载对应的虚拟负载位置为p_V＝p_Q+ρq，ρ为负常数，定义虚拟负载位置控制偏差ε＝p_V-(p_d+ρr₃)，r₃＝(0,0,1)^T，所设计的扰动观测器如下式所示

$\stackrel{\·}{\hat{d}}=K_{1}sign\left(\tilde{h}\right)+\frac{4p^2+\mathrm{\λ}}{2\mathrm{\λ}}\mathrm{\α}\stackrel{\·}{\mathrm{\ϵ}}$

$\stackrel{\·}{\hat{h}}=f+\hat{d}+K_2|\tilde{h}{|}^{\frac{1}{2}}sign\left(\tilde{h}\right)+2\frac{\mathrm{\α}p}{\mathrm{\λ}}diag\left(\stackrel{\·}{\mathrm{\ϵ}}\right)|\tilde{h}{|}^{\frac{1}{2}},$

其中K₁＝diag{k_1i}∈R^3×3,K₂＝diag{k_2i}∈R^3×3表示对角增益矩阵，表示估计扰动项d的估计值，表示的估计值，表示的估计偏差，符号sign(·)表示变量·的符号函数，符号diag(·)表示由变量·生成对角矩阵，α，p,λ分别表示大于零的常数，对角增益矩阵K₁，K₂要满足以下不等式条件

$\left\{\begin{array}{c}{k}_{1i}={\mathrm{pk}}_{2i}\\ k_{2i}>-\frac{p(\frac{4{\mathrm{\δ}}_2}{{\mathrm{lm}}_L}+1)}{\mathrm{\λ}}+\frac{{(\frac{2{\mathrm{\δ}}_2}{{\mathrm{lm}}_L}+\mathrm{\λ}+4p^2)}^2}{12p\mathrm{\λ}}\end{array}\right.$

其中δ₂为扰动力D的一阶时间导数上界；

带有扰动补偿机制的非线性鲁棒控制器设计步骤：

针对所提出的多旋翼无人机悬挂负载运动学模型，设计非线性鲁棒控制器如下所示

$\begin{array}{c}F=-K_{p1}\mathrm{\ϵ}-diag\left(K_{p2}\right)\tanh \left(\mathrm{\ϵ}\right)-(m_Q+m_L)G\\ -k_{d1}\stackrel{\·}{\mathrm{\ϵ}}-K_{d2}\tanh \left(\stackrel{\·}{\mathrm{\ϵ}}\right)-{\mathrm{lm}}_L\hat{d}\end{array}$

其中K_p1＝diag{k_p1i}∈R^3×3，K_d1＝diag{k_d1i}∈R^3×3，K_d2＝diag{k_d2i}∈R^3×3均表示对角增益矩阵，K_p2＝{k_p2i}∈R^1×3表示增益向量，增益均大于零，该控制器实现多旋翼无人机的位置控制误差收敛，即p_Q→p_d，以及悬挂负载摆角的消除，即q→(0,0,1)^T。

2.如权利要求1所述的用于无人机悬挂负载***的抗扰控制方法，其特征是，还包括如下验证步骤：利用多旋翼无人机悬挂负载***实现，所述***由两部分组成，其一为四旋翼无人机，其二为质量为悬挂负载；利用运动捕捉***实时获取悬挂负载以及四旋翼无人机的三维位置信息，并通过Xbee模块将位置信息实时传输至无人机机载微处理器，按照权利要求1所述的步骤设计的扰动观测器、带有扰动补偿机制的非线性鲁棒控制器运行于无人机机载微处理器单元，采用两台工业风扇模拟强交变风扰。两台工业风扇分别位于四旋翼无人机的两侧，与四旋翼无人机期望控制位置水平相距约2.5m，在扰动模拟开始后，每台工业风扇前各自设置一块挡风板，每台挡风板均以3s为周期轮流遮挡/移开风扇出风口，以此形成方向周期***替，强度随时间变化的时变风扰。