CN115556111B - 基于变惯性参数建模的飞行机械臂耦合扰动控制方法 - Google Patents
基于变惯性参数建模的飞行机械臂耦合扰动控制方法 Download PDFInfo
- Publication number
- CN115556111B CN115556111B CN202211319418.XA CN202211319418A CN115556111B CN 115556111 B CN115556111 B CN 115556111B CN 202211319418 A CN202211319418 A CN 202211319418A CN 115556111 B CN115556111 B CN 115556111B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- representing
- coordinate system
- aerial vehicle
- unmanned aerial
- 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
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种基于变惯性参数建模的飞行机械臂耦合扰动控制方法,属于空中作业无人***控制技术领域。本发明针对现有旋翼飞行机械臂抗扰动控制中的反馈补偿存在延时,影响***稳定性的问题。包括:建立动态变化质心在机体坐标系下的表达式和动态变化惯量在机体坐标系下的表达式;利用多刚体质点系的动量与动量矩定理对旋翼飞行机械臂***进行动力学建模,得到***动力学模型;基于质心变化与惯量变化对***动力学模型进行处理,建立质心变化和惯量变化对旋翼飞行机械臂***的耦合扰动的映射关系,得到基于变惯性参数的耦合扰动模型;将耦合扰动模型计算获得的耦合扰动估计值作为前馈补偿量输入位置环和姿态环实现耦合扰动控制。本发明可以抑制耦合扰动。
Description
技术领域
本发明涉及基于变惯性参数建模的飞行机械臂耦合扰动控制方法,属于空中作业无人***控制技术领域。
背景技术
近年来,旋翼飞行机械臂作为一种新型的空中机器人***,因其在三维空间灵活的机动性和在其工作空间强大的操纵性引起了研究人员较为广泛的研究和注意。旋翼飞行机械臂可以将机械臂的操纵能力有效拓展至三维空间,同时具备无人机的灵活机动性,使得一些高空特种无人作业任务成为可能,具有非常有价值的应用潜力和广阔的应用前景。
耦合扰动问题是摆在旋翼飞行机械臂稳定精确控制面前最大的挑战之一,其主要是因为无人机与机械臂之间存在相对运动导致***质心和惯量发生快速变化而产生的,特别是在一些需要无人机或机械臂在大范围快速运动的实际动态场景中,耦合扰动影响会异常强烈和明显,甚至所产生的耦合扰动力矩数值上可能达到无人机螺旋桨能产生的用于做姿态机动总力矩的1/3左右。对于搭载多自由度机械臂的旋翼飞行机械臂,其作为典型的强耦合、欠驱动和非线性动态***,无人机与机械臂之间的耦合扰动会直接通过它们之间的刚性连接而传递,造成***姿态的偏差,而姿态偏差又会因该***的欠驱动特性进一步影响到其位置控制的偏差,最终导致旋翼飞行机械臂的无人机基座和机械臂末端执行器控制精度均受到严重影响。是故,强烈的耦合扰动会严重影响旋翼飞行机械臂执行空中作业任务的准确性,更严重的情况可能威胁***的稳定性,甚至出现***失控造成事故的严重后果。所以,旋翼飞行机械臂精确稳定地执行空中作业任务的前提便是无人机基座能够在强烈的耦合扰动影响下仍然保持稳定和高精度的位姿控制,在此条件下,机械臂末端执行器准确稳定地达到期望位姿状态才成为可能。因此,研究和分析应对强烈耦合扰动的旋翼飞行机械臂***的精确补偿与稳定控制显得尤为重要。
在现有相关研究中,如专利申请号为CN201910916380的专利中提出了一种基于解耦控制的飞行机械臂***,该发明在处理耦合扰动时采用传统机械臂迭代动力学方程来计算驱动关节的力矩,并将其视为机械臂运动对飞行器造成的外力干扰;这种处理方式只能适用于传统与固定基座固连的机械臂,对于旋翼飞行机械臂内以浮动的无人机为基座的机械臂是不准确的,因为该耦合扰动不仅仅只与机械臂的状态有关,而是与无人机和机械臂的状态都耦合相关的。专利申请号为CN202110994016的专利中提出了一种抗干扰和饱和特性的飞行机械臂位姿控制方法,专利申请号为CN202010801707的专利中提出了一种针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法,专利申请号为CN201810094313的专利中提出了一种基于动态重心补偿的旋翼飞行机械臂***及算法,这些方法在处理旋翼飞行机械臂的耦合扰动时均存在一个共性的问题,就是都只考虑了机械臂运动所带来的质心偏移,并未考虑***惯量也会因机械臂运动而动态变化的情况,所以这些方法对耦合扰动的实际抑制效果会相对打折扣。另外,专利申请号为CN201810271456的专利中提出了一种旋翼飞行机械臂自适应RBFNNs测噪自抗扰控制方法,该发明利用自适应RBFNNs对耦合扰动进行估计和反馈补偿;这类根据***受扰后的状态偏差进行估计和反馈补偿的方法是需要一定的响应时间才能估计和抑制扰动的,而对于旋翼飞行机械臂来说这段响应时间可能是致命的,因为瞬时产生的强烈的耦合扰动可能使得***还未等到准确的反馈补偿量被估计出来就已经受到严重影响,所以该类反馈补偿方法在实际***应用时难免受到一定的限制。
发明内容
针对现有旋翼飞行机械臂抗扰动控制中的反馈补偿存在延时,影响***稳定性的问题,本发明提供一种基于变惯性参数建模的飞行机械臂耦合扰动控制方法。
本发明的一种基于变惯性参数建模的飞行机械臂耦合扰动控制方法,包括,
步骤一:对旋翼飞行机械臂***的质心变化,建立动态变化质心在机体坐标系下的表达式;同时,对旋翼飞行机械臂***的惯量变化,根据平行轴移定理,建立动态变化惯量在机体坐标系下的表达式;
步骤二:利用多刚体质点系的动量与动量矩定理对旋翼飞行机械臂***进行动力学建模,得到***动力学模型;
步骤三:基于质心变化与惯量变化对***动力学模型进行处理,建立质心变化和惯量变化对旋翼飞行机械臂***的耦合扰动的映射关系,得到基于变惯性参数的耦合扰动模型;
步骤四:将耦合扰动模型计算获得的耦合扰动估计值作为前馈补偿量输入位置环和姿态环,实现耦合扰动控制。
根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,步骤一中,质心在机体坐标系下的表达式为:
式中Broc表示旋翼飞行机械臂***在机体坐标系下的质心向量,表示旋翼飞行机械臂***质心变化的速度,ms表示旋翼飞行机械臂***的总质量;旋翼飞行机械臂***包括无人机与机械臂;mi表示机械臂第i个连杆的质量,n表示机械臂连杆的总数量,i=1,2…n,Bpci表示机械臂第i个连杆在机体坐标系下的质心位置,Bvci表示机械臂第i个连杆在机体坐标系下的质心变化速度。
根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,步骤一中,惯量在机体坐标系下的表达式为:
式中表示机械臂相对于无人机基座与机械臂的连接点在机体坐标系下的转动惯量矩阵,/>表示/>的导数,BRi表示机械臂第i个连杆坐标系到机体坐标系的旋转矩阵,/>表示机械臂第i个连杆在第i个连杆坐标系的惯量矩阵,I3×3表示3x3的单位矩阵,Bωi表示第i个连杆在机体坐标系下的角速度,Skew(Bωi)表示Bωi的反对称矩阵,iRB表示BRi的转置;Bpci,Bvci,Bωi的获得方法为:
式中q表示机械臂的关节角度,表示机械臂的角速度向量,BJci(q)表示机械臂第i个连杆质心相对于机体坐标系的雅可比矩阵,irci表示机械臂第i个连杆质心在第i个连杆坐标系的位置向量,BTi(q)表示从第i个连杆坐标系到机体坐标系的转换矩阵。
根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,步骤二中得到***动力学模型包括得到无人机机体坐标系到惯性系的转换矩阵IRB:
首先进行参考坐标系定义,以∑I表示惯性系,以∑B表示机体坐标系,∑I和∑B中均以横轴指向机头方向,竖轴指向地,∑B中原点O位于无人机质心处,即无人机基座与机械臂的连接点;∑i(i=1,2,,,n)分别表示n自由度的机械臂各连杆坐标系,根据craig法则改进的DH参数法建立;
假设p0点为旋翼飞行机械臂***中任一质点,p0点相对于惯性系∑I的绝对位置向量为rp0,无人机相对于惯性系∑I的绝对位置向量为ro,p0点相对于无人机的质心位置为则得到以下关系式:
式中表示机体坐标系下p0点相对于o点的位置矢量,/>为/>的导数,Bωb表示无人机机体实际角速度向量,/>表示/>的导数,/>为/>在机体坐标系的表示,×代表叉乘运算;则得到IRB为:
式中φ表示无人机俯仰角,θ表示无人机滚转角,ψ表示无人机偏航角。
根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,步骤二中得到***动力学模型还包括得到旋翼飞行机械臂***的动量和动量矩:
根据多刚体质点系的动量和动量矩定义,得到旋翼飞行机械臂***的动量P和动量矩L:
式中mb表示无人机质量,mman表示机械臂质量,表示p0点所在位置的质量微分量;
对上式变形,得到:
式中Bromc表示机体坐标系下的机械臂质心向量,roc表示旋翼飞行机械臂***质心向量,BIb表示无人机转动惯量矩阵,表示机体坐标系下机械臂相对于o点的转动惯量矩阵;
机体坐标系下的机械臂质心向量Bromc和机体坐标系下的旋翼飞行机械臂***质心向量Broc之间的关系为:
根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,对旋翼飞行机械臂***的动量P和动量矩L求导得到:
按照动量与动量矩定理,得到:
式中IFext表示惯性系中作用在质点系上的合外力,表示IFext相对于o点产生的合外力矩;则对于旋翼飞行机械臂***,得到:
式中Fl表示无人机螺旋桨产生的升力,e3表示竖直方向单位向量,e3=[0 0 1]T;g表示重力加速度,τ表示无人机螺旋桨产生的力矩。
根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,结合所有关系式推导得到***动力学模型:
式中Pb=[x,y,z]表示无人机在惯性坐标系下的位置向量,其中x,y,z为无人机在惯性坐标系下的横轴、纵轴、竖轴坐标;表示无人机在惯性坐标系下的速度向量;Φb为无人机机体角速度:Φb=[φ,θ,ψ];
T(Φb)表示无人机机体角速度到姿态角速度的旋转矩阵;
Bro表示机体坐标系下机械臂与无人机基座连接点的位置,所述机械臂安装在无人机质心位置;
Bωb=[p q r]T,其中p、q、r分别为无人机的滚转角速度、俯仰角速度和偏航角速度。
根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,所述耦合扰动模型的具体形式如下:
式中Fdis表示基于变惯性参数的耦合扰动模型估计的耦合扰动力向量,Bτdis表示基于变惯性参数的耦合扰动模型估计的耦合扰动力矩向量。
根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,步骤四中,位置环和姿态环的基础控制器均采用级联PID控制算法;
级联PID位置内环控制器的表达式为:
upos_basic=[ux uy uz]T=Kp_v(vb_d-vb)+Ki_v∫(vb_d-vb)dt+Kd_v(d(vb_d-vb)/dt),
式中upos_basic表示由级联PID位置内环控制器生成的三个方向上的基础控制量,其中ux、uy、uz分别为横轴、纵轴和竖轴的基础控制量;vb_d为期望速度向量,vb为实际速度向量;
Kp_v表示位置内环控制器比例参数矩阵,Ki_v表示位置内环控制器积分参数矩阵,Kd_v表示位置内环控制器微分参数矩阵;
级联PID姿态内环控制器的表达式为:
式中uatt_basic表示由级联PID姿态内环控制器生成的三个方向上的基础控制量,其中τφ表示俯仰角方向的基础控制量,τθ表示滚转角方向的基础控制量,τψ表示偏航角方向的基础控制量,Bωb_d为期望角速度向量;Kp_ω表示姿态内环控制器比例参数矩阵,Ki_ω表示姿态内环控制器积分参数矩阵,Kd_ω表示姿态内环控制器微分参数矩阵。
根据本发明的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,由级联PID位置内环控制器和级联PID姿态内环控制器得到整体控制器的最终形式为:
式中upos表示位置内环最终控制量,uatt表示姿态内环最终控制量。
本发明的有益效果:本发明方法具体应用在搭载多自由度机械臂的旋翼飞行机械臂***中,可对旋翼飞行机械臂内存在的耦合扰动影响进行精确的模型描述与前馈补偿,实现旋翼飞行机械臂在强烈的耦合扰动影响下的精确补偿与稳定控制。
本发明方法主要以旋翼飞行器为基座搭载多自由度机械臂的新型空中机器人***为研究对象,研究该***面临的强烈的耦合扰动问题。从耦合扰动产生机理层面展开深入分析,综合考虑了该***由于机械臂运动所带来的***质心与惯量的动态变化,并以变惯性参数的方式来描述机械臂运动与***质心和惯量变化的映射关系。在此基础上,基于多刚体的动量与动量矩定理,进一步推导了精确的***动力学模型和基于变惯性参数的耦合扰动模型,该模型充分利用了无人机与机械臂的状态信息,能准确反应无人机与机械臂之间耦合扰动的静态和动态关系,并实现在不借助外部传感器的情况下对耦合扰动的准确估计。
另外,基于该精确的耦合扰动模型结合基础控制器的前馈补偿方法,相较于其他反馈补偿方法,本发明方法可及时准确地补偿由耦合扰动带给***的干扰影响,有效避免了反馈补偿方法需要一定响应时间的延迟且很难做到准确估计的弊端。因此,本发明方法可以有效抑制强烈的耦合扰动对***的影响,显著提高旋翼飞行机械臂***的抗扰能力、控制精度及稳定性。
附图说明
图1是本发明所述基于变惯性参数建模的飞行机械臂耦合扰动控制方法的流程图;
图2是旋翼飞行机械臂***建模过程参考坐标系定义示意图;图中1表示无人机的第一个电机,2表示无人机的第二个电机,3表示无人机的第三个电机,4表示无人机的第四个电机,5表示无人机的第五个电机,六表示无人机的第六个电机;XB、YB、ZB表示机体坐标系∑B的横轴、纵轴和竖轴;XI、YI、ZI表示惯性系∑I的横轴、纵轴和竖轴;Xi、Yi、Zi表示机械臂第i个连杆坐标系∑i的横轴、纵轴和竖轴;
图3是基于本发明方法进行前馈补偿的飞行机械臂耦合扰动控制的控制框图;图中xd、yd、zd分别为x、y、z轴期望位置信号,ψd为偏航角期望信号,φd为滚转角期望信号,θd为俯仰角期望信号,um为得到的升力方向控制量。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其它实施例,都属于本发明保护的范围。
需要说明的是,在不冲突的情况下,本发明中的实施例及实施例中的特征可以相互组合。
下面结合附图和具体实施例对本发明作进一步说明,但不作为本发明的限定。
具体实施方式一、结合图1至图3所示,本发明提供了一种基于变惯性参数建模的飞行机械臂耦合扰动控制方法,整体技术方案由耦合扰动产生机理分析、变惯性参数设计、***动力学建模、耦合扰动模型建立及结合控制器进行前馈补偿这五个部分组成。对于搭载多自由度机械臂的旋翼飞行机械臂***,无人机基座与机械臂之间所产生的耦合扰动将直接影响***位置与姿态控制表现,甚至威胁到***飞行的安全。本实施方式首先从耦合扰动产生机理层面对耦合扰动进行了深入分析,考虑质心和惯量的动态变化,利用变惯性参数对该***及其耦合扰动进行了精确建模,所推到的耦合扰动模型充分利用了无人机与机械臂的状态信息,能准确反应无人机与机械臂之间耦合扰动的静态和动态关系,可实现在不借助外部传感器的情况下对耦合扰动的准确估计。利用耦合扰动模型在控制器中进行前馈补偿,可实现旋翼飞行机械臂在强烈的耦合扰动影响下的精确补偿与控制。
本实施方式对耦合扰动产生机理进行深入分析。旋翼飞行机械臂***中无人机与机械臂之间的耦合扰动,不同于传统机械臂对于固定基座施加的扰动力和力矩,其是与无人机状态和机械臂状态紧密相关的一种强烈的耦合影响。在旋翼飞行机械臂***中,当无人机与机械臂之间存在相对运动时,该耦合扰动便会产生,且随着两者之间相对运动越剧烈越快速,这种耦合扰动影响越强烈,带给整个***的干扰影响越显著。这是因为机械臂的运动会引起旋翼飞行机械臂***的质心和惯量发生变化,且机械臂运动范围越大越快速,所引起的质心和惯量变化越大越剧烈,从而产生越强烈的耦合扰动力和力矩作用于***。
具体实现步骤包括:
步骤一:变惯性参数设计:对旋翼飞行机械臂***的质心变化,建立动态变化质心在机体坐标系下的表达式;同时,对旋翼飞行机械臂***的惯量变化,根据平行轴移定理,建立动态变化惯量在机体坐标系下的表达式;
步骤二:考虑旋翼飞行机械臂是由旋翼无人机搭载一个n自由度的机械臂构成的整体***,这时不再将旋翼飞行机械臂***统一作为单个刚体,而是看作为三维空间的多个刚体组成的***。利用多刚体质点系的动量与动量矩定理对旋翼飞行机械臂***进行动力学建模,得到***动力学模型;
步骤三:基于质心变化与惯量变化对***动力学模型进行处理,建立质心变化和惯量变化对旋翼飞行机械臂***的耦合扰动的映射关系,得到基于变惯性参数的耦合扰动模型;
步骤四:结合控制器进行前馈补偿。在通过精确的建模步骤得到了基于变惯性参数的耦合扰动模型后,只需在基础控制器设计过程中加入耦合扰动模型所计算出来的耦合扰动估计值作为前馈补偿,即可实现旋翼飞行机械臂在强烈的耦合扰动影响下的精确补偿与控制。所述基础控制器可以是任意线性或非线性控制器,只要其在没有扰动情况下的闭环***是稳定的即可。将耦合扰动模型计算获得的耦合扰动估计值作为前馈补偿量输入位置环和姿态环,实现耦合扰动控制。
进一步,步骤一中,质心在机体坐标系下的表达式为:
式中Broc表示旋翼飞行机械臂***在机体坐标系下的质心向量,表示旋翼飞行机械臂***质心变化的速度,ms表示旋翼飞行机械臂***的总质量;旋翼飞行机械臂***包括无人机与机械臂;mi表示机械臂第i个连杆的质量,n表示机械臂连杆的总数量,i=1,2…n,Bpci表示机械臂第i个连杆在机体坐标系下的质心位置,Bvci表示机械臂第i个连杆在机体坐标系下的质心变化速度。
步骤一中,惯量在机体坐标系下的表达式为:
式中表示机械臂相对于无人机基座与机械臂的连接点在机体坐标系下的转动惯量矩阵,/>表示/>的导数,BRi表示机械臂第i个连杆坐标系到机体坐标系的旋转矩阵,/>表示机械臂第i个连杆在第i个连杆坐标系的惯量矩阵,I3×3表示3x3的单位矩阵,Bωi表示第i个连杆在机体坐标系下的角速度,Skew(Bωi)表示Bωi的反对称矩阵,iRB表示BRi的转置;
上面两个表达式中,除了Bpci,Bvci,Bωi,其余变量均为机械臂固有惯性参数或可通过机械臂DH参数模型获取。Bpci,Bvci,Bωi的获得方法为:
式中q表示机械臂的关节角度,表示机械臂的角速度向量,BJci(q)表示机械臂第i个连杆质心相对于机体坐标系的雅可比矩阵,irci表示机械臂第i个连杆质心在第i个连杆坐标系的位置向量;BTi(q)表示从第i个连杆坐标系到机体坐标系的转换矩阵,可通过DH参数模型获得。
再进一步,步骤二中得到***动力学模型包括得到无人机机体坐标系到惯性系的转换矩阵IRB:
结合图2所示,首先进行参考坐标系定义,以∑I表示惯性系,以∑B表示机体坐标系,∑I和∑B中均以横轴指向机头方向,竖轴指向地,∑B中原点O位于无人机质心处,即无人机基座与机械臂的连接点;∑i(i=1,2,,,n)分别表示n自由度的机械臂各连杆坐标系,根据craig法则改进的DH参数法建立;
假设p0点为旋翼飞行机械臂***中任一质点,p0点相对于惯性系∑I的绝对位置向量为无人机相对于惯性系∑I的绝对位置向量为ro,p0点相对于无人机的质心位置为则得到以下关系式:
式中表示机体坐标系下p0点相对于o点的位置矢量,/>为/>的导数,Bωb表示无人机机体实际角速度向量,/>表示/>的导数,/>为/>在机体坐标系的表示,×代表叉乘运算;则得到IRB为:
式中φ表示无人机俯仰角,θ表示无人机滚转角,ψ表示无人机偏航角。
步骤二中得到***动力学模型还包括得到旋翼飞行机械臂***的动量和动量矩:
根据多刚体质点系的动量和动量矩定义,得到旋翼飞行机械臂***的动量P和动量矩L:
式中mb表示无人机质量,mman表示机械臂质量,表示p0点所在位置的质量微分量;
对上式变形,得到:
式中Bromc表示机体坐标系下的机械臂质心向量,roc表示旋翼飞行机械臂***质心向量,BIb表示无人机转动惯量矩阵,表示机体坐标系下机械臂相对于o点的转动惯量矩阵;
机体坐标系下的机械臂质心向量Bromc和机体坐标系下的旋翼飞行机械臂***质心向量Broc之间的关系为:
本实施方式中只保留了中的主导项Lman表示机械臂运动相对于o点的动量矩,这是因为可以认为机械臂质心的运动相对于o点的动量矩可主要代表机械臂整体运动对o点的动量矩,这样做是合理的。
对旋翼飞行机械臂***的动量P和动量矩L求导得到:
按照动量与动量矩定理,得到:
式中IFext表示惯性系中作用在质点系上的合外力,表示IFext相对于o点产生的合外力矩;则对于旋翼飞行机械臂***,得到:
式中Fl表示无人机螺旋桨产生的升力,e3表示竖直方向单位向量,e3=[0 0 1]T;g表示重力加速度,τ表示无人机螺旋桨产生的力矩。
结合所有关系式推导得到***动力学模型:
式中Pb=[x,y,z]表示无人机在惯性坐标系下的位置向量,其中x,y,z为无人机在惯性坐标系下的横轴、纵轴、竖轴坐标;表示无人机在惯性坐标系下的速度向量;Φb为无人机机体角速度:Φb=[φ,θ,ψ];
T(Φb)表示无人机机体角速度到姿态角速度的旋转矩阵;
Bro表示机体坐标系下机械臂与无人机基座连接点的位置,所述机械臂安装在无人机质心位置;
Bωb=[p q r]T,其中p、q、r分别为无人机的滚转角速度、俯仰角速度和偏航角速度。
基于变惯性参数推导的旋翼飞行机械臂***动力学模型不同于传统旋翼飞行平台动力学模型,其含有动态变化的惯性参数及其引起的附加耦合扰动项,它们与无人机和机械臂的状态量及其导数紧密耦合在一起。从理论上讲,该模型可以很好的描述出旋翼飞行机械臂***在受耦合扰动情况下的真实动力学特性。
机械臂运动会导致旋翼飞行机械臂***的质心Broc和惯量发生变化,且机械臂运动越剧烈,所带来的***质心和惯量变化越剧烈,而这种变化和整个***间的耦合扰动之间的映射关系即为所需要建立的耦合扰动模型。将步骤二中的***动力学模型做进一步处理,可推导出基于变惯性参数的耦合扰动模型具体形式如下:
式中Fdis表示基于变惯性参数的耦合扰动模型估计的耦合扰动力向量,Bτdis表示基于变惯性参数的耦合扰动模型估计的耦合扰动力矩向量。
耦合扰动模型中所需用到的无人机及机械臂各状态量、状态量导数及其二阶导都是可以通过旋翼飞行机械臂***自身的传感器直接测量得到或间接通过一些估计方法得到,是易于在实际***中使用的。
基于变惯性参数的耦合扰动模型是与无人机及机械臂各状态量、状态量导数及其二阶导直接相关的,其充分利用了***内部各部分的状态量,且这些状态量都是可以通过旋翼飞行机械臂***上自带的传感器直接测量或间接估计出来的,可以很好地反应无人机和机械臂之间耦合扰动的静态与动态关系。
再进一步,结合控制器进行前馈补偿。
考虑到现行大多数的实物无人机***的控制器均还是采用级联PID控制算法,为了本发明所提出方法在实际***的泛化使用,选取级联PID控制算法作为基础控制器来说明结合控制器进行前馈补偿的具体实现。
步骤四中,位置环和姿态环的基础控制器均采用级联PID控制算法;
级联PID位置内环控制器的表达式为:
upos_basic=[ux uy uz]T=Kp_v(vb_d-vb)+Ki_v∫(vb_d-vb)dt+Kd_v(d(vb_d-vb)/dt),
式中upos_basic表示由级联PID位置内环控制器生成的三个方向上的基础控制量,其中ux、uy、uz分别为横轴、纵轴和竖轴的基础控制量;vb_d为期望速度向量,vb为实际速度向量;
Kp_v表示位置内环控制器比例参数矩阵,Ki_v表示位置内环控制器积分参数矩阵,Kd_v表示位置内环控制器微分参数矩阵;
级联PID姿态内环控制器的表达式为:
式中uatt_basic表示由级联PID姿态内环控制器生成的三个方向上的基础控制量,其中τφ表示俯仰角方向的基础控制量,τθ表示滚转角方向的基础控制量,τψ表示偏航角方向的基础控制量,Bωb_d为期望角速度向量;Kp_ω表示姿态内环控制器比例参数矩阵,Ki_ω表示姿态内环控制器积分参数矩阵,Kd_ω表示姿态内环控制器微分参数矩阵。
基于变惯性参数的耦合扰动模型进行前馈补偿,并结合基础控制器所生成的控制量,可以得到控制器最终形式:
由级联PID位置内环控制器和级联PID姿态内环控制器得到整体控制器的最终形式为:
式中upos表示位置内环最终控制量,uatt表示姿态内环最终控制量。
至此,完成了对本发明方法的详细阐述,控制***框架图如图3所示。图3中首先,被控对象为旋翼飞行机械臂***,由无人机基座搭载多自由度机械臂构成,其内部存在强烈的耦合扰动;然后,对于控制流程,基于变惯性参数的耦合扰动模型前馈补偿量部分主要负责根据无人机与机械臂的各状态量、状态量导数和采用指令滤波器等微分估计方法获取的状态量二阶导,通过耦合扰动模型计算出前馈补偿力和力矩;然后结合基础控制器生成的控制量进行前馈补偿,从而形成最终的控制量作用于旋翼飞行机械臂***,使其实现在强烈的耦合扰动影响下的精确补偿与控制。
虽然在本文中参照了特定的实施方式来描述本发明,但是应该理解的是,这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是,可以对示例性的实施例进行许多修改,并且可以设计出其他的布置,只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是,可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是,结合单独实施例所描述的特征可以使用在其它所述实施例中。
Claims (8)
1.一种基于变惯性参数建模的飞行机械臂耦合扰动控制方法,其特征在于包括,
步骤一:对旋翼飞行机械臂***的质心变化,建立动态变化质心在机体坐标系下的表达式;同时,对旋翼飞行机械臂***的惯量变化,根据平行轴移定理,建立动态变化惯量在机体坐标系下的表达式;
步骤二:利用多刚体质点系的动量与动量矩定理对旋翼飞行机械臂***进行动力学建模,得到***动力学模型;
步骤三:基于质心变化与惯量变化对***动力学模型进行处理,建立质心变化和惯量变化对旋翼飞行机械臂***的耦合扰动的映射关系,得到基于变惯性参数的耦合扰动模型;
步骤四:将耦合扰动模型计算获得的耦合扰动估计值作为前馈补偿量输入位置环和姿态环,实现耦合扰动控制;
步骤一中,质心在机体坐标系下的表达式为:
式中Broc表示旋翼飞行机械臂***在机体坐标系下的质心向量,表示旋翼飞行机械臂***质心变化的速度,ms表示旋翼飞行机械臂***的总质量;旋翼飞行机械臂***包括无人机与机械臂;mi表示机械臂第i个连杆的质量,n表示机械臂连杆的总数量,i=1,2···n,Bpci表示机械臂第i个连杆在机体坐标系下的质心位置,Bvci表示机械臂第i个连杆在机体坐标系下的质心变化速度;
步骤一中,惯量在机体坐标系下的表达式为:
式中表示机械臂相对于无人机基座与机械臂的连接点在机体坐标系下的转动惯量矩阵,/>表示/>的导数,BRi表示机械臂第i个连杆坐标系到机体坐标系的旋转矩阵,表示机械臂第i个连杆在第i个连杆坐标系的惯量矩阵,I3×3表示3x3的单位矩阵,Bωi表示第i个连杆在机体坐标系下的角速度,Skew(Bωi)表示Bωi的反对称矩阵,iRB表示BRi的转置;Bpci,Bvci,Bωi的获得方法为:
式中q表示机械臂的关节角度,表示机械臂的角速度向量,BJci(q)表示机械臂第i个连杆质心相对于机体坐标系的雅可比矩阵,irci表示机械臂第i个连杆质心在第i个连杆坐标系的位置向量,BTi(q)表示从第i个连杆坐标系到机体坐标系的转换矩阵。
2.根据权利要求1所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,其特征在于,步骤二中得到***动力学模型包括得到无人机机体坐标系到惯性系的转换矩阵IRB:
首先进行参考坐标系定义,以∑I表示惯性系,以∑B表示机体坐标系,∑I和∑B中均以横轴指向机头方向,竖轴指向地,∑B中原点O位于无人机质心处,即无人机基座与机械臂的连接点;∑i分别表示n自由度的机械臂各连杆坐标系,根据craig法则改进的DH参数法建立,i=1,2,,,n;
假设p0点为旋翼飞行机械臂***中任一质点,p0点相对于惯性系∑I的绝对位置向量为无人机相对于惯性系∑I的绝对位置向量为ro,p0点相对于无人机的质心位置为/>则得到以下关系式:
式中表示机体坐标系下p0点相对于o点的位置矢量,/>为/>的导数,Bωb表示无人机机体实际角速度向量,/>表示/>的导数,/>为/>在机体坐标系的表示,×代表叉乘运算;则得到IRB为:
式中φ表示无人机俯仰角,θ表示无人机滚转角,ψ表示无人机偏航角。
3.根据权利要求2所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,其特征在于,步骤二中得到***动力学模型还包括得到旋翼飞行机械臂***的动量和动量矩:
根据多刚体质点系的动量和动量矩定义,得到旋翼飞行机械臂***的动量P和动量矩L:
式中mb表示无人机质量,mman表示机械臂质量,mp0表示p0点所在位置的质量微分量;
对上式变形,得到:
式中Bromc表示机体坐标系下的机械臂质心向量,roc表示旋翼飞行机械臂***质心向量,BIb表示无人机转动惯量矩阵,表示机体坐标系下机械臂相对于o点的转动惯量矩阵;
机体坐标系下的机械臂质心向量Bromc和机体坐标系下的旋翼飞行机械臂***质心向量Broc之间的关系为:
4.根据权利要求3所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,其特征在于,对旋翼飞行机械臂***的动量P和动量矩L求导得到:
按照动量与动量矩定理,得到:
式中IFext表示惯性系中作用在质点系上的合外力,表示IFext相对于o点产生的合外力矩;则对于旋翼飞行机械臂***,得到:
式中Fl表示无人机螺旋桨产生的升力,e3表示竖直方向单位向量,e3=[0 0 1]T;g表示重力加速度,τ表示无人机螺旋桨产生的力矩。
5.根据权利要求4所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,其特征在于,结合所有关系式推导得到***动力学模型:
式中Pb=[x,y,z]表示无人机在惯性坐标系下的位置向量,其中x,y,z为无人机在惯性坐标系下的横轴、纵轴、竖轴坐标;表示无人机在惯性坐标系下的速度向量;Φb为无人机机体角速度:Φb=[φ,θ,ψ];
T(Φb)表示无人机机体角速度到姿态角速度的旋转矩阵;
Bro表示机体坐标系下机械臂与无人机基座连接点的位置,所述机械臂安装在无人机质心位置;
Bωb=[p q r]T,其中p、q、r分别为无人机的滚转角速度、俯仰角速度和偏航角速度。
6.根据权利要求5所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,其特征在于,所述耦合扰动模型的具体形式如下:
式中Fdis表示基于变惯性参数的耦合扰动模型估计的耦合扰动力向量,Bτdis表示基于变惯性参数的耦合扰动模型估计的耦合扰动力矩向量。
7.根据权利要求6所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,其特征在于,步骤四中,位置环和姿态环的基础控制器均采用级联PID控制算法;
级联PID位置内环控制器的表达式为:
upos_basic=[ux uy uz]T=Kp_v(vb_d-vb)+Ki_v∫(vb_d-vb)dt+Kd_v(d(vb_d-vb)/dt),
式中upos_basic表示由级联PID位置内环控制器生成的三个方向上的基础控制量,其中ux、uy、uz分别为横轴、纵轴和竖轴的基础控制量;vb_d为期望速度向量,vb为实际速度向量;
Kp_v表示位置内环控制器比例参数矩阵,Ki_v表示位置内环控制器积分参数矩阵,Kd_v表示位置内环控制器微分参数矩阵;
级联PID姿态内环控制器的表达式为:
式中uatt_basic表示由级联PID姿态内环控制器生成的三个方向上的基础控制量,其中τφ表示俯仰角方向的基础控制量,τθ表示滚转角方向的基础控制量,τψ表示偏航角方向的基础控制量,Bωb_d为期望角速度向量;Kp_ω表示姿态内环控制器比例参数矩阵,Ki_ω表示姿态内环控制器积分参数矩阵,Kd_ω表示姿态内环控制器微分参数矩阵。
8.根据权利要求7所述的基于变惯性参数建模的飞行机械臂耦合扰动控制方法,其特征在于,
由级联PID位置内环控制器和级联PID姿态内环控制器得到整体控制器的最终形式为:
式中upos表示位置内环最终控制量,uatt表示姿态内环最终控制量。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211319418.XA CN115556111B (zh) | 2022-10-26 | 2022-10-26 | 基于变惯性参数建模的飞行机械臂耦合扰动控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211319418.XA CN115556111B (zh) | 2022-10-26 | 2022-10-26 | 基于变惯性参数建模的飞行机械臂耦合扰动控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115556111A CN115556111A (zh) | 2023-01-03 |
CN115556111B true CN115556111B (zh) | 2023-08-18 |
Family
ID=84769188
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211319418.XA Active CN115556111B (zh) | 2022-10-26 | 2022-10-26 | 基于变惯性参数建模的飞行机械臂耦合扰动控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115556111B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115364255B (zh) * | 2022-09-27 | 2023-09-01 | 江苏理工学院 | 一种适用范围广且消毒稳定的病毒消杀装置及控制方法 |
CN116909199B (zh) * | 2023-09-11 | 2023-12-22 | 华东交通大学 | 一种基于连杆配置的可重构无人机的控制方法 |
Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0733095A (ja) * | 1993-07-16 | 1995-02-03 | Mitsubishi Electric Corp | 宇宙機の制御装置、およびそのスラスタ制御方法 |
WO2015180171A1 (en) * | 2014-05-30 | 2015-12-03 | SZ DJI Technology Co., Ltd. | Aircraft attitude control methods |
JP2018010671A (ja) * | 2017-09-05 | 2018-01-18 | エスゼット ディージェイアイ テクノロジー カンパニー リミテッドSz Dji Technology Co.,Ltd | 航空機姿勢制御方法 |
CN108803639A (zh) * | 2018-05-29 | 2018-11-13 | 南京理工大学 | 一种基于反步法的四旋翼飞行器飞行控制方法 |
CN109358504A (zh) * | 2018-10-24 | 2019-02-19 | 中北大学 | 基于自适应积分鲁棒的四旋翼轨迹/姿态复合抗干扰跟踪控制方法 |
CN111098299A (zh) * | 2019-12-06 | 2020-05-05 | 上海航天控制技术研究所 | 一种空间机器人扰动补偿的方法 |
CN111880553A (zh) * | 2020-08-11 | 2020-11-03 | 北京航空航天大学 | 一种考虑惯量不确定性的四旋翼无人机姿态控制方法 |
CN111923047A (zh) * | 2020-08-11 | 2020-11-13 | 北京航空航天大学 | 一种针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法 |
CN112558621A (zh) * | 2019-09-26 | 2021-03-26 | 西南科技大学 | 一种基于解耦控制的飞行机械臂*** |
CN113703320A (zh) * | 2021-08-27 | 2021-11-26 | 北京航空航天大学杭州创新研究院 | 一种抗干扰和饱和特性的飞行机械臂位姿控制方法 |
CN114035598A (zh) * | 2021-11-22 | 2022-02-11 | 青岛理工大学 | 一种多旋翼吊挂***的视觉摆角检测与减摆方法 |
CN114734445A (zh) * | 2022-05-09 | 2022-07-12 | 中兵智能创新研究院有限公司 | 一种四足机器人动态适应负载的控制方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105480413B (zh) * | 2016-02-03 | 2019-01-22 | 英华达(上海)科技有限公司 | 无人旋翼机及控制无人旋翼机的飞行方法 |
-
2022
- 2022-10-26 CN CN202211319418.XA patent/CN115556111B/zh active Active
Patent Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0733095A (ja) * | 1993-07-16 | 1995-02-03 | Mitsubishi Electric Corp | 宇宙機の制御装置、およびそのスラスタ制御方法 |
WO2015180171A1 (en) * | 2014-05-30 | 2015-12-03 | SZ DJI Technology Co., Ltd. | Aircraft attitude control methods |
JP2018010671A (ja) * | 2017-09-05 | 2018-01-18 | エスゼット ディージェイアイ テクノロジー カンパニー リミテッドSz Dji Technology Co.,Ltd | 航空機姿勢制御方法 |
CN108803639A (zh) * | 2018-05-29 | 2018-11-13 | 南京理工大学 | 一种基于反步法的四旋翼飞行器飞行控制方法 |
CN109358504A (zh) * | 2018-10-24 | 2019-02-19 | 中北大学 | 基于自适应积分鲁棒的四旋翼轨迹/姿态复合抗干扰跟踪控制方法 |
CN112558621A (zh) * | 2019-09-26 | 2021-03-26 | 西南科技大学 | 一种基于解耦控制的飞行机械臂*** |
CN111098299A (zh) * | 2019-12-06 | 2020-05-05 | 上海航天控制技术研究所 | 一种空间机器人扰动补偿的方法 |
CN111880553A (zh) * | 2020-08-11 | 2020-11-03 | 北京航空航天大学 | 一种考虑惯量不确定性的四旋翼无人机姿态控制方法 |
CN111923047A (zh) * | 2020-08-11 | 2020-11-13 | 北京航空航天大学 | 一种针对质心偏移和基座浮动的带臂旋翼无人机姿态控制方法 |
CN113703320A (zh) * | 2021-08-27 | 2021-11-26 | 北京航空航天大学杭州创新研究院 | 一种抗干扰和饱和特性的飞行机械臂位姿控制方法 |
CN114035598A (zh) * | 2021-11-22 | 2022-02-11 | 青岛理工大学 | 一种多旋翼吊挂***的视觉摆角检测与减摆方法 |
CN114734445A (zh) * | 2022-05-09 | 2022-07-12 | 中兵智能创新研究院有限公司 | 一种四足机器人动态适应负载的控制方法 |
Non-Patent Citations (1)
Title |
---|
吴仪政.变负载无人机的自适应模糊自抗扰控制研究.中国优秀硕士论文全文数据库.2022,全文. * |
Also Published As
Publication number | Publication date |
---|---|
CN115556111A (zh) | 2023-01-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN115556111B (zh) | 基于变惯性参数建模的飞行机械臂耦合扰动控制方法 | |
Martins et al. | Feedback linearization with zero dynamics stabilization for quadrotor control | |
CN109696830B (zh) | 小型无人直升机的强化学习自适应控制方法 | |
CN109460596B (zh) | 一种飞翼无人机非线性载荷计算方法 | |
CN110427043B (zh) | 基于作业飞行机器人重心偏移的位姿控制器设计方法 | |
CN109116860B (zh) | 三旋翼无人机的非线性鲁棒控制方法 | |
CN111708377B (zh) | 基于惯导/飞控***信息融合的飞行控制方法 | |
CN112558621A (zh) | 一种基于解耦控制的飞行机械臂*** | |
CN110908278A (zh) | 一种折叠翼飞行器的动力学建模与稳定控制方法 | |
CN112346470A (zh) | 一种基于改进自抗扰控制的四旋翼姿态控制方法 | |
Li et al. | Visual servoing of flexible-link manipulators by considering vibration suppression without deformation measurements | |
Muniraj et al. | Path-following control of small fixed-wing unmanned aircraft systems with H∞ type performance | |
CN115826597A (zh) | 基于自适应神经网络的旋翼飞行器抗扰控制方法及设备 | |
CN115366109A (zh) | 一种旋翼飞行机械臂复合分层抗干扰方法 | |
CN115431271A (zh) | 一种飞行机械臂末端抗干扰指向控制方法 | |
CN115480583A (zh) | 飞行作业机器人的视觉伺服跟踪与阻抗控制方法 | |
Bazylev et al. | Design of control system for a four-rotor UAV equipped with robotic arm | |
CN113580127A (zh) | 一种多旋翼飞行器仿人双机械臂***及其动态自平衡控制设计方法 | |
CN116382332B (zh) | 一种基于ude的战斗机大机动鲁棒飞行控制方法 | |
CN116679548A (zh) | 基于时变观测器的三自由度直升机鲁棒输出反馈控制方法 | |
Alqaisi et al. | Vision-based leader-follower approach for uncertain quadrotor dynamics using feedback linearisation sliding mode control | |
CN116449694A (zh) | 基于自适应变参的三自由度直升机鲁棒输出反馈控制方法 | |
Abrougui et al. | Flight Controller Design Based on Sliding Mode Control for Quadcopter Waypoints Tracking | |
CN113442118A (zh) | 一种可穿戴外肢体机器人碰撞响应控制方法及*** | |
Huang et al. | Design, simulation and experimental study of a force observer for a flying–perching quadrotor |
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 |