CN112817328B - 一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法 - Google Patents

一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法 Download PDF

Info

Publication number
CN112817328B
CN112817328B CN202011631309.2A CN202011631309A CN112817328B CN 112817328 B CN112817328 B CN 112817328B CN 202011631309 A CN202011631309 A CN 202011631309A CN 112817328 B CN112817328 B CN 112817328B
Authority
CN
China
Prior art keywords
load
swing
rotors
coordinate system
angle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011631309.2A
Other languages
English (en)
Other versions
CN112817328A (zh
Inventor
张栋
杨云霄
郗厚印
韩衡志
胡玉斌
张国庆
张学智
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Qingdao University of Technology
Original Assignee
Qingdao University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Qingdao University of Technology filed Critical Qingdao University of Technology
Priority to CN202011631309.2A priority Critical patent/CN112817328B/zh
Publication of CN112817328A publication Critical patent/CN112817328A/zh
Application granted granted Critical
Publication of CN112817328B publication Critical patent/CN112817328B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/04—Control of altitude or depth
    • G05D1/042—Control of altitude or depth specially adapted for aircraft
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control And Safety Of Cranes (AREA)

Abstract

本发明公开了一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法，包括如下步骤：建立四旋翼的数学模型和所吊挂负载的摆动模型；基于所建立的模型，对四旋翼和负载的状态进行约束，根据约束条件计算样条曲线的参数，得到规划路径；根据得到的规划路径，利用二分法求出满足摆角约束条件的最小运输时间；采用基于积分反步的串级控制方法，通过跟踪所规划的路径，实现了负载的精确定位和负载的摆动角度抑制，在最短的时间内将负载送达预定位置。本发明所公开的方法通过规划一个合适的平面运动轨迹，使得整个吊挂系统可以在最短的时间从初始位置到达期望位置，同时保持负载的摆角被限制在较小的范围内，既提高了运输效率，又保证了运输的安全性。

Description

一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法
技术领域
本发明涉及一种四旋翼控制方法，特别涉及一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法。
背景技术
近年来，四旋翼飞行器悬挂操纵装置已成为研究热点，并广泛应用于商业和科学领域。通常，操纵装置主要分为四种类型：夹持器、机械手、缆绳和其他类，不同的操纵装置适用于不同的应用场景。单自由度夹持器是使用最广泛的操纵装置，它直接安装在四旋翼无人机的机身上或机身下。这种操纵装置有三个优点：(1)易于制造，(2)建模和控制比较方便，(3)造价相对便宜。机械手主要由两部分组成：附在无人机机身上的一个或多个多自由度手臂和带有各种传感器的抓手，一般来说，手臂和夹持器由伺服电机驱动。相对于夹持器而言，机械手显著扩展了工作空间，并且可以利用机械手的冗余度来补偿无人机运动的位置误差。对于复杂的任务来说，这是一个更好的选择。然而在利用四旋翼无人机运输货物时，缆绳会被更多的使用，因为它具有质量轻，适用范围广，更容易被广大用户接受等特点，所以在执行一般性的运输任务中，缆绳会成为首选。除了以上三种类型的操纵装置以外，还有其他针对不同应用场景需求而设置的特殊的操纵装置。例如，一种由一个三自由度三角架、一个万向节和一个末端执行器组成的操纵装置用于与环境交互并进行超声无损实验的新型机械设计方案。
从使用四旋翼无人机运输货物的角度来说，使用缆绳是一种更大众化的选择，与外加夹持器和机械手相比，这种方式不会过大的增加四旋翼的携带重量，从而能够承载更重的货物，提高了运输效率。另一方面，使用缆绳运输货物对四旋翼无人机的姿态变化不会产生太大的影响，四旋翼的姿态响应不会因需要带动负载产生相同的姿态变化而变得迟缓。因此，对于使用绳索吊挂货物的四旋翼吊挂系统的研究是具有重要的现实意义。现有的关于四旋翼吊挂负载的研究中都做出了绳索长度不会变化的假设，事实上，这一假设大大的限制了用四旋翼吊挂系统运输货物的时效性。和直升机往下系绳索相似，在四旋翼下安装变绳长装置，实现在即将到达目的地时边飞行边往地面放下绳索，可以大大节省了装卸货物的时间，一定程度上提高了运输效率。可见，对变绳长四旋翼吊挂系统的研究是一个新的方向。
在现有的研究中，由于假设操纵装置(主要是抓手)和目标的质量比无人机要小，研究者忽略了操纵装置质心和惯性的变化，在此基础上直接设计飞行控制器。这种不考虑四旋翼无人机与负载间的相互作用的建模方法被称为“整体建模法”。然而，这是一种简单但不精确的简化建模方法。与此不同的一种建模方法是“分离建模法”，将四旋翼和所吊挂的负载分离成独立的两个部分，单独建立他们的运动学模型，利用他们之间的耦合关系设计飞行控制器，这种情况下通常将负载运动对四旋翼无人机的影响看作一个外部干扰，所设计的控制器应考虑对干扰的抑制作用。
在以上的建模方式中，基本都会做出吊挂负载的缆绳长度是固定的，这样在推导整个系统的动力学方程时不用考虑绳长的变化，绳长作为一个固定值就避免了其各阶导数的出现，因此系统模型不会过于复杂。正如前文所说，对于变绳长四旋翼吊挂系统的研究是有一定现实意义的，因此，在建模时将绳长的变化考虑在内，是现阶段我们要做的工作之一。
使用绳索吊挂货物虽然有它的一定优势，但是也会带来它特有的问题，比如负载的摆动。负载的摆动对四旋翼飞行器的飞行有负面影响。当负载摆角过大时，四旋翼控制系统的性能会受到较大的影响。因此，负载摆角应控制在合理范围内，以保证四旋翼吊挂系统的飞行性能。对于减摆方面的研究，也有许多文献资料可查，摆角抑制的方式主要分为两种：主动控制和被动控制。主动控制是提前分析目标出现偏差的可能性，采取相应的防护性措施，使目标得以实现，常见的控制方法有最优控制和输入整形，但最优控制对模型建立的精准度较为依赖，对于非线性、强耦合的四旋翼吊挂系统来说，建立十分精确的模型尚存在困难。输入整形技术对系统模型的精准度要求不高，但对于负载的摆动特性需要准确的预测，因而对于外界的干扰鲁棒性不高。被动控制是通过对系统进行实时监控，将系统输出得到的速度、加速度信息进行处理，反馈传送给控制器的输入端，控制与输入端的偏差，以实现目标，最常见的有PID控制和自适应控制等，但以上方法都是在摆角过大时，才产生较为明显的控制效果，且整个调节过程需要较长的时间。
发明内容
为解决上述技术问题，本发明提供了一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法，以实现以最优时间使得整个系统从初始位置到达期望位置，且在整个过程中，负载的摆动可被限制在一定范围以达到有效减摆的目的。
为达到上述目的，本发明的技术方案如下：
一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法，包括如下步骤：
步骤一，建立四旋翼的数学模型和所吊挂负载的摆动模型；
步骤二，基于所建立的模型，对四旋翼和负载的初始状态、结束状态，以及负载的摆动角度和摆动速度进行约束，利用5阶参数未知的三角样条曲线和4阶参数未知的三角样条曲线对四旋翼和负载的期望位置进行参数化，根据初始状态和结束状态的约束条件计算三角样条曲线的参数，得到满足状态约束的规划路径；
步骤三，根据得到的规划路径，利用负载的摆角与规划路径之间的关系，利用二分法求出满足摆动角度和摆动速度约束条件的最小运输时间；
步骤四，采用基于积分反步的串级控制方法，外环控制四旋翼的水平位置，内环控制四旋翼的姿态和高度，通过跟踪所规划的路径，实现了负载的精确定位和负载的摆动角度抑制，在最短的时间内将负载送达预定位置。
上述方案中，步骤一的具体方法如下：
(1)首先建立坐标系，包括惯性坐标系{I}＝{XI,YI,ZI}，四旋翼固定坐标系{B}＝{XB,YB,ZB}和负载坐标系{H}＝{XH,YH,ZH}，并假设负载坐标系原点与四旋翼固定坐标系重合，方向始终与惯性坐标系平行，所有坐标系的方向都遵循右手定则；
(2)建立四旋翼的数学模型：
Figure BDA0002880117470000031
其中，ωx，ωy，ωz为四旋翼在x，y，z轴的姿态角速度，
Figure BDA0002880117470000032
为四旋翼在x，y，z轴的姿态角加速度，Jx，Jy，Jz为四旋翼在x，y，z轴的转动惯量，Jr为整个电机转子和螺旋桨绕转轴的总转动惯量，Ωr为螺旋桨角速度的矢量和，d为电机中心到四旋翼中心的距离，
Figure BDA0002880117470000033
为四旋翼在x，y，z方向上的加速度，欧拉角Θ＝[φ,θ,ψ]T表示四旋翼的滚转角，俯仰角和偏航角，U1,U2,U3,U4为输入控制量，fox,foy,foz为吊绳对四旋翼的拉力在坐标轴方向上的分量，M为四旋翼的质量，g为重力加速度；
(3)建立负载的摆动模型：
将负载看作一个运动的质点，将其摆角分解为投影到两个平面内与竖直方向的夹角，其中，α为吊绳在XHOHZH平面内的投影与竖直方向的夹角，β为吊绳在YHOHZH平面内的投影与竖直方向的夹角；此外，吊绳长度的变化被认为是均匀的，即其一阶导数是一个常数，其二阶导数为零；
空间中，负载在惯性坐标系下的位置表示为：
Figure BDA0002880117470000041
其中，
Figure BDA0002880117470000042
为负载在惯性坐标系下的位置，
Figure BDA0002880117470000043
为四旋翼在惯性坐标系下的位置，
Figure BDA0002880117470000044
为负载在负载坐标系中的位置，RP为负载坐标系到惯性坐标系的旋转矩阵，其具体形式为
Figure BDA0002880117470000045
Rot(α),Rot(β)分别表示在负载坐标系中，负载围绕XH轴转β度和围绕YH转α度的旋转矩阵；
将公式(3)带入到公式(2)中后，最终负载在惯性坐标系下的位置表示为
Figure BDA0002880117470000046
其中，si＝sin(i),ci＝cos(i)，L表示吊绳的长度，(x，y，z)代表四旋翼在惯性坐标系下的位置；
对公式(4)求二阶导数，可得
Figure BDA0002880117470000047
其中，
Figure BDA0002880117470000048
表示负载在惯性坐标系x，y，z方向上的加速度，
Figure BDA0002880117470000049
为α的摆动速度，
Figure BDA00028801174700000410
为β的摆动速度，
Figure BDA00028801174700000411
为α的加速度，
Figure BDA00028801174700000412
为β的加速度，
Figure BDA00028801174700000414
为绳长变化率；
设吊绳上的拉力为f，则在惯性坐标系下拉力f沿各轴的分量为[-fsαcβ,fsβ,-fcαcβ]；
根据牛顿第二定律：
Figure BDA00028801174700000413
其中，m为负载的质量；
消去中间变量f，可得吊绳长度变化时负载的摆动模型为：
Figure BDA0002880117470000051
Figure BDA0002880117470000052
上述方案中，步骤二的具体方法如下：
设四旋翼的初始水平位置为x(0)＝xr0,y(0)＝yr0,负载的初始水平位置为xP(0)＝xr0,，yP(0)＝yr0，经过时间T后四旋翼到达期望位置x(T)＝xr,y(T)＝yr，负载到达期望位置xP(T)＝xr,yP(T)＝yr；
在初始状态时，控制整个吊挂系统处于静止状态，即初始时刻的状态约束：
Figure BDA0002880117470000053
在结束时刻，控制整个系统也处于静止状态，即结束时刻的状态约束：
Figure BDA0002880117470000054
在运输过程中，对负载的摆角以及摆角的摆动速度进行约束，即摆角的状态约束：
Figure BDA0002880117470000055
其中，αmax为摆角α的最大值，
Figure BDA0002880117470000056
为α的摆动速度，
Figure BDA0002880117470000057
为摆角α摆动速度的最大值，βmax为摆角β的最大值，
Figure BDA0002880117470000058
为β的摆动速度，
Figure BDA0002880117470000059
为摆角β摆动速度的最大值；
选择5阶参数未知的三角样条曲线和4阶参数未知的三角样条曲线对负载和四旋翼的期望位置进行参数化：
Figure BDA00028801174700000510
Figure BDA00028801174700000511
其中，τ1＝πt/(2T)，0≤t≤T，a0～a5，b1～b4，m0～m5，n1～n4均为待定参数；
Figure BDA0002880117470000061
Figure BDA0002880117470000062
其中，τ2＝πt/T，0≤t≤T，p0～p4，r0～r4 q1～q3，s1～s3均为待定参数；结合状态约束公式(9)～(11)，将公式(56)～(57)改写为如下：
Figure BDA0002880117470000063
以
Figure BDA0002880117470000064
为例，其计算过程如下：
当t＝0时，带入样条曲线
Figure BDA0002880117470000065
中可得
Figure BDA0002880117470000066
Figure BDA0002880117470000067
Figure BDA0002880117470000068
Figure BDA0002880117470000069
Figure BDA00028801174700000610
当t＝T时，带入样条曲线
Figure BDA00028801174700000611
中可得
Figure BDA00028801174700000612
Figure BDA00028801174700000613
Figure BDA0002880117470000071
Figure BDA0002880117470000072
Figure BDA0002880117470000073
同样，对样条曲线x*(t)，样条曲线
Figure BDA00028801174700000711
和样条曲线y*(t)采用同样的方法进行计算；利用matlab求解出公式(19)～(28)的解，得到满足约束的样条曲线参数为：
Figure BDA0002880117470000074
Figure BDA0002880117470000075
Figure BDA0002880117470000076
将以上参数带入到样条曲线中，得到即可获取样条曲线的具体表达式；
Figure BDA0002880117470000077
Figure BDA0002880117470000078
Figure BDA0002880117470000079
Figure BDA00028801174700000710
至此，规划的路径已经满足初始状态和结束状态的约束。
上述方案中，步骤三的具体方法如下：
负载的摆角与吊挂系统的路径有如下的关系：
Figure BDA0002880117470000081
将样条曲线(29)～(32)带入到(33)中，求出满足摆角约束条件的T值。
进一步的，二分法求满足摆动角度和摆动速度约束条件的最小运输时间T值的方法如下：
(1)给定输入：设定T的最大值和最小值、设定摆角及其摆速的最大值和最小值，设定精度；
(2)进行迭代：取T的最大值和最小值的平均值，带入到样条曲线中，判断是否满足角度约束条件，若满足，说明T值仍可取小，令当前最大值等于平均值，继续迭代；若不满足，说明T需取较大值，令当前最小值等于平均值，继续迭代；
(3)每次迭代后求出最大值与最小值之差，判断是否满足设定的精度，如满足，则输出结果，不满足，则继续迭代搜索；直至求出满足精度的T值，输出结果。
上述方案中，步骤四的具体方法如下：
将系统模型写成状态方程的形式：
Figure BDA0002880117470000082
Figure BDA0002880117470000083
其中，ζ1,ζ2为状态变量，ux＝cosφsinθcosψ+sinφsinψ，uy＝cosφsinθsinψ-sinφcosψ为虚拟控制输入；
Figure BDA0002880117470000084
ζ3＝[φ θ ψ]T,ζ4＝[ωx ωy ωz]T (76)
Figure BDA0002880117470000091
Figure BDA0002880117470000092
采用积分反步控制，设计的位置控制率为：
Figure BDA0002880117470000093
所设计的姿态控制率为：
Figure BDA0002880117470000094
其中，e1＝ζ1d-ζ1表示位置跟踪误差，ζ1d＝[x*,y*,zd]T表示期望位置，e2＝v1-ζ2表示虚拟控制误差，
Figure BDA0002880117470000095
Figure BDA0002880117470000096
表示误差导数，同理，e3＝ζ3d-ζ3表示姿态跟踪误差，e4＝v2-ζ4表示虚拟控制误差，
Figure BDA0002880117470000097
Figure BDA0002880117470000098
表示误差导数，p1为位置误差积分
Figure BDA0002880117470000099
p2为姿态误差积分
Figure BDA00028801174700000910
A1,A3均为3维单位矩阵，Λ1,Λ2,A2,A4为非负正定斜对称矩阵；
Figure BDA00028801174700000911
通过上述技术方案，本发明提供的一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法考虑了在运输负载的过程中，绳长变化对负载摆动特性的影响，通过规划一个合适的平面运动轨迹，使得整个吊挂系统可以在最短的时间从初始位置到达期望位置，同时保持负载的摆角被限制在较小的范围内，既提高了运输效率，又保证了运输的安全性。本发明在积分反步控制的基础上，和路径规划相结合，在实现整个系统的精准轨迹跟踪的同时，有效抑制负载的摆动，且对外部干扰的抑制都有较好的效果。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。
图1为本发明所公开的一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法流程图；
图2为本发明实施例所公开的一种四旋翼变绳长吊挂系统原理示意图；
图3为本发明所公开的控制方法原理框图；
图4为本发明所公开的二分法流程图；
图5为本发明实施例1仿真得到的四旋翼在x轴方向上的运动轨迹；
图6为本发明实施例1仿真得到的四旋翼在y轴方向上的运动轨迹；
图7为本发明实施例1仿真得到的四旋翼在z轴方向上的运动轨迹；
图8为本发明实施例1仿真得到的四旋翼滚转角姿态变化图；
图9为本发明实施例1仿真得到的四旋翼俯仰角姿态变化图；
图10为本发明实施例1仿真得到的四旋翼偏航角姿态变化图；
图11为本发明实施例1仿真得到的负载摆角α的运动情况图；
图12为本发明实施例1仿真得到的负载摆角β的运动情况图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。
本发明提供了一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法，如图1所示，包括如下步骤：
步骤一，建立四旋翼的数学模型和所吊挂负载的摆动模型，如图2所示；
(1)首先建立坐标系，包括惯性坐标系{I}＝{XI,YI,ZI}，四旋翼固定坐标系{B}＝{XB,YB,ZB}和负载坐标系{H}＝{XH,YH,ZH}，并假设负载坐标系原点与四旋翼固定坐标系重合，方向始终与惯性坐标系平行，所有坐标系的方向都遵循右手定则；
(2)建立四旋翼的数学模型：
Figure BDA0002880117470000111
其中，ωx，ωy，ωz为四旋翼在x，y，z轴的姿态角速度，
Figure BDA0002880117470000112
为四旋翼在x，y，z轴的姿态角加速度，Jx，Jy，Jz为四旋翼在x，y，z轴的转动惯量，Jr为整个电机转子和螺旋桨绕转轴的总转动惯量，Ωr为螺旋桨角速度的矢量和，d为电机中心到四旋翼中心的距离，
Figure BDA0002880117470000113
为四旋翼在x，y，z方向上的加速度，欧拉角Θ＝[φ,θ,ψ]T表示四旋翼的滚转角，俯仰角和偏航角，U1,U2,U3,U4为输入控制量，fox,foy,foz为吊绳对四旋翼的拉力在坐标轴方向上的分量，M为四旋翼的质量，g为重力加速度；
(3)建立负载的摆动模型：
将负载看作一个运动的质点，将其摆角分解为投影到两个平面内与竖直方向的夹角，其中，α为吊绳在XHOHZH平面内的投影与竖直方向的夹角，β为吊绳在YHOHZH平面内的投影与竖直方向的夹角；此外，吊绳长度的变化被认为是均匀的，即其一阶导数是一个常数，其二阶导数为零；
空间中，负载在惯性坐标系下的位置表示为：
Figure BDA0002880117470000114
其中，
Figure BDA0002880117470000115
为负载在惯性坐标系下的位置，
Figure BDA0002880117470000116
为四旋翼在惯性坐标系下的位置，
Figure BDA0002880117470000117
为负载在负载坐标系中的位置，RP为负载坐标系到惯性坐标系的旋转矩阵，其具体形式为
Figure BDA0002880117470000118
Rot(α),Rot(β)分别表示在负载坐标系中，负载围绕XH轴转β度和围绕YH转α度的旋转矩阵；
将公式(3)带入到公式(2)中后，最终负载在惯性坐标系下的位置表示为
Figure BDA0002880117470000121
其中，si＝sin(i),ci＝cos(i)，L表示吊绳的长度，(x，y，z)代表四旋翼在惯性坐标系下的位置；
对公式(4)求二阶导数，可得
Figure BDA0002880117470000122
其中，
Figure BDA0002880117470000123
表示负载在惯性坐标系x，y，z方向上的加速度，
Figure BDA0002880117470000124
为α的摆动速度，
Figure BDA0002880117470000125
为β的摆动速度，
Figure BDA0002880117470000126
为α的加速度，
Figure BDA0002880117470000127
为β的加速度，
Figure BDA0002880117470000128
为绳长变化率；
设吊绳上的拉力为f，则在惯性坐标系下拉力f沿各轴的分量为[-fsαcβ,fsβ,-fcαcβ]；
根据牛顿第二定律：
Figure BDA0002880117470000129
其中，m为负载的质量；
消去中间变量f，可得吊绳长度变化时负载的摆动模型为：
Figure BDA00028801174700001210
Figure BDA00028801174700001211
步骤二，基于所建立的模型，对四旋翼和负载的初始状态、结束状态，以及负载的摆动角度和摆动速度进行约束，利用5阶参数未知的三角样条曲线和4阶参数未知的三角样条曲线对四旋翼和负载的期望位置进行参数化，根据初始状态和结束状态的约束条件计算三角样条曲线的参数，得到满足状态约束的规划路径；
设四旋翼的初始水平位置为x(0)＝xr0,y(0)＝yr0,负载的初始水平位置为xP(0)＝xr0,，yP(0)＝yr0，经过时间T后四旋翼到达期望位置x(T)＝xr,y(T)＝yr，负载到达期望位置xP(T)＝xr,yP(T)＝yr；
在初始状态时，控制整个吊挂系统处于静止状态，即初始时刻的状态约束：
Figure BDA0002880117470000131
在结束时刻，控制整个系统也处于静止状态，即结束时刻的状态约束：
Figure BDA0002880117470000132
在运输过程中，对负载的摆角以及摆角的摆动速度进行约束，即摆角的状态约束：
Figure BDA0002880117470000133
其中，αmax为摆角α的最大值，
Figure BDA0002880117470000134
为α的摆动速度，
Figure BDA0002880117470000135
为摆角α摆动速度的最大值，βmax为摆角β的最大值，
Figure BDA0002880117470000136
为β的摆动速度，
Figure BDA0002880117470000137
为摆角β摆动速度的最大值；
选择5阶参数未知的三角样条曲线和4阶参数未知的三角样条曲线对负载和四旋翼的期望位置进行参数化：
Figure BDA0002880117470000138
Figure BDA0002880117470000139
其中，τ1＝πt/(2T)，0≤t≤T，a0～a5，b1～b4，m0～m5，n1～n4均为待定参数；
Figure BDA00028801174700001310
Figure BDA00028801174700001311
其中，τ2＝πt/T，0≤t≤T，p0～p4，r0～r4 q1～q3，s1～s3均为待定参数；结合状态约束公式(9)～(11)，将公式(96)～(97)改写为如下：
Figure BDA0002880117470000141
以
Figure BDA0002880117470000142
为例，其计算过程如下：
当t＝0时，带入样条曲线
Figure BDA0002880117470000143
中可得
Figure BDA0002880117470000144
Figure BDA0002880117470000145
Figure BDA0002880117470000146
Figure BDA0002880117470000147
Figure BDA0002880117470000148
当t＝T时，带入样条曲线
Figure BDA0002880117470000149
中可得
Figure BDA00028801174700001410
Figure BDA00028801174700001411
Figure BDA00028801174700001412
Figure BDA00028801174700001413
Figure BDA00028801174700001414
同样，对样条曲线x*(t)，样条曲线
Figure BDA00028801174700001415
和样条曲线y*(t)采用同样的方法进行计算；
利用matlab求解出公式(19)～(28)的解，得到满足约束的样条曲线参数为：
Figure BDA0002880117470000151
Figure BDA0002880117470000152
Figure BDA0002880117470000153
将以上参数带入到样条曲线中，得到即可获取样条曲线的具体表达式；
Figure BDA0002880117470000154
Figure BDA0002880117470000155
Figure BDA0002880117470000156
Figure BDA0002880117470000157
至此，规划的路径已经满足初始状态和结束状态的约束。
步骤三，根据得到的规划路径，利用负载的摆角与规划路径之间的关系，利用二分法求出满足摆动角度和摆动速度约束条件的最小运输时间；
负载的摆角与吊挂系统的路径有如下的关系：
Figure BDA0002880117470000158
将样条曲线(29)～(32)带入到(33)中，求出满足摆角约束条件的T值。
如图4所示，二分法求满足摆动角度和摆动速度约束条件的最小运输时间T值的方法如下：
(1)给定输入：设定T的最大值和最小值、设定摆角及其摆速的最大值和最小值，设定精度；
(2)进行迭代：取T的最大值和最小值的平均值，带入到样条曲线中，判断是否满足角度约束条件，若满足，说明T值仍可取小，令当前最大值等于平均值，继续迭代；若不满足，说明T需取较大值，令当前最小值等于平均值，继续迭代；
(3)每次迭代后求出最大值与最小值之差，判断是否满足设定的精度，如满足，则输出结果，不满足，则继续迭代搜索；直至求出满足精度的T值，输出结果。
步骤四，采用基于积分反步的串级控制方法，如图3所示，外环控制四旋翼的水平位置，内环控制四旋翼的姿态和高度，通过跟踪所规划的路径，实现了负载的精确定位和负载的摆动角度抑制，在最短的时间内将负载送达预定位置。
为了便于控制器设计，将系统模型写成状态方程的形式：
Figure BDA0002880117470000161
Figure BDA0002880117470000162
其中，ζ1,ζ2为状态变量，ux＝cosφsinθcosψ+sinφsinψ，uy＝cosφsinθsinψ-sinφcosψ为虚拟控制输入；
Figure BDA0002880117470000163
ζ3＝[φ θ ψ]T,ζ4＝[ωx ωy ωz]T (116)
Figure BDA0002880117470000171
Figure BDA0002880117470000172
采用积分反步控制，设计的位置控制率为：
Figure BDA0002880117470000173
所设计的姿态控制率为：
Figure BDA0002880117470000174
其中，e1＝ζ1d-ζ1表示位置跟踪误差，ζ1d＝[x*,y*,zd]T表示期望位置，e2＝v1-ζ2表示虚拟控制误差，
Figure BDA0002880117470000175
Figure BDA0002880117470000176
表示误差导数，同理，e3＝ζ3d-ζ3表示姿态跟踪误差，e4＝v2-ζ4表示虚拟控制误差，
Figure BDA0002880117470000177
Figure BDA0002880117470000178
表示误差导数，p1为位置误差积分
Figure BDA0002880117470000179
p2为姿态误差积分
Figure BDA00028801174700001710
A1,A3均为3维单位矩阵，Λ1,Λ2,A2,A4为非负正定斜对称矩阵；
Figure BDA00028801174700001711
实施例1
设定参数：四旋翼质量M＝1.4kg，负载质量m＝0.25kg，重力加速度g＝9.81m/s2，转动惯量Jx＝0.0087kg·m2,Jy＝0.0087kg·m2,Jz＝0.016kg·m2,Jr＝9.89e-5kg·m2，吊绳长度随时间变化为L＝1+0.1t m，电机距离四旋翼中心的距离d＝0.18m，四旋翼的初始位置为[xr0,yr0,zr0]＝[0,0,0]，期望位置为[xr,yr,zr]＝[3,4,5]，运输时间T的上下限分别为Tl＝4s，Tu＝7s，限制摆角最大值为αmax＝βmax＝0.3rad，最大摆动速率为
Figure BDA0002880117470000181
经过二分法计算出的T值为5.0313。
按照上述方法计算出的四旋翼参考轨迹为：
Figure BDA0002880117470000182
Figure BDA0002880117470000183
仿真实验：利用matlab进行仿真分析，仿真结果见图5-图12。结果表明：四旋翼能够携带负载在5秒左右运动到指定位置，在运输过程中，摆角的幅度被有效的限制在0.3rad之内，由此说明本发明方法的有效性。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (2)

1.一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法，其特征在于，包括如下步骤：
步骤一，建立四旋翼的数学模型和所吊挂负载的摆动模型；
步骤二，基于所建立的模型，对四旋翼和负载的初始状态、结束状态，以及负载的摆动角度和摆动速度进行约束，利用5阶参数未知的三角样条曲线和4阶参数未知的三角样条曲线对四旋翼和负载的期望位置进行参数化，根据初始状态和结束状态的约束条件计算三角样条曲线的参数，得到满足状态约束的规划路径；
步骤三，根据得到的规划路径，利用负载的摆角与规划路径之间的关系，利用二分法求出满足摆动角度和摆动速度约束条件的最小运输时间；
步骤四，采用基于积分反步的串级控制方法，外环控制四旋翼的水平位置，内环控制四旋翼的姿态和高度，通过跟踪所规划的路径，实现了负载的精确定位和负载的摆动角度抑制，在最短的时间内将负载送达预定位置；
步骤一的具体方法如下：
(1)首先建立坐标系，包括惯性坐标系{I}＝{XI,YI,ZI}，四旋翼固定坐标系{B}＝{XB,YB,ZB}和负载坐标系{H}＝{XH,YH,ZH}，并假设负载坐标系原点与四旋翼固定坐标系重合，方向始终与惯性坐标系平行，所有坐标系的方向都遵循右手定则；
(2)建立四旋翼的数学模型：
Figure FDA0003613395050000011
其中，ωx，ωy，ωz为四旋翼在x，y，z轴的姿态角速度，
Figure FDA0003613395050000012
为四旋翼在x，y，z轴的姿态角加速度，Jx，Jy，Jz为四旋翼在x，y，z轴的转动惯量，Jr为整个电机转子和螺旋桨绕转轴的总转动惯量，Ωr为螺旋桨角速度的矢量和，d为电机中心到四旋翼中心的距离，
Figure FDA0003613395050000013
为四旋翼在x，y，z方向上的加速度，欧拉角Θ＝[φ,θ,ψ]T表示四旋翼的滚转角，俯仰角和偏航角，U1,U2,U3,U4为输入控制量，fox,foy,foz为吊绳对四旋翼的拉力在坐标轴方向上的分量，M为四旋翼的质量，g为重力加速度；
(3)建立负载的摆动模型：
将负载看作一个运动的质点，将其摆角分解为投影到两个平面内与竖直方向的夹角，其中，α为吊绳在XHOHZH平面内的投影与竖直方向的夹角，β为吊绳在YHOHZH平面内的投影与竖直方向的夹角；此外，吊绳长度的变化被认为是均匀的，即其一阶导数是一个常数，其二阶导数为零；
空间中，负载在惯性坐标系下的位置表示为：
Figure FDA0003613395050000021
其中，
Figure FDA0003613395050000022
为负载在惯性坐标系下的位置，
Figure FDA0003613395050000023
为四旋翼在惯性坐标系下的位置，
Figure FDA0003613395050000024
为负载在负载坐标系中的位置，RP为负载坐标系到惯性坐标系的旋转矩阵，其具体形式为
Figure FDA0003613395050000025
Rot(α),Rot(β)分别表示在负载坐标系中，负载围绕XH轴转β度和围绕YH转α度的旋转矩阵；
将公式(3)带入到公式(2)中后，最终负载在惯性坐标系下的位置表示为
Figure FDA0003613395050000026
其中，si＝sin(i),ci＝cos(i)，L表示吊绳的长度，(x，y，z)代表四旋翼在惯性坐标系下的位置；
对公式(4)求二阶导数，可得
Figure FDA0003613395050000027
其中，
Figure FDA0003613395050000028
表示负载在惯性坐标系x，y，z方向上的加速度，
Figure FDA0003613395050000029
为α的摆动速度，
Figure FDA00036133950500000210
为β的摆动速度，
Figure FDA00036133950500000211
为α的加速度，
Figure FDA00036133950500000212
为β的加速度，
Figure FDA00036133950500000213
为绳长变化率；
设吊绳上的拉力为f，则在惯性坐标系下拉力f沿各轴的分量为[-fsαcβ,fsβ,-fcαcβ]；
根据牛顿第二定律：
Figure FDA00036133950500000214
其中，m为负载的质量；
消去中间变量f，可得吊绳长度变化时负载的摆动模型为：
Figure FDA0003613395050000031
Figure FDA0003613395050000032
步骤二的具体方法如下：
设四旋翼的初始水平位置为x(0)＝xr0,y(0)＝yr0,负载的初始水平位置为xP(0)＝xr0，yP(0)＝yr0，经过时间T后四旋翼到达期望位置x(T)＝xr,y(T)＝yr，负载到达期望位置xP(T)＝xr,yP(T)＝yr；
在初始状态时，控制整个吊挂系统处于静止状态，即初始时刻的状态约束：
Figure FDA0003613395050000033
在结束时刻，控制整个系统也处于静止状态，即结束时刻的状态约束：
Figure FDA0003613395050000034
在运输过程中，对负载的摆角以及摆角的摆动速度进行约束，即摆角的状态约束：
Figure FDA0003613395050000035
其中，αmax为摆角α的最大值，
Figure FDA0003613395050000036
为α的摆动速度，
Figure FDA0003613395050000037
为摆角α摆动速度的最大值，βmax为摆角β的最大值，
Figure FDA0003613395050000038
为β的摆动速度，
Figure FDA0003613395050000039
为摆角β摆动速度的最大值；
选择5阶参数未知的三角样条曲线和4阶参数未知的三角样条曲线对负载和四旋翼的期望位置进行参数化：
Figure FDA00036133950500000310
Figure FDA00036133950500000311
其中，τ1＝πt/(2T)，0≤t≤T，a0～a5，b1～b4，m0～m5，n1～n4均为待定参数；
Figure FDA0003613395050000041
Figure FDA0003613395050000042
其中，τ2＝πt/T，0≤t≤T，p0～p4，r0～r4 q1～q3，s1～s3均为待定参数；结合状态约束公式(9)～(11)，将公式(16)～(17)改写为如下：
Figure FDA0003613395050000043
以
Figure FDA0003613395050000044
为例，其计算过程如下：
当t＝0时，带入样条曲线
Figure FDA0003613395050000045
中可得
Figure FDA0003613395050000046
Figure FDA0003613395050000047
Figure FDA0003613395050000048
Figure FDA0003613395050000049
Figure FDA00036133950500000410
当t＝T时，带入样条曲线
Figure FDA00036133950500000411
中可得
Figure FDA00036133950500000412
Figure FDA00036133950500000413
Figure FDA0003613395050000051
Figure FDA0003613395050000052
Figure FDA0003613395050000053
同样，对样条曲线x*(t)，样条曲线
Figure FDA0003613395050000054
和样条曲线y*(t)采用同样的方法进行计算；
利用matlab求解出公式(19)～(28)的解，得到满足约束的样条曲线参数为：
Figure FDA0003613395050000055
a4＝m4＝0,
Figure FDA0003613395050000056
Figure FDA0003613395050000057
b2＝n2＝0,
Figure FDA0003613395050000058
Figure FDA0003613395050000059
p2＝r2＝p4＝r4＝q1＝s1＝q2＝s2＝q3＝s3＝0；
将以上参数带入到样条曲线中，得到即可获取样条曲线的具体表达式；
Figure FDA00036133950500000510
Figure FDA00036133950500000511
Figure FDA00036133950500000512
Figure FDA00036133950500000513
至此，规划的路径已经满足初始状态和结束状态的约束；
步骤三的具体方法如下：
负载的摆角与吊挂系统的路径有如下的关系：
Figure FDA0003613395050000061
将样条曲线(29)～(32)带入到(33)中，求出满足摆角约束条件的T值；
步骤四的具体方法如下：
将系统模型写成状态方程的形式：
Figure FDA0003613395050000062
Figure FDA0003613395050000063
其中，ζ1,ζ2为状态变量，ux＝cosφsinθcosψ+sinφsinψ，uy＝cosφsinθsinψ-sinφcosψ为虚拟控制输入；
Figure FDA0003613395050000064
ζ3＝[φ θ ψ]T,ζ4＝[ωx ωy ωz]T (36)
Figure FDA0003613395050000065
Figure FDA0003613395050000071
采用积分反步控制，设计的位置控制率为：
Figure FDA0003613395050000072
所设计的姿态控制率为：
Figure FDA0003613395050000073
其中，e1＝ζ1d-ζ1表示位置跟踪误差，ζ1d＝[x*,y*,zd]T表示期望位置，e2＝v1-ζ2表示虚拟控制误差，
Figure FDA0003613395050000074
Figure FDA0003613395050000075
表示误差导数，同理，e3＝ζ3d-ζ3表示姿态跟踪误差，e4＝v2-ζ4表示虚拟控制误差，
Figure FDA0003613395050000076
Figure FDA0003613395050000077
表示误差导数，p1为位置误差积分
Figure FDA0003613395050000078
p2为姿态误差积分
Figure FDA0003613395050000079
A1,A3均为3维单位矩阵，Λ1,Λ2,A2,A4为非负正定斜对称矩阵；
Figure FDA00036133950500000710
2.根据权利要求1所述的一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法，其特征在于，二分法求满足摆动角度和摆动速度约束条件的最小运输时间T值的方法如下：
(1)给定输入：设定T的最大值和最小值、设定摆角及其摆速的最大值和最小值，设定精度；
(2)进行迭代：取T的最大值和最小值的平均值，带入到样条曲线中，判断是否满足角度约束条件，若满足，说明T值仍可取小，令当前最大值等于平均值，继续迭代；若不满足，说明T需取较大值，令当前最小值等于平均值，继续迭代；
(3)每次迭代后求出最大值与最小值之差，判断是否满足设定的精度，如满足，则输出结果，不满足，则继续迭代搜索；直至求出满足精度的T值，输出结果。
CN202011631309.2A 2020-12-31 2020-12-31 一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法 Active CN112817328B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011631309.2A CN112817328B (zh) 2020-12-31 2020-12-31 一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011631309.2A CN112817328B (zh) 2020-12-31 2020-12-31 一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法

Publications (2)

Publication Number Publication Date
CN112817328A CN112817328A (zh) 2021-05-18
CN112817328B true CN112817328B (zh) 2022-08-02

Family

ID=75855157

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011631309.2A Active CN112817328B (zh) 2020-12-31 2020-12-31 一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法

Country Status (1)

Country Link
CN (1) CN112817328B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114035598B (zh) * 2021-11-22 2023-11-24 青岛理工大学 一种多旋翼吊挂系统的视觉摆角检测与减摆方法
CN114779649B (zh) * 2022-06-16 2022-09-06 南京理工大学 四旋翼无人机悬挂负载运输控制方法
CN115686038B (zh) * 2022-10-14 2024-05-28 青岛理工大学 一种四旋翼吊挂系统的摆角观测与减摆控制系统及方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2006056648A (ja) * 2004-08-19 2006-03-02 Mitsubishi Heavy Ind Ltd 吊荷の振れ量検出装置およびこれを備えたクレーン
CN101985343A (zh) * 2009-07-08 2011-03-16 嫩青利勃海尔-维克股份有限公司 用于操纵悬吊在承载缆上的负载的起重机
CN105372992A (zh) * 2015-12-01 2016-03-02 哈尔滨理工大学 一种悬吊漂浮物随动系统的欠驱动滑模控制算法
CN108345217A (zh) * 2018-02-12 2018-07-31 南开大学 变绳长起重机系统时间最优轨迹规划方法、装置及系统
CN111008467A (zh) * 2019-11-29 2020-04-14 天津大学 无人机吊挂飞行运输系统的减摆轨迹在线规划方法
CN111061282A (zh) * 2019-11-29 2020-04-24 天津大学 基于能量法的四旋翼无人机吊挂飞行系统控制方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110928322B (zh) * 2019-12-09 2021-07-06 大连理工大学 四旋翼飞行器运送悬挂负载的非线性运动控制方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2006056648A (ja) * 2004-08-19 2006-03-02 Mitsubishi Heavy Ind Ltd 吊荷の振れ量検出装置およびこれを備えたクレーン
CN101985343A (zh) * 2009-07-08 2011-03-16 嫩青利勃海尔-维克股份有限公司 用于操纵悬吊在承载缆上的负载的起重机
CN105372992A (zh) * 2015-12-01 2016-03-02 哈尔滨理工大学 一种悬吊漂浮物随动系统的欠驱动滑模控制算法
CN108345217A (zh) * 2018-02-12 2018-07-31 南开大学 变绳长起重机系统时间最优轨迹规划方法、装置及系统
CN111008467A (zh) * 2019-11-29 2020-04-14 天津大学 无人机吊挂飞行运输系统的减摆轨迹在线规划方法
CN111061282A (zh) * 2019-11-29 2020-04-24 天津大学 基于能量法的四旋翼无人机吊挂飞行系统控制方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
Adaptive_boundary_control_of_an_axially_moving_string_system_Application_to_container_cranes;Quang Hieu Ngo;《IEEE International Symposium on Industrial Electronics (ISlE 2009)》;20091231;全文 *
Swing-free stop control of the slewing motion of a mobile crane;JacekKłosiński;《Control Engineering Practice》;20050430;第13卷(第4期);全文 *
基于B样条曲线的四旋翼系绳运输系统轨迹规划;梁晓;《飞行力学》;20191031;第37卷(第5期);全文 *
带吊挂负载的四旋翼无人机的控制系统设计;代苏艳;《内蒙古科技大学学报》;20171231;第36卷(第4期);全文 *

Also Published As

Publication number Publication date
CN112817328A (zh) 2021-05-18

Similar Documents

Publication Publication Date Title
CN112817328B (zh) 一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法
US11378983B2 (en) Stable flight control method for multi-rotor unmanned aerial vehicle based on finite-time neurodynamics
Jimenez-Cano et al. Aerial manipulator for structure inspection by contact from the underside
Suarez et al. Lightweight and human-size dual arm aerial manipulator
Ritz et al. Carrying a flexible payload with multiple flying vehicles
Ariyibi et al. Quaternion-based nonlinear attitude control of quadrotor formations carrying a slung load
de Angelis et al. Two-time-scale control of a multirotor aircraft for suspended load transportation
Godbole et al. Nonlinear control of unmanned aerial vehicles with cable suspended payloads
WO2020177267A1 (zh) 四旋翼无人机的控制方法、装置、设备及可读介质
CN109725643B (zh) 一种基于主动建模的旋翼飞行器非平衡负载吊运系统的控制方法
Son et al. Model predictive control of a multi-rotor with a suspended load for avoiding obstacles
CN111459188B (zh) 一种基于四元数的多旋翼非线性飞行控制方法
Zhang et al. Cascaded control for balancing an inverted pendulum on a flying quadrotor
Zha et al. Quaternion-based nonlinear trajectory tracking control of a quadrotor unmanned aerial vehicle
Korpela et al. Towards the realization of mobile manipulating unmanned aerial vehicles (MM-UAV): Peg-in-hole insertion tasks
Ali et al. Controlling of an under-actuated quadrotor UAV equipped with a manipulator
Sun et al. Dynamic modeling and hardware-in-loop simulation for a tail-sitter unmanned aerial vehicle in hovering flight
CN116069050A (zh) 一种抗扰动的旋翼无人机吊载系统减摇控制方法及装置
Abuzayed et al. Design of lightweight aerial manipulator with a CoG compensation mechanism
Roque et al. Fast model predictive image-based visual servoing for quadrotors
Barakou et al. Real-time applicable cooperative aerial manipulation: A survey
CN113031655B (zh) 一种伴有负载升降的飞行吊运系统及其控制方法
Sun et al. Trajectory planning of quadrotor uav with suspended payload based on predictive control
CN110221543A (zh) 一种利用系绳/推力协调的无人机吊挂系统摆动抑制方法
CN114237291B (zh) 一种柔性约束多无人机编队控制方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant