CN113917937A - 基于轴角矢量的多旋翼姿态自抗扰控制方法及系统 - Google Patents

基于轴角矢量的多旋翼姿态自抗扰控制方法及系统 Download PDF

Info

Publication number
CN113917937A
CN113917937A CN202111153127.3A CN202111153127A CN113917937A CN 113917937 A CN113917937 A CN 113917937A CN 202111153127 A CN202111153127 A CN 202111153127A CN 113917937 A CN113917937 A CN 113917937A
Authority
CN
China
Prior art keywords
target
attitude
quaternion
angle
angular velocity
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.)
Pending
Application number
CN202111153127.3A
Other languages
English (en)
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.)
Puzhou Technology Shenzhen Co ltd
Original Assignee
Puzhou Technology Shenzhen Co ltd
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 Puzhou Technology Shenzhen Co ltd filed Critical Puzhou Technology Shenzhen Co ltd
Priority to CN202111153127.3A priority Critical patent/CN113917937A/zh
Publication of CN113917937A publication Critical patent/CN113917937A/zh
Pending legal-status Critical Current

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
    • G05D1/0833—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability using limited authority control

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Security & Cryptography (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

本发明涉及一种基于轴角矢量的多旋翼姿态自抗扰控制方法及系统，包括以下步骤：基于预先得到的机体目标姿态角、机体当前姿态角，获取三轴的目标角速度；获取当前机体角速度，估计当前角加速度和扰动量，并基于所述目标角速度向量获取最终的姿态控制量。本发明将姿态运动视为一种空间内基于旋转矢量的旋转运动，通过目标角速度、当前机体角速度，并估计当前角加速度和扰动量，得到最终的姿态控制量，克服了丢失控制精度的问题。同时引入扰动量，从而具有自抗扰功能，以提高姿态的响应速度和控制器的适应性。

Description

基于轴角矢量的多旋翼姿态自抗扰控制方法及系统
技术领域
本发明涉及多旋翼无人机技术领域，尤其涉及基于轴角矢量的多旋翼姿态自抗扰控制方法及系统。
背景技术
目前常用的多旋翼姿态控制方式为串级PID控制，每个控制通道(包括俯仰、横滚、航向)相互独立，每个控制通道包含两个控制环路，分别为外环角度环和内环角速度环，角度环输入目标姿态角度和当前实际姿态角度，通过PID控制器生成目标角速度，将生成的目标角速度和当前实际角速度输入到角速度环PID控制器生成最终的姿态控制量。
PID控制器存在参数的适应性较差，跟踪响应慢和易超调问题，另外多旋翼飞行器的姿态之间是相互耦合的，将各个通道独立来控制会在一定程度上丢失控制精度。
发明内容
本发明所要解决的技术问题在于提供一种基于轴角矢量的多旋翼姿态自抗扰控制方法及系统，以解决丢失控制精度的问题。
本发明通过以下技术手段实现解决上述技术问题的：
一种基于轴角矢量的多旋翼姿态自抗扰控制方法，包括以下步骤：
基于预先得到的机体目标姿态角、机体当前姿态角，获取三轴的目标角速度；
获取当前机体角速度，估计当前角加速度和扰动量，并基于所述目标角速度向量获取最终的姿态控制量。
将姿态运动视为一种空间内基于旋转矢量的旋转运动，通过目标角速度、当前机体角速度，并估计当前角加速度和扰动量，得到最终的姿态控制量，克服了丢失控制精度的问题。同时引入扰动量，从而具有自抗扰功能，以提高姿态的响应速度和控制器的适应性。
作为本发明进一步的方案：所述获取三轴的目标角速度包括：
将机体目标姿态角、机体当前姿态角带入欧拉角转四元数公式可得到目标姿态角的四元数、当前姿态角的四元数。
根据目标姿态角的四元数，获取目标姿态四元数；
基于当前姿态角的四元数、目标姿态四元数，获取姿态误差四元数，并计算得到目标角速度向量。
作为本发明进一步的方案：
所述获取目标姿态四元数包括：
基于目标姿态角的四元数，利用公式（1）计算倾转误差（倾转误差中通常只包含横 滚和俯仰没有航向）四元数向量
Figure DEST_PATH_IMAGE001
；
Figure 507092DEST_PATH_IMAGE002
（1）
其中，
Figure DEST_PATH_IMAGE003
，
Figure 383781DEST_PATH_IMAGE004
表示目标姿态坐标系的Z轴在NED坐标系下的 向量且有
Figure 238867DEST_PATH_IMAGE005
（2）
其中，
Figure 37058DEST_PATH_IMAGE006
表示目标姿态坐标系的Z轴在NED坐标系下的向量且有
Figure DEST_PATH_IMAGE007
（3）
可得到倾转角（包括俯仰和横滚）目标四元数：
Figure 298275DEST_PATH_IMAGE009
（4）
通过公式（5）获取多旋翼绕Z轴的旋转运动目标四元数；
Figure DEST_PATH_IMAGE010
（5）
利用公式（6）对旋转运动目标四元数进行，得到限制后的四元数：
Figure 763892DEST_PATH_IMAGE012
（6）
根据倾转角目标四元数、限制后的四元数，利用公式（7）最终得到目标姿态四元数；
Figure DEST_PATH_IMAGE013
（7）。
作为本发明进一步的方案：
所述获取姿态误差四元数包括：
利用公式（8）获取最终的姿态误差四元数；
Figure 819573DEST_PATH_IMAGE014
（8）。
作为本发明进一步的方案：所述计算得到目标角速度向量包括：
基于姿态误差四元数，利用公式（9）可得到目标角速度向量
Figure 606525DEST_PATH_IMAGE015
，
Figure 874696DEST_PATH_IMAGE016
的计算公式为：
Figure 725977DEST_PATH_IMAGE018
（9）
其中
Figure 686980DEST_PATH_IMAGE019
、
Figure 459764DEST_PATH_IMAGE020
、
Figure 469308DEST_PATH_IMAGE021
为可调参数。
作为本发明进一步的方案：获取最终的姿态控制量的步骤包括：
采用快速跟踪函数fhan得到目标角加速度，计算公式为：
Figure 643937DEST_PATH_IMAGE023
（10）
其中
Figure 510262DEST_PATH_IMAGE024
为计算出的目标角速度，
Figure 68545DEST_PATH_IMAGE025
为计算出的目标角加速度，
Figure 678518DEST_PATH_IMAGE026
为式9计算出 的目标角速度，k为第k个时刻，
Figure 707653DEST_PATH_IMAGE027
为执行周期时间，
Figure 10459DEST_PATH_IMAGE028
（11）
其中，
Figure 492256DEST_PATH_IMAGE029
均为函数输入，
Figure 905920DEST_PATH_IMAGE030
为跟踪步长
Figure 789562DEST_PATH_IMAGE030
越大跟踪速度越快，sign 为符号函数，
Figure 499154DEST_PATH_IMAGE031
，
Figure 468247DEST_PATH_IMAGE033
（12）
其中
Figure 420023DEST_PATH_IMAGE034
，
Figure 423751DEST_PATH_IMAGE035
（13）；
据测量得到的当前机体角速度
Figure 68359DEST_PATH_IMAGE036
，使用状态观测器计算出k时刻到k+1时刻的观 测角加速度和观测扰动量，计算公式为：
Figure 524748DEST_PATH_IMAGE038
（14）
其中
Figure 280214DEST_PATH_IMAGE039
为观测角速度，
Figure 872870DEST_PATH_IMAGE040
为观测角加速度，
Figure 201563DEST_PATH_IMAGE041
为观测扰动量，
Figure 145248DEST_PATH_IMAGE042
为控制器给 出的控制量，
Figure 704405DEST_PATH_IMAGE043
、
Figure 417146DEST_PATH_IMAGE044
、
Figure 137977DEST_PATH_IMAGE045
、b均为可调参数，
Figure 568959DEST_PATH_IMAGE046
为观测角速度和当前角速度 的观测误差；
根据目标角速度、角加速度和当前实际的角速度、角加速度可得到最终控制量
Figure 666228DEST_PATH_IMAGE047
为
Figure 499055DEST_PATH_IMAGE048
（15）
Figure 892252DEST_PATH_IMAGE049
（16）
其中
Figure 810530DEST_PATH_IMAGE050
为目标角速度与观测角速度之间的误差，
Figure 711489DEST_PATH_IMAGE051
为目标 角加速度和观测角加速度之间的误差，c为阻尼系数。
一种基于轴角矢量的多旋翼姿态自抗扰控制系统，包括：
获取模块，用于基于机体目标姿态角、机体当前姿态角，获取三轴的目标角速度；
计算模块，用于获取当前机体角速度，估计当前角加速度和扰动量，并基于所述目标角速度向量获取最终的姿态控制量。
作为本发明进一步的方案：
将机体目标姿态角、机体当前姿态角带入欧拉角转四元数公式可得到目标姿态角的四元数、当前姿态角的四元数。
根据目标姿态角的四元数，获取目标姿态四元数；
基于当前姿态角的四元数、目标姿态四元数，获取姿态误差四元数，并计算得到目标角速度向量。
作为本发明进一步的方案：所述获取目标姿态四元数包括：
基于目标姿态角的四元数，利用公式（1）计算倾转误差（倾转误差中通常只包含横 滚和俯仰没有航向）四元数向量
Figure 133244DEST_PATH_IMAGE052
；
Figure 461457DEST_PATH_IMAGE053
（1）
其中，
Figure 601451DEST_PATH_IMAGE054
，
Figure 306102DEST_PATH_IMAGE055
表示目标姿态坐标系的Z轴在NED坐标系下的 向量且有
Figure 474040DEST_PATH_IMAGE056
（2）
其中，
Figure 973155DEST_PATH_IMAGE057
表示目标姿态坐标系的Z轴在NED坐标系下的向量且有
Figure 600445DEST_PATH_IMAGE058
（3）
可得到倾转角（包括俯仰和横滚）目标四元数：
Figure 843208DEST_PATH_IMAGE059
（4）
通过公式（5）获取多旋翼绕Z轴的旋转运动目标四元数；
Figure 239554DEST_PATH_IMAGE060
（5）
利用公式（6）对旋转运动目标四元数进行，得到限制后的四元数：
Figure 801247DEST_PATH_IMAGE062
（6）
根据倾转角目标四元数、限制后的四元数，利用公式（7）最终得到目标姿态四元数；
Figure 650255DEST_PATH_IMAGE063
（7）
作为本发明进一步的方案：所述获取姿态误差四元数包括：
利用公式（8）获取最终的姿态误差四元数；
Figure 431129DEST_PATH_IMAGE064
（8）。
本发明的优点在于：
1、本发明将姿态运动视为一种空间内基于旋转矢量的旋转运动，通过目标角速度、当前机体角速度，并估计当前角加速度和扰动量，得到最终的姿态控制量，克服了丢失控制精度的问题。
2、本发明通过引入扰动量，从而具有自抗扰功能，以提高姿态的响应速度和控制器的适应性，不仅可以提高飞行器姿态控制器的响应速度和控制精度，还具有较强的适应性。
附图说明
图1为本发明提供的基于轴角矢量的多旋翼姿态自抗扰控制方法的流程方框示意图。
图2位本发明提供的基于轴角矢量的多旋翼姿态自抗扰控制系统的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本公开方案，下面将结合本公开示例性实施例中的附图，对本公开示例性实施例中的技术方案进行清楚、完整地描述。
在本公开的说明书和权利要求书及上述附图中的描述的一些流程中，包含了按照特定顺序出现的多个操作，但是应该清楚了解，这些操作可以不按照其在本文中出现的顺序来执行或并行执行，操作的序号如10、20、等，仅仅是用于区分开各个不同的操作，序号本身不代表任何的执行顺序。另外，这些流程可以包括更多或更少的操作，并且这些操作可以按顺序执行或并行执行。
需要说明的是，本文中的“第一”、“第二”等描述，是用于区分不同的消息、设备、模块等，不代表先后顺序，也不限定“第一”和“第二”是不同的类型。
下面将结合本公开示例性实施例中的附图，对本公开示例性实施例中的技术方案进行清楚、完整地描述，显然，所描述的示例性实施例仅仅是本公开一部分实施例，而不是全部的实施例。基于本公开中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本公开保护的范围。
实施例1
参阅图1，图1为本发明实施例1提供的基于轴角矢量的多旋翼姿态自抗扰控制方法的流程方框示意图，应用于多旋翼无人机，所述多旋翼无人机包括用于飞行的三轴，该方法主要包括以下步骤：
S10、基于预先得到的机体目标姿态角、机体当前姿态角，获取三轴的目标角速度；
此处需要说明的是，多旋翼的目标姿态角为
Figure 947561DEST_PATH_IMAGE065
、
Figure 289943DEST_PATH_IMAGE066
、
Figure 891825DEST_PATH_IMAGE067
；
Figure 273128DEST_PATH_IMAGE068
表示目标姿态角中 的横滚、
Figure 644067DEST_PATH_IMAGE069
表示目标姿态角中的俯仰、
Figure 655885DEST_PATH_IMAGE070
表示目标姿态角中的航向；
机体当前姿态角为
Figure 213905DEST_PATH_IMAGE071
、
Figure 602161DEST_PATH_IMAGE072
、
Figure 329071DEST_PATH_IMAGE073
，
Figure 777370DEST_PATH_IMAGE074
表示机体当前姿态角中的横滚、
Figure 353845DEST_PATH_IMAGE075
表示机体 当前姿态角中的俯仰、
Figure 280213DEST_PATH_IMAGE076
表示机体当前姿态角中的航向。
需要强调的是，所述目标姿态角是通过已有的手段进行获取的，而机体当前姿态角是可以直接测量得到的，所以此处不再对如何获取机体目标姿态角、机体当前姿态角进行详细的描述。
进一步地，步骤S10中，还包括：
S11、将机体目标姿态角、机体当前姿态角带入欧拉角转四元数公式可得到目标姿态角的四元数、当前姿态角的四元数。
S12、根据目标姿态角的四元数，获取目标姿态四元数；
S13、基于当前姿态角的四元数、目标姿态四元数，获取姿态误差四元数，并计算得到目标角速度向量。
在步骤S11中；已知机体目标姿态角（横滚
Figure 360164DEST_PATH_IMAGE077
、俯仰
Figure 979364DEST_PATH_IMAGE078
、航向
Figure 777556DEST_PATH_IMAGE079
）、机体当前姿态 角（横滚
Figure 507615DEST_PATH_IMAGE080
、俯仰
Figure 943537DEST_PATH_IMAGE081
、航向
Figure 733639DEST_PATH_IMAGE082
），根据欧拉角转四元数公式可得到目标姿态角的四元数
Figure 19127DEST_PATH_IMAGE083
和当前姿态角的四元数
Figure 287297DEST_PATH_IMAGE084
。
四元数
Figure 341841DEST_PATH_IMAGE085
与欧拉角
Figure 302843DEST_PATH_IMAGE086
、
Figure 810048DEST_PATH_IMAGE087
、
Figure 881909DEST_PATH_IMAGE088
之间的转换关系为
Figure 558004DEST_PATH_IMAGE090
其中，四元数是姿态角的另一种表示方式，欧拉角表示绕XYZ轴的旋转运动，四元数表示绕三维空间中的旋转矢量的旋转运动。
在步骤S12中，包括：
S121、基于目标姿态角的四元数，利用公式（1）计算倾转误差（倾转误差中通常只 包含横滚和俯仰没有航向）四元数
Figure 424328DEST_PATH_IMAGE091
；
Figure 684408DEST_PATH_IMAGE092
（1）
其中，
Figure 294381DEST_PATH_IMAGE093
，
Figure 323517DEST_PATH_IMAGE094
表示目标姿态坐标系的Z轴在NED坐标系下的 向量且有
Figure 360743DEST_PATH_IMAGE095
（2）
其中，
Figure 108120DEST_PATH_IMAGE096
表示目标姿态坐标系的Z轴在NED坐标系下的向量且有
Figure 521783DEST_PATH_IMAGE097
（3）
可得到倾转角（包括俯仰和横滚）目标四元数：
Figure 608688DEST_PATH_IMAGE098
（4）
S121、通过公式（5）获取多旋翼绕Z轴的旋转运动目标四元数；
Figure 368878DEST_PATH_IMAGE100
（5）
考虑到倾转运动较为省力而旋转运动比较费力而且倾转运动是保证飞机安全飞 行的主要因素，因此需要对旋转运动做一定的限制保证倾转运动优先。所以本申请实施例 方式中，取调节因子
Figure 541234DEST_PATH_IMAGE101
，利用公式（6）对旋转运动目标四元数进行，得到限制后的 四元数：
Figure 493009DEST_PATH_IMAGE103
（6）
根据倾转角目标四元数、限制后的四元数，利用公式（7）最终得到目标姿态四元数；
Figure 496737DEST_PATH_IMAGE104
（7）
S13、根据上述方法得到的目标姿态四元数
Figure 875766DEST_PATH_IMAGE105
和当前姿态角的四元数
Figure 128893DEST_PATH_IMAGE106
，利用 公式（8）获取最终的姿态误差四元数；
Figure 884359DEST_PATH_IMAGE108
（8）
其中，基于姿态误差四元数，利用公式（9）可得到目标角速度向量
Figure 244059DEST_PATH_IMAGE109
，
Figure 59568DEST_PATH_IMAGE110
的计算公式为：
Figure 3253DEST_PATH_IMAGE112
（9）
其中
Figure 296831DEST_PATH_IMAGE113
、
Figure 9572DEST_PATH_IMAGE114
、
Figure 995983DEST_PATH_IMAGE115
为可调参数。
S20、获取当前机体角速度，估计当前角加速度和扰动量，并基于所述目标角速度向量获取最终的姿态控制量。
在进行步骤S20时，需要将目标角速度向量转换为目标角速度向量，该转换步骤为现有技术，不再详细进行描述。
在步骤S20中，包括：
S21、首先采用快速跟踪函数fhan得到目标角加速度，计算公式为：
Figure 630227DEST_PATH_IMAGE117
（10）
其中
Figure 727496DEST_PATH_IMAGE118
为计算出的目标角速度，
Figure 294743DEST_PATH_IMAGE119
为计算出的目标角加速度，
Figure 953520DEST_PATH_IMAGE120
为式9计算 出的目标角速度，k为第k个时刻，
Figure 871797DEST_PATH_IMAGE121
为执行周期时间，
Figure 507178DEST_PATH_IMAGE122
（11）
其中，
Figure 194511DEST_PATH_IMAGE123
均为函数输入，
Figure 522725DEST_PATH_IMAGE124
为跟踪步长
Figure 662719DEST_PATH_IMAGE124
越大跟踪速度越快， sign为符号函数，
Figure 367370DEST_PATH_IMAGE125
，
Figure 909209DEST_PATH_IMAGE127
（12）
其中
Figure 909789DEST_PATH_IMAGE128
，
Figure 802658DEST_PATH_IMAGE129
（13）
S22、根据测量得到的当前机体角速度
Figure 779842DEST_PATH_IMAGE130
，使用状态观测器计算出k时刻到k+1时 刻的观测角加速度和观测扰动量，计算公式为：
Figure 176188DEST_PATH_IMAGE132
（14）
其中
Figure 908521DEST_PATH_IMAGE133
为观测角速度，
Figure 23107DEST_PATH_IMAGE134
为观测角加速度，
Figure 102184DEST_PATH_IMAGE135
为观测扰动量，
Figure 821878DEST_PATH_IMAGE136
为控制器给 出的控制量，
Figure 662795DEST_PATH_IMAGE137
、
Figure 264678DEST_PATH_IMAGE138
、
Figure 849243DEST_PATH_IMAGE139
、b均为可调参数，
Figure 220181DEST_PATH_IMAGE140
为观测角速度和当前角 速度的观测误差。
根据目标角速度、角加速度和当前实际的角速度、角加速度可得到最终控制量
Figure 232000DEST_PATH_IMAGE141
为
Figure 55599DEST_PATH_IMAGE142
（15）
Figure 945320DEST_PATH_IMAGE143
为式14的观测扰动量，b与式14中的b相同，
Figure 170765DEST_PATH_IMAGE144
（16）
其中
Figure 353485DEST_PATH_IMAGE145
为目标角速度与观测角速度之间的误差，
Figure 929960DEST_PATH_IMAGE146
为 目标角加速度和观测角加速度之间的误差，c为阻尼系数可根据实际情况进行调节。
实施例2
参阅图2，本实施例提供一种基于轴角矢量的多旋翼姿态自抗扰控制系统，包括：
获取模块，用于基于机体目标姿态角、机体当前姿态角，获取三轴的目标角速度；还用于：
S11、将机体目标姿态角、机体当前姿态角带入欧拉角转四元数公式可得到目标姿态角的四元数、当前姿态角的四元数。
S12、根据目标姿态角的四元数，获取目标姿态四元数；
S13、基于当前姿态角的四元数、目标姿态四元数，获取姿态误差四元数，并计算得到目标角速度向量。
在步骤S11中；基于机体目标姿态角、机体当前姿态角，根据欧拉角转四元数公式 可得到目标姿态角的四元数
Figure 856327DEST_PATH_IMAGE147
和当前姿态角的四元数
Figure 936279DEST_PATH_IMAGE148
。
在步骤S12中，包括：
S121、利用公式（1）计算倾转误差（倾转误差中通常只包含横滚和俯仰没有航向） 四元数向量
Figure 555479DEST_PATH_IMAGE149
；
Figure 353671DEST_PATH_IMAGE151
（1）
其中，
Figure 319615DEST_PATH_IMAGE152
，
Figure 519652DEST_PATH_IMAGE153
表示目标姿态坐标系的Z轴在NED坐标系 下的向量且有
Figure 309754DEST_PATH_IMAGE155
（2）
其中，
Figure 329662DEST_PATH_IMAGE156
表示目标姿态坐标系的Z轴在NED坐标系下的向量且有
Figure 863412DEST_PATH_IMAGE158
（3）
可得到倾转角（包括俯仰和横滚）目标四元数可表示为
Figure 917955DEST_PATH_IMAGE159
（4）
S121、通过公式（5）获取多旋翼绕Z轴的旋转运动目标四元数；
Figure 613379DEST_PATH_IMAGE161
（5）
考虑到倾转运动较为省力而旋转运动比较费力而且倾转运动是保证飞机安全飞 行的主要因素，因此需要对旋转运动做一定的限制保证倾转运动优先。所以本申请实施例 方式中，取调节因子
Figure 386163DEST_PATH_IMAGE162
，利用公式（6）对旋转运动目标四元数进行，得到限制后的 四元数：
Figure 959489DEST_PATH_IMAGE164
（6）
根据倾转角目标四元数、限制后的四元数，利用公式（7）最终得到目标姿态四元数；
Figure 868539DEST_PATH_IMAGE165
（7）
S13、根据上述方法得到的目标姿态四元数
Figure 443DEST_PATH_IMAGE166
和当前姿态角的四元数
Figure 994944DEST_PATH_IMAGE167
，利用 公式（8）获取最终的姿态误差四元数；
Figure 870496DEST_PATH_IMAGE169
（8）
其中，基于姿态误差四元数，利用公式（9）可得到目标角速度向量
Figure 165211DEST_PATH_IMAGE170
，
Figure 202437DEST_PATH_IMAGE171
的计算公式为：
Figure 949813DEST_PATH_IMAGE173
（9）
其中
Figure 689161DEST_PATH_IMAGE174
、
Figure 572803DEST_PATH_IMAGE175
、
Figure 780931DEST_PATH_IMAGE176
为可调参数。
计算模块，用于获取当前机体角速度，估计当前角加速度和扰动量，并基于所述目标角速度向量获取最终的姿态控制量。
在进行计算模块时，需要将目标角速度向量转换为目标角速度向量，该转换步骤为现有技术，不再详细进行描述。
在计算模块中，包括：
S21、首先采用快速跟踪函数fhan得到目标角加速度，计算公式为：
Figure 750024DEST_PATH_IMAGE178
（10）
其中
Figure 701799DEST_PATH_IMAGE179
为计算出的目标角速度，
Figure 236686DEST_PATH_IMAGE180
为计算出的目标角加速度，
Figure 445075DEST_PATH_IMAGE181
为式9计算 出的目标角速度，k为第k个时刻，
Figure 901465DEST_PATH_IMAGE182
为执行周期时间，
Figure 656931DEST_PATH_IMAGE183
的函数表达式为：
Figure 515166DEST_PATH_IMAGE185
（11）
其中，
Figure 330675DEST_PATH_IMAGE186
均为函数输入，
Figure 8781DEST_PATH_IMAGE187
为跟踪步长
Figure 567938DEST_PATH_IMAGE187
越大跟踪速度越快， sign为符号函数，
Figure 782144DEST_PATH_IMAGE188
，
Figure DEST_PATH_IMAGE190
（12）
其中
Figure DEST_PATH_IMAGE191
，
Figure DEST_PATH_IMAGE192
（13）
S22、根据测量得到的当前机体角速度
Figure DEST_PATH_IMAGE193
，使用状态观测器计算出k时刻到k+1时 刻的观测角加速度和观测扰动量，计算公式为：
Figure DEST_PATH_IMAGE195
（14）
其中
Figure DEST_PATH_IMAGE196
为观测角速度，
Figure DEST_PATH_IMAGE197
为观测角加速度，
Figure DEST_PATH_IMAGE198
为观测扰动量，
Figure DEST_PATH_IMAGE199
为控制器给出 的控制量，
Figure DEST_PATH_IMAGE200
、
Figure DEST_PATH_IMAGE201
、
Figure DEST_PATH_IMAGE202
、b均为可调参数，
Figure DEST_PATH_IMAGE203
为观测角速度和当前角 速度的观测误差。根据目标角速度、角加速度和当前实际的角速度、角加速度可得到最终控 制量
Figure DEST_PATH_IMAGE204
为
Figure DEST_PATH_IMAGE205
（15）
Figure DEST_PATH_IMAGE206
为式14的观测扰动量，b与式14中的b相同，
Figure DEST_PATH_IMAGE207
（16）
其中
Figure DEST_PATH_IMAGE208
为目标角速度与观测角速度之间的误差，
Figure DEST_PATH_IMAGE209
为 目标角加速度和观测角加速度之间的误差，c为阻尼系数可根据实际情况进行调节。
通过上述方法，最终获取得到最终控制量
Figure DEST_PATH_IMAGE210
，即可实现保证控制精度的同时，提高 了适应性。
以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种基于轴角矢量的多旋翼姿态自抗扰控制方法，其特征在于，包括：
基于预先得到的机体目标姿态角、机体当前姿态角，获取三轴的目标角速度；
获取当前机体角速度，估计当前角加速度和扰动量，并基于所述目标角速度向量，获取最终的姿态控制量。
2.根据权利要求1所述的基于轴角矢量的多旋翼姿态自抗扰控制方法，其特征在于，所述获取三轴的目标角速度包括：
将机体目标姿态角、机体当前姿态角带入欧拉角转四元数公式可得到目标姿态角的四元数、当前姿态角的四元数；
根据目标姿态角的四元数，获取目标姿态四元数；
基于当前姿态角的四元数、目标姿态四元数，获取姿态误差四元数，并计算得到目标角速度向量。
3.根据权利要求2所述的基于轴角矢量的多旋翼姿态自抗扰控制方法，其特征在于，
所述获取目标姿态四元数包括：
基于目标姿态角的四元数，利用公式（1）计算倾转误差（倾转误差中通常只包含横滚和 俯仰没有航向）四元数向量
Figure 984092DEST_PATH_IMAGE001
；
Figure 829688DEST_PATH_IMAGE002
（1）
其中，
Figure 734977DEST_PATH_IMAGE003
，
Figure 827567DEST_PATH_IMAGE004
表示目标姿态坐标系的Z轴在NED坐标系下的向量 且有
Figure 877693DEST_PATH_IMAGE005
（2）
其中，
Figure 58139DEST_PATH_IMAGE006
表示目标姿态坐标系的Z轴在NED坐标系下的向量且有
Figure 595431DEST_PATH_IMAGE007
（3）
可得到倾转角（包括俯仰和横滚）目标四元数：
Figure 921239DEST_PATH_IMAGE008
（4）
通过公式（5）获取多旋翼绕Z轴的旋转运动目标四元数；
Figure 973508DEST_PATH_IMAGE009
（5）
利用公式（6）对旋转运动目标四元数进行限制，得到限制后的四元数：
Figure 957645DEST_PATH_IMAGE010
（6）
根据倾转角目标四元数、限制后的四元数，利用公式（7）最终得到目标姿态四元数；
Figure 893984DEST_PATH_IMAGE011
（7）。
4.根据权利要求1所述的基于轴角矢量的多旋翼姿态自抗扰控制方法，其特征在于，
所述获取姿态误差四元数包括：
利用公式（8）获取最终的姿态误差四元数；
Figure 672584DEST_PATH_IMAGE012
（8）。
5.根据权利要求1所述的基于轴角矢量的多旋翼姿态自抗扰控制方法，其特征在于，所述计算得到目标角速度向量包括：
基于姿态误差四元数，利用公式（9）可得到目标角速度向量
Figure 477729DEST_PATH_IMAGE013
，
Figure 999977DEST_PATH_IMAGE014
的 计算公式为：
Figure 557866DEST_PATH_IMAGE015
（9）
其中
Figure 241788DEST_PATH_IMAGE016
、
Figure 737492DEST_PATH_IMAGE017
、
Figure 548584DEST_PATH_IMAGE018
为可调参数。
6.根据权利要求4所述的基于轴角矢量的多旋翼姿态自抗扰控制方法，其特征在于，获取最终的姿态控制量的步骤包括：
采用快速跟踪函数fhan得到目标角加速度，计算公式为：
Figure 711712DEST_PATH_IMAGE019
（10）
其中
Figure 97694DEST_PATH_IMAGE020
为计算出的目标角速度，
Figure 346273DEST_PATH_IMAGE021
为计算出的目标角加速度，
Figure 459591DEST_PATH_IMAGE022
为式9计算出的目标 角速度，k为第k个时刻，
Figure 742805DEST_PATH_IMAGE023
为执行周期时间，
Figure 34109DEST_PATH_IMAGE024
（11）
其中，
Figure 769984DEST_PATH_IMAGE025
均为函数输入，
Figure 179669DEST_PATH_IMAGE026
为跟踪步长
Figure 317389DEST_PATH_IMAGE027
越大跟踪速度越快，sign为符号 函数，
Figure 779595DEST_PATH_IMAGE028
，
Figure 2765DEST_PATH_IMAGE029
（12）
其中
Figure 457886DEST_PATH_IMAGE030
，
Figure 450113DEST_PATH_IMAGE031
（13）；
据测量得到的当前机体角速度
Figure 614378DEST_PATH_IMAGE032
，使用状态观测器计算出k时刻到k+1时刻的观测角加 速度和观测扰动量，计算公式为：
Figure 324845DEST_PATH_IMAGE033
（14）
其中
Figure 819543DEST_PATH_IMAGE034
为观测角速度，
Figure 931855DEST_PATH_IMAGE035
为观测角加速度，
Figure 735863DEST_PATH_IMAGE036
为观测扰动量，
Figure 933626DEST_PATH_IMAGE037
为控制器给出的 控制量，
Figure 730550DEST_PATH_IMAGE038
、
Figure 697369DEST_PATH_IMAGE039
、
Figure 32798DEST_PATH_IMAGE040
、b均为可调参数，
Figure 827446DEST_PATH_IMAGE041
为观测角速度和当前角速度的观 测误差；
根据目标角速度、角加速度和当前实际的角速度、角加速度可得到最终控制量
Figure 647634DEST_PATH_IMAGE042
为
Figure 187069DEST_PATH_IMAGE043
（15）
Figure 864038DEST_PATH_IMAGE044
（16）
其中
Figure 770814DEST_PATH_IMAGE045
为目标角速度与观测角速度之间的误差，
Figure 660272DEST_PATH_IMAGE046
为目标角加 速度和观测角加速度之间的误差，c为阻尼系数。
7.一种基于权利要求1-6任一所述的基于轴角矢量的多旋翼姿态自抗扰控制方法的系统，其特征在于，包括：
获取模块，用于基于机体目标姿态角、机体当前姿态角，获取三轴的目标角速度；
计算模块，用于获取当前机体角速度，估计当前角加速度和扰动量，并基于所述目标角速度向量获取最终的姿态控制量。
8.根据权利要求7所述的基于轴角矢量的多旋翼姿态自抗扰控制系统，其特征在于，所述获取三轴的目标角速度包括：
将机体目标姿态角、机体当前姿态角带入欧拉角转四元数公式可得到目标姿态角的四元数、当前姿态角的四元数；
根据目标姿态角的四元数，获取目标姿态四元数；
基于当前姿态角的四元数、目标姿态四元数，获取姿态误差四元数，并计算得到目标角速度向量。
9.根据权利要求7所述的基于轴角矢量的多旋翼姿态自抗扰控制系统，其特征在于，
所述获取目标姿态四元数包括：
基于目标姿态角的四元数，利用公式（1）计算倾转误差（倾转误差中通常只包含横滚和 俯仰没有航向）四元数向量
Figure 352416DEST_PATH_IMAGE047
；
Figure 137969DEST_PATH_IMAGE048
（1）
其中，
Figure 532042DEST_PATH_IMAGE049
，
Figure 474459DEST_PATH_IMAGE050
表示目标姿态坐标系的Z轴在NED坐标系下的向量且 有
Figure 4797DEST_PATH_IMAGE051
（2）
其中，
Figure 757990DEST_PATH_IMAGE052
表示目标姿态坐标系的Z轴在NED坐标系下的向量且有
Figure 639358DEST_PATH_IMAGE053
（3）
可得到倾转角（包括俯仰和横滚）目标四元数：
Figure 87263DEST_PATH_IMAGE054
（4）
通过公式（5）获取多旋翼绕Z轴的旋转运动目标四元数；
Figure 190217DEST_PATH_IMAGE055
（5）
利用公式（6）对旋转运动目标四元数进行，得到限制后的四元数：
Figure 848732DEST_PATH_IMAGE056
（6）
根据倾转角目标四元数、限制后的四元数，利用公式（7）最终得到目标姿态四元数；
Figure 217396DEST_PATH_IMAGE057
（7）。
10.根据权利要求7所述的基于轴角矢量的多旋翼姿态自抗扰控制系统，其特征在于，
所述获取姿态误差四元数包括：
利用公式（8）获取最终的姿态误差四元数；
Figure 252348DEST_PATH_IMAGE058
（8）。
CN202111153127.3A 2021-09-29 2021-09-29 基于轴角矢量的多旋翼姿态自抗扰控制方法及系统 Pending CN113917937A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111153127.3A CN113917937A (zh) 2021-09-29 2021-09-29 基于轴角矢量的多旋翼姿态自抗扰控制方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111153127.3A CN113917937A (zh) 2021-09-29 2021-09-29 基于轴角矢量的多旋翼姿态自抗扰控制方法及系统

Publications (1)

Publication Number Publication Date
CN113917937A true CN113917937A (zh) 2022-01-11

Family

ID=79237146

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111153127.3A Pending CN113917937A (zh) 2021-09-29 2021-09-29 基于轴角矢量的多旋翼姿态自抗扰控制方法及系统

Country Status (1)

Country Link
CN (1) CN113917937A (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130317741A1 (en) * 2012-05-23 2013-11-28 Vectornav Technologies, Llc System on a chip inertial navigation system
CN108196563A (zh) * 2018-02-09 2018-06-22 深圳禾苗通信科技有限公司 一种多旋翼无人机自抗扰补偿控制方法及系统
CN109263870A (zh) * 2018-10-11 2019-01-25 浙江大学 基于矢量多旋翼系统的六自由度运动平台及其控制方法
CN109507890A (zh) * 2019-01-09 2019-03-22 中南大学 一种基于eso的无人机动态逆广义预测控制器
CN111522352A (zh) * 2020-05-09 2020-08-11 广东工业大学 多旋翼飞行器单参数自抗扰姿态控制器设计方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130317741A1 (en) * 2012-05-23 2013-11-28 Vectornav Technologies, Llc System on a chip inertial navigation system
CN108196563A (zh) * 2018-02-09 2018-06-22 深圳禾苗通信科技有限公司 一种多旋翼无人机自抗扰补偿控制方法及系统
CN109263870A (zh) * 2018-10-11 2019-01-25 浙江大学 基于矢量多旋翼系统的六自由度运动平台及其控制方法
CN109507890A (zh) * 2019-01-09 2019-03-22 中南大学 一种基于eso的无人机动态逆广义预测控制器
CN111522352A (zh) * 2020-05-09 2020-08-11 广东工业大学 多旋翼飞行器单参数自抗扰姿态控制器设计方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
GUSTAV O¨ MAN LUNDIN 等: "《A fault tolerant attitude estimation architecture with GLR-based disturbance rejection》", 《2019 4TH CONFERENCE ON CONTROL AND FAULT TOLERANT SYSTEMS》, 20 September 2019 (2019-09-20), pages 1 - 7 *
WEI BAI 等: "《Visual Landing System Of UAV Based On ADRC》", 《IEEE》, 31 December 2017 (2017-12-31), pages 1 - 6 *
时来富 等: "《四旋翼飞行器的模糊自抗扰姿态控制》", 《电气与自动化》, 20 June 2021 (2021-06-20), pages 1 - 6 *

Similar Documents

Publication Publication Date Title
CN106249745B (zh) 四轴无人机的控制方法
Kondak et al. Autonomously flying VTOL-robots: Modeling and control
Raza et al. Intelligent flight control of an autonomous quadrotor
CN112346470A (zh) 一种基于改进自抗扰控制的四旋翼姿态控制方法
Derafa et al. Super twisting control algorithm for the four rotors helicopter attitude tracking problem
CN112180960B (zh) 一种面向执行器故障的无人机容错飞行方法及飞行系统
De Plinval et al. Visual servoing for underactuated VTOL UAVs: A linear, homography-based approach
CN107247459B (zh) 抗干扰飞行控制方法及装置
CN111880410B (zh) 一种针对电机故障的四旋翼无人机容错控制方法
CN107063262A (zh) 一种用于无人机姿态解算的互补滤波方法
CN112558621A (zh) 一种基于解耦控制的飞行机械臂系统
CN111459188B (zh) 一种基于四元数的多旋翼非线性飞行控制方法
CN111522352A (zh) 多旋翼飞行器单参数自抗扰姿态控制器设计方法
CN113759706B (zh) 基于轴角矢量的多旋翼姿态自抗扰控制方法及系统
CN113359472A (zh) 一种四旋翼无人机自适应鲁棒轨迹跟踪控制方法
CN112198885A (zh) 一种满足机动平台自主降落需求的无人机控制方法
CN115826394A (zh) 基于分数阶pid与分数阶终端滑模的四旋翼无人机控制方法
Khebbache et al. Robust stabilization of a quadrotor UAV in presence of actuator and sensor faults
CN116643578B (zh) 一种微小型尾座式无人机多模态统一控制方法
CN111580534B (zh) 飞行器控制方法、装置、电子设备及存储介质
Khebbache et al. Robust stabilization of a quadrotor aerial vehicle in presence of actuator faults
CN113917937A (zh) 基于轴角矢量的多旋翼姿态自抗扰控制方法及系统
Song et al. Research on attitude control of quadrotor uav based on active disturbance rejection control
Joukhadar et al. Integral LQR-based 6DOF autonomous quadrocopter balancing system control
CN110928321A (zh) 一种四旋翼无人机姿态的鲁棒控制方法

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