CN110427043B - 基于作业飞行机器人重心偏移的位姿控制器设计方法 - Google Patents

基于作业飞行机器人重心偏移的位姿控制器设计方法 Download PDF

Info

Publication number
CN110427043B
CN110427043B CN201910833440.8A CN201910833440A CN110427043B CN 110427043 B CN110427043 B CN 110427043B CN 201910833440 A CN201910833440 A CN 201910833440A CN 110427043 B CN110427043 B CN 110427043B
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
gravity center
vehicle platform
gravity
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.)
Expired - Fee Related
Application number
CN201910833440.8A
Other languages
English (en)
Other versions
CN110427043A (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.)
Fuzhou University
Original Assignee
Fuzhou University
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 Fuzhou University filed Critical Fuzhou University
Priority to CN201910833440.8A priority Critical patent/CN110427043B/zh
Publication of CN110427043A publication Critical patent/CN110427043A/zh
Application granted granted Critical
Publication of CN110427043B publication Critical patent/CN110427043B/zh
Expired - Fee Related 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/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
    • G05D1/0816—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability

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)

Abstract

本发明涉及一种基于作业飞行机器人重心偏移的位姿控制器设计方法，包括如下步骤：步骤S1：考虑重心偏移，对四旋翼无人机搭载机械臂系统进行建模；步骤S2：通过引入二阶滑模函数，在滑模面上求解出位置控制律，使无人机平台可以按目标轨迹飞行；步骤S3：姿态解耦时考虑重心偏移系统参数，解算出无人机平台按目标轨迹飞行所需的翻滚角、俯仰角和升力；步骤S4：在姿态控制器中考虑重心偏移控制参数，在反演控制器中加入自适应，使控制律自适应重心偏移控制参数，解算出翻滚、俯仰、偏航的输入力矩；步骤S5：通过升力、翻滚力矩、俯仰力矩、偏航力矩解算出四个旋翼的转速。该方法有利于提高无人机的控制精度。

Description

基于作业飞行机器人重心偏移的位姿控制器设计方法
技术领域
本发明涉及无人机技术领域，具体涉及一种基于作业飞行机器人重心偏移的位姿控制器设计方法。
背景技术
无人机实现无人驾驶的方式从遥控驾驶，到机载计算机自主控制。无人机已经是成熟的飞行平台，可以在飞行平台上搭载不同组件扩展飞行平台在不同领域的应用。比如，农业、遥感、物流、探伤甚至清洁方面都有无人机应用的潜能。其中，这些应用不乏需要在无人机平台上搭载机械臂，将二者结合起来就是空中智能机器人，如此高端的设备可使工业获得很大的便利。随着研究员对这块领域的深入，已经有学者实现了无人机搭载机械臂在实际中的应用。设计了一个双臂空中机械手来拧紧阀门；引入冲击装置，能够将扭矩增加到原来的六倍，抓住物体后通过使用飞行机器人绕z轴旋转来执行扭转工作，可以把天花板上的灯泡拧下来、摘取庄稼、高空拧螺丝等；使用一种基于图像的圆柱体检测算法，能自主抓取圆柱形物体；从鹰捕猎汲取灵感，实现高速抓取；多个无人机协作执行运输任务。
上述的这些应用都有一个飞行抓取的动作。而想要凭借指令飞行抓取，就还有一些技术难点需要攻克。稳定抓取，是控制工程对飞行抓取首要解决的问题。由于抓取后的重心发生偏移，若重心偏移量过大将导致悬停的无人机产生水平分量，竖直分量减少而偏离规划好的位置甚至失控。
对于飞行抓取的重心偏移问题，已经有学者引用了多种方法对重心偏移进行补偿。例如，用移动电池盒在短时间补偿偏移的方法；建立一个广义的重心补偿方案，对位置漂移进行补偿；为了减小重心偏移，机械手的几何参数的选择经过优化，使机械臂质量最小化等方法。上述学者大部分从外部补偿的角度去解决问题，对作业型飞行机器人系统会造成额外的设计负担.。因此，本发明对系统进行完善，从系统建模的角度将重心偏移引起的时变参数列为研究对象，将重心偏移的时变参数分为重心偏移系统参数和重心偏移控制参数。在姿态解耦时考虑重心偏移系统参数并设计控制器去自适应重心偏移控制参数,，以此来解决重心偏移的问题.。在此基础上,在姿态控制器设计过程中，通过加入自适应律调整重心偏移控制参数，实现了飞行姿态的高精度轨迹跟踪性能。
发明内容
本发明的目的在于提供一种基于作业飞行机器人重心偏移的位姿控制器设计方法，该方法有利于提高无人机的控制精度。
为实现上述目的，本发明采用如下技术方案：一种基于作业飞行机器人重心偏移的位姿控制器设计方法，包括如下步骤：
步骤S1：考虑重心偏移，对搭载机械臂的四旋翼无人机系统进行建模；
步骤S2：通过引入二阶滑模函数，在滑模面上求解出位置控制律，使无人机平台可以按目标轨迹dT飞行；
步骤S3：姿态解耦时考虑重心偏移系统参数，解算出无人机平台按目标轨迹dT飞行所需的翻滚角
Figure BDA0002191466800000021
俯仰角θd和升力
Figure BDA0002191466800000022
步骤S4：在姿态控制器中考虑重心偏移控制参数，在反演控制器中加入自适应，使控制律自适应重心偏移控制参数，解算出翻滚、俯仰、偏航的输入力矩
Figure BDA0002191466800000023
步骤S5：通过升力
Figure BDA0002191466800000024
翻滚力矩
Figure BDA0002191466800000025
俯仰力矩
Figure BDA0002191466800000026
偏航力矩
Figure BDA0002191466800000027
解算出四个旋翼的转速ωi，i＝1,2,3,4。
进一步地，利用牛顿-欧拉方程法对搭载机械臂的四旋翼无人机系统进行建模，根据力平衡和力矩平衡得到：
Figure BDA0002191466800000028
其中，F为系统所受的外力，M为系统所受的外力矩，m为系统总质量，r'为重心偏移在无人机平台坐标系内的位置，r0为无人机平台在世界坐标系的位置，B为系统的推动力，Ω为无人机平台在世界坐标系的角速度矢量，I为系统的惯性张量，
Figure BDA0002191466800000029
表示对Ω进行一次微分，
Figure BDA00021914668000000210
表示对r0进行二次微分。
进一步地，所述步骤S2具体包括以下步骤：
步骤S21：输入无人机平台在惯性坐标的目标轨迹dT＝[xT yT zT]T；
步骤S22：利用卡尔曼滤波融合超宽带定位测量无人机平台，获取当前无人机平台位置信息d＝[x y z]T；
步骤S23：定义一个跟踪误差向量δ，实时测量无人机平台与目标轨迹的误差；
δ＝dT-d，δ∈R3 (2)
步骤S24：用一个二阶滑模函数，定义状态空间中的时变曲面s：
Figure BDA0002191466800000039
Figure BDA0002191466800000031
步骤S25：在滑模面上，求解出控制律；令s＝0，有：
Figure BDA0002191466800000032
其中，
Figure BDA0002191466800000033
对于期望dT的跟踪问题等价于当t>0时轨迹必须进入滑模面，一旦进入滑模面，就不会离开，一直待在滑模面上。
进一步地，所述步骤S3具体包括以下步骤：
步骤S31：当机械臂开始作业时，机械臂各连杆的重心在无人机平台坐标系FUAV内的坐标发生改变，r'的位置也发生改变，即造成了重心的偏移；估算无人机平台坐标系内的重心偏移坐标r'＝[x'y'z']T：
Figure BDA0002191466800000034
其中，APj是第j个关节臂的重心在坐标系FUAV内的坐标，
Figure BDA0002191466800000035
是抓取目标物在坐标系FUAV内的坐标，无人机搭载的机械臂有四个关节臂，即有四个连杆，每个连杆的质量为mj，j＝1,2,3,4，每个连杆质心在无人机平台的坐标为APj，抓取目标物的质量为mT，系统总质量为ms；
步骤S32：在系统建模时考虑重心位置发生偏移，从而在姿态解耦时多出了重心偏移系统参数，重心偏移系统参数的估计值为：
Figure BDA0002191466800000036
其中，
Figure BDA0002191466800000037
为无人机平台在世界坐标系内的速度矢量，由
Figure BDA0002191466800000038
积分解得，
Figure BDA0002191466800000041
为速度矢量是位置d的一阶微分，其中u为速度矢量
Figure BDA0002191466800000042
在x轴方向的分量，v为速度矢量
Figure BDA0002191466800000043
在y轴方向的分量，w为速度矢量
Figure BDA0002191466800000044
在z轴方向的分量；通过卡尔曼滤波融合三轴惯性测量传感器实时测得，微分解得Ω＝[p q r]T，其中p为角速度矢量Ω在x轴方向的分量，q为角速度矢量Ω在y轴方向的分量，r为角速度矢量Ω在z轴方向的分量；
步骤S33：结合式(5)、(7)，求解位置追踪时所需要的偏航角ψd、翻滚角
Figure BDA0002191466800000045
俯仰角θd，设定偏航角ψd为已知值，从而输出所需要翻滚角
Figure BDA0002191466800000046
俯仰角θd：
Figure BDA0002191466800000047
其中，g为重力加速度，
Figure BDA0002191466800000048
为重心偏移系统参数的估计值；
步骤S34：结合式(5)、(7)、(8)，求得升力
Figure BDA0002191466800000049
Figure BDA00021914668000000410
进一步地，所述步骤S4具体包括以下步骤：
步骤S41：按式(10)求得无人机平台当前的俯仰角的一次微分
Figure BDA00021914668000000411
和绕y轴俯仰的角速度的一次微分
Figure BDA00021914668000000412
Figure BDA00021914668000000413
其中，Ix、Iy、Iz分别为系统绕无人机平台坐标系的x、y、z轴的转动惯量，c2为重心偏移控制参数；
步骤S42：通过卡尔曼滤波融合三轴惯性测量传感器，获得无人机平台当前的俯仰角θ；
步骤S43：定义跟踪误差e1，实时测量无人机平台与期望俯仰角的误差：
Figure BDA0002191466800000051
步骤S44：定义跟踪误差ε1，实时测量无人机平台与期望俯仰角速度的误差：
ε1＝q-qd (12)
步骤S45：通过卡尔曼滤波融合三轴惯性测量传感器实时测得，微分解得Ω＝[p qr]T；
步骤S46：对期望俯仰角θd求二阶微分得
Figure BDA0002191466800000052
步骤S47：为了使李雅普诺夫函数正定，一阶微分李雅普诺夫半负定，使重心偏移控制参数的估计值
Figure BDA0002191466800000053
为：
Figure BDA0002191466800000054
其中，t为时间；
步骤S48：求得俯仰力矩
Figure BDA0002191466800000055
为：
Figure BDA0002191466800000056
其中，k1、k2为正标量参数；
步骤S49：同理，求得翻滚力矩
Figure BDA0002191466800000057
偏航力矩
Figure BDA0002191466800000058
为：
Figure BDA0002191466800000059
Figure BDA00021914668000000510
同俯仰角的输出一样，k3、k4、k5、k6为正标量参数。
进一步地，所述步骤S5具体包括以下步骤：
步骤S51：四个旋翼的角速度与升力
Figure BDA00021914668000000511
翻滚力矩
Figure BDA00021914668000000512
俯仰力矩
Figure BDA00021914668000000513
偏航力矩
Figure BDA00021914668000000514
的关系为：
Figure BDA0002191466800000061
步骤S52：解算出四个旋翼的的转速ωi，i＝1,2,3,4。
相较于现有技术，本发明具有以下有益效果：本发明不从补偿重心偏移去考虑问题，而在系统建模时考虑重心偏移引起的重心偏移参数，重心偏移参数分为重心偏移系统参数和重心偏移控制参数，求解期望翻滚角、俯仰角和偏航角时考虑重心偏移系统参数，位姿控制器分为两部分设计，一部分为位置控制器，引入二阶滑模函数，在滑模面上求解出控制律，以此来解决位置控制器调参繁琐的问题，另一部分为姿态控制器，引入自适应反演控制器，去自适应重心偏移控制参数，以此来解决重心偏移的问题，从而提高了无人机的控制精度，克服了现有技术中飞行抓取后重心位置发生改变导致控制效果不理想的问题，具有很强的实用性和广阔的应用前景。
附图说明
图1是本发明实施例的流程结构示意图。
图2是本发明实施例的位置控制器中X轴分量的控制效果示意图。
图3是本发明实施例的位置控制器中Y轴分量的控制效果示意图。
图4是本发明实施例的位置控制器中Z轴分量的控制效果示意图。
图5是本发明实施例的姿态控制器中对翻滚角
Figure BDA0002191466800000063
的控制效果示意图。
图6是本发明实施例的姿态控制器中对俯仰角θ的控制效果示意图。
图7是本发明实施例的姿态控制器中对翻滚角ψ的控制效果示意图。
具体实施方式
下面结合附图及实施例对本发明做进一步说明。
本发明提供一种基于作业飞行机器人重心偏移的位姿控制器设计方法，如图1所示，包括如下步骤：
步骤S1：考虑重心偏移，对搭载机械臂的四旋翼无人机系统进行建模。
其中，利用牛顿-欧拉方程法对搭载机械臂的四旋翼无人机系统进行建模，根据力平衡和力矩平衡得到：
Figure BDA0002191466800000062
其中，F为系统所受的外力，M为系统所受的外力矩，m为系统总质量，r'为重心偏移在无人机平台坐标系内的位置，r0为无人机平台在世界坐标系的位置，B为系统的推动力，Ω为无人机平台在世界坐标系的角速度矢量，I为系统的惯性张量，
Figure BDA0002191466800000071
表示对Ω进行一次微分，
Figure BDA0002191466800000072
表示对r0进行二次微分。
步骤S2：通过引入二阶滑模函数，在滑模面上求解出位置控制律，使无人机平台可以按目标轨迹dT飞行。具体包括以下步骤：
步骤S21：输入无人机平台在惯性坐标的目标轨迹dT＝[xT yT zT]T；
步骤S22：利用卡尔曼滤波融合超宽带定位测量无人机平台，获取当前无人机平台位置信息d＝[x y z]T；
步骤S23：定义一个跟踪误差向量δ，实时测量无人机平台与目标轨迹的误差；
δ＝dT-d，δ∈R3 (2)
步骤S24：用一个二阶滑模函数，定义状态空间中的时变曲面s：
Figure BDA0002191466800000073
Figure BDA0002191466800000074
步骤S25：在滑模面上，求解出控制律；令s＝0，有：
Figure BDA0002191466800000075
其中，
Figure BDA0002191466800000076
对于期望dT的跟踪问题等价于当t>0时轨迹必须进入滑模面，一旦进入滑模面，就不会离开，一直待在滑模面上。
综上，位置控制律如下：
Figure BDA0002191466800000077
Figure BDA0002191466800000078
Figure BDA0002191466800000079
上述控制器的控制效果如图2-图4。
步骤S3：姿态解耦时考虑重心偏移系统参数，解算出无人机平台按目标轨迹dT飞行所需的翻滚角
Figure BDA0002191466800000081
俯仰角θd和升力
Figure BDA0002191466800000082
具体包括以下步骤：
步骤S31：当机械臂开始作业时，机械臂各连杆的重心在无人机平台坐标系FUAV内的坐标发生改变，r'的位置也发生改变，即造成了重心的偏移；估算无人机平台坐标系内的重心偏移坐标r'＝[x'y'z']T：
Figure BDA0002191466800000083
其中，APj是第j个关节臂的重心在坐标系FUAV内的坐标，
Figure BDA0002191466800000084
是抓取目标物在坐标系FUAV内的坐标，无人机搭载的机械臂有四个关节臂，即有四个连杆，每个连杆的质量为mj，j＝1,2,3,4，每个连杆质心在无人机平台的坐标为APj，抓取目标物的质量为mT，系统总质量为ms；
步骤S32：在系统建模时考虑重心位置发生偏移，从而在姿态解耦时多出了重心偏移系统参数，重心偏移系统参数的估计值为：
Figure BDA0002191466800000085
其中，
Figure BDA0002191466800000086
为无人机平台在世界坐标系内的速度矢量，由
Figure BDA0002191466800000087
积分解得，
Figure BDA0002191466800000088
为速度矢量是位置d的一阶微分，其中u为速度矢量
Figure BDA0002191466800000089
在x轴方向的分量，v为速度矢量
Figure BDA00021914668000000810
在y轴方向的分量，w为速度矢量
Figure BDA00021914668000000811
在z轴方向的分量；通过卡尔曼滤波融合三轴惯性测量传感器实时测得，微分解得Ω＝[p q r]T，其中p为角速度矢量Ω在x轴方向的分量，q为角速度矢量Ω在y轴方向的分量，r为角速度矢量Ω在z轴方向的分量；
步骤S33：结合式(5)、(7)，求解位置追踪时所需要的偏航角ψd、翻滚角
Figure BDA00021914668000000812
俯仰角θd，设定偏航角ψd为已知值，从而输出所需要翻滚角
Figure BDA00021914668000000813
俯仰角θd：
Figure BDA0002191466800000091
其中，g为重力加速度，
Figure BDA0002191466800000092
为重心偏移系统参数的估计值；
步骤S34：结合式(5)、(7)、(8)，求得升力
Figure BDA0002191466800000093
Figure BDA0002191466800000094
步骤S4：在姿态控制器中考虑重心偏移控制参数，在反演控制器中加入自适应，使控制律自适应重心偏移控制参数，解算出翻滚、俯仰、偏航的输入力矩
Figure BDA0002191466800000095
步骤4中，在姿态控制器中考虑重心偏移控制参数c1、c2、c3，在反演控制器中加入自适应，使控制率自适应重心偏移控制参数得
Figure BDA0002191466800000096
解算出输出力矩
Figure BDA0002191466800000097
其具体为：
步骤S41：按式(10)求得无人机平台当前的俯仰角的一次微分
Figure BDA0002191466800000098
和绕y轴俯仰的角速度的一次微分
Figure BDA0002191466800000099
Figure BDA00021914668000000910
其中，Ix、Iy、Iz分别为系统绕无人机平台坐标系的x、y、z轴的转动惯量，c2为重心偏移控制参数；
步骤S42：通过卡尔曼滤波融合三轴惯性测量传感器，获得无人机平台当前的俯仰角θ；
步骤S43：定义跟踪误差e1，实时测量无人机平台与期望俯仰角的误差：
Figure BDA00021914668000000911
步骤S44：定义跟踪误差ε1，实时测量无人机平台与期望俯仰角速度的误差：
ε1＝q-qd (12)
步骤S45：通过卡尔曼滤波融合三轴惯性测量传感器实时测得，微分解得Ω＝[p qr]T；
步骤S46：对期望俯仰角θd求二阶微分得
Figure BDA0002191466800000101
步骤S47：设计李雅普诺夫函数：
Figure BDA0002191466800000102
Figure BDA0002191466800000103
分析系统可知，该系统是一个链式的系统。输入力矩
Figure BDA0002191466800000104
控制输出俯仰角速度q，而俯仰角速度q控制俯仰角度θ。期望俯仰角速度qd
Figure BDA0002191466800000105
Figure BDA0002191466800000106
为了满足李雅普诺夫稳定性定理，
Figure BDA0002191466800000107
为重心偏移控制参数的估计值，输入力矩
Figure BDA0002191466800000108
为：
Figure BDA0002191466800000109
k1、k2为正标量参数。
一阶微分新李雅普诺夫函数
Figure BDA00021914668000001010
为：
Figure BDA00021914668000001011
因为重心偏移控制参数c2属于慢变不确定参数，
Figure BDA00021914668000001012
为重心偏移控制参数的估计误差，数学表示为：
Figure BDA00021914668000001013
在李雅普诺夫函数V1的基础上建立新的李雅普诺夫函数V
Figure BDA00021914668000001014
Figure BDA00021914668000001015
为了使李雅普诺夫函数正定，一阶微分李雅普诺夫半负定，使重心偏移控制参数的估计值
Figure BDA0002191466800000111
为：
Figure BDA0002191466800000112
其中，t为时间；
李雅普诺夫函数V正定，而一阶微分李雅普诺夫函数
Figure BDA0002191466800000113
只是半负定，不能证明随着时间的推移误差e1＝0、ε1＝0。引入Barbalat引理作类李雅普诺夫稳定性分析证明控制器的有效性，证明如下：
因为
Figure BDA0002191466800000114
可微且有上界，
Figure BDA0002191466800000115
连续，所以时间t趋于无穷时，
Figure BDA0002191466800000116
解得e1＝0、ε1＝0。随着时间的推移俯仰角度误差e1和俯仰角速度ε1以指数的形式衰减为零。综上，该控制律可以使系统达到渐进稳定，当且仅当k1、k2为正标量参数。
步骤S48：综上，求得俯仰力矩
Figure BDA0002191466800000117
为：
Figure BDA0002191466800000118
其中，k1、k2为正标量参数；
步骤S49：同理，求得翻滚力矩
Figure BDA0002191466800000119
偏航力矩
Figure BDA00021914668000001110
为：
Figure BDA00021914668000001111
Figure BDA00021914668000001112
同俯仰角的输出一样，k3、k4、k5、k6为正标量参数。
步骤S5：通过升力
Figure BDA00021914668000001113
翻滚力矩
Figure BDA00021914668000001114
俯仰力矩
Figure BDA00021914668000001115
偏航力矩
Figure BDA00021914668000001116
解算出四个旋翼的转速ωi，i＝1,2,3,4。具体包括以下步骤：
步骤S51：四个旋翼的角速度与升力
Figure BDA00021914668000001117
翻滚力矩
Figure BDA00021914668000001118
俯仰力矩
Figure BDA00021914668000001119
偏航力矩
Figure BDA00021914668000001120
的关系为：
Figure BDA0002191466800000121
其中，除了角速度外的所有常数项收集到正标量参数C1、C2；
步骤S52：解算出四个旋翼的的转速ωi，i＝1,2,3,4。
上述控制器控制效果如图5-图7。
以下用一个具体的应用实例对本发明的操作进行详细说明，本发明的基于考虑重心偏移参数的系统模型进行位姿控制器设计，主要从位置控制器和姿态控制器两个方面来体现其有效性。具体设置如下：
1)模拟时长100s模拟。为了增加模拟环境的多样性，体现本发明的有效性。在该过程中，给定机械臂各关节一个运动规划：
Figure BDA0002191466800000122
2)抓取目标物体后，给定期望轨迹使无人机平台按期望轨迹飞行。无人机的其实位置为d＝[0 0 0]T，期望轨迹其实位置dT＝[0 6 2]T。在重心偏移的情况下，给定无人的期望轨迹dT如下：
dT＝[3sin(0.02πt)6cos(0.02πt)2sin(0.04πt)+2]T (0≤t≤100) (a2)
3)硬件参数如表1所示：
表1硬件参数
Figure BDA0002191466800000131
4)控制参数如表2所示：
表2控制参数
Figure BDA0002191466800000141
图2-图4是位置控制器的控制效果。可以看出，在30s内就能完美的跟踪目标轨迹。图5-图7是姿态控制器的控制效果。因为对翻滚角和俯仰角控制时的抖振将直接反应到无人机上，所以在控制器设计时需极力避免这种情况。在本发明中，图5图6可以看出翻滚角和俯仰角在没有抖振的情况下20s内平滑的追踪到了期望轨迹。由于在仿真时，人为设定了偏航角为零。并且在实际中，偏航角对无人机平台的位置控制影响极小，希望其一直为零。图7可以看出，偏航角在10s内有很明显的抖振。但超调量小、响应时间的短。仍认为控制器效果好。图2-图7证明了本发明的有效性和优越性。
以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

Claims (2)

1.一种基于作业飞行机器人重心偏移的位姿控制器设计方法，其特征在于，包括如下步骤：
步骤S1：考虑重心偏移，对搭载机械臂的四旋翼无人机系统进行建模；
步骤S2：通过引入二阶滑模函数，在滑模面上求解出位置控制律，使无人机平台可以按目标轨迹dT飞行；
步骤S3：姿态解耦时考虑重心偏移系统参数，解算出无人机平台按目标轨迹dT飞行所需的翻滚角
Figure FDA0003191374110000011
俯仰角θd和升力
Figure FDA0003191374110000012
步骤S4：在姿态控制器中考虑重心偏移控制参数，在反演控制器中加入自适应，使控制律自适应重心偏移控制参数，解算出翻滚、俯仰、偏航的输入力矩
Figure FDA0003191374110000013
步骤S5：通过升力
Figure FDA0003191374110000014
翻滚力矩
Figure FDA0003191374110000015
俯仰力矩
Figure FDA0003191374110000016
偏航力矩
Figure FDA0003191374110000017
解算出四个旋翼的转速ωi，i＝1,2,3,4；
利用牛顿-欧拉方程法对搭载机械臂的四旋翼无人机系统进行建模，根据力平衡和力矩平衡得到：
Figure FDA0003191374110000018
其中，F为系统所受的外力，M为系统所受的外力矩，m为系统总质量，r'为重心偏移在无人机平台坐标系内的位置，r0为无人机平台在世界坐标系的位置，B为系统的推动力，Ω为无人机平台在世界坐标系的角速度矢量，I为系统的惯性张量，
Figure FDA0003191374110000019
表示对Ω进行一次微分，
Figure FDA00031913741100000110
表示对r0进行二次微分；
所述步骤S2具体包括以下步骤：
步骤S21：输入无人机平台在惯性坐标的目标轨迹dT＝[xT yT zT]T；
步骤S22：利用卡尔曼滤波融合超宽带定位测量无人机平台，获取当前无人机平台位置信息d＝[x y z]T；
步骤S23：定义一个跟踪误差向量δ，实时测量无人机平台与目标轨迹的误差；
δ＝dT-d，δ∈R3 (2)
步骤S24：用一个二阶滑模函数，定义状态空间中的时变曲面s：
Figure FDA0003191374110000021
Figure FDA0003191374110000022
步骤S25：在滑模面上，求解出控制律；令s＝0，有：
Figure FDA0003191374110000023
其中，
Figure FDA0003191374110000024
对于期望dT的跟踪问题等价于当t>0时轨迹必须进入滑模面，一旦进入滑模面，就不会离开，一直待在滑模面上；
所述步骤S3具体包括以下步骤：
步骤S31：当机械臂开始作业时，机械臂各连杆的重心在无人机平台坐标系FUAV内的坐标发生改变，r'的位置也发生改变，即造成了重心的偏移；估算无人机平台坐标系内的重心偏移坐标r'＝[x' y' z']T：
Figure FDA0003191374110000025
其中，APj是第j个关节臂的重心在坐标系FUAV内的坐标，
Figure FDA0003191374110000026
是抓取目标物在坐标系FUAV内的坐标，无人机搭载的机械臂有四个关节臂，即有四个连杆，每个连杆的质量为mj，j＝1,2,3,4，每个连杆重心在无人机平台的坐标为APj，抓取目标物的质量为mT，系统总质量为ms；
步骤S32：在系统建模时考虑重心位置发生偏移，从而在姿态解耦时多出了重心偏移系统参数，重心偏移系统参数的估计值为：
Figure FDA0003191374110000027
其中，
Figure FDA0003191374110000028
为无人机平台在世界坐标系内的速度矢量，由
Figure FDA0003191374110000029
积分解得，
Figure FDA00031913741100000210
为速度矢量是位置d的一阶微分，其中u为速度矢量
Figure FDA00031913741100000211
在x轴方向的分量，v为速度矢量
Figure FDA00031913741100000212
在y轴方向的分量，w为速度矢量
Figure FDA00031913741100000213
在z轴方向的分量；通过卡尔曼滤波融合三轴惯性测量传感器实时测得，微分解得Ω＝[p q r]T，其中p为角速度矢量Ω在x轴方向的分量，q为角速度矢量Ω在y轴方向的分量，r为角速度矢量Ω在z轴方向的分量；
步骤S33：结合式(5)、(7)，求解位置追踪时所需要的偏航角ψd、翻滚角
Figure FDA0003191374110000031
俯仰角θd，设定偏航角ψd为已知值，从而输出所需要翻滚角
Figure FDA0003191374110000032
俯仰角θd：
Figure FDA0003191374110000033
其中，g为重力加速度，
Figure FDA0003191374110000034
为重心偏移系统参数的估计值；
步骤S34：结合式(5)、(7)、(8)，求得升力
Figure FDA0003191374110000035
Figure FDA0003191374110000036
所述步骤S4具体包括以下步骤：
步骤S41：按式(10)求得无人机平台当前的俯仰角的一次微分
Figure FDA0003191374110000037
和绕y轴俯仰的角速度的一次微分
Figure FDA0003191374110000038
Figure FDA0003191374110000039
其中，Ix、Iy、Iz分别为系统绕无人机平台坐标系的x、y、z轴的转动惯量，c2为重心偏移控制参数；
步骤S42：通过卡尔曼滤波融合三轴惯性测量传感器，获得无人机平台当前的俯仰角θ；
步骤S43：定义跟踪误差e1，实时测量无人机平台与期望俯仰角的误差：
Figure FDA00031913741100000310
步骤S44：定义跟踪误差ε1，实时测量无人机平台与期望俯仰角速度的误差：
ε1＝q-qd (12)
步骤S45：通过卡尔曼滤波融合三轴惯性测量传感器实时测得，微分解得Ω＝[p q r]T；
步骤S46：对期望俯仰角θd求二阶微分得
Figure FDA0003191374110000041
步骤S47：为了使李雅普诺夫函数正定，一阶微分李雅普诺夫半负定，使重心偏移控制参数的估计值
Figure FDA0003191374110000042
为：
Figure FDA0003191374110000043
其中，t为时间；
步骤S48：求得俯仰力矩
Figure FDA0003191374110000044
为：
Figure FDA0003191374110000045
其中，k1、k2为正标量参数；
步骤S49：同理，求得翻滚力矩
Figure FDA0003191374110000046
偏航力矩
Figure FDA0003191374110000047
为：
Figure FDA0003191374110000048
Figure FDA0003191374110000049
同俯仰角的输出一样，k3、k4、k5、k6为正标量参数。
2.根据权利要求1所述的基于作业飞行机器人重心偏移的位姿控制器设计方法，其特征在于，所述步骤S5具体包括以下步骤：
步骤S51：四个旋翼的角速度与升力
Figure FDA00031913741100000410
翻滚力矩
Figure FDA00031913741100000411
俯仰力矩
Figure FDA00031913741100000412
偏航力矩
Figure FDA00031913741100000413
的关系为：
Figure FDA00031913741100000414
其中，C1为无人机平台外部力系数，C2为无人机平台外部力矩系数；
步骤S52：解算出四个旋翼的的转速ωi，i＝1,2,3,4。
CN201910833440.8A 2019-09-04 2019-09-04 基于作业飞行机器人重心偏移的位姿控制器设计方法 Expired - Fee Related CN110427043B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910833440.8A CN110427043B (zh) 2019-09-04 2019-09-04 基于作业飞行机器人重心偏移的位姿控制器设计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910833440.8A CN110427043B (zh) 2019-09-04 2019-09-04 基于作业飞行机器人重心偏移的位姿控制器设计方法

Publications (2)

Publication Number Publication Date
CN110427043A CN110427043A (zh) 2019-11-08
CN110427043B true CN110427043B (zh) 2021-09-28

Family

ID=68417422

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910833440.8A Expired - Fee Related CN110427043B (zh) 2019-09-04 2019-09-04 基于作业飞行机器人重心偏移的位姿控制器设计方法

Country Status (1)

Country Link
CN (1) CN110427043B (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111185907B (zh) * 2020-01-13 2021-07-13 福州大学 一种作业型飞行机器人抓取后的位姿稳定控制方法
CN111650836B (zh) * 2020-06-18 2021-08-31 福州大学 基于作业飞行机器人动态滑翔抓取物体的控制方法
CN111984024B (zh) * 2020-08-25 2022-08-09 福州大学 基于作业型飞行机器人的扰动和不确定性控制方法
CN112527008B (zh) * 2020-12-21 2023-05-02 福州大学 基于低复杂度规定性能的作业型飞行机器人控制方法
CN113467501B (zh) * 2021-07-20 2023-03-28 福州大学 作业飞行机器人动态滑翔抓取与力位混合控制方法
CN114063626B (zh) * 2021-09-18 2024-01-09 航天时代飞鹏有限公司 基于重心检测的四旋翼货运无人机飞行姿态控制方法
CN114047774B (zh) * 2021-11-11 2023-11-14 中国电子科技集团公司第二十八研究所 一种四旋翼无人机在低空分层空域运行仿真的验证方法
CN114967717B (zh) * 2022-04-30 2024-06-07 清华大学 飞行器的安全控制方法、装置及存储介质
CN116540785B (zh) * 2023-07-05 2023-09-15 北京未来宇航空间科技研究院有限公司 一种飞行器质心的偏移控制方法及飞行器

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9938001B1 (en) * 2015-09-28 2018-04-10 Amazon Technologies, Inc. Unmanned aerial vehicle (UAV) deployment of passive control stabilizers
CN108453738A (zh) * 2018-03-30 2018-08-28 东南大学 一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法
CN108555935A (zh) * 2018-05-23 2018-09-21 东南大学 一种机器人及控制方法
CN108608512A (zh) * 2018-03-08 2018-10-02 南京太司德智能科技有限公司 一种作业臂可动态伸缩的树障清理空中机器人和调节方法
CN109308074A (zh) * 2017-07-28 2019-02-05 深圳禾苗通信科技有限公司 一种无人机重心偏移的补偿方法及系统
CN109343369A (zh) * 2018-11-19 2019-02-15 南京邮电大学 一种基于非线性观测器的四旋翼容错控制器设计方法
RU2682944C1 (ru) * 2018-03-16 2019-03-22 Акционерное Общество "Государственное Машиностроительное Конструкторское Бюро "Радуга" Имени А.Я. Березняка" Способ выведения беспилотного летательного аппарата на высотную траекторию полета
CN109725643A (zh) * 2019-01-08 2019-05-07 南开大学 一种基于主动建模的旋翼飞行器非平衡负载吊运系统的控制方法
CN109795682A (zh) * 2018-12-10 2019-05-24 清华大学 一种高效尾坐式垂直起降固定翼飞行器及其控制方法
CN109895099A (zh) * 2019-03-28 2019-06-18 哈尔滨工业大学（深圳） 一种基于自然特征的飞行机械臂视觉伺服抓取方法
CN109934871A (zh) * 2019-02-18 2019-06-25 武汉大学 一种面向高危环境的智能无人机抓取目标的系统和方法
CN110083062A (zh) * 2019-04-23 2019-08-02 西安工业大学 一种基于速度扰动观测器和Fuzzy-PID的视轴稳定复合控制方法
CN110077586A (zh) * 2019-05-22 2019-08-02 福州大学 一种复合式飞行器及其控制方法
CN110155316A (zh) * 2019-06-09 2019-08-23 西北工业大学 一种变质心控制共轴双螺旋桨垂直起降飞行器及其控制方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1901153A1 (en) * 2006-09-12 2008-03-19 OFFIS e.V. Control system for unmanned 4-rotor-helicopter
US8306674B2 (en) * 2009-10-01 2012-11-06 Raytheon Company System and method for divert and attitude control in flight vehicles
SE539760C2 (en) * 2014-12-23 2017-11-21 Husqvarna Ab Control of downhill movement for a robotic work tool
CN105353762B (zh) * 2015-09-25 2017-12-26 南京航空航天大学 基于双余度姿态传感器的六旋翼无人机的控制方法
CN107656530B (zh) * 2016-07-26 2022-04-19 深圳华清精密科技有限公司 变参数开架式海洋水下机器人的轨迹跟踪控制方法、装置和系统
US10556677B2 (en) * 2017-02-16 2020-02-11 Amazon Technologies, Inc. Maintaining attitude control of unmanned aerial vehicles using pivoting propulsion motors
US10427790B2 (en) * 2017-06-12 2019-10-01 David A. Verkade Adaptive aerial vehicle
CN108375988A (zh) * 2018-05-25 2018-08-07 哈尔滨工业大学 一种带有不平衡负载的四旋翼无人机位姿控制方法

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9938001B1 (en) * 2015-09-28 2018-04-10 Amazon Technologies, Inc. Unmanned aerial vehicle (UAV) deployment of passive control stabilizers
CN109308074A (zh) * 2017-07-28 2019-02-05 深圳禾苗通信科技有限公司 一种无人机重心偏移的补偿方法及系统
CN108608512A (zh) * 2018-03-08 2018-10-02 南京太司德智能科技有限公司 一种作业臂可动态伸缩的树障清理空中机器人和调节方法
RU2682944C1 (ru) * 2018-03-16 2019-03-22 Акционерное Общество "Государственное Машиностроительное Конструкторское Бюро "Радуга" Имени А.Я. Березняка" Способ выведения беспилотного летательного аппарата на высотную траекторию полета
CN108453738A (zh) * 2018-03-30 2018-08-28 东南大学 一种基于Opencv图像处理的四旋翼飞行器空中自主抓取作业的控制方法
CN108555935A (zh) * 2018-05-23 2018-09-21 东南大学 一种机器人及控制方法
CN109343369A (zh) * 2018-11-19 2019-02-15 南京邮电大学 一种基于非线性观测器的四旋翼容错控制器设计方法
CN109795682A (zh) * 2018-12-10 2019-05-24 清华大学 一种高效尾坐式垂直起降固定翼飞行器及其控制方法
CN109725643A (zh) * 2019-01-08 2019-05-07 南开大学 一种基于主动建模的旋翼飞行器非平衡负载吊运系统的控制方法
CN109934871A (zh) * 2019-02-18 2019-06-25 武汉大学 一种面向高危环境的智能无人机抓取目标的系统和方法
CN109895099A (zh) * 2019-03-28 2019-06-18 哈尔滨工业大学（深圳） 一种基于自然特征的飞行机械臂视觉伺服抓取方法
CN110083062A (zh) * 2019-04-23 2019-08-02 西安工业大学 一种基于速度扰动观测器和Fuzzy-PID的视轴稳定复合控制方法
CN110077586A (zh) * 2019-05-22 2019-08-02 福州大学 一种复合式飞行器及其控制方法
CN110155316A (zh) * 2019-06-09 2019-08-23 西北工业大学 一种变质心控制共轴双螺旋桨垂直起降飞行器及其控制方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
3自由度旋翼飞行机械臂系统动力学建模与预测控制方法;宋大雷，等;《机器人》;20150331;第37卷(第2期);152-160页 *
Nonlinear Control of a Quadrotor With Deviated Center of Gravity;Bin Xian，等;《Journal of Dynamic Systems, Measurement, and Control》;20170131;第139卷;1-8页 *
Nonlinear Model Predictive Control of a Fully-actuated UAV on SE(3) using Acceleration Characteristics of the Structure;Yuichi Tadokoro，等;《2019 12th Asian Control Conference (ASCC)》;20190612;283-288页 *
Stable Under-Actuated Manipulator Design For Mobile Manipulating Unmanned Aerial Vehicle (MM-UAV);Osama M. Abdul Hafez，等;《2017 7th ICMSAO》;20171231;1-6页 *
Vision-Guided Aerial Manipulation Using a Multirotor with a Robotic Arm;Suseong Kim，等;《IEEE TRANSACTIONS ON MECHATRONICS》;20151231;1-13页 *
一种带有重心调节机构的作业型飞行机器人建模与控制;连杰，等;《机器人》;20190131;第41卷(第1期);1-8页 *

Also Published As

Publication number Publication date
CN110427043A (zh) 2019-11-08

Similar Documents

Publication Publication Date Title
CN110427043B (zh) 基于作业飞行机器人重心偏移的位姿控制器设计方法
CN111185907B (zh) 一种作业型飞行机器人抓取后的位姿稳定控制方法
CN110125936B (zh) 一种空间机器人的地面实验验证系统
Hamel et al. Visual servo trajectory tracking for a four rotor VTOL aerial vehicle
WO2019024303A1 (zh) 一种基于有限时间神经动力学的多旋翼无人飞行器的稳定飞行控制方法
CN112558621A (zh) 一种基于解耦控制的飞行机械臂系统
Meng et al. Vibration suppression control of free-floating space robots with flexible appendages for autonomous target capturing
CN108638068B (zh) 一种携带冗余度机械臂的飞行机器人控制系统设计方法
CN112578805B (zh) 一种旋翼飞行器的姿态控制方法
Zhang et al. Cascaded control for balancing an inverted pendulum on a flying quadrotor
CN111923049B (zh) 基于球面模型的飞行机械臂视觉伺服与多任务控制方法
CN115556111B (zh) 基于变惯性参数建模的飞行机械臂耦合扰动控制方法
CN115480583B (zh) 飞行作业机器人的视觉伺服跟踪与阻抗控制方法
CN113467501B (zh) 作业飞行机器人动态滑翔抓取与力位混合控制方法
CN116069050A (zh) 一种抗扰动的旋翼无人机吊载系统减摇控制方法及装置
CN110673623A (zh) 一种基于双环pd控制算法控制的四旋翼无人机着陆方法
CN113580127B (zh) 一种多旋翼飞行器仿人双机械臂系统及其动态自平衡控制设计方法
CN115826597A (zh) 基于自适应神经网络的旋翼飞行器抗扰控制方法及设备
CN112817328A (zh) 一种四旋翼变绳长吊挂系统的路径规划与减摆控制方法
Panetsos et al. A deep reinforcement learning motion control strategy of a multi-rotor uav for payload transportation with minimum swing
Khosravian et al. Design of an intelligent controller for station keeping, attitude control, and path tracking of a quadrotor using recursive neural networks
Liang et al. Active disturbance rejection attitude control for a bird-like flapping wing micro air vehicle during automatic landing
CN111650836B (zh) 基于作业飞行机器人动态滑翔抓取物体的控制方法
Erasmus et al. Linear quadratic gaussian control of a quadrotor with an unknown suspended payload
CN108279562A (zh) 一种基于滑模pid控制的飞行机械臂

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
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20210928

CF01 Termination of patent right due to non-payment of annual fee