CN108614573B - 六旋翼无人机的自动容错姿态控制方法 - Google Patents

六旋翼无人机的自动容错姿态控制方法 Download PDF

Info

Publication number
CN108614573B
CN108614573B CN201810460050.6A CN201810460050A CN108614573B CN 108614573 B CN108614573 B CN 108614573B CN 201810460050 A CN201810460050 A CN 201810460050A CN 108614573 B CN108614573 B CN 108614573B
Authority
CN
China
Prior art keywords
motor
aerial vehicle
unmanned aerial
angle
rotor unmanned
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
CN201810460050.6A
Other languages
English (en)
Other versions
CN108614573A (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.)
Shanghai Clobotics Technology Co ltd
Original Assignee
Shanghai Clobotics Technology 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 Shanghai Clobotics Technology Co ltd filed Critical Shanghai Clobotics Technology Co ltd
Priority to CN201810460050.6A priority Critical patent/CN108614573B/zh
Publication of CN108614573A publication Critical patent/CN108614573A/zh
Application granted granted Critical
Publication of CN108614573B publication Critical patent/CN108614573B/zh
Active 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
    • 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
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft

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

本发明提供了一种六旋翼无人机的自动容错姿态控制方法，包括如下步骤：采集六旋翼无人机上多个电机的电流和/或电压，根据所述电流和/或电压判断每一所述电机是否存在故障；当判断出一所述电机为故障电机时，关闭另一与所述故障电机相邻且转动方向相反的电机；根据所述故障电机所驱动的桨翼i的序号选择相对应的映射矩阵Ri，并根据所述映射矩阵Ri将输入姿态角转换为控制姿态角,i为大于等于0小于等于6的自然数；将所述控制姿态角输入所述六旋翼无人机的飞行控制模块以控制所述六旋翼无人机飞行。本发明在六旋翼无人机中每一电机的具有倾斜的安装角度时，能够根据电机的故障情况优化动力分配，实现六旋翼无人机的容错控制。

Description

六旋翼无人机的自动容错姿态控制方法
技术领域
本发明涉及多旋翼无人机，具体地，涉及一种六旋翼无人机的自动容错姿态控制方法。
背景技术
标准的六旋翼无人机比四轴旋翼无人机相比，有着更好的稳定性。安装有更多的电机以获得更好的控制效果。六旋翼无人机在一个电机失效的情况下，仍然比四轴旋翼无人机多一个电机。而此时常被错误的认为失效一个电机的飞行器仍然可控。
但是事实上当六旋翼无人机在损失一个电机后是无法严格保证悬停的。而针对将电机具有安装倾角的无人机，在6个螺旋桨正常工作以及1个损坏的情况下，通用无人机的飞控算法并不支持电机有安装倾角的无人机以及坏桨的控制。因此需要设计一种控制算法和架构来实现六旋翼无人机的容错控制。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种六旋翼无人机的自动容错姿态控制方法。本发明针改变了六旋翼无人机电机的安装角度，根据电机的故障情况优化动力分配，实现六旋翼无人机的容错控制。其中六旋翼无人机的位置和姿态控制可以描述如下：遥控器发出俯仰、横滚、偏航角以及油门命令，六旋翼无人机接收到遥控器的命令，在六个电机正常以及一个电机故障的时候可以实现自动容错控制。
根据本发明提供的六旋翼无人机的自动容错姿态控制方法，所述六旋翼无人机的每一电机具有相对于水平面沿一方倾斜安装角α，沿另一方向的倾斜安装角β，包括如下步骤：
步骤S1：采集六旋翼无人机上多个电机的电流和/或电压，根据所述电流和/或电压判断每一所述电机是否存在故障；
步骤S2：当判断出一所述电机为故障电机时，关闭另一与所述故障电机相邻且转动方向相反的电机；
步骤S3：根据所述故障电机所驱动的桨翼i的序号选择相对应的映射矩阵Ri，并根据所述映射矩阵Ri将输入姿态角转换为控制姿态角,i为大于等于0小于等于6的自然数；
步骤S4：将所述控制姿态角输入所述六旋翼无人机的飞行控制模块以控制所述六旋翼无人机飞行。
优选地，还包括如下步骤：
步骤S5：通过传感器采集所述六旋翼无人机的飞行姿态角；
步骤S6：根据所述控制姿态角和所述飞行姿态角之间的偏差值，计算需对所述六旋翼无人机施加的目标推力
Figure BDA0001660666020000021
和目标转矩τr，进而根据所述目标推力
Figure BDA0001660666020000022
和目标转矩τr计算出每一电机的转速。
优选地，当一电机的电流和/或电压存在如下方式时，判定所述电机存在故障：
-所述电流大于电流上限阈值；
-所述电流小于电流下限阈值；
-所述电流变化速率大于电流变化速率阈值；
-所述电压大于电压上限阈值；
-所述电压小于电压下限阈值；
-所述电压变化速率大于电压变化速率阈值。
优选地，在所述步骤S1之前还包括如下步骤：
步骤M1：建立机体坐标系FB，FB＝{OB，(xB,yB,zB)}，其中，OB为机体的中心，zB、xB、yB分别为六旋翼无人机的Z轴、X轴、Y轴；zB表示六旋翼无人机在平衡状态时，垂直于地面的方向，xB表示穿过所述机体的中心沿所述机体的机头延伸的方向，yB表示垂直于zB和xB的方向；
在电机i水平安装时，建立初始电机坐标系，在所述初始电机坐标系中zC为Z轴，表示电机i的轴线方向且垂直于地面的方向，xC为X轴，表示穿过所述电机i的中心沿所述机臂延伸的方向，yC为Y轴，表示垂直于zC和xC形成平面的方向；
步骤M2：对于每一电机i，当该电机i驱动浆翼i顺时针转动时，将该电机i绕xC轴逆时针旋转αi角度以及绕yC轴逆时针旋转βi角度；当该电机i驱动浆翼i逆时针转动时，将该电机i绕xC轴顺时针旋转αi角度以及绕yC轴逆时针旋转βi角度；在本实施例，所述逆时针旋转和所述顺时针旋转的观测位置位于所述六旋翼无人机的外侧。
步骤M3：对于电机i，建立旋转后电机坐标系
Figure BDA0001660666020000023
Figure BDA0001660666020000024
其中，i表示电机的序号，i为大于0小于等于6的自然数，
Figure BDA0001660666020000031
表示电机i的中心，
Figure BDA0001660666020000032
Figure BDA0001660666020000033
为电机i的Z轴、X轴、Y轴；
Figure BDA0001660666020000034
为Z轴，表示电机i的轴线方向，
Figure BDA0001660666020000035
为X轴，表示穿过所述电机i的中心与所述机臂延伸的方向呈βi角度的方向，
Figure BDA0001660666020000036
为Y轴，表示垂直于
Figure BDA0001660666020000037
和
Figure BDA0001660666020000038
的方向；
步骤M4：将每一
Figure BDA0001660666020000039
轴与zB轴之间的夹角通过倾斜安装角α、β来表示，由于第i桨翼的zpi轴与该翼的旋转轴的轴向同方向，第i桨翼的zpi轴与机体坐标系zB轴之间的旋转关系为，
zpi＝Ry(βi)Rx(αi)zB
其中，Ry(βi)表示yB方向的旋转矩阵，Rx(αi)表示xB方向的旋转矩，分别表示如下：
Figure BDA00016606660200000310
Figure BDA00016606660200000311
当zB＝[0 0 1]的时候，能够得到：
Figure BDA00016606660200000312
矩阵的第三个元素；
步骤M5：第i桨翼会产生沿zpi方向的推力fi和转矩τi，在
Figure BDA00016606660200000313
坐标系中可以表示为，
Figure BDA00016606660200000314
Figure BDA00016606660200000315
其中，kfi、kτi为与桨翼物理特性相关的常系数，kfi为推力与转速的关系参数，kτi为力矩与转速的关系参数，ωi为每个桨翼的转速，+表示该桨翼的旋转方向为顺时针，-表示该桨翼的旋转方向为逆时针。
六旋翼无人机的所有能够转动的螺旋桨所产生的总推力
Figure BDA00016606660200000316
和总扭矩τc在地面坐标系
Figure BDA00016606660200000317
下可以表示为
Figure BDA00016606660200000318
Figure BDA00016606660200000319
其中，pi表示原点
Figure BDA00016606660200000320
在机体坐标系FB中的位置，u＝[ω1|ω1|,ω2|ω2|…,ω6|ω6|]，F1为控制输入u到施加到无人机的推力的转换矩阵，F2为控制输入u到施加到无人机的转矩的转换矩阵，n为能够转动的电机的总数，n小于等于i，R为地面坐标系与旋转后电机坐标系
Figure BDA00016606660200000321
之间的转换矩阵,
Figure BDA00016606660200000322
Figure BDA00016606660200000323
Figure BDA00016606660200000324
表示3×n的有理数矩阵；
根据牛顿欧拉公式，带有倾角的六旋翼无人机的动力学模型如下，
Figure BDA0001660666020000041
其中，六旋翼无人机的质量为m，g为重力加速度，ω为六旋翼无人机的姿态角，
Figure BDA0001660666020000042
为六旋翼无人机的角速度，J为转动惯量，
Figure BDA0001660666020000043
为加速度，e3为地面坐标系Z轴(0，0，1)。
优选地，所述Ri包括R0、R1、R2、R3、R4、R5以及R6；
当不存在电机故障时，采用映射矩阵R0，
Figure BDA0001660666020000044
所述六旋翼无人机包括第一号电机、第二号电机、第三号电机、第四号电机、第五号电机以及第六号电机；
当第一号电机、第二号电机故障时，采用映射矩阵R1、R2，
Figure BDA0001660666020000045
Figure BDA0001660666020000046
当第三号电机、第四号电机故障时，采用映射矩阵R3、R4，
Figure BDA0001660666020000047
Figure BDA0001660666020000048
当第五号电机、第六号电机故障时，采用映射矩阵R5、R6，
Figure BDA0001660666020000049
Figure BDA00016606660200000410
优选地，根据所述映射矩阵Ri将输入姿态角转换为控制姿态角，具体为：
[Φ θ ψ]control＝Ri[Φ θ ψ]pilot
[Φ θ ψ]pilot为输入姿态角，[Φ θ ψ]control为控制姿态角，Φ表示横滚角，θ表示俯仰角，ψ表示偏航角。
优选地，所述步骤S5包括如下步骤：
步骤S501：求解稳态解，具体为，当六旋翼无人机在悬停姿态的时候满足下式：
Figure BDA00016606660200000411
由F2u＝0可知，稳态解u属于转换矩阵F2的核空间，能够确定u的解缩小为一条直线，根据RF1u＝mge3能够确定u唯一的解；
步骤S502：使得总扭矩τc尽量接近目标扭矩τr，总推力
Figure BDA00016606660200000412
尽量接近目标推力
Figure BDA00016606660200000413
通过如下优化目标求解动力分配，
Figure BDA0001660666020000051
进而通过合并简化可以得到，
Figure BDA0001660666020000052
其中，c为标量系数。
优选地，所述电机与带倾角的基座相连接，所述基座固定在六旋翼无人机电机的水平机架上。
与现有技术相比，本发明具有如下的有益效果：
本发明在六旋翼无人机中每一电机的具有倾斜的安装角度时，能够根据电机的故障情况优化动力分配，实现六旋翼无人机的容错控制，即在六个电机正常以及一个电机故障的时候可以实现自动容错控制。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明中六旋翼无人机的模块示意图；
图2为本发明中六旋翼无人机的自动容错姿态控制方法的步骤流程图；
图3为本发明中输入姿态角的映射算法的集成流程图；
图4为本发明中动力分配模块的集成流程图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。
在本实施例中，本发明针对六旋翼无人机安全性的缺陷和不足，改变了六旋翼无人机电机的安装角度，根据电机的特点优化动力分配，实现六旋翼无人机的容错控制。其中六旋翼无人机的位置和姿态控制可以描述如下：遥控器发出俯仰、横滚、偏航角以及油门命令，六旋翼无人机接收到遥控器的命令，在六个电机正常以及一个电机故障的时候可以实现自动容错控制。
在本实施例中，当进行故障诊断时，根据监测到的飞行数据以及先验知识来决定当前飞行器状态，并将输出结果发送至姿态命令映射模块以及动力分配模块。姿态命令映射模块根据当前飞行器状态以及飞手的控制命令，自动映射成姿态控制的控制姿态角。姿态控制模块通过计算映射之后的控制姿态角与传感器采集到的飞行姿态角之间偏差，决定需要施加的推力以及力矩。动力分配模块通过求解非线性优化问题，使力矩和推力的测量值与设定值的偏差最小，达到六旋翼无人机的自动容错姿态控制。
本发明提供的六旋翼无人机的自动容错姿态控制方法，所述六旋翼无人机的每一电机具有相对于水平面沿一方倾斜安装角α，沿另一方向的倾斜安装角β，包括以下几个步骤：
步骤S1：采集六旋翼无人机上多个电机的电流和/或电压，根据所述电流和/或电压判断每一所述电机是否存在故障；
步骤S2：当判断出一所述电机为故障电机时，关闭另一与所述故障电机相邻且转动方向相反的电机；
步骤S3：根据所述故障电机所驱动的桨翼i的序号选择相对应的映射矩阵Ri，并根据所述映射矩阵Ri将输入姿态角转换为控制姿态角,i为大于等于0小于等于6的自然数；
步骤S4：将所述控制姿态角输入所述六旋翼无人机的飞行控制模块以控制所述六旋翼无人机飞行，进而通过传感器采集所述六旋翼无人机的飞行姿态角；
步骤S5：根据所述控制姿态角和所述飞行姿态角之间的偏差值，计算需对所述六旋翼无人机施加的目标推力
Figure BDA0001660666020000061
和目标转矩τr，进而根据所述目标推力
Figure BDA0001660666020000062
和目标转矩τr计算出每一电机的转速。
在本实施例中，当一电机的电流和/或电压存在如下方式时，判定所述电机存在故障：
-所述电流大于电流上限阈值；
-所述电流小于电流下限阈值；
-所述电流变化速率大于电流变化速率阈值；
-所述电压大于电压上限阈值；
-所述电压小于电压下限阈值；
-所述电压变化速率大于电压变化速率阈值。
在本实施例中，所述电流上限阈值为10A，电流下限阈值为0.5A，电流变化速率阈值为3A/ms，电压上限阈值为12v，电压下限阈值为11v，电压变化速率阈值2v/ms。
在本实施例中，故障诊断模块采集每一电机的电流和电压，当一电机的电流和电压出现如上异常跳动时，判定该电机为故障电机；定位所述故障电机，立即关闭与所述故障电机相对应的配合电机，具体为即1号电机与2号电机向对应，3号电机与4号电机相对于，5号电机与6号电机相对于；根据所述故障电机驱动桨翼i的序号选择相应的映射矩阵Ri，通过映射矩阵Ri矩阵转换将输入姿态角转换为控制姿态角[Φ θ ψ]control输入姿态控制模块。姿态控制模块计算映射之后的控制姿态角与传感器输出飞行姿态角之间的偏差值，并使偏差值最小，即姿态控制模块解算出的控制力矩与实际姿态稳定所需的飞行力矩之差的模加上姿态控制模块输入的控制推力与飞机高度稳定所需的飞行推力之差的模最小。进而动力优化模块根据姿态控制模块的目标控制姿态角施加的最优推力以及力矩，最后得到电机最优转速。
建立六旋翼无人机的动力学模型
在本实施例中，本发明基于空气动力学和旋翼飞行力学理论，建立了带有倾角的六旋翼无人机的动态模型，所述动态模型用于姿态控制的平衡点、动力分配优化算法以及***可控性分析。
步骤M1：建立机体坐标系FB，FB＝{OB，(xB,yB,zB)}，其中，OB为机体的中心，zB、xB、yB分别为六旋翼无人机的Z轴、X轴、Y轴；zB表示六旋翼无人机在平衡状态时，垂直于地面的方向，xB表示穿过所述机体的中心沿所述机体的机头延伸的方向，yB表示垂直于zB和xB的方向；
在电机i水平安装时，建立初始电机坐标系，在所述初始电机坐标系中zC为Z轴，表示电机i的轴线方向且垂直于地面的方向，xC为X轴，表示穿过所述电机i的中心沿所述机臂延伸的方向，yC为Y轴，表示垂直于zC和xC形成平面的方向。
步骤M2：对于每一电机i，当该电机i驱动浆翼i顺时针转动时，将该电机i绕xC轴逆时针旋转αi角度以及绕yC轴逆时针旋转βi角度；当该电机i驱动浆翼i逆时针转动时，将该电机i绕xC轴顺时针旋转αi角度以及绕yC轴逆时针旋转βi角度；在本实施例，所述逆时针旋转和所述顺时针旋转的观测位置位于所述六旋翼无人机的外侧。
步骤M3：对于电机i，建立旋转后电机坐标系
Figure BDA0001660666020000071
Figure BDA0001660666020000072
其中，i表示电机的序号，i为大于0小于等于6的自然数，
Figure BDA00016606660200000712
表示电机i的中心，
Figure BDA0001660666020000073
Figure BDA0001660666020000074
为电机i的Z轴、X轴、Y轴。
Figure BDA0001660666020000075
为Z轴，表示电机i的轴线方向，
Figure BDA0001660666020000076
为X轴，表示穿过所述电机i的中心与所述机臂延伸的方向呈βi角度的方向，
Figure BDA0001660666020000077
为Y轴，表示垂直于
Figure BDA0001660666020000078
和
Figure BDA0001660666020000079
的方向。
步骤M4：将每一
Figure BDA00016606660200000710
轴与zB轴之间的夹角通过安装角α、β来表示，由于第i桨翼的
Figure BDA00016606660200000711
轴与该翼的旋转轴的轴向同方向，第i桨翼的zpi轴与机体坐标系zB轴之间的旋转关系为，
zpi＝Ry(βi)Rx(αi)zB
其中，Ry(βi)表示yB方向的旋转矩阵，Rx(αi)表示xB方向的旋转矩，分别表示如下：
Figure BDA0001660666020000081
Figure BDA0001660666020000082
当zB＝[0 0 1]的时候，能够得到：
Figure BDA0001660666020000083
矩阵的第三个元素；
步骤S105：第i桨翼会产生沿zpi方向的推力fi和转矩τi，在
Figure BDA0001660666020000084
坐标系中可以表示为，
Figure BDA0001660666020000085
Figure BDA0001660666020000086
其中，kfi、kτi为与桨翼物理特性相关的常系数，kfi为推力与转速的关系参数，kτi为力矩与转速的关系参数，ωi为每个桨翼的转速，+表示该桨翼的旋转方向为顺时针，-表示该桨翼的旋转方向为逆时针。fi为推力的数值，fi为推力的向量，τi为转矩的数值，τi为转矩的向量。
六旋翼无人机的所有能够转动的螺旋桨所产生的总推力
Figure BDA0001660666020000087
和总扭矩τc在地面坐标系
Figure BDA0001660666020000088
下可以表示为
Figure BDA0001660666020000089
Figure BDA00016606660200000810
其中，pi表示原点
Figure BDA00016606660200000811
在机体坐标系FB中的位置，u＝[ω1|ω1|,ω2|ω2|…,ω6|ω6|]，F1为控制输入u到施加到无人机的推力的转换矩阵，F2为控制输入u到施加到无人机的转矩的转换矩阵，n小于等于i，R为地面坐标系与旋转后电机坐标系
Figure BDA00016606660200000812
之间的转换矩阵，
Figure BDA00016606660200000813
Figure BDA00016606660200000814
Figure BDA00016606660200000815
表示3×n的有理数矩阵；
根据牛顿欧拉公式，带有倾角的六旋翼无人机的动力学模型如下，
Figure BDA00016606660200000816
其中，六旋翼无人机的质量为m，g为重力加速度，ω为六旋翼无人机的姿态角，
Figure BDA00016606660200000817
为六旋翼无人机的角速度，J为转动惯量，
Figure BDA00016606660200000818
为加速度，e3为地面坐标系Z轴(0，0，1)，R为地面坐标系与旋转后电机坐标系
Figure BDA00016606660200000819
之间的转换矩阵。
带有倾角的六旋翼无人机模型可控性分析
根据六旋翼无人机的动力学模型分析可知，六旋翼无人机的姿态主要由姿态角ω表示，而六旋翼无人机的姿态是由桨翼施加在六旋翼无人机的总推力
Figure BDA0001660666020000091
和总扭矩τc通过转换矩阵F2进行调节。如果六旋翼无人机的姿态完全可控，则需要转换矩阵F2满秩。
针对桨翼水平安装的六旋翼无人机，在6个螺旋桨都正常工作的情况下，转换矩阵F2满秩；当1个电机发生故障的时候，转换矩阵F2的秩降为2，因此六旋翼无人机的姿态不完全可控，该理论分析在实物飞行中也得到验证，六旋翼无人机在正常飞行过程中，突然发生坏桨的时候，六旋翼无人机飞手无法通过正常操作控制六旋翼无人机，最终六旋翼无人机由于姿态和位置无法正常控制而坠毁。
而针对具有安装倾角的六旋翼无人机，在6个螺旋桨正常工作以及1个损坏的情况下，转换矩阵F2均满秩，理论上六旋翼无人机姿态完全可控。但通用的六旋翼无人机飞控算法并不支持有安装倾角以及坏桨的控制。因此需要特殊的控制算法和架构来实现六旋翼无人机的容错控制。
映射矩阵算法
对于有安装倾角的六旋翼无人机，在悬停姿态的时候满足下式，
Figure BDA0001660666020000092
由F2u＝0可知，u属于转换矩阵F2的核空间，可确定u的解缩小为一条直线，根据RF1u＝mge3能够确定u唯一的解。
当不同桨翼损坏的时候，可以得到不同的稳态解u1s,u2s,u3s,u4s,u5s,u6s，u1s为1号电机驱动的桨翼损坏时的稳定解，u2s为2号电机驱动的桨翼损坏时的稳定解，u3s为3号电机驱动的桨翼损坏时的稳定解，u4s为4号电机驱动的桨翼损坏时的稳定解，u5s为5号电机驱动的桨翼损坏时的稳定解，u6s为6号电机驱动的桨翼损坏时的稳定解。在电机没有故障的时候的稳态解为u0s。在正常情况下悬停姿态时，机体坐标系的zB轴与地面坐标系的zw轴重合，此时映射矩阵R0为单位矩阵。
当电机存在故障的时候，根据之前所得的稳态解可知，机体坐标系的zB轴与地面坐标系的zw轴不重合，存在一定的夹角，可求解得到不同坏桨对应的映射矩阵R1,R2,…R6。
[Φ θ ψ]control＝Ri[Φ θ ψ]pilot
飞手通过遥控器给出的输入姿态角为[Φ θ ψ]pilot，Φ表示六旋翼无人机的横滚角，θ表示六旋翼无人机的俯仰角，ψ表示六旋翼无人机的偏航角。通过映射矩阵Ri转换为姿态控制模块的控制姿态角[Φ θ ψ]control。在故障诊断模块检测到不同坏桨的情况时，切换至不同的映射矩阵R0,R1,R2,…R6。
表1六旋翼无人机的参数测量如下
Figure BDA0001660666020000101
可得到各个模式的稳态解如下：
表2六旋翼无人机姿态映射表
Figure BDA0001660666020000102
5.动力最优分配算法
姿态控制算法为常规的PID算法，根据目标姿态和实际姿态的误差，计算所需的转矩τr，动力分配的目标之一为使总扭矩τc尽量接近目标扭矩τr。动力分配的另一个算法是使总推力
Figure BDA0001660666020000103
能够平衡重力，或者满足飞手给定的推力设定值需求，即总推力
Figure BDA0001660666020000104
尽量接近目标推力
Figure BDA0001660666020000105
通过如下优化目标求解动力分配，
Figure BDA0001660666020000106
进而通过合并简化可以得到，
Figure BDA0001660666020000107
其中，c为标量系数，u为稳态解，u的取值范围为u0s，u1s,u2s,u3s,u4s,u5s,u6s。在故障诊断模块检测到不同故障桨翼的情况，切换相应的配置参数，通过该优化算法计算出最优的动力输出，进行姿态的调节。
在本实施例中，所述电机与带倾角的基座相连接，所述基座固定在六旋翼无人机电机的水平机架上，从而实现倾斜安装角α、β。
基于本发明内容，可以有多种实现方式，以下是几种模块实现方式举例：
在开源飞控软件PX4中代码集成
输入姿态角的映射算法的集成流程图如图3所示，该输入姿态角的映射算法集成在位置控制回路中，位置控制回路需要接收飞手的遥控器输入的设定值作为相应的目标设定值，该位置控制回路需要判断是否存在故障的电机，当无电机故障情况下直接使用遥控器的设定值作为位置和姿态设定值，当电机发生故障的情况下需要调度映射矩阵就被激活，然后查找到相应的故障电机对应的映射矩阵计算出新的遥控器位置和输入姿态角。
改进的动力分配模块的集成可以有两种方式，如图4所示，第一种方式为该动力分配模块中需要判断是否发生电机故障，在发生故障的情况下使用新的动力分配优化算法，否则使用原来默认的PX4的动力分配算法。第二种方式为直接修改PX4中动力分配算法，在该PX4中动力分配算法加入故障判断以及是否激活相应动力分配算法的逻辑。两种实施方式都可以实现动力分配算法。
Gazebo仿真
A、按照第一种方式PX4中加入针对不同电机故障的姿态命令映射模块和映射调度模块，在PX4的动力分配模块中加入相应的动力分配优化算法。
B、在gazebo模型中某个六旋翼无人机中修改相应的模型参数，设置相应的电机安装角。
C、通过stil仿真，用游戏摇杆测试算法。
D、通过地面站软件收集数据进行分析算法调参。
Matlab仿真
A、matlab中建立六旋翼无人机的动态模型，包含力的平衡和力矩平衡的模型，加入电机的安装角。
B、matlab中建立PX4对应的无人机的控制器，包括外回路的位置控制器，内回路的姿态控制模块以及混控器。
C、matlab中建立针对不同执行机构故障的姿态命令映射模块以及映射调度模块，在的混控器中加入相应的动力分配优化算法。
D、仿真倾角对控制的影响，分析结果。
无人机实际实施方案
A、PX4中加入针对不同电机故障的姿态命令映射模块以及映射调度模块，在PX4的混控器中加入相应的动力分配优化算法。
B、3D打印电机打印设计出来的电机安装角α、β
C、实际飞行，通过地面站收集数据进行算法更新参数。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。

Claims (7)

1.一种六旋翼无人机的自动容错姿态控制方法，所述六旋翼无人机的每一电机具有相对于水平面沿一方倾斜安装角α，沿另一方向的倾斜安装角β，其特征在于，包括如下步骤：
步骤S1：采集六旋翼无人机上多个电机的电流和/或电压，根据所述电流和/或电压判断每一所述电机是否存在故障；
步骤S2：当判断出一所述电机为故障电机时，关闭另一与所述故障电机相邻且转动方向相反的电机；
步骤S3：根据所述故障电机所驱动的桨翼i的序号选择相对应的映射矩阵Ri，并根据所述映射矩阵Ri将输入姿态角转换为控制姿态角，i为大于等于0小于等于6的自然数；
步骤S4：将所述控制姿态角输入所述六旋翼无人机的飞行控制模块以控制所述六旋翼无人机飞行；
在所述步骤S1之前还包括如下步骤：
步骤M1：建立机体坐标系FB，FB＝{OB，(xB，yB，zB)}，其中，OB为机体的中心，zB、xB、yB分别为六旋翼无人机的Z轴、X轴、Y轴；zB表示六旋翼无人机在平衡状态时，垂直于地面的方向，xB表示穿过所述机体的中心沿所述机体的机头延伸的方向，yB表示垂直于zB和xB的方向；
在电机i水平安装时，建立初始电机坐标系，在所述初始电机坐标系中zC为Z轴，表示电机i的轴线方向且垂直于地面的方向，xC为X轴，表示穿过所述电机i的中心沿机臂延伸的方向，yC为Y轴，表示垂直于zC和xC形成平面的方向；
步骤M2：对于每一电机i，当该电机i驱动浆翼i顺时针转动时，将该电机i绕xC轴逆时针旋转αi角度以及绕yC轴逆时针旋转βi角度；当该电机i驱动浆翼i逆时针转动时，将该电机i绕xC轴顺时针旋转αi角度以及绕yC轴逆时针旋转βi角度；在本实施例，所述逆时针旋转和所述顺时针旋转的观测位置位于所述六旋翼无人机的外侧；
步骤M3：对于电机i，建立旋转后电机坐标系
Figure FDA0003095121450000011
Figure FDA0003095121450000012
其中，i表示电机的序号，i为大于0小于等于6的自然数，
Figure FDA0003095121450000013
表示电机i的中心，
Figure FDA0003095121450000014
Figure FDA0003095121450000015
为电机i的Z轴、X轴、Y轴；
Figure FDA0003095121450000016
为Z轴，表示电机i的轴线方向，
Figure FDA0003095121450000017
为X轴，表示穿过所述电机i的中心与机臂延伸的方向呈βi角度的方向，
Figure FDA0003095121450000018
为Y轴，表示垂直于
Figure FDA0003095121450000019
和
Figure FDA00030951214500000110
的方向；
步骤M4：将每一
Figure FDA0003095121450000021
轴与zB轴之间的夹角通过倾斜安装角α、β来表示，由于第i桨翼的zpi轴与该翼的旋转轴的轴向同方向，第i桨翼的zpi轴与机体坐标系zB轴之间的旋转关系为，
zpi＝Ry(βi)Rx(αi)zB
其中，Ry(βi)表示yB方向的旋转矩阵，Rx(αi)表示xB方向的旋转矩，分别表示如下：
Figure FDA0003095121450000022
Figure FDA0003095121450000023
当zB＝[0 0 1]的时候，能够得到：
Figure FDA0003095121450000024
Figure FDA0003095121450000025
矩阵的第三个元素；
步骤M5：第i桨翼会产生沿zpi方向的推力fi和转矩τi，在
Figure FDA0003095121450000026
些标系中可以表示为，
Figure FDA0003095121450000027
Figure FDA0003095121450000028
其中，kfi、kτi为与桨翼物理特性相关的常系数，kfi为推力与转速的关系参数，kτi为力矩与转速的关系参数，ωi为每个桨翼的转速，+表示该桨翼的旋转方向为顺时针，-表示该桨翼的旋转方向为逆时针；
六旋翼无人机的所有能够转动的螺旋桨所产生的总推力
Figure FDA0003095121450000029
和总扭矩τc在地面坐标系
Figure FDA00030951214500000210
下可以表示为
Figure FDA00030951214500000211
Figure FDA00030951214500000212
其中，pi表示原点
Figure FDA00030951214500000220
在机体坐标系FB中的位置，u＝[ω1|ω1|，ω2|ω2|…，ω6|ω6|]，F1为控制输入u到施加到无人机的推力的转换矩阵，F2为控制输入u到施加到无人机的转矩的转换矩阵，n为能够转动的电机的总数，n小于等于i，R为地面坐标系与旋转后电机坐标系
Figure FDA00030951214500000213
之间的转换矩阵，
Figure FDA00030951214500000214
Figure FDA00030951214500000215
Figure FDA00030951214500000216
表示3×n的有理数矩阵；
根据牛顿欧拉公式，带有倾角的六旋翼无人机的动力学模型如下，
Figure FDA00030951214500000217
其中，六旋翼无人机的质量为m，g为重力加速度，ω为六旋翼无人机的姿态角，
Figure FDA00030951214500000218
为六旋翼无人机的角速度，J为转动惯量，
Figure FDA00030951214500000219
为加速度，e3为地面坐标系Z轴(0，0，1)。
2.根据权利要求1所述的六旋翼无人机的自动容错姿态控制方法，其特征在于，还包括如下步骤：
步骤S5：通过传感器采集所述六旋翼无人机的飞行姿态角；
步骤S6：根据所述控制姿态角和所述飞行姿态角之间的偏差值，计算需对所述六旋翼无人机施加的目标推力
Figure FDA0003095121450000031
和目标转矩τr，进而根据所述目标推力
Figure FDA0003095121450000032
和目标转矩τr计算出每一电机的转速。
3.根据权利要求1所述的六旋翼无人机的自动容错姿态控制方法，其特征在于，当一电机的电流和/或电压存在如下方式时，判定所述电机存在故障：
-所述电流大于电流上限阈值；
-所述电流小于电流下限阈值；
-所述电流变化速率大于电流变化速率阈值；
-所述电压大于电压上限阈值；
-所述电压小于电压下限阈值；
-所述电压变化速率大于电压变化速率阈值。
4.根据权利要求1所述的六旋翼无人机的自动容错姿态控制方法，其特征在于，所述Ri包括R0、R1、R2、R3、R4、R5以及R6；
当不存在电机故障时，采用映射矩阵Rn，
Figure FDA0003095121450000033
所述六旋翼无人机包括第一号电机、第二号电机、第三号电机、第四号电机、第五号电机以及第六号电机；
当第一号电机、第二号电机故障时，采用映射矩阵R1、R2，
Figure FDA0003095121450000034
Figure FDA0003095121450000035
当第三号电机、第四号电机故障时，采用映射矩阵R3、R4，
Figure FDA0003095121450000036
Figure FDA0003095121450000037
当第五号电机、第六号电机故障时，采用映射矩阵R5、R6，
Figure FDA0003095121450000038
Figure FDA0003095121450000039
5.根据权利要求1所述的六旋翼无人机的自动容错姿态控制方法，其特征在于，根据所述映射矩阵Ri将输入姿态角转换为控制姿态角，具体为：
[Ф θ ψ]control＝Ri[Ф θ ψ]pilot
[Ф θ ψ]pilot为输入姿态角，[Ф θ ψ]control为控制姿态角，Φ表示横滚角，θ表示俯仰角，ψ表示偏航角。
6.根据权利要求2所述的六旋翼无人机的自动容错姿态控制方法，其特征在于，所述步骤S5包括如下步骤：
步骤S501：求解稳态解，具体为，当六旋翼无人机在悬停姿态的时候满足下式：
Figure FDA0003095121450000041
由F2u＝0可知，稳态解u属于转换矩阵F2的核空间，能够确定u的解缩小为一条直线，根据RF1u＝mge3能够确定u唯一的解；
步骤S502：使得总扭矩τc尽量接近目标扭矩τr，总推力
Figure FDA0003095121450000042
尽量接近目标推力
Figure FDA0003095121450000043
通过如下优化目标求解动力分配，
Figure FDA0003095121450000044
进而通过合并简化可以得到，
Figure FDA0003095121450000045
其中，c为标量系数。
7.根据权利要求1所述的六旋翼无人机的自动容错姿态控制方法，其特征在于，所述电机与带倾角的基座相连接，所述基座固定在六旋翼无人机电机的水平机架上。
CN201810460050.6A 2018-05-15 2018-05-15 六旋翼无人机的自动容错姿态控制方法 Active CN108614573B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810460050.6A CN108614573B (zh) 2018-05-15 2018-05-15 六旋翼无人机的自动容错姿态控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810460050.6A CN108614573B (zh) 2018-05-15 2018-05-15 六旋翼无人机的自动容错姿态控制方法

Publications (2)

Publication Number Publication Date
CN108614573A CN108614573A (zh) 2018-10-02
CN108614573B true CN108614573B (zh) 2021-08-20

Family

ID=63663342

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810460050.6A Active CN108614573B (zh) 2018-05-15 2018-05-15 六旋翼无人机的自动容错姿态控制方法

Country Status (1)

Country Link
CN (1) CN108614573B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109263979A (zh) * 2018-10-26 2019-01-25 珠海银通无人机科技有限公司 一种双冗余动力飞行器
JPWO2021024591A1 (zh) * 2019-08-02 2021-02-11
CN110888451B (zh) * 2019-12-20 2022-12-06 国网山西省电力公司电力科学研究院 一种多旋翼无人机容错控制方法及***
CN112327896A (zh) * 2020-10-29 2021-02-05 东北大学 旋翼容错控制方法、装置、计算机存储介质及计算机设备
CN112373677B (zh) * 2020-11-18 2021-06-22 三生万物(北京)人工智能技术有限公司 一种六旋翼无人机动力缺失保护***、保护方法
CN116190975B (zh) * 2023-01-13 2023-10-27 中国科学院自动化研究所 用于大口径射电望远镜的容错位姿分配方法及***
CN115857309A (zh) * 2023-02-27 2023-03-28 华东交通大学 一种可倾斜六旋翼无人机的容错控制方法及***
CN116088557B (zh) * 2023-03-31 2023-06-23 湖南大学 一种全驱动型六旋翼无人机位姿控制方法及装置

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101907893A (zh) * 2010-07-02 2010-12-08 北京航空航天大学 基于六自由度并联机构飞机部件调姿装配***及调试方法
CN102902277A (zh) * 2012-10-19 2013-01-30 北京航空航天大学 一种六旋翼飞行器有一个电机停机时的安全着陆方法
CN103135553A (zh) * 2013-01-21 2013-06-05 南京航空航天大学 四旋翼飞行器容错控制方法
CN103365297A (zh) * 2013-06-29 2013-10-23 天津大学 基于光流的四旋翼无人机飞行控制方法
CN104216417A (zh) * 2014-09-22 2014-12-17 北京航空航天大学 一种基于四矢量螺旋桨的平流层飞艇的动态控制分配方法
CN104699108A (zh) * 2013-12-10 2015-06-10 中国航空工业第六一八研究所 一种多旋翼飞行器的控制分配方法
CN105473442A (zh) * 2013-06-09 2016-04-06 瑞士苏黎世联邦理工学院 遭遇影响效应器的故障的多旋翼器的受控飞行
CN106970642A (zh) * 2017-04-20 2017-07-21 佛山科学技术学院 一种多旋翼无人机状态监测装置
CN107077142A (zh) * 2016-10-25 2017-08-18 深圳市大疆创新科技有限公司 多旋翼飞行器及其控制方法、控制装置及飞行控制***

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101907893A (zh) * 2010-07-02 2010-12-08 北京航空航天大学 基于六自由度并联机构飞机部件调姿装配***及调试方法
CN102902277A (zh) * 2012-10-19 2013-01-30 北京航空航天大学 一种六旋翼飞行器有一个电机停机时的安全着陆方法
CN103135553A (zh) * 2013-01-21 2013-06-05 南京航空航天大学 四旋翼飞行器容错控制方法
CN105473442A (zh) * 2013-06-09 2016-04-06 瑞士苏黎世联邦理工学院 遭遇影响效应器的故障的多旋翼器的受控飞行
CN103365297A (zh) * 2013-06-29 2013-10-23 天津大学 基于光流的四旋翼无人机飞行控制方法
CN104699108A (zh) * 2013-12-10 2015-06-10 中国航空工业第六一八研究所 一种多旋翼飞行器的控制分配方法
CN104216417A (zh) * 2014-09-22 2014-12-17 北京航空航天大学 一种基于四矢量螺旋桨的平流层飞艇的动态控制分配方法
CN107077142A (zh) * 2016-10-25 2017-08-18 深圳市大疆创新科技有限公司 多旋翼飞行器及其控制方法、控制装置及飞行控制***
CN106970642A (zh) * 2017-04-20 2017-07-21 佛山科学技术学院 一种多旋翼无人机状态监测装置

Also Published As

Publication number Publication date
CN108614573A (zh) 2018-10-02

Similar Documents

Publication Publication Date Title
CN108614573B (zh) 六旋翼无人机的自动容错姿态控制方法
Oosedo et al. Flight control systems of a quad tilt rotor unmanned aerial vehicle for a large attitude change
US10836467B2 (en) Tilt-rotor multicopters with variable pitch propellers
Bapst et al. Design and implementation of an unmanned tail-sitter
Matouk et al. Quadrotor position and attitude control via backstepping approach
CN108594839B (zh) 基于多矢量技术的控制方法、飞机及存储介质
Riddick An overview of NASA’s learn-to-fly technology development
CN110333733A (zh) 一种四旋翼飞行器的串级变论域模糊pid姿态控制***及方法
CN113039502A (zh) 多旋翼无人机及其控制方法、控制装置和计算机可读存储介质
CN115016268A (zh) 一种基于滑模理论的可倾转四旋翼无人机容错控制方法
Sridhar et al. Tilt-rotor quadcopter xplored: Hardware based dynamics, smart sliding mode controller, attitude hold & wind disturbance scenarios
Qin et al. Gemini ii: Design, modeling, and control of a compact yet efficient servoless bi-copter
CN109308064A (zh) 一种四旋翼无人机的故障容错控制方法及***
Fang et al. Design and nonlinear control of an indoor quadrotor flying robot
Zhu et al. Design and experimental testing of safe flight control system for novel vertical take-off and landing aircraft
Jothiraj et al. Enabling bidirectional thrust for aggressive and inverted quadrotor flight
CN108681251B (zh) 六旋翼无人机电机倾角的确定方法
CN111572762A (zh) 一种旋翼航空器及其螺旋桨力矩的转换方法
CN115906295B (zh) 基于数字孪生的无人机健康监测方法和装置
Jothiraj et al. Control allocation of bidirectional thrust quadrotor subject to actuator constraints
CN111338369A (zh) 一种基于非线性逆补偿的多旋翼飞行控制方法
CN115857309A (zh) 一种可倾斜六旋翼无人机的容错控制方法及***
WO2019241617A2 (en) Systems and methods for controlling a vehicle
CN113296533B (zh) 广义作动器控制分配与重构方法、装置及其相关组件
CN113238572B (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
GR01 Patent grant
GR01 Patent grant