四旋翼无人机悬挂负载运输控制方法

1.本发明涉及无人机飞行控制技术领域，尤其涉及一种四旋翼无人机悬挂负载运输控制方法。


背景技术：

2.无人机发展已有百余年历史，随着科学技术的不断进步和相关学科的快速发展，无人机逐渐能够胜任越来越多复杂困难的飞行任务，目前已经应用于各行各业中。无人机的类型多种多样，其中四旋翼无人机因其结构简单，控制性能优越，成为用途最为广泛的机型。
3.与此同时，随着物流行业的规模逐渐扩大和物联网技术的不断更新，借助无人机实现物流运输终端环节的无人化精准安全配送是未来的发展趋势。旋翼式无人机悬挂负载运输的方式，可以为货物的装卸提供便利，可以在不需要降落无人机的情况下进行装卸。同时，这种通过绳索悬挂负载的方式对无人机的影响也相对较小，不会改变无人机的重心、转动惯量等重要参数；只要在无人机载荷范围内，对负载的形状也没有过多的要求。
4.关于获取目标物体距离信息是实现自动驾驶以及自由移动机器人等技术中最为重要的一个环节，其技术方式根据传感器类型可以分为两大类：接触式测量技术和非接触式测量技术。其中接触式测量技术通过使用接触式传感器来获取障碍物信息，尽管该技术设计和实现过程相对简单，但是由接触式测量技术设计而成的系统可预见性弱，自主控制程度低。如果在复杂的环境中使用该技术的测距系统，导致的结果是避障效率比较低，而且可能对测量系统本身以及周围环境的物品造成难以预估的结果，此项技术不能很好融入人类生活。非接触方式则是通过一些非接触式的传感器来获取障碍物距离信息，其种类多、可控性高、应用场景广，因此非接触式是一个热门的研究领域。
5.双目相机是最常用的一种测距方法，其实现过程是模拟人类眼睛的成像模型。通过计算实际场景中的物体在左右相机成像，并融合左右图像信息而带来的视差信息来计算距离信息。双口相机获得的距离信息与视差信息成反比，当采集得到的视差数据越大时，实际测得的距离信息越近。双目相机的测距方法相对与其它方法，其设计成本低，普通的相机就能完成测距效果，由于是模拟人眼，其实现过程很直观，而且对于远距离的口标，测量结果比较准确。
6.目前，四旋翼无人机悬挂负载运输时，存在负载的摆动幅度大，无法实现位置的精确控制，到达指定位置费时费力的问题。


技术实现要素：

7.本发明提供了一种四旋翼无人机悬挂负载运输控制方法，实现了四旋翼无人机悬挂运输负载到指定位置并使负载的摆动程度降到最小以保护负载的目的。
8.为了实现本发明的目的，所采用的技术方案是：四旋翼无人机悬挂负载运输控制方法，控制方法包括如下步骤：
s1、在无人机底部安装双目相机组成四旋翼无人机悬挂负载运输系统，通过双目相机测量绳索与惯性坐标系重力方向之间的夹角；s2、建立四旋翼无人机悬挂负载运输系统平面模型的动力学模型；s3、根据lyapunov稳定性定理，建立状态误差子系统，状态误差子系统包括实际位置与期望位置的误差和实际负载摆角与期望摆角的误差；s4、根据无人机的位置误差和负载摆动抑制选择合适的lyapunov函数，以设计无人机位置控制器，针对负载质量不确定的情况，在无人机位置控制器中加入了自适应控制部分；s5、根据lyapunov稳定性定理证明无人机位置控制器的稳定性，从而使得无人机的实际位置最终收敛到期望位置。
9.作为本发明的优化方案，控制方法还包括，根据四旋翼无人机欠驱动特性，对四旋翼无人机的位置控制采用外环控制，由以下公式计算四旋翼无人机的期望姿态横滚角以及期望总升力：
ꢀꢀꢀ
（1）（2）其中，uy(t)为无人机的位置外环控制关于惯性坐标系yi轴方向的控制输入，uz(t)为与uy(t)相对应的惯性坐标系zi轴方向的控制输入，
ϕd(t)为根据位置外环控制所计算得到的期望姿态横滚角，fd(t)为根据位置外环控制所计算得到的期望总升力。
10.作为本发明的优化方案，在步骤s1中，通过双目相机获取负载距离无人机重心所在的水平平面的距离，接合绳索的长度，根据三角形函数关系计算绳索与惯性系重力方向之间的夹角。
11.作为本发明的优化方案，步骤s2中，四旋翼无人机悬挂负载运输系统平面模型的动力学模型为：此外，m(q(t))的表达式为：此外，m(q(t))的表达式为：的表达式为：g(q(t))的表达式为：u(t)的表达式为：
式（3）中， 表示系统的广义坐标，y(t)是无人机惯性坐标系yi轴方向上的位移，z(t)则是与y(t)相对应的zi轴方向上的位移，θ(t)是悬挂绳索与惯性坐标系重力方向之间的夹角；m(q(t)) 表示系统的惯量矩阵， 表示系统的向心力矩阵，g(q(t))表示系统的重力向量，u(t)表示系统的螺旋桨升力向量，，，j表示无人机的转动惯量，
ϕ
(t)表示无人机的实时滚转角，τ(t)表示无人机的姿态驱动力矩；式（4）中，mq为无人机的质量，m
p
为所运输负载的质量，l代表绳索的长度；式（6）中，g=9.8m/s2，g表示重力加速度。
12.作为本发明的优化方案，步骤s3中，实际位置与期望位置的误差如下：（8）式（8）中，ey(t)表示当前时刻惯性系下yi轴的实际位置与期望位置的差值，ez(t)则表示当前时刻惯性系下zi轴方向上的实际位置与期望位置差值，e(t)为无人机在三个广义自由度上的误差信号向量，，yd表示期望yi轴位置，zd表示期望zi轴位置。
13.作为本发明的优化方案，步骤s4中，无人机位置控制器的控制输入uy(t)，uz(t)如下：（9）式中， 为控制增益系数，k
py
为y轴的控制增益系数，k
pz
为z轴的控制增益系数，是ω的在线估计值，ω=(mq m
p
)g表示四旋翼无人机与负载的总重力，。
14.作为本发明的优化方案，在无人机位置控制器中加入的自适应控制部分为：（10）（11）其中：是正实数参数，均为权重系数，ρ(s)是一个微分饱和函数，s就是函数的自变量。
15.作为本发明的优化方案，步骤s4的lyapunov候选函数v(t)：
（12）式（12）中ρe(e(t)/2)定义如下：（13）其中：是ω的在线估计误差，。
16.本发明具有积极的效果：1）本发明基于能量法设计了一种非线性控制策略，在负载质量准确数值未知但其质量范围可以确定的情况下，实现了对无人机位置的精确控制和飞行过程中负载摆动的快速抑制；2）本发明在不影响四旋翼无人机飞行性能的前提下，通过双目测距摄像头可以获取准确的负载摆角信息，同时所设计的运输控制方法能够使无人机悬挂负载运输系统准确、稳定的跟踪期望轨迹，并且使负载的摆动减至最小，以保证所运输负载的安全；3）本发明方法不需要依赖于高精度的传感器，不需要搭载高性能的机载计算机，有效减轻无人机的负载，提高无人机续航能力，具备重量轻、高可靠性、高精度的优点。
附图说明
17.下面结合附图和具体实施方式对本发明作进一步详细的说明。
18.图1是本发明四旋翼无人机悬挂负载运输系统的示意图；图2是本发明四旋翼无人机悬挂负载运输控制方法的流程图；图3是绳索与惯性坐标系重力方向之间的夹角的获取示意图；图4是双目相机的测距原理。
具体实施方式
19.如图1-3所示，本发明公开了四旋翼无人机悬挂负载运输控制方法，控制方法包括如下步骤：s1、在无人机底部安装双目相机组成四旋翼无人机悬挂负载运输系统，通过双目相机测量绳索与惯性坐标系重力方向之间的夹角；目前，大多关于悬挂负载摆动抑制的无人机位置控制都需要测量负载摆角的信息，考虑到这一点，本发明首先提供了四旋翼无人机悬挂负载运输系统的组成，除了四旋翼无人机机载集成imu、gps等常规传感器外，通过在无人机底部安装双目相机组成四旋翼无人机悬挂负载运输系统。
20.在步骤s1中，通过双目相机获取负载距离无人机重心所在的水平平面的距离，接合绳索的长度，根据三角形函数关系计算绳索与惯性系重力方向之间的夹角。
21.如图2所示，为定义的惯性坐标系，ge3表示重力加速度向量，为单位向量，θ(t)是悬挂绳索与惯性坐标系重力方向之间的夹角。
22.四旋翼无人机悬挂负载运输系统（简称系统）是一个非线性、欠驱动、强耦合的系统，四旋翼无人机（简称无人机）的位置、姿态和负载摆动状态的控制，都只能通过改变四个电机的转速来进行控制。无人机的位置、姿态等状态可以通过机载飞控单元中集成的惯性测量单元（inertial measurement unit，imu）、全球定位系统（global position system，gps）、陀螺仪等元件测量得到。但是负载是通过绳索固定在机身重心位置，考虑到负载装卸的便利性和绳索本身的限制不方便设计安装测量元件，因此关于负载的位置、绳索摆动相对于重力方向的角度、角速度等状态信息是不容易获得的。
23.针对如何测量绳索摆动与重力加速度向量之间的夹角如何获取，本发明依赖于机器视觉技术，通过在无人机底部安装双目相机以获取负载距离无人机重心所在水平平面xioyi的距离，进而计算出绳索与惯性坐标系重力方向之间的夹角。在步骤s1中，通过双目相机获取负载距离无人机重心所在的水平平面的距离，接合绳索的长度，根据三角形函数关系计算绳索与惯性系重力方向之间的夹角。基于机器视觉的测距技术获取负载到水平平面的距离d，l代表绳索的长度可以精确测量。
24.双目成像模型测距原理：双目相机是由左右两个相机组合而成，使用两个相机拍摄实际场景的某一点，将得到左右两幅图像数据，然后利用这一点在左右图像数据中存在的视线偏差，结合三角测量原理进行三维场景信息的恢复，实现重建三维场景，从而计算得到空间某一点到相机的距离。建立四旋翼无人机悬挂负载运输系统的动力学模型。
25.假设双目成像是一个理想的平行双目立体视觉模型，其成像模型如图4所示。左右两个相机位于同一个平面（光轴是相互平行），并且左右两个相机的参数是相同的。点o为双目相机坐标系的原点，o
l
、or分别是左右相机的中心点。点p为实际场景中的一个点，其与成像面相交于点p
l
和pr，分别代表在左右两个相机的成像点。将三个点p、p
l
和pr分别投影到相机坐标系x轴上，交点依次标记为、m和n。用d来表示点p到相机的距离。b是两个相机间的距离，也被称为基距。在理想的模型中，两个相机的焦距是相同的，都用f来表示。
26.根据三角形相似原理，可以得三角形pqpr相似于三角形，三角形pqp
l
相似于三角形，可以得到下列等式关系：
ꢀꢀꢀꢀ
（14）
ꢀꢀꢀꢀꢀꢀꢀꢀ
（15）式中，记|qpr|=a，并令理想模型中的|nor|=m，|mo
l
|=n，于是，式（14）和（15）可以整理为：
ꢀꢀꢀꢀꢀ
（16）将式（16）进一步化简可以得：
ꢀꢀꢀꢀꢀꢀꢀꢀ
（17）设点p
l
和pr的像素坐标系上的位置为(u
l
、v
l
)和(ur、vr)，从长度数量关系上可以得到u
l-ur=m n，式（17）可以进一步整理为：（18）最后，记u
l-ur=d，式（18）可以表示为（19）式中，d被称为视差，即点p在左右两个相机上像素坐标系u轴上的差值。
27.根据式（19）可知，在双目立体视觉中某一点到相机的距离，与相机的焦距f，两个相机间的基距b和视差d有关。焦距和基距是固定且已知的两个参数，只有视差是未知量。从距离计算公式可以看出，视差与距离成反比的关系，某一点在左右图像上的视差值越小，其实际距离就越远。左右图像的视差获取的过程较为复杂，需要通过左右相机获取的图像进行匹配，才能找到某一点的视差值。
28.s2、建立四旋翼无人机悬挂负载运输系统平面模型的动力学模型；针对四旋翼无人机悬挂负载运输系统的控制设计在二维动力学模型的基础上进行研究，即只考虑无人机在yi轴和zi轴两个自由度（注：坐标轴符号带上标“i”表示是对应惯性坐标系（inertial frame）下；带上标“b”则表示对应为机体坐标系（body frame），文中没有做特殊说明的符号均为惯性坐标系下的表示），并考虑负载摆角的快速抑制。仅考虑上述两个自由度在化简动力学模型的同时也可以实现无人机的位置控制。关于部分符号的说明，表示n维向量，则表示m行n列的矩阵。
29.通过推导，步骤s2中，四旋翼无人机悬挂负载运输系统平面模型的动力学模型为：式（3）中， 表示系统的广义坐标，y(t)是无人机惯性坐标系yi轴方向上的位移，z(t)则是与y(t)相对应的zi轴方向上的位移，θ(t)是悬挂绳索与惯性坐标系重力方向之间的夹角；m(q(t)) 表示系统的惯量矩阵， 表示系统的向心力矩阵，g(q(t))表示系统的重力向量，u(t)表示系统的螺旋桨升力向量，j表示无人机的转动惯量，
ϕ
(t)表示无人机的实时滚转角，τ(t)表示无人机的姿态驱动力矩；此外，m(q(t))的表达式为：
式（4）中，mq为无人机的质量，m
p
为所运输负载的质量，l代表绳索的长度的表达式为：g(q(t))的表达式为：式（6）中，g=9.8m/s2，g表示重力加速度；u(t)的表达式为：式中，将式（4）~（7）代入式（3）中，可得到运输系统平面模型的动力学模型的展开式为： （20）对于上述的运输系统平面模型的动力学模型，可以推导出如下性质：（1）惯性矩阵m(q(t))是对称的正定矩阵，且对于任意的向量，存在两个整数λm和λm，可以理解为惯性矩阵的上界和下界，则有下式成立： （21）（2）惯性矩阵m(q(t))和向心力矩阵具有如下关系：
ꢀꢀ
(22)（3）本发明在无法得知悬挂负载的精确质量，但是可以明确负载质量数值范围的情况下，数学表述如下：
ꢀꢀꢀꢀꢀꢀ
（23）式中，表示负载质量范围的下界，而则表示范围的上界。
30.（4）在实际飞行过程中，可以假设负载始终处于无人机机体坐标系xboyb平面的下
方，也即负载绳索的摆角满足以下条件：
ꢀꢀꢀ
（24）四旋翼无人机悬挂飞行运输系统的飞行控制目标是保证无人机飞行纵向和垂直方向的运动飞行到目标期望位置，在飞行过程中期望绳索的摆动角度最小并最终收敛到0，表示如下：（25）式中，表示无人机的目标期望位置。
31.s3、根据lyapunov稳定性定理，建立状态误差子系统，状态误差子系统包括实际位置与期望位置的误差和实际负载摆角与期望摆角的误差；步骤s3中，实际位置与期望位置的误差如下：（8）式（8）中，ey(t)表示当前时刻惯性系下yi轴的实际位置与期望位置的差值，ez(t)则表示当前时刻惯性系下zi轴方向上的实际位置与期望位置差值，e(t)为无人机在三个广义自由度上的误差信号向量，，yd表示期望yi轴位置，zd表示期望zi轴位置。
32.并进一步可得：（26）s4、根据无人机的位置误差和负载摆动抑制选择合适的lyapunov函数，以设计无人机位置控制器，针对负载质量不确定的情况，在无人机位置控制器中加入了自适应控制部分；选择合适的lyapunov函数，该函数考虑了无人机的位置误差和负载摆动抑制，以设计无人机位置控制器，在这一步骤中还考虑了负载质量这一参数存在不确定性，在无人机位置控制器中加入了自适应控制部分。
33.四旋翼无人机悬挂负载运输系统的机械能e(t)可以定义如下：
ꢀꢀ
（27）基于上式对时间t求一阶导数，可得：
ꢀꢀꢀꢀꢀꢀꢀ
（28）式中ω=(mq m
p
)g表示四旋翼无人机与负载的总重力，根据系统的能量无源性特性，
步骤s4中，无人机位置控制器的控制输入uy(t)，uz(t)如下：
ꢀꢀ
（9）式中， 为控制增益系数，k
py
为y轴的控制增益系数，k
pz
为z轴的控制增益系数，是ω的在线估计值。
34.将控制器式（9）带入式（28）中，可得：
ꢀꢀꢀꢀꢀ
（29）其中是ω的在线估计误差，定义如下：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（30）对式（30）求导，可得：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（31）的参数自适应律设计如下：
ꢀꢀ
(10)其中，是正实数参数，为权重系数，是可以任意取值的系数。式（10）中ρ(s)是一个微分饱和函数，此处s为微分饱和函数的变量用以解释函数性质，具体使用该微分饱和函数时可以将s替换为对应的变量或者函数。其具体定义如下：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（11）可以证明，式（11）中的ρ(s)函数具有如下性质：（1）对任意；（2）定义ρ(s)关于s的导数为：
ꢀꢀꢀ
（32）且对任意；（3）对任意。
35.s5、根据lyapunov稳定性定理证明无人机位置控制器的稳定性，从而使得无人机的实际位置最终收敛到期望位置。
36.步骤s5上述所设计的飞行控制器进行系统稳定性的证明分析。
37.对于如式（20）所示的四旋翼无人机悬挂负载运输系统，考虑负载质量m
p
确切值未知但其范围可以确定的情况下，运用式（10）中的负载质量估计自适应更新律，可以使系统中负载质量的估计值渐进收敛于实际值；式（9）中所设计的非线性位置外环控制器可以使四旋翼无人机从当前初始位置渐进收敛到目标期望位置，过程中还加入了能够使负载摆角渐进收敛到零的摆动抑制项，使其摆角最终为零，数学表示如下：
ꢀꢀꢀꢀꢀ
（33）对于所设计控制器的稳定性证明如下：根据lyapunov稳定性定理，若代表系统的能量函数在初始状态时是存在且大于或等于零的，而且能量函数的导数是小于等于零的，则可以推导得到系统的能量逐渐减小并收敛到平衡稳定的状态。所以在证明所设计控制器的稳定性时，首先需要选取合适的系统能量lyapunov函数，本发明选取如下lyapunov候选函数 v(t)：（12）在上式中，项表示悬挂负载的势能，与θ(t)有关，考虑当θ(t)等于期望摆角时，悬挂负载的势能达到一个平衡。在这一项中，e(t)也包含θ(t)-θd(t)，所述控制器设计考虑了实际负载摆角与期望摆角的误差。式（12）中ρe(e(t)/2)的定义如下：
ꢀꢀ
（13）对于v(t)中的项，根据式（32）可知，其符号与的符号一致，所以，易得v(t)≥0，可知lyapunov候选函数是半正定的。
38.根据lyapunov方法构造系统能量函数，考虑系统动能、负载势能、误差子系统能量、负载重力估计误差以及辅助项，确定以下公式为lyapunov候选函数：其中，公式（12）等式右端第一项表示为加权重后的系统总动能，第二项为加权后的负载势能，第三项为y轴位置误差所对应的能量，第四项为z轴位置误差所对应的能量，第五项为辅助项，第六项为负载重力信息在线估计的误差能量；lyapunov函数，设计相关位置
外环控制器，其中包含了悬挂负载的摆动抑制控制，通过外环控制得到姿态内环控制的期望输入；根据所述lyapunov函数，依据lyapunov稳定性定理证明了所设计控制器的稳定性。
39.对式（12）求一阶时间导数，可得：（34）对于式（34）中项，由式（3）可得： （35）
ꢀꢀ
其中，q(t)的表达式为： （36）根据式（35），可得：（37）对于式（34）中的，将式（22）代入，整理得：
ꢀꢀꢀꢀꢀꢀ
（38）对于式（34）中的，将式（10）代入，有：（39）将式（35）~（39）代入式（34），并合并同类项做化简可得：
（40）对于式（40）中的：项，由ρ(s)函数的性质（3）可得：（41）对于式（40）中的项，有下列不等式成立：（42）对于式（40）中的项，将式（9）和式（33）代入，整理可得： （43）对于式（40）中的项，由式（21）可得（44）令，则由式（44）和ρ(s)函数的性质（2）可得：（45）将式（41）~（43）和式（45）代入式（40）中，可得：
（46）当时，。
40.根据lyapunov稳定性定理，以及式（12）与（46）可知，所设计的飞行控制器是稳定的。
41.控制方法还包括，根据四旋翼无人机欠驱动特性，对四旋翼无人机的位置控制采用外环控制，由以下公式计算四旋翼无人机的期望姿态横滚角以及期望总升力：
ꢀꢀꢀꢀꢀ
（1）
ꢀꢀꢀꢀꢀꢀ
（2）其中，uy(t)为无人机的位置外环控制关于惯性坐标系yi轴方向的控制输入，uz(t)为与uy(t)相对应的惯性坐标系zi轴方向的控制输入，
ϕd(t)为根据位置外环控制所计算得到的期望姿态横滚角，fd(t)为根据位置外环控制所计算得到的期望总升力。因为四旋翼无人机本身具有欠驱动特性，通常采用串级控制策略，位置控制为外环控制，姿态控制为内环控制。内环姿态控制沿用传统pid控制方法。
42.根据所述姿态内环控制的期望输入，可以计算得出四旋翼无人机的期望姿态驱动力矩，进而配合上述所得到的期望总升力计算得出无人机四个螺旋桨各自的期望旋转速度。
43.具体实施时，四旋翼无人机选择轴距350机型。机架选择350mm轴距四旋翼机架，电池为锂聚合物动力电芯，容量为2200mah，电压为7.4v，持续放电倍率为20c，尺寸（mm）为106*34*17，重量约为110g。
44.以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保
护范围之内。