CN107728470B - 一种无速率测量的机械臂姿态控制方法 - Google Patents

一种无速率测量的机械臂姿态控制方法 Download PDF

Info

Publication number
CN107728470B
CN107728470B CN201710773971.3A CN201710773971A CN107728470B CN 107728470 B CN107728470 B CN 107728470B CN 201710773971 A CN201710773971 A CN 201710773971A CN 107728470 B CN107728470 B CN 107728470B
Authority
CN
China
Prior art keywords
signal
attitude
mechanical arm
estimator
derivative
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
CN201710773971.3A
Other languages
English (en)
Other versions
CN107728470A (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.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
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 University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN201710773971.3A priority Critical patent/CN107728470B/zh
Publication of CN107728470A publication Critical patent/CN107728470A/zh
Application granted granted Critical
Publication of CN107728470B publication Critical patent/CN107728470B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明公开了一种无速率测量的机械臂姿态控制方法，属于机械臂控制技术领域。本发明的控制步骤为：首先，建立机械臂的动力学与运动学模型；然后，进行反馈线性化；其次，根据上述线性模型设置速率估计器与比例‑积分‑微分控制器，将该的控制信号转换成相应物理量输入至机械臂作动器。为了获得更好的控制效果，利用极点配置方法对PID控制器参数进行调参。本发明可在不依赖的速率信号测量前提下实现对机械臂姿态的控制，且控制方式简单易用。

Description

一种无速率测量的机械臂姿态控制方法
技术领域
本发明属于机械臂控制技术领域，具体涉及一种在速率不可测条件下的机械臂姿态控制方法。
背景技术
目前，机械臂广泛应用于航天工程、生产制造业以及机器人技术创新等领域。机械臂的姿态控制是机械臂完成指定动作任务的基础与核心。为满足姿态控制的需要，常常需要获得准确的姿态速率信息。但是，机械臂在工作过程中，其速率测量系统失灵，测量信号噪声过大以及速率传感器的几何尺寸过大无法安装在机械臂等原因，会导致不能或不能完全测量准确的姿态速率信息，若直接对姿态信号进行微分，则高频噪声容易导致角微分信号不可用，高增益微分器尤其明显。
基于机械臂的动力学与运动学模型，可以对机械臂多个姿态通道设置反馈控制器，常用的反馈控制器为比例-积分-微分控制器，此线性反馈控制器设置较为简单，但其控制参数若选择不当，很有可能造成系统不稳定，如何根据系统特性选取合适的控制参数是提高姿态控制效果的一个关键问题。
发明内容
本发明的发明目的在于：针对上述存在的问题，提供一种基于速率估计器的机械臂姿态控制方法，以获得更好的控制效果。
本发明的基于速率估计器的机械臂姿态控制方法包括下列步骤：
步骤1：基于待控制机械臂的动力学与运动学模型，构造受扰二阶积分器链。
根据机械结构特点，选取合适的状态变量，列出系统非线性方程如下：
Figure BDA0001395569080000011
其中x为系统状态向量，u为系统输入向量(即系统输入信号)，y为系统输出向量，f(x)、g(x)以及h(x)为非线性项。
对输出方程求二阶导并利用牛顿-欧拉方程建立机械臂动力学与运动学模型：
Figure BDA0001395569080000012
其中，
Figure BDA0001395569080000021
表示函数h(x)f(x)对x求导过程中函数h(x)对自变量x求偏导部分，
Figure BDA0001395569080000022
表示函数对自变量x求偏导，
Figure BDA0001395569080000023
Figure BDA0001395569080000024
且
Figure BDA0001395569080000025
利用非线性系统中输入-输出反馈线性化方法将式(2)中的u变量替换为uρ，替换方程如下式所示：
Figure BDA0001395569080000026
变量替换后，考虑到机械臂所受外界干扰，可得受扰二阶积分器链形式：
Figure BDA0001395569080000027
上式为线性方程，其中uρ为替换输入，dρ为外界干扰，后续需利用PID控制律对其进行设置。外界干扰dρ需要满足
Figure BDA0001395569080000028
条件(即外界干扰的一阶导数在有限时间内趋近于0),通常dρ可设为常值信号(例如
Figure BDA0001395569080000029
)或者dρ＝e-t。
步骤2：根据上述线性状态方程，设置速率估计器与比例-积分-微分控制器。
速率估计器由两部分组成：
1)以期望姿态信号ρd(例如机械臂期望的姿态角信号、位移信号或速度信号)作为输入的三阶有限时间收敛估计器(finite-time convergent estimators，FTC)，此估计器输出期望姿态信号估计值、期望姿态信号速率估计值、期望姿态信号加速度估计值，所述估计器具体形式如下：
Figure BDA00013955690800000210
其中，
Figure BDA00013955690800000211
作为三阶FTC估计器的输出信号，分别是期望姿态信号ρd的估计值、期望姿态速率信号
Figure BDA00013955690800000212
的估计值和期望姿态加速度信号
Figure BDA00013955690800000213
的估计值，在计算时通常设其初始值都为0。
Figure BDA0001395569080000031
代表期望姿态信号估计值的一阶导数，
Figure BDA0001395569080000032
代表期望姿态速率信号估计值的一阶导数，
Figure BDA0001395569080000033
代表期望姿态加速度信号估计值的一阶导数。sgn()表示符号函数。λi和μi＞0(i＝0,1,2)是FTC估计器的设置参数。
Figure BDA0001395569080000034
是基础信号ρd0二阶导数的Lipschitz常数。
2)以姿态信号与其期望信号的误差量作为输入的二阶有限时间收敛估计器，此估计器输出姿态速率信号误差量的估计值，所述估计器具体形式如下：
Figure BDA0001395569080000035
其中，eρ(t)为姿态信号误差量，即eρ(t)＝ρ(t)-ρd(t)，ρ(t)为姿态信号。
eρ作为FTC估计器的输入信号，
Figure BDA0001395569080000036
和
Figure BDA0001395569080000037
作为二阶FTC估计器的输出信号，分别是姿态信号误差量eρ的估计值和姿态速率信号误差量
Figure BDA0001395569080000038
的估计值，在计算时通常设其初始值都为0。
Figure BDA0001395569080000039
表示姿态信号误差量估计值的一阶导数，
Figure BDA00013955690800000310
表示姿态速率信号误差量估计值的一阶导数，λi和μi＞0(i＝0,1)是FTC估计器的设置参数。
Figure BDA00013955690800000311
是基础信号eρ0一阶导数的Lipschitz常数。
设置比例-积分-微分控制器如下式：
Figure BDA00013955690800000312
其中，控制器输入为姿态信号加速度估计值
Figure BDA00013955690800000313
姿态信号误差量eρ(t)、姿态信号速率估计值
Figure BDA00013955690800000314
Figure BDA00013955690800000315
是比例项系数，
Figure BDA00013955690800000316
是微分项系数，
Figure BDA00013955690800000317
是积分项系数，uρ(t)为控制器输出。
步骤3：利用极点配置方法整定PID控制器参数
将PID控制信号uρ(t)引入机械臂线性状态方程(4)中，为了使得符号统一，令(4)中
Figure BDA00013955690800000318
得到闭环误差方程如下式所示：
Figure BDA00013955690800000319
其中，
Figure BDA00013955690800000320
eρ(t)＝ρ(t)-ρd(t)，在有限时间内：
Figure BDA0001395569080000041
所以式(8)在有限时间内可以化成如下方程：
Figure BDA0001395569080000042
为了消除积分符号，对上式两边求导可得如下方程：
Figure BDA0001395569080000043
定义
Figure BDA0001395569080000044
Figure BDA0001395569080000045
则闭环误差方程(11)可写成
Figure BDA0001395569080000046
因为
Figure BDA0001395569080000047
所以
Figure BDA0001395569080000048
闭环系统Aρ阵的特征值多项式为：
Figure BDA0001395569080000049
其中I表示单位矩阵，λ表示特征值；
设置需要配置的极点p0，p1与p2，得到期望特征值多项式如下：
Figure BDA00013955690800000410
比较f(λ)与f*(λ)各对应系数，可解得：
Figure BDA00013955690800000411
将计算出的
Figure BDA00013955690800000412
代入PID控制控制器，得到控制信号。
步骤4：将PID控制器的控制信号转换成相应物理量(例如电压)输入至待控制机械臂的作动器。
替换控制量uρ(t)，即PID控制器的输出信号，转换成相应物理量输入至机械臂作动器。通过观察机械臂姿态控制实际效果，重复步骤3中PID参数整定方法。直至达到控制目的。
综上所述，由于采用了上述技术方案，本发明的有益效果是：
(1)本发明提供的速率估计器可以在有限时间内使得速率信号估计值快速收敛至实际速率信号，其结构简单清晰，便于计算机编程实现。
(2)本发明提供的PID控制器可以将速率信号估计值作为输入，减轻了控制器设置对速率信号测量的依赖性。
(3)本发明提供的PID调参方法简单，便于快速整定控制参数。
(4)根据本发明提供的控制方法简单易用，便于工程实现。
附图说明
图1：本发明姿态控制结构示意图。
图2：三自由度直升机结构示意图。
图3：相同实验条件下，升降角速度估计效果对比图。
图4：相同实验条件下，期望升降角速度估计效果对比图。
图5：极点配置为p0＝-20.9810，p1＝-0.5095+0.6i，p2＝-0.5095-0.6i时的升降角跟踪效果图。
图6：极点配置为p0＝-20.9810，p1＝-0.5095+0.6i，p2＝-0.5095-0.6i时的升降角速率估计值曲线图。
图7：极点配置为p0＝-20.9810，p1＝-0.5095+0.6i，p2＝-0.5095-0.6i时的期望升降角速率估计值曲线图。
图8：极点配置为p0＝-20.9810，p1＝-0.5095+0.6i，p2＝-0.5095-0.6i时的升降角误差值曲线图。
图9：极点配置为p0＝-25，p1＝-2，p2＝-2时的升降角跟踪效果图。
图10：极点配置为p0＝-25，p1＝-2，p2＝-2时的升降角速率估计值曲线图。
图11：极点配置为p0＝-25，p1＝-2，p2＝-2时的期望升降角速率估计值曲线图。
图12：极点配置为p0＝-25，p1＝-2，p2＝-2时的升降角误差值曲线图。
图13：极点配置为p0＝-23，p1＝-1.5，p2＝-1.5时的俯仰角跟踪效果图。
图14：极点配置为p0＝-23，p1＝-1.5，p2＝-1.5时的俯仰角角误差值曲线图。
图中的标号、符号和线条等说明如下：
图1中，ρd为期望姿态信号；ρ为实际姿态信号；eρ为姿态信号误差量；
Figure BDA0001395569080000061
为期望姿态信号速率估计值；
Figure BDA0001395569080000062
为期望姿态信号加速度估计值；
Figure BDA0001395569080000063
为姿态速率信号误差量估计值；uρ为替换输入信号；u为系统输入信号；p0，p1，p2是需要配置的极点；
Figure BDA0001395569080000064
是通过极点配置出来的PID控制参数，f(·)、g(·)、h(·)表示非线性项。
图2中，E点和G点表示前后电机，Fb与Ff表示前后电机产生的升力，
机身EG通过CD与平衡杆BC连接，且机身可以绕杆BC旋转，旋转角度定义为俯仰角(φ)。主杆AG连接在基座G上并垂直于地面，平衡杆BC可以绕主杆旋转，旋转角度定义为航向角(ψ)。同时改变两个旋桨的转速，可以使机体绕升降轴AH旋转，产生升降角(θ)。平衡杆末端有一个配重块，用来平衡机体产生的升降力矩。
图5中，虚线为升降角跟踪信号，实线为期望升降角信号，右上角小图为时间30秒至35秒的数据放大图。
图8中，右下角小图为时间30秒至35秒的误差数据放大图。
图9中，虚线为升降角跟踪信号，实线为期望升降角信号，右上角小图为时间30秒至35秒的数据放大图。
图12中，右下角小图为时间30秒至35秒的误差数据放大图。
图13中，虚线为俯仰角跟踪信号，实线为期望俯仰角信号，右下角小图为时间30秒至35秒的数据放大图。
图14中，右下角小图为时间30秒至35秒的误差数据放大图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面结合实施方式和附图，对本发明作进一步地详细描述。
在实现本发明的无速率测量的机械臂姿态控制方法时，首先，建立机械臂的动力学与运动学模型；然后，进行反馈线性化；其次，根据上述线性模型设置速率估计器与比例-积分-微分控制器(proportional-integral-derivative，PID)，将PID控制器的控制信号转换成相应物理量(例如电压)输入至机械臂作动器。为了获得更好的控制效果，利用极点配置方法对PID控制器参数进行调参，其对应的控制结构示意图如附图1所示。
本具体实施方式采用的机械臂为中国固高科技(深圳)有限公司研制的实时三自由度直升机模型仿真试验平台作为验证试验的具体实施对象。此实验平台可以利用Simulink/Matlab进行积木式搭建控制模型，其主要机械外形如附图2所示。从图中可以看出，实验平台由多个机械连杆组成，可以看成一种简单的机械臂。它拥有3个自由度，分别是绕俯仰轴BC旋转的俯仰角φ(pitch)，绕升降轴AH旋转的升降角θ(elevation)，绕偏航轴AG旋转的航向角ψ(travel)。当前后电机转速同时增加或减小时，会使升降角发生变化，当前后电机差速旋转时，会使俯仰角发生变化，进而带动平衡杆BC，产生偏航力矩，使偏航角发生变化。本发明只考虑模型的升降通道与俯仰通道，将其视为二自由度机械臂，以此模型详细介绍本发明的特点。
在无速率测量的情况下，对图2所示的机械臂的姿态控制步骤如下：
步骤1：建立直升机姿态角动力学模型，并对其进行反馈线性化。
根据机械结构特点，建立机械臂升降通道与俯仰通道动力学模型如下：
Figure BDA0001395569080000071
Figure BDA0001395569080000072
Figure BDA0001395569080000073
其中，θ，φ，ψ分别对应直升机的升降角(elevation)，俯仰角(pitch)和航向角(travel)，单位均为rad；Jyy，Jxx与Jzz分别为绕升降轴、俯仰轴与航向轴的转动惯量，单位均为kg·m2；fθ，fφ与fψ分别为作用在升降通道，俯仰通道与航向通道的不确定项及干扰项之和；m为旋翼动力装置质量，M为整个直升机装置质量，以上单位均为kg；g为重力加速度，单位为m/s2；la为升降轴轴心和航向轴轴心到直升机体中心的距离(对应附图2中的AC),lh为俯仰轴轴心到每个螺旋桨的距离(对应附图2中的DE)，lφ为俯仰轴轴心到升降轴的距离(对应附图2中的CD)，以上单位均为m；Vf与Vb分别为作用于前、后电机的电压，Vd为前后电机的差动电压，Vs为前后电机的合电压，单位均为V；Kf称之为升力系数，单位为N/V。
选取系统变量x＝(θ,φ)T，选取系统输出y＝(θ,φ)T，所以系统非线性方程如下：
y＝x (21)
对非线性系统(21)进行输入-输出反馈线性化，即对系统输出y求二阶导，如下式所示：
Figure BDA0001395569080000081
利用输入变量替换使得式(22)转换成更简单的线性双积分模型：
作如下输入变量替换：
Figure BDA0001395569080000082
及变量替换：
Figure BDA0001395569080000083
通过式(23)和(24)可将式(22)简化为下式：
Figure BDA0001395569080000084
步骤2：根据上述线性状态方程，设置速率估计器与比例-积分-微分控制器。
速率估计器由两部分组成：
1)以期望姿态角信号ρd作为输入的三阶有限时间收敛估计器(finite-timeconvergent estimators，FTC)，此估计器输出期望姿态信号估计值、期望姿态信号速率估计值、期望姿态信号加速度估计值，所述估计器具体形式如下：
Figure BDA0001395569080000091
其中，
Figure BDA0001395569080000092
作为三阶FTC估计器的输出信号，分别是期望姿态角信号ρd的估计值、期望姿态角速率信号
Figure BDA0001395569080000093
的估计值和期望姿态角加速度信号
Figure BDA0001395569080000094
的估计值，在计算时通常设其初始值都为0。
Figure BDA0001395569080000095
代表期望姿态角信号估计值的一阶导数，
Figure BDA0001395569080000096
代表期望姿态角速率信号估计值的一阶导数，
Figure BDA0001395569080000097
代表期望姿态角加速度信号估计值的一阶导数。sgn()表示符号函数。λi和μi＞0(i＝0,1,2)是FTC估计器的设置参数。
Figure BDA0001395569080000098
是基础信号ρd0二阶导数的Lipschitz常数。
2)以姿态信号与其期望信号的误差量作为输入的二阶有限时间收敛估计器，此估计器输出姿态速率信号误差量的估计值，所述估计器具体形式如下：
Figure BDA0001395569080000099
其中，eρ(t)为姿态信号误差量，即eρ(t)＝ρ(t)-ρd(t)，ρ(t)为姿态信号。
eρ作为FTC估计器的输入信号，
Figure BDA00013955690800000910
和
Figure BDA00013955690800000911
作为二阶FTC估计器的输出信号，分别是姿态角信号误差量eρ的估计值和姿态角速率信号误差量
Figure BDA00013955690800000912
的估计值，在计算时通常设其初始值都为0。
Figure BDA00013955690800000913
表示姿态角信号误差量估计值的一阶导数，
Figure BDA00013955690800000914
表示姿态角速率信号误差量估计值的一阶导数，λi和μi＞0(i＝0,1)是FTC估计器的设置参数。
Figure BDA00013955690800000915
是基础信号eρ0一阶导数的Lipschitz常数。
设置比例-积分-微分控制器如下式：
Figure BDA00013955690800000916
其中，控制器输入为姿态角信号加速度估计值
Figure BDA00013955690800000917
姿态角信号误差量eρ(t)、姿态角信号速率估计值
Figure BDA00013955690800000918
Figure BDA00013955690800000919
是比例项系数，
Figure BDA00013955690800000920
是微分项系数，
Figure BDA00013955690800000921
是积分项系数，uρ(t)为控制器输出。
步骤3：利用极点配置方法整定PID控制器参数
将PID控制信号uρ(t)引入机械臂线性状态方程(25)中，为了使得符号统一，令(25)中
Figure BDA0001395569080000101
得到闭环误差方程如下式所示：
Figure BDA0001395569080000102
其中，
Figure BDA0001395569080000103
eρ(t)＝ρ(t)-ρd(t)，在有限时间内：
Figure BDA0001395569080000104
所以式(29)在有限时间内可以化成如下方程
Figure BDA0001395569080000105
为了消除积分符号，对上式两边求导可得如下方程
Figure BDA0001395569080000106
定义
Figure BDA0001395569080000107
Bρ＝[0 0 1]T∈R3×1 (34)
则闭环误差方程(32)可写成
Figure BDA0001395569080000108
因为
Figure BDA0001395569080000109
所以
Figure BDA00013955690800001010
闭环系统Aρ阵的特征值多项式为：
Figure BDA00013955690800001011
设置需要配置的极点p0，p1与p2，得到期望特征值多项式如下：
Figure BDA00013955690800001012
比较f(λ)与f*(λ)各对应系数，可解得：
Figure BDA0001395569080000111
将计算出的
Figure BDA0001395569080000112
代入PID控制控制器。
步骤4：将PID控制器的控制信号转换成相应物理量(例如电压)输入至机械臂作动器。
结合机械臂动力学方程式(18)-(20)，可以通过如下公式计算得到Vf和Vb并输入给模型电机：
Figure BDA0001395569080000113
通过观察机械臂姿态控制实际效果，重复步骤3中PID参数整定方法。直至达到控制目的。
实施例1
为了更好地说明速率估计器的估计效果，在同等试验条件下利用Simulink/Matlab的一阶微分模块分别对升降角与期望升降角微分，算出其角速率，并与速率估计器的估算结果作对比。
选取初始姿态角为[θ,φ]T＝[00，00]T，将期望俯仰角姿态信号设为0°，期望升降角值设为6sin(0.041πt)+10，单位为度。将极点配置为p0＝-23，p1＝-1，p2＝-1，此时升降通道的
Figure BDA0001395569080000114
为47、
Figure BDA0001395569080000115
为25，
Figure BDA0001395569080000116
为23速率估计器的参数设置如下表所示：
Figure BDA0001395569080000117
Figure BDA0001395569080000121
升降角速度估计效果对比图与期望升降角速度估计效果对比图如附图3与附图4所示。
实施例2
为了更好地说明利用极点配置整定PID控制参数方法，本实施例对机械臂系统的升降通道与俯仰通道姿态进行控制试验。选取初始姿态角为[θ,φ]T＝[00，00]T，将期望俯仰角姿态信号设为0°，望值升降角期设为6sin(0.041πt)+10，单位为度。步骤2中速率估计器的参数设置与实施例1一致。
对PID控制器的参数进行初步调试，在步骤4中，首先，将极点配置为p0＝-20.9810，p1＝-0.5095+0.6i，p2＝-0.5095-0.6i，此时升降通道的
Figure BDA0001395569080000122
为22、
Figure BDA0001395569080000123
为22，
Figure BDA0001395569080000124
为13，升降角跟踪图见附图5，升降角速度估计值见附图6，期望升降角速度估计值见附图7，升降角误差信号见附图8。
再将极点配置为p0＝-25，p1＝-2，p2＝-2，此时升降通道的
Figure BDA0001395569080000125
为104、
Figure BDA0001395569080000126
为29，
Figure BDA0001395569080000127
为100，升降角跟踪图见附图9，升降角速度估计值见附图10，期望升降角速度估计值见附图11，升降角误差信号见附图12。
附图5至附图12表明，将极点配置成p0＝-25，p1＝-2，p2＝-2时，机械臂的升降角能快速、准确的跟踪给期望升降角信号，速率估计器能够有效地对姿态角速度进行估计，且此时动态效果与稳定性都优于极点配置为p0＝-20.9810，p1＝-0.5095+0.6i，p2＝-0.5095-0.6i时的状态，试验结果同时验证了将系统误差闭环方程的极点向左偏离虚轴越远，系统的收敛速度越快这一理论。但是此时升降通道的PID控制参数的高增益会导致作动器输入电压饱和，长期以此会缩短作动器的使用寿命，所以应避免将极点设置离虚轴过远。
实施例3
考察俯仰角跟踪非零常值信号，设期望俯仰角姿态信号为5°，期望升降角姿态信号为5°，将极点配置成p0＝-23，p1＝-1.5，p2＝-1.5，此时俯仰通道的
Figure BDA0001395569080000131
为71.25、
Figure BDA0001395569080000132
为26，
Figure BDA0001395569080000133
为51.75，俯仰角跟踪图见附图13，俯仰角误差信号见附图14。
试验例结果表明，本发明所设置的速率估计器能够快速、准确地估计速率信号，所设置的姿态控制方法有良好的稳态性能与动态性能。
以上所述，仅为本发明的具体实施方式，本说明书中所公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换；所公开的所有特征、或所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以任何方式组合。

Claims (1)

1.一种无速率测量的机械臂姿态控制方法，其特征在于，包括下列步骤：
步骤1：基于待控制机械臂的动力学与运动学模型，构造受扰二阶积分器链：
Figure FDA0002491050310000011
其中uρ为系统输入信号u的替换输入，dρ为外界干扰；
待控制机械臂的动力学与运动学模型为：
Figure FDA0002491050310000012
Figure FDA0002491050310000013
Vs＝Vf+Vb；
Vd＝Vf-Vb；
其中，φ、θ、ψ分别表示待控制机械臂的三个自由度，φ、θ、ψ分别对应的旋转轴为第一、二和三轴；
Jyy，Jxx分别为绕第二轴、第一轴的转动惯量；
fθ，fφ分别为作用在第二轴、第一轴对应的通道的不确定项及干扰项之和；
m为动力装置质量，M为整个装置质量，g为重力加速度；
la为第二轴轴心和第三轴轴心分别到待控制机械臂中心的距离；
lh为第一轴轴心到每个推动装置的距离，lφ为第一轴心到第二轴的距离；
Vf与Vb分别为作用于前、后电机的电压，Vd为前后电机的差动电压，Vs为前后电机的合电压；Kf为升力系数；
选取系统变量x＝(θ,φ)T，选取系统输出y＝(θ,φ)T，得到系统非线性方程：y＝x；并对系统输出y求二阶导：
Figure FDA0002491050310000014
利用输入变量替换使得系统输出y的二阶导转换成线性双积分模型：
Figure FDA0002491050310000021
及变量替换：
Figure FDA0002491050310000022
从而得到
Figure FDA0002491050310000023
ρ∈θ,φ；
步骤2：设置速率估计器与比例-积分-微分控制器：
201：设置速率估计器：
以待控制机械臂的期望姿态信号ρd作为输入的三阶有限时间收敛FTC估计器：
Figure FDA0002491050310000024
Figure FDA0002491050310000025
Figure FDA0002491050310000026
其中，
Figure FDA0002491050310000027
分别表示三阶FTC估计器的输出信号，分别是期望姿态信号ρd的估计值、期望姿态速率信号
Figure FDA0002491050310000028
的估计值、期望姿态加速度信号
Figure FDA0002491050310000029
的估计值，且初始值都为预设值；
Figure FDA00024910503100000210
分别表示估计值
Figure FDA00024910503100000211
的一阶导数；sgn()表示符号函数；λi、μi表示三阶FTC估计器的设置参数，且λi、μi均大于0，下标i＝0,1,2；
Figure FDA00024910503100000212
表示基础信号ρd0二阶导数的Lipschitz常数；
以待控制机械臂的姿态信号ρ与其ρd的误差量作为输入的二阶FTC估计器：
Figure FDA00024910503100000213
Figure FDA00024910503100000214
其中，eρ表示姿态信号误差量，即eρ＝ρ-ρd，ρ(t)表示姿态信号，
Figure FDA00024910503100000215
和
Figure FDA00024910503100000216
分别表示二阶FTC估计器的输出信号，分别是姿态信号误差量eρ的估计值和姿态速率信号误差量
Figure FDA00024910503100000217
的估计值，且初始值都为预设值；
Figure FDA00024910503100000218
分别为eρ、
Figure FDA00024910503100000219
的一阶导数；λi、μi表示二阶FTC估计器的设置参数，且λi、μi均大于0，下标i＝0,1；
Figure FDA00024910503100000220
表示基础信号eρ0一阶导数的Lipschitz 常数；
202：设置比例-积分-微分控制器为：
Figure FDA0002491050310000031
所述比例-积分-微分控制器的输入为姿态信号加速度估计值
Figure FDA0002491050310000032
姿态信号误差量eρ(t)和姿态信号速率估计值
Figure FDA0002491050310000033
输出为uρ(t)，其中
Figure FDA0002491050310000034
表示比例项系数，
Figure FDA0002491050310000035
表示微分项系数，
Figure FDA0002491050310000036
表示积分项系数；
步骤3：利用极点配置方法整定比例-积分-微分控制器的参数：
基于步骤1构造的方程
Figure FDA0002491050310000037
得到闭环误差方程：
Figure FDA0002491050310000038
其中，
Figure FDA0002491050310000039
Bρ＝[0 0 1]T∈R3×1；
将Aρ的特征值多项式表示为：
Figure FDA00024910503100000310
其中λ表示特征值；
设置待配置的极点p0，p1与p2，得到期望特征值多项式为：
f*(λ)＝λ3-(p0+p1+p2)λ2+(p0p1+p0p2+p2p1)λ-p0p1p2，
比较f(λ)与f*(λ)各对应系数，得到：
Figure FDA00024910503100000311
Figure FDA00024910503100000312
Figure FDA00024910503100000313
将计算得到的
Figure FDA00024910503100000314
代入比例-积分-微分控制器，得到控制信号；
步骤4：将步骤3得到的控制信号转换成相应物理量输入至待控制机械臂的作动器。
CN201710773971.3A 2017-08-31 2017-08-31 一种无速率测量的机械臂姿态控制方法 Active CN107728470B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710773971.3A CN107728470B (zh) 2017-08-31 2017-08-31 一种无速率测量的机械臂姿态控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710773971.3A CN107728470B (zh) 2017-08-31 2017-08-31 一种无速率测量的机械臂姿态控制方法

Publications (2)

Publication Number Publication Date
CN107728470A CN107728470A (zh) 2018-02-23
CN107728470B true CN107728470B (zh) 2020-07-21

Family

ID=61205658

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710773971.3A Active CN107728470B (zh) 2017-08-31 2017-08-31 一种无速率测量的机械臂姿态控制方法

Country Status (1)

Country Link
CN (1) CN107728470B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110744548B (zh) * 2019-11-08 2021-02-19 山东大学 一种多线驱连续体机械臂驱动线耦合关系的统一解耦方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101339404A (zh) * 2008-08-07 2009-01-07 北京航空航天大学 飞行器姿态动力学简化模型增益切换比例-微分控制的设计方法
CN103732363A (zh) * 2011-08-19 2014-04-16 株式会社安川电机 机器人系统、机器人控制装置、机器人手部及机器人控制方法
CN104035328A (zh) * 2014-06-21 2014-09-10 电子科技大学 一种采用干扰估计器的多运动体跟踪控制方法
CN104260107A (zh) * 2014-09-04 2015-01-07 北京邮电大学 一种实现空间机械臂柔性关节齿隙补偿的方法
CN105171758A (zh) * 2015-10-21 2015-12-23 孟强 一种机器人的自适应有限时间收敛滑模控制方法
CN105182990A (zh) * 2015-09-30 2015-12-23 南京航空航天大学 具有输出受限的三自由度模型直升机的鲁棒控制方法
CN106094510A (zh) * 2016-06-30 2016-11-09 电子科技大学 一种基于干扰补偿器的pid参数调节方法
CN106292297A (zh) * 2016-10-26 2017-01-04 成都市优艾维机器人科技有限公司 基于pid控制器和l1自适应控制器的姿态控制方法
RU2615587C1 (ru) * 2016-03-18 2017-04-05 Общество с ограниченной ответственностью "ЛЕ ТАЛО РОБОТИКС" Способ точной посадки беспилитного летательного аппарата

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101339404A (zh) * 2008-08-07 2009-01-07 北京航空航天大学 飞行器姿态动力学简化模型增益切换比例-微分控制的设计方法
CN103732363A (zh) * 2011-08-19 2014-04-16 株式会社安川电机 机器人系统、机器人控制装置、机器人手部及机器人控制方法
CN104035328A (zh) * 2014-06-21 2014-09-10 电子科技大学 一种采用干扰估计器的多运动体跟踪控制方法
CN104260107A (zh) * 2014-09-04 2015-01-07 北京邮电大学 一种实现空间机械臂柔性关节齿隙补偿的方法
CN105182990A (zh) * 2015-09-30 2015-12-23 南京航空航天大学 具有输出受限的三自由度模型直升机的鲁棒控制方法
CN105171758A (zh) * 2015-10-21 2015-12-23 孟强 一种机器人的自适应有限时间收敛滑模控制方法
RU2615587C1 (ru) * 2016-03-18 2017-04-05 Общество с ограниченной ответственностью "ЛЕ ТАЛО РОБОТИКС" Способ точной посадки беспилитного летательного аппарата
CN106094510A (zh) * 2016-06-30 2016-11-09 电子科技大学 一种基于干扰补偿器的pid参数调节方法
CN106292297A (zh) * 2016-10-26 2017-01-04 成都市优艾维机器人科技有限公司 基于pid控制器和l1自适应控制器的姿态控制方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Continuous Differentiator Based on Adaptive Second-Order Sliding-Mode Control for a 3-DOF Helicopter;Herman Casta等;《IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS》;20160930;第63卷(第9期);第5786-5793页 *
三自由度双旋翼直升机飞行姿态的控制方法研究与实现;鞠彬彬;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20130415(第04期);第1-71页 *

Also Published As

Publication number Publication date
CN107728470A (zh) 2018-02-23

Similar Documents

Publication Publication Date Title
Huang et al. Adaptive tracking control of underactuated quadrotor unmanned aerial vehicles via backstepping
CN112904728B (zh) 一种基于改进型趋近律的机械臂滑模控制轨迹跟踪方法
Diao et al. A nonlinear adaptive control approach for quadrotor UAVs
CN112346470A (zh) 一种基于改进自抗扰控制的四旋翼姿态控制方法
CN111258216B (zh) 一种适用于四旋翼飞行器的滑模重复控制器
De Simone et al. Multibody model of a UAV in presence of wind fields
Figueroa et al. Reinforcement learning for balancing a flying inverted pendulum
CN109885074B (zh) 四旋翼无人机有限时间收敛姿态控制方法
He et al. Extended state observer-based robust backstepping sliding mode control for a small-size helicopter
CN112558621A (zh) 一种基于解耦控制的飞行机械臂系统
CN110333733A (zh) 一种四旋翼飞行器的串级变论域模糊pid姿态控制系统及方法
Cao et al. Eso-based robust and high-precision tracking control for aerial manipulation
CN107608368B (zh) 一种无人机在极端初始状态的快速平衡控制方法
CN116069050A (zh) 一种抗扰动的旋翼无人机吊载系统减摇控制方法及装置
Safaei et al. Lyapunov-based nonlinear controller for quadrotor position and attitude tracking with GA optimization
Akbar et al. Adaptive modified super-twisting control for a quadrotor helicopter with a nonlinear sliding surface
CN107728470B (zh) 一种无速率测量的机械臂姿态控制方法
CN112016162B (zh) 一种四旋翼无人机pid控制器参数优化方法
Gonzalez-Hernandez et al. Altitude control improvement for a Quadrotor UAV using integral action in a sliding-mode controller
Seah et al. Bilinear modelling and attitude control of a quadrotor
CN116679548A (zh) 基于时变观测器的三自由度直升机鲁棒输出反馈控制方法
CN113741502B (zh) 输出约束下带负载四旋翼无人机的神经网络控制方法
Babaei et al. Robust backstepping control of a quadrotor uav using extended kalman bucy filter
Anuar et al. Super-twisting sliding mode control with adaptive gain of quadrotor with rigid manipulator
Liu et al. Large-angle and High-speed Trajectory Tracking Control of a Quadrotor UAV based on Reachability

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
CB03 Change of inventor or designer information

Inventor after: Peng Chen

Inventor after: Rao Xinyuan

Inventor after: Wang Jia

Inventor after: Zhu Bo

Inventor after: Lin Chen

Inventor before: Zhu Bo

Inventor before: Rao Xinyuan

Inventor before: Wang Jia

Inventor before: Peng Chen

Inventor before: Lin Chen

CB03 Change of inventor or designer information
GR01 Patent grant
GR01 Patent grant