CN105739513B - 一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法 - Google Patents
一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法 Download PDFInfo
- Publication number
- CN105739513B CN105739513B CN201610082286.1A CN201610082286A CN105739513B CN 105739513 B CN105739513 B CN 105739513B CN 201610082286 A CN201610082286 A CN 201610082286A CN 105739513 B CN105739513 B CN 105739513B
- Authority
- CN
- China
- Prior art keywords
- com
- flying robot
- attitude
- control subsystem
- coordinate system
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 28
- 238000013461 design Methods 0.000 claims abstract description 18
- 239000011159 matrix material Substances 0.000 claims description 27
- 238000012937 correction Methods 0.000 claims description 25
- 239000013598 vector Substances 0.000 claims description 20
- 238000004364 calculation method Methods 0.000 claims description 14
- 230000000694 effects Effects 0.000 claims description 10
- 238000013016 damping Methods 0.000 claims description 7
- 230000005484 gravity Effects 0.000 claims description 6
- 238000012546 transfer Methods 0.000 claims description 4
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 2
- 238000001228 spectrum Methods 0.000 claims 1
- 230000003993 interaction Effects 0.000 abstract description 6
- 238000005516 engineering process Methods 0.000 abstract description 3
- 230000007812 deficiency Effects 0.000 abstract 1
- 230000007613 environmental effect Effects 0.000 abstract 1
- 230000007547 defect Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 238000012938 design process Methods 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000004611 spectroscopical analysis Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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/08—Control of attitude, i.e. control of roll, pitch, or yaw
-
- 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
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 Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Feedback Control In General (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法,属于飞行机器人控制技术领域。所述控制器包括位置控制子***和姿态控制子***。位置控制子***包含外环和内环伪动态逆和PI控制器、位置配置环节;姿态控制子***也包含外环和内环伪动态逆和PI控制器、姿态配置环节。本发明避免了基于欧拉角控制器存在的奇异点和线性控制器制约机器人性能发挥的不足;简化了控制器的设计,提高了控制器***跟踪精度。所设计的控制器不仅具有全姿态轨迹跟踪能力,而且抗干扰能力强,跟踪精度高,显著提高了四旋翼飞行机器人机动性和环境交互能力,为四旋翼飞行机器人的进一步推广应用奠定了技术基础。
Description
技术领域
本发明属于飞行机器人控制技术领域,涉及轨迹自动跟踪技术,具体地说,是指一种四旋翼飞行机器人非线性轨迹跟踪控制***的设计方法。
背景技术
随着飞行机器人技术的快速发展,飞行机器人的应用越来越广泛。特别是四旋翼飞行机器人因为具有垂直起降功能,对场地无特别要求,特别适合侦察、营救、航拍等领域,目前已逐渐在民用和军事等领域得到了应用。为了提高四旋翼飞行机器人作业能力和应用范围,人们不再满足于四旋翼飞行机器人能平稳飞行等基本要求,而对其机动能力和环境交互能力的要求越来越高,这就需要四旋翼飞行机器人控制器具有很高的轨迹跟踪能力。四旋翼飞行机器人是一个典型的欠驱动、强耦合的非线性***,这给其飞行轨迹跟踪控制器的设计带来了挑战。目前普遍使用的四旋翼飞行机器人飞行轨迹跟踪控制器存在着如下的不足:通常是基于欧拉角进行姿态表达的,采用欧拉角进行姿态表达的控制器,虽然有着姿态表达直观等特点,但在俯仰角为90度时,存在奇异点,此时飞行机器人失控,限制了飞行机器人的机动性和环境交互能力。另外一个问题是常用的控制器,通常采用的控制方法是通过对***进行线性化后再采用线性控制方法进行设计,该方法的优点是简化了控制器的设计过程,但由于四旋翼飞行机器人本身是一个非线性***,采用线性控制方法进行控制,严重制约了其性能的发挥,特别是机动性和环境交互能力。
发明内容
为了解决基于四旋翼飞行机器人轨迹跟踪控制器存在的上述问题,本发明提出了一种四旋翼飞行机器人非线性轨迹跟踪控制***设计方法,包括跟踪控制器及其跟踪控制方法。本发明采用基于四元素的姿态表达,通过建立四旋翼飞行机器人六自由度运动学和动力学模型,使用非线性控制方法-轨迹线性化控制方法具体设计了四旋翼飞行机器人六自由度非线性轨迹跟踪控制器,达到了避免基于欧拉角控制器存在的奇异点和线性控制器制约机器人性能发挥的不足。采用经过误差修正控制量的命令值代替标称值对各个控制环进行设计,不仅简化了控制器的设计,而且提高了控制器***跟踪精度。所设计的控制器不仅具有全姿态轨迹跟踪能力,而且抗干扰能力强,跟踪精度高,显著提高了四旋翼飞行机器人机动性和环境交互能力,为四旋翼飞行机器人的进一步推广应用奠定了技术基础。
本发明首先提供一种四旋翼飞行机器人非线性轨迹跟踪控制器,整个跟踪控制器由两部分组成:位置控制子***和姿态控制子***。位置控制子***中又包含内外两个控制环和一个位置配置环节,每个控制环都包含一个完整的轨迹线性化控制法控制结构:一个伪动态逆和一个起镇定调节作用的PI控制器。位置配置环节的主要作用是根据力指令Fcom计算得到姿态指令值βcom和四个旋翼应该产生的总拉力Tcom。姿态控制子***中包含内外两个控制环和姿态配置环节,每个控制环也都包含一个完整的轨迹线性化控制法控制结构,姿态配置环节的主要作用是根据力矩指令Mcom和位置控制子***输出的总拉力Tcom来映射得到四个旋翼的转速,从而实现对四旋翼飞行机器人的控制。
本发明的跟踪控制器接收位置指令Pcom,位置控制子***第一外环伪动态逆根据该值计算出标称速度vnom,第一外环PI控制器根据位置误差Perr计算出位置误差修正控制量vctrl,标称速度vnom和位置误差修正控制量vctrl之和构成速度指令vcom;速度指令vcom作为位置控制子***内环的输入。位置控制子***第一内环伪动态逆根据速度指令vcom计算出标称力Fnom,第一内环PI控制器根据速度误差verr计算出速度误差修正控制量Fctrl,标称力Fnom和速度误差修正控制量Fctrl之和构成力指令Fcom;力指令Fcom作为位置控制子***中位置配置环节的输入,位置配置环节根据力指令Fcom值计算出姿态指令βcom和总拉力指令Tcom,并作为姿态控制子***的输入;所述的总拉力指令Tcom直接输出给姿态控制子***中的姿态配置环节。姿态指令βcom作为姿态控制子***中第二外环伪动态逆的输入量,第二外环伪动态逆根据该姿态指令计算出标称角速度ωnom;第二外环PI控制器根据姿态误差βerr计算出姿态误差修正控制量ωctrl,标称角速度ωnom和姿态误差修正控制量ωctrl之和构成角速度指令ωcom;角速度指令ωcom作为姿态控制子***内环的输入,姿态控制子***中第二内环伪动态逆根据角速度指令ωcom值计算出标称力矩Mnom,第二内环PI控制器根据角速度误差ωerr计算出角速度误差修正控制量Mctrl,标称力矩Mnom和角速度误差修正控制量Mctrl之和构成力矩指令Mcom;力矩指令Mcom输出给姿态配置环节,姿态配置环节根据输入的力矩指令Mcom和总拉力指令Tcom计算出四旋翼飞行机器人四个旋翼的转速,从而实现对四旋翼飞行机器人的控制。其中,四旋翼飞行机器人的位置、速度、姿态、角速度的测量值*sen(*为P、v、β、ω)由组合导航***测量给出。
本发明一种四旋翼飞行机器人轨迹跟踪控制***设计方法的优点在于:
1、采用四元素来表达四旋翼飞行机器人姿态进行控制器设计,有效避免基于欧拉角传统控制器存在奇异点的不足。
2、采用轨迹线性化非线性控制方法设计轨迹跟踪控制器,在设计中不需要对方程进行小扰动线性化处理,因而避免了一般飞行控制***设计中由于线性化所带来的误差,并具有自动解耦能力,有效提高了控制器的控制精度,避免了线性化的不足,从而为飞行机器人的高机动飞行和环境交互能力的提高奠定了基础。
3、在控制器结构上,通过采用经过误差控制量修正的命令值代替标称值作为下一个控制环的输入,不仅简化了设计步骤,还提高了控制器轨迹跟踪性能。
附图说明
图1为本发明所应用的四旋翼飞行机器人坐标系与配置图;
图2为本发明四旋翼飞行机器人非线性轨迹跟踪控制器总体结构图;
图3为本发明四旋翼飞行机器人非线性轨迹跟踪控制器位置跟踪效果图;
图4为本发明四旋翼飞行机器人非线性轨迹跟踪控制器速度跟踪效果图;
图5为本发明四旋翼飞行机器人非线性轨迹跟踪控制器姿态跟踪效果图;
图6为本发明四旋翼飞行机器人非线性轨迹跟踪控制器角速度跟踪效果图。
具体实施方式
下面结合附图对本发明做详细说明。
本发明提供一种四旋翼飞行机器人非线性轨迹跟踪控制器及其控制方法,采用命令值代替标称值进行控制环路设计,如图2所示,本发明提供的跟踪控制器由两部分组成:位置控制子***和姿态控制子***。所述的位置控制子***中包含第一外环伪动态逆、第一外环PI控制器、第一内环伪动态逆、第一内环PI控制器和位置配置环节,位置配置环节根据力指令Fcom计算得到姿态指令值βcom和四个旋翼应该产生的总拉力Tcom,输出给姿态控制子***。所述的姿态控制子***中包含第二外环伪动态逆、第二外环PI控制器、第二内环伪动态逆、第二内环PI控制器和姿态配置环节。姿态配置环节根据力矩指令Mcom和位置控制子***输出的总拉力Tcom来映射得到四个旋翼的转速,从而实现对四旋翼飞行机器人的控制。
本发明的跟踪控制器接收位置指令Pcom,位置控制子***第一外环伪动态逆根据该值计算出标称速度vnom,第一外环PI控制器根据位置误差Perr计算出位置误差修正控制量vctrl,标称速度vnom和位置误差修正控制量vctrl之和构成速度指令vcom;速度指令vcom作为位置控制子***内环的输入。位置控制子***第一内环伪动态逆根据速度指令vcom计算出标称力Fnom,第一内环PI控制器根据速度误差verr计算出速度误差修正控制量Fctrl,标称力Fnom和速度误差修正控制量Fctrl之和构成力指令Fcom;力指令Fcom作为位置控制子***中位置配置环节的输入,位置配置环节根据力指令Fcom值计算出姿态指令βcom和总拉力指令Tcom,并作为姿态控制子***的输入;所述的总拉力指令Tcom直接输出给姿态控制子***中的姿态配置环节。姿态指令βcom作为姿态控制子***中第二外环伪动态逆的输入量,第二外环伪动态逆根据该姿态指令计算出标称角速度ωnom;第二外环PI控制器根据姿态误差βerr计算出姿态误差修正控制量ωctrl,标称角速度ωnom和姿态误差修正控制量ωctrl之和构成角速度指令ωcom;角速度指令ωcom作为姿态控制子***内环的输入,姿态控制子***中第二内环伪动态逆根据角速度指令ωcom值计算出标称力矩Mnom,第二内环PI控制器根据角速度误差ωerr计算出角速度误差修正控制量Mctrl,标称力矩Mnom和角速度误差修正控制量Mctrl之和构成力矩指令Mcom;力矩指令Mcom输出给姿态配置环节,姿态配置环节根据输入的力矩指令Mcom和总拉力指令Tcom计算出四旋翼飞行机器人四个旋翼的转速,从而实现对四旋翼飞行机器人的控制。其中,四旋翼飞行机器人的位置、速度、姿态、角速度的测量值*sen(*为P、v、β、ω)由组合导航***测量给出。
基于所述的跟踪控制器,本发明还提供一种四旋翼飞行机器人非线性轨迹跟踪控制方法,包括以下步骤:
第一步,基于四元素四旋翼飞行机器人建模,包括运动学建模与动力学建模;
由于旋翼的质量相对于四旋翼飞行机器人可以忽略,此时可以把四旋翼飞行机器人看成一个6自由度的刚体,其运动可以分解为平移运动和旋转运动,建立的坐标系和配置如图1所示的。图1中ΣE表示地面坐标系ODXNYEZD,Σb为机体坐标系xyz,T1、T2、T3、T4分别表示四个旋翼所产生的拉力,总拉力为T。用Ω1、Ω2、Ω3、Ω4分别表示四旋翼飞行机器人的四个旋翼的转速,用P表示地面坐标系下飞行机器人重心的位置矢量,v=(vx,vy,vz)表示地面坐标系下飞行机器人重心的速度矢量,F表示地面坐标系下飞行机器人所受合外力总力(包括重力),M表示机体坐标系下飞行机器人的总力矩,ω表示机体坐标系下的角速度矢量,表示机体坐标系相对于地面坐标系的姿态旋转矩阵,是一个3×3的矩阵,具体计算见公式(20)~(25),用四元素来表示姿态旋转矩阵则为:
其中a,b,c,d为四元素β的四个分量,四旋翼飞行机器人在地面坐标系下平移运动的运动学方程为:
动力学方程为:
式中的m为四旋翼飞行机器人的质量,基于四元素的四旋翼飞行机器人旋转运动的运动学方程为:
近似为刚体的四旋翼飞行机器人的旋转动力学方程为:
上式中,J为四旋翼飞行机器人在机体坐标系下描述的惯性张量,定义如下:
其中,Jxx、Jyy、Jzz、Jzx、Jyz、Jxy为飞行机器人在机体坐标系下描述的转动惯量和惯性积。由于四旋翼飞行机器人的对称性,有Jxy=Jyz=0,通过整理,式(5)可以改写为如下所示的以ω为状态变量的状态方程:
其中,和与飞行机器人在机体坐标系下的惯性参数有关,和可以用Jxx,Jyy,Jzz,Jxz来表示,具体表达如下:
四旋翼飞行机器人四个旋翼产生的力矩Ma和总拉力T为:
其中,kt和kd分别为旋翼产生拉力和扭矩的系数,dr是两个对角旋翼之间的距离,
每个旋翼产生的陀螺效应为:
其中Jr是电机转动惯量,ez=(0,0,1)T,则四旋翼飞行机器人所受的总力矩M为:
M=Ma+MG (9)
四旋翼飞行机器人在地面坐标系下所受的总力F为:
其中,表示姿态旋转矩阵,m为四旋翼飞行机器人的质量,g是重力常数,vx、vy,vz分别是四旋翼飞行机器人在地面坐标系下的三个方向的速度分量,CDv是阻力系数。
第二步,基于以上所建立的四旋翼飞行机器人的运动和动力学模型,采用轨迹线性化控制方法进行控制器的具体设计。
位置控制子***根据位置指令Pcom,计算出四旋翼飞行机器人的姿态指令βcom和总拉力指令Tcom,并作为指令输出给姿态控制子***。
如图2所示,位置控制子***的外环在控制结构上,由第一外环伪动态逆和第一外环PI控制器组成。地面坐标系下的标称速度vnom由方程(2)求伪逆得到。
式中Pcom是地面坐标系下的位置指令,为了保证的因果性,由位置指令Pcom利用二阶伪微分运算器得到,二阶伪微分运算器如下式传递函数所示:
其中,ζd、ωn,d分别是伪微分运算器阻尼比和带宽频率,s为传递函数的符号,复参数。
位置跟踪误差Perr定义为:
Perr=Psen-Pcom (13)
其中,Psen是传感器测量的四旋翼飞行机器人位置的测量值。
位置跟踪误差的动力学方程为:
其中vctrl为位置误差修正控制量,根据轨迹线性化的设计方法,位置控制子***第一外环PI控制器的控制率为:
vctrl=-KP1(t)Perr-KI1(t)∫Perr (15)
其中,
Acl11=diag{-α1j1},Acl12=diag{-α1j2}和α1jk(j=1,2,3;k=1,2)是控制参数,根据PD谱理论,系数α1jk(j=1,2,3;k=1,2)由期望的闭环***阻尼比和带宽频率得到,具体计算方法为:
其中,ωn,j(t)是带宽频率,ζj是闭环***阻尼比,t是时间。位置控制子***的外环输出是速度指令值vcom,计算公式为:
vcom=vnom+vctrl (19)
由于各个控制环设计过程基本相似,就不再赘述,下面分别对两个分配环节进行设计。
位置配置环节是把位置控制子***的内环输出的力指令Fcom作为输入值,计算输出四旋翼飞行机器人姿态命令βcom和沿着飞行机器人机体坐标系Z轴的总拉力指令Tcom。四旋翼飞行机器人的姿态旋转矩阵表达为方向余弦矩阵的形式为:
其中,r1、r2、r3分别表示姿态旋转矩阵的三个列向量,cij为方向余弦矩阵参数,i=1,2,3;j=1,2,3。
把上式(20)代入式(10),可以计算得到r3:
其中,Fcom=[Fx,com Fy,com Fz,com],Fx,com、Fy,com、Fz,com分别为力指令在地面坐标系三个方向上的分量。
从图1中我们可以看到,四旋翼飞行机器人四个旋翼所产生的合力总是在机体坐标系的Z轴方向上,并不依赖于飞行机器人的航向,因此,航向角可以设置为任意值。但考虑到工程实际,通常让机体坐标系的x轴始终指向位置指令轨迹的切线方向上,此时,航向角ψ按下式进行计算:
其中Xcom和Ycom是四旋翼飞行机器人的命令位置。
此时,姿态旋转矩阵的r1列向量投影到参考面XNODYE上的单位向量可以表示为:
h=[cos(ψ),sin(ψ),0]T (23)
考虑到单位向量r2正交于向量h和r3所构成的平面,因此r2列向量的计算公式为:
由于方向余弦矩阵是一个正交阵,根据右手定则,单位列向量r2的计算公式为:
r1=r2×r3 (25)
由式(21)、(24)和(25)则可计算出姿态旋转矩阵对于小角度的旋转,用来表示姿态标称值的四元素βnom的各个分量anom、bnom、cnom、dnom与姿态旋转矩阵Rb e参数cij(i=1,2,3,j=1,2,3)之间的关系式为:
其中,cij是由式(20)定义的以方向余弦形式表示的姿态旋转矩阵的参数。
从控制结构框图2中可以看到,位置配置环节并不需要单独计算出姿态误差,因此,位置控制子***输出给姿态控制子***的姿态指令就是姿态的标称值,即βcom=βnom。总拉力指令Tcom直接输出给姿态控制子***的姿态配置环节,由于总拉力的方向总是沿着四旋翼飞行机器人的机体坐标系的Z轴上,Tcom大小为:
其中Fx、Fy、Fz由下式给出:
Fx=Fx,com+CDv|vx,com|vx,com
Fy=Fy,com+CDv|vy,com|vy,com
Fz=Fz,com-mg+CDv|vz,com|vz,com
vx,com、vy,com、vz,com分别为速度指令在地面坐标系上的三个分量。
至此,完成了整个位置控制子***的设计,位置控制子***以位置指令作为输入值,输出姿态和总拉力指令值,并作为姿态控制子***的输入量。
第三步,姿态控制子***接收位置控制子***的内环输出的总拉力Tcom和姿态指令βcom,姿态配置环节结合总拉力Tcom和力矩Mcom,计算输出四旋翼飞行机器人四个旋翼的转速指令Ωi,com(i=1,2,3,4,分别代表四个旋翼),从而实现对四旋翼飞行机器人的飞行控制。
姿态控制子***的姿态配置环节的设计非常简单,直接把方程(7)求逆即可,从中可以看出,基于轨迹线性化的控制方法,具有直接解耦的功能,简化了设计过程。四个旋翼转速指令计算公式为:
至此,完成了整个四旋翼飞行机器人的飞行建模和基于四元素姿态表达和轨迹线性化控制方法的非线性全姿态飞行控制器的设计。
采用该控制方法对四旋翼飞行机器人的实际飞行跟踪效果进行了测试,所使用的阻尼系数和角频率等控制参数如表1所示。
表1控制参数
跟踪效果如图3~图6所示,其中图3为位置跟踪,图4为速度跟踪,图5为姿态跟踪,图6为角速度跟踪,其中*com、*sen(*为P、v、γ、ω)分别表示指令值和测量值。在姿态跟踪效果图中,由于基于四元素的姿态表达不够直观,在作图时,利用四元素与欧拉角之间的转换关系,转换为欧拉角,但在整个控制过程中,依然是以四元素的形式来表达飞行机器人的姿态,如图5所示,Ф、θ、ψ分别表示横滚、俯仰和方向角。从这些效果图中可以看到,四个控制环路的跟踪误差都非常小,表明采用本发明控制方法所设计的四旋翼飞行机器人六自由度非线性轨迹跟踪控制器具有良好的轨迹跟踪能力。
Claims (3)
1.一种四旋翼飞行机器人非线性轨迹跟踪控制器,其特征在于:包括位置控制子***和姿态控制子***;所述的位置控制子***中包含第一外环伪动态逆、第一外环PI控制器、第一内环伪动态逆、第一内环PI控制器和位置配置环节,位置配置环节根据力指令Fcom计算得到姿态指令值βcom和四个旋翼应该产生的总拉力Tcom,输出给姿态控制子***;所述的姿态控制子***中包含第二外环伪动态逆、第二外环PI控制器、第二内环伪动态逆、第二内环PI控制器和姿态配置环节;姿态配置环节根据力矩指令Mcom和位置控制子***输出的总拉力Tcom来映射得到四个旋翼的转速,从而实现对四旋翼飞行机器人的控制。
2.根据权利要求1所述的一种四旋翼飞行机器人非线性轨迹跟踪控制器,其特征在于:跟踪控制器接收位置指令Pcom,位置控制子***第一外环伪动态逆根据该值计算出标称速度vnom,第一外环PI控制器根据位置误差Perr计算出位置误差修正控制量vctrl,标称速度vnom和位置误差修正控制量vctrl之和构成速度指令vcom;速度指令vcom作为位置控制子***内环的输入;位置控制子***第一内环伪动态逆根据速度指令vcom计算出标称力Fnom,第一内环PI控制器根据速度误差verr计算出速度误差修正控制量Fctrl,标称力Fnom和速度误差修正控制量Fctrl之和构成力指令Fcom;力指令Fcom作为位置控制子***中位置配置环节的输入,位置配置环节根据力指令Fcom值计算出姿态指令βcom和总拉力指令Tcom,并作为姿态控制子***的输入;所述的总拉力指令Tcom直接输出给姿态控制子***中的姿态配置环节;姿态指令βcom作为姿态控制子***中第二外环伪动态逆的输入量,第二外环伪动态逆根据该姿态指令计算出标称角速度ωnom;第二外环PI控制器根据姿态误差βerr计算出姿态误差修正控制量ωctrl,标称角速度ωnom和姿态误差修正控制量ωctrl之和构成角速度指令ωcom;角速度指令ωcom作为姿态控制子***内环的输入,姿态控制子***中第二内环伪动态逆根据角速度指令ωcom值计算出标称力矩Mnom,第二内环PI控制器根据角速度误差ωerr计算出角速度误差修正控制量Mctrl,标称力矩Mnom和角速度误差修正控制量Mctrl之和构成力矩指令Mcom;力矩指令Mcom输出给姿态配置环节,姿态配置环节根据输入的力矩指令Mcom和总拉力指令Tcom计算出四旋翼飞行机器人四个旋翼的转速;其中,四旋翼飞行机器人的位置、速度、姿态、角速度的测量值*sen由组合导航***测量给出,*为P、v、β、ω。
3.一种四旋翼飞行机器人非线性轨迹跟踪控制方法,包括以下步骤:
第一步,基于四元素四旋翼飞行机器人建模,包括运动学建模与动力学建模;
把四旋翼飞行机器人看成一个6自由度的刚体,其运动分解为平移运动和旋转运动,建立的坐标系,ΣE表示地面坐标系ODXNYEZD,Σb为机体坐标系xyz,T1、T2、T3、T4分别表示四个旋翼所产生的拉力,总拉力为T,用Ω1、Ω2、Ω3、Ω4分别表示四旋翼飞行机器人的四个旋翼的转速,用P表示地面坐标系下飞行机器人重心的位置矢量,v=(vx,vy,vz)表示地面坐标系下飞行机器人重心的速度矢量,F表示地面坐标系下飞行机器人所受合外力总力,M表示机体坐标系下飞行机器人的总力矩,ω表示机体坐标系下的角速度矢量,表示机体坐标系相对于地面坐标系的姿态旋转矩阵,是一个3×3的矩阵,用四元素来表示姿态旋转矩阵则为:
其中a,b,c,d为四元素β的四个分量,四旋翼飞行机器人在地面坐标系下平移运动的运动学方程为:
动力学方程为:
式中的m为四旋翼飞行机器人的质量,基于四元素的四旋翼飞行机器人旋转运动的运动学方程为:
其中p、q、r为角速度ω的三个分量,分别表示机体坐标系下四旋翼飞行机器人横滚、俯仰和航向角速度,
四旋翼飞行机器人的旋转动力学方程为:
上式中,J为四旋翼飞行机器人在机体坐标系下描述的惯性张量,定义如下:
其中,Jxx、Jyy、Jzz、Jzx、Jyz、Jxy为飞行机器人在机体坐标系下描述的转动惯量和惯性积;由于四旋翼飞行机器人的对称性,有Jxy=Jyz=0,通过整理,式(5)改写为如下所示的以ω为状态变量的状态方程:
其中,和与飞行机器人在机体坐标系下的惯性参数有关,和用Jxx,Jyy,Jzz,Jxz来表示,具体表达如下:
四旋翼飞行机器人四个旋翼产生的力矩Ma和总拉力T为:
其中,kt和kd分别为旋翼产生拉力和扭矩的系数,dr是两个对角旋翼之间的距离,
每个旋翼产生的陀螺效应为:
其中Jr是电机转动惯量,ez=(0,0,1)T,则四旋翼飞行机器人所受的总力矩M为:
M=Ma+MG (9)
四旋翼飞行机器人在地面坐标系下所受的总力F为:
其中,表示姿态旋转矩阵,g是重力常数,vx、vy,vz分别是四旋翼飞行机器人在地面坐标系下的三个方向的速度分量,CDv是阻力系数;
第二步,基于以上所建立的四旋翼飞行机器人的运动和动力学模型,采用轨迹线性化控制方法进行控制器的具体设计;
位置控制子***根据位置指令Pcom,计算出四旋翼飞行机器人的姿态指令βcom和总拉力指令Tcom,并作为指令输出给姿态控制子***;
位置控制子***的外环在控制结构上,由第一外环伪动态逆和第一外环PI控制器组成,地面坐标系下的标称速度vnom由方程(11)求伪逆得到;
式中Pcom是地面坐标系下的位置指令,由位置指令Pcom利用二阶伪微分运算器得到,二阶伪微分运算器如下式传递函数所示:
其中,ζd、ωn,d分别是伪微分运算器阻尼比和带宽频率,s为传递函数的符号,复参数;
位置跟踪误差Perr定义为:
Perr=Psen-Pcom (13)
其中,Psen是传感器测量的四旋翼飞行机器人位置的测量值;
位置跟踪误差的动力学方程为:
其中vctrl为位置误差修正控制量,根据轨迹线性化的设计方法,位置控制子***第一外环PI控制器的控制律为:
vctrl=-KP1(t)Perr-KI1(t)∫Perr (15)
其中,
Acl11=diag{-α1j1},Acl12=diag{-α1j2}和α1jk是控制参数,根据PD谱理论,系数α1jk由期望的闭环***阻尼比和带宽频率得到,j=1,2,3;k=1,2,具体计算方法为:
其中,ωn,j(t)是带宽频率,ζj是闭环***阻尼比,t是时间,位置控制子***的外环输出是速度指令值vcom,计算公式为:
vcom=vnom+vctrl (19)
位置配置环节是把位置控制子***的内环输出的力指令Fcom作为输入值,计算输出四旋翼飞行机器人姿态命令βcom和沿着飞行机器人机体坐标系Z轴的总拉力指令Tcom,四旋翼飞行机器人的姿态旋转矩阵表达为方向余弦矩阵的形式为:
其中,r1、r2、r3分别表示姿态旋转矩阵的三个列向量,cij为方向余弦矩阵参数,i=1,2,3;j=1,2,3;
把上式(20)代入式(10),计算得到r3:
记
则得
其中,Fcom=[Fx,com Fy,com Fz,com],Fx,com、Fy,com、Fz,com分别为力指令在地面坐标系三个方向上的分量;
让机体坐标系的x轴始终指向位置指令轨迹的切线方向上,此时,航向角ψ按下式进行计算:
其中Xcom和Ycom是四旋翼飞行机器人的命令位置;
此时,姿态旋转矩阵的r1列向量投影到参考面XNODYE上的单位向量表示为:
h=[cos(ψ),sin(ψ),0]T (23)
考虑到单位向量r2正交于向量h和r3所构成的平面,因此r2列向量的计算公式为:
由于方向余弦矩阵是一个正交阵,根据右手定则,单位列向量r2的计算公式为:
r1=r2×r3 (25)
由式(21)、(24)和(25)计算出姿态旋转矩阵对于小角度的旋转,用来表示姿态标称值的四元素βnom的各个分量anom、bnom、cnom、dnom与姿态旋转矩阵参数cij之间的关系式为:
总拉力指令Tcom直接输出给姿态控制子***的姿态配置环节,由于总拉力的方向总是沿着四旋翼飞行机器人的机体坐标系的Z轴上,Tcom大小为:
其中Fx、Fy、Fz由下式给出:
Fx=Fx,com+CDv|vx,com|vx,com
Fy=Fy,com+CDv|vy,com|vy,com
Fz=Fz,com-mg+CDv|vz,com|vz,com
vx,com、vy,com、vz,com分别为速度指令在地面坐标系上的三个分量;
至此,位置控制子***以位置指令作为输入值,输出姿态和总拉力指令值,并作为姿态控制子***的输入量;
第三步,姿态控制子***接收位置控制子***的内环输出的总拉力Tcom和姿态指令βcom,姿态配置环节结合总拉力Tcom和力矩Mcom,计算输出四旋翼飞行机器人四个旋翼的转速指令Ωi,com,i=1,2,3,4,从而实现对四旋翼飞行机器人的飞行控制;
四个旋翼转速指令计算公式为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610082286.1A CN105739513B (zh) | 2016-02-05 | 2016-02-05 | 一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610082286.1A CN105739513B (zh) | 2016-02-05 | 2016-02-05 | 一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105739513A CN105739513A (zh) | 2016-07-06 |
CN105739513B true CN105739513B (zh) | 2018-06-12 |
Family
ID=56245886
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610082286.1A Active CN105739513B (zh) | 2016-02-05 | 2016-02-05 | 一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105739513B (zh) |
Families Citing this family (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106873382B (zh) * | 2016-08-11 | 2020-01-07 | 广东工业大学 | 一种基于四轴飞行器的数学模型构建方法及装置 |
CN106933104B (zh) * | 2017-04-21 | 2020-05-19 | 苏州工业职业技术学院 | 一种基于dic-pid的四旋翼飞行器姿态与位置的混合控制方法 |
CN109308074B (zh) * | 2017-07-28 | 2021-10-08 | 深圳禾苗通信科技有限公司 | 一种无人机重心偏移的补偿方法及*** |
CN108205321B (zh) * | 2017-12-20 | 2021-05-25 | 成都纵横自动化技术股份有限公司 | 滑跑纠偏控制方法及相关装置 |
CN108803350B (zh) * | 2018-08-16 | 2020-06-23 | 居鹤华 | 基于轴不变量的动基座多轴机器人动力学建模与解算方法 |
CN110488875B (zh) * | 2019-09-02 | 2022-07-12 | 中国人民解放军海军航空大学 | 基于动态逆的无人机跟踪目标初始段航向误差修正方法 |
CN110850887B (zh) * | 2019-10-10 | 2021-03-16 | 南京航空航天大学 | 一种四旋翼无人机复合动态逆抗干扰姿态控制方法 |
CN111240357B (zh) * | 2020-01-15 | 2023-06-30 | 兰州大学 | 一种微型扑翼飞行器可避免奇异状态的位置跟踪控制方法 |
CN111338369B (zh) * | 2020-03-19 | 2022-08-12 | 南京理工大学 | 一种基于非线性逆补偿的多旋翼飞行控制方法 |
CN111650951B (zh) * | 2020-05-22 | 2021-09-17 | 南京航空航天大学 | 一种四旋翼无人机复杂轨迹的全回路复合动态逆跟踪控制方法 |
CN113375670A (zh) * | 2021-04-12 | 2021-09-10 | 国网江苏省电力有限公司兴化市供电分公司 | 基于三轴陀螺仪和加速度计值使用四元数解算姿态的方法 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101937233A (zh) * | 2010-08-10 | 2011-01-05 | 南京航空航天大学 | 近空间高超声速飞行器非线性自适应控制方法 |
CN102193557A (zh) * | 2010-03-04 | 2011-09-21 | 南京航空航天大学 | 无人机的鲁棒受限飞行控制方法 |
CN102880060A (zh) * | 2012-10-25 | 2013-01-16 | 北京理工大学 | 再入飞行器自适应指数时变滑模姿态控制方法 |
CN102955477A (zh) * | 2012-10-26 | 2013-03-06 | 南京信息工程大学 | 一种四旋翼飞行器姿态控制***及控制方法 |
CN104199456A (zh) * | 2014-09-04 | 2014-12-10 | 中国科学院自动化研究所 | 水上无人机水面自主操纵控制方法及*** |
CN104536448A (zh) * | 2014-11-17 | 2015-04-22 | 哈尔滨工程大学 | 一种基于Backstepping法的无人机姿态***控制方法 |
CN104765272A (zh) * | 2014-03-05 | 2015-07-08 | 北京航空航天大学 | 一种基于pid神经元网络控制(pidnn)的四旋翼飞行器控制方法 |
CN104950899A (zh) * | 2015-06-10 | 2015-09-30 | 北京理工大学 | 一种固定时间收敛的飞行器姿态控制方法 |
CN105159305A (zh) * | 2015-08-03 | 2015-12-16 | 南京航空航天大学 | 一种基于滑模变结构的四旋翼飞行控制方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102439646B (zh) * | 2009-03-26 | 2014-11-26 | 俄亥俄州立大学 | 轨迹跟踪飞行控制器 |
US10025320B2 (en) * | 2012-02-15 | 2018-07-17 | Sikorsky Aircraft Corporation | Control system for reconfigurable rotary wing aircraft |
-
2016
- 2016-02-05 CN CN201610082286.1A patent/CN105739513B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102193557A (zh) * | 2010-03-04 | 2011-09-21 | 南京航空航天大学 | 无人机的鲁棒受限飞行控制方法 |
CN101937233A (zh) * | 2010-08-10 | 2011-01-05 | 南京航空航天大学 | 近空间高超声速飞行器非线性自适应控制方法 |
CN102880060A (zh) * | 2012-10-25 | 2013-01-16 | 北京理工大学 | 再入飞行器自适应指数时变滑模姿态控制方法 |
CN102955477A (zh) * | 2012-10-26 | 2013-03-06 | 南京信息工程大学 | 一种四旋翼飞行器姿态控制***及控制方法 |
CN104765272A (zh) * | 2014-03-05 | 2015-07-08 | 北京航空航天大学 | 一种基于pid神经元网络控制(pidnn)的四旋翼飞行器控制方法 |
CN104199456A (zh) * | 2014-09-04 | 2014-12-10 | 中国科学院自动化研究所 | 水上无人机水面自主操纵控制方法及*** |
CN104536448A (zh) * | 2014-11-17 | 2015-04-22 | 哈尔滨工程大学 | 一种基于Backstepping法的无人机姿态***控制方法 |
CN104950899A (zh) * | 2015-06-10 | 2015-09-30 | 北京理工大学 | 一种固定时间收敛的飞行器姿态控制方法 |
CN105159305A (zh) * | 2015-08-03 | 2015-12-16 | 南京航空航天大学 | 一种基于滑模变结构的四旋翼飞行控制方法 |
Non-Patent Citations (2)
Title |
---|
An acceleration measurements-based approach for helicopter nonlinear flight control using Incremental Nonlinear Dynamic Inversion;P. Simplício 等;《Control Engineering Practice》;20130831;第1065-1077页 * |
一种改进的微型飞行器非线性动态逆解耦控制研究;邹小志 等;《航空兵器》;20100831;第16-20页 * |
Also Published As
Publication number | Publication date |
---|---|
CN105739513A (zh) | 2016-07-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105739513B (zh) | 一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法 | |
Das et al. | Dynamic inversion with zero-dynamics stabilisation for quadrotor control | |
CN107957730B (zh) | 一种无人飞行器稳定飞行控制方法 | |
Islam et al. | Robust control of four-rotor unmanned aerial vehicle with disturbance uncertainty | |
Derafa et al. | Super twisting control algorithm for the attitude tracking of a four rotors UAV | |
Das et al. | Backstepping approach for controlling a quadrotor using lagrange form dynamics | |
CN102830622B (zh) | 一种四旋翼飞行器自抗扰自动飞行控制方法 | |
CN106155076B (zh) | 一种多旋翼无人飞行器的稳定飞行控制方法 | |
Bonna et al. | Trajectory tracking control of a quadrotor using feedback linearization | |
CN112558621A (zh) | 一种基于解耦控制的飞行机械臂*** | |
CN107491083B (zh) | 一种基于饱和自适应滑模控制的四旋翼自主着船方法 | |
CN109032156B (zh) | 一种基于状态观测的吊挂载荷四旋翼无人机悬停控制方法 | |
CN109885074B (zh) | 四旋翼无人机有限时间收敛姿态控制方法 | |
CN111459188B (zh) | 一种基于四元数的多旋翼非线性飞行控制方法 | |
JP2015024705A (ja) | 小型電動ヘリコプタの自動離着陸制御方法 | |
CN111338369B (zh) | 一种基于非线性逆补偿的多旋翼飞行控制方法 | |
Jung et al. | A comprehensive flight control design and experiment of a tail-sitter UAV | |
Yacef et al. | Adaptive fuzzy backstepping control for trajectory tracking of unmanned aerial quadrotor | |
Liang et al. | Unmanned aerial transportation system with flexible connection between the quadrotor and the payload: modeling, controller design, and experimental validation | |
Sanchez et al. | Continuous reactive-based position-attitude control of quadrotors | |
CN111813140A (zh) | 一种具有高精度的四旋翼无人机轨迹跟踪控制方法 | |
CN112198885A (zh) | 一种满足机动平台自主降落需求的无人机控制方法 | |
CN109976364B (zh) | 一种六旋翼飞行器姿态解耦控制方法 | |
Long et al. | Design and quaternion-based attitude control of the omnicopter MAV using feedback linearization | |
Zawiski et al. | Modelling and optimal control system design for quadrotor platform–an extended approach |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |