CN103712598A - 一种小型无人机姿态确定***与确定方法 - Google Patents

一种小型无人机姿态确定***与确定方法 Download PDF

Info

Publication number
CN103712598A
CN103712598A CN201310748375.1A CN201310748375A CN103712598A CN 103712598 A CN103712598 A CN 103712598A CN 201310748375 A CN201310748375 A CN 201310748375A CN 103712598 A CN103712598 A CN 103712598A
Authority
CN
China
Prior art keywords
attitude
chip
aerial vehicle
unmanned aerial
angular rate
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.)
Granted
Application number
CN201310748375.1A
Other languages
English (en)
Other versions
CN103712598B (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.)
Harbin Institute of Technology
Bohai University
Original Assignee
Harbin Institute of Technology
Bohai 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 Harbin Institute of Technology, Bohai University filed Critical Harbin Institute of Technology
Priority to CN201310748375.1A priority Critical patent/CN103712598B/zh
Publication of CN103712598A publication Critical patent/CN103712598A/zh
Application granted granted Critical
Publication of CN103712598B publication Critical patent/CN103712598B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Gyroscopes (AREA)

Abstract

一种低成本、低功耗、算法简单、计算量小的小型无人机姿态确定***与确定方法，确定***包括：角速率陀螺芯片，用于测量无人机的滚动、俯仰与偏航角速率；三轴电子罗盘芯片，用于测量无人机飞行时的航迹方位角即偏航角；三轴加速度计芯片量重力在无人机本体坐标轴的分量；微控器芯片，通过I2C总线分别与角速率陀螺芯片、三轴电子罗盘芯片和三轴加速度计芯片连接，用于采集上述传感器的测量数据，并根据采集数据姿态确定方法确定无人机实时姿态。确定方法包括以下步骤：包括建立无人机本体坐标系与惯性坐标系之间初始方向余弦矩阵、更新姿态方向余弦矩阵、标准化方向余弦矩阵、计算无人机的修正三轴角速度和计算无人机三轴姿态角。

Description

一种小型无人机姿态确定***与确定方法
技术领域
 本发明涉及无人机飞行姿态确定技术领域，具体涉及一种基于多传感器数据融合的低成本、低功耗的小型无人机姿态确定***与确定方法。
背景技术
无人机由于其成本低、无人员伤亡以及战场生存能力强等特点而广泛应用于军事目标的侦查与打击等众多领域，成为航空技术的重要发展方向，受到各国的高度关注。无人机的姿态控制***是无人机正常运行并完成飞行任务的重要保障***之一，而无人机姿态的确定是实现姿态控制的基础。因此，针对小型无人机低成本等特点，如何设计低成本高可靠性的姿态确定***是目前研究的热点。
目前大多数的军事侦查无人机采用高精度的角速率陀螺进行姿态确定。该方法将角速率陀螺的测量值进行积分从而获得无人机的姿态信息。由于积分计算中存在一定的偏差，因此高精度的姿态确定需要高精度的陀螺。由于高精度陀螺存在成本高、重量大以及结构复杂等缺陷，从而使得采用高精度陀螺进行姿态确定的方案并不适用于低成本的小型无人机。事实上，低成本小型无人机通常采用低成本、体积小的微型电子机械传感器，但是此类传感器的精度低，积分角速率陀螺输出值将产生较大的漂移，从而无法提供高精度的姿态确定。
为提高低成本无人机的姿态确定精度，目前小型无人机常采用传感器融合技术将加速度计、磁强计及GPS等组成的无陀螺***与角速率陀螺进行融合，确定无人机姿态。该方法利用角速率陀螺提供的姿态信息来“平滑”无陀螺***，而无陀螺***则用于校正角速率陀螺产生的漂移。因此采用适当的滤波器可将角速率陀螺与无陀螺***结合，从而获得无偏的姿态信息。采用多传感器数据融合技术进行无人机姿态确定的核心在于融合不同传感器的滤波器。无人机姿态确定中常采用扩展卡尔曼滤波器或者非线性预测滤波器。然而这些滤波算法计算量大，算法复杂，显然不太适用于低成本、计算能力小的小型无人机姿态确定。
在低成本传感器的制约条件下，无人机的小型化、低成本化，迫切要求形成一种结构简单、计算量小、廉价但又具有较高精度和可靠性的姿态确定***与确定方法。
发明内容
本发明的目的是为了解决现有技术存在的上述问题，提供一种低成本、低功耗、算法简单、计算量小的小型无人机姿态确定***与确定方法，实现小型无人机低成本的姿态确定。
为解决上述技术问题，本发明采用如下技术方案：
一种小型无人机姿态确定***，其包括： 
角速率陀螺芯片，用于测量无人机的滚动、俯仰与偏航角速率；
三轴电子罗盘芯片，用于测量无人机飞行时的航迹方位角即偏航角；
三轴加速度计芯片量重力在无人机本体坐标轴的分量；
微控器芯片，通过I2C总线分别与角速率陀螺芯片、三轴电子罗盘芯片和三轴加速度计芯片连接，用于采集上述传感器的测量数据，并根据采集数据姿态确定方法确定无人机实时姿态。
一种小型无人机姿态确定方法，包括以下步骤：
步骤一、采用欧拉角321坐标系旋转建立无人机本体坐标系与参考惯性坐标系之间的无人机姿态，并建立两坐标系之间的初始方向余弦矩阵                                                ；
步骤二、更新姿态方向余弦矩阵。根据修正的三轴角速度值
Figure 333079DEST_PATH_IMAGE002
，步骤一中建立的初始方向余弦值
Figure 634747DEST_PATH_IMAGE003
，微控器芯片采样时间
Figure 792059DEST_PATH_IMAGE004
，以及时刻的方向余弦信息，计算
Figure 283400DEST_PATH_IMAGE006
时刻的方向余弦矩阵
Figure 970734DEST_PATH_IMAGE007
为：
Figure 767788DEST_PATH_IMAGE008
当
Figure 376624DEST_PATH_IMAGE009
时，修正的三轴角速度
Figure 284537DEST_PATH_IMAGE010
直接采用三轴角速率陀螺芯片测量的角速率值；
步骤三、标准化方向余弦矩阵。根据方向余弦阵
Figure 826377DEST_PATH_IMAGE007
的第一列
Figure 794333DEST_PATH_IMAGE011
、第二列
Figure 890465DEST_PATH_IMAGE012
与第三列
Figure 133228DEST_PATH_IMAGE013
，标准化方向余弦阵
Figure 998415DEST_PATH_IMAGE007
为
Figure 137273DEST_PATH_IMAGE014
，
Figure 767154DEST_PATH_IMAGE016
Figure 486849DEST_PATH_IMAGE017
式中
Figure 327766DEST_PATH_IMAGE018
Figure 398490DEST_PATH_IMAGE019
Figure 451896DEST_PATH_IMAGE020
其中“
Figure 822835DEST_PATH_IMAGE021
”表示向量叉乘运算，且
Figure 303495DEST_PATH_IMAGE022
；
步骤四、根据三轴角速率陀螺芯片测量的无人机角速度以及比例积分反馈输出修正项
Figure 984192DEST_PATH_IMAGE024
，计算无人机修正的三轴角速度
Figure 678478DEST_PATH_IMAGE025
为：
Figure 595619DEST_PATH_IMAGE026
；
步骤五、根据步骤三获得的标准化后的方向余弦矩阵
Figure 906514DEST_PATH_IMAGE027
，计算无人机姿态滚动
Figure 301724DEST_PATH_IMAGE028
、俯仰
Figure 116096DEST_PATH_IMAGE029
与偏航角
Figure 938558DEST_PATH_IMAGE030
分别为：
Figure 736750DEST_PATH_IMAGE031
，
Figure 935650DEST_PATH_IMAGE032
，
Figure 604529DEST_PATH_IMAGE033
其中
Figure 140770DEST_PATH_IMAGE034
分别表示方向余弦矩阵
Figure 895099DEST_PATH_IMAGE027
的第
Figure 632111DEST_PATH_IMAGE035
第
Figure 686655DEST_PATH_IMAGE036
的元素。
步骤一中初始方向余弦矩阵
Figure 116499DEST_PATH_IMAGE003
的建立过程为：
步骤a、设定无人机初始的滚动与俯仰姿态角均为零度，即
Figure 358125DEST_PATH_IMAGE037
与，根据电子罗盘芯片输出的三轴测量值，计算该测量值在无人机本体系x b 与y b 轴的投影分别为
Figure 143044DEST_PATH_IMAGE040
、
Figure 403124DEST_PATH_IMAGE041
；
步骤b、根据步骤a获得的
Figure 481939DEST_PATH_IMAGE042
与
Figure 979916DEST_PATH_IMAGE043
值计算初始偏航角的正余弦值分别为
Figure 233360DEST_PATH_IMAGE045
，
Figure 115865DEST_PATH_IMAGE046
；
步骤c、根据步骤a与b的姿态角以及欧拉角321坐标系旋转，建立无人机初始方向余弦矩阵为：
Figure 942056DEST_PATH_IMAGE047
。
步骤四中获得比例积分反馈输出修正项
Figure 379990DEST_PATH_IMAGE024
为：
Figure 800607DEST_PATH_IMAGE048
，
，
其中
Figure 843016DEST_PATH_IMAGE051
与分别为比例、积分因子，当时
Figure 944330DEST_PATH_IMAGE053
，
Figure 888015DEST_PATH_IMAGE054
为检测出的角速率陀螺总的偏移值。
计算角速率陀螺总的偏移值
Figure 916014DEST_PATH_IMAGE054
为：
Figure 97597DEST_PATH_IMAGE055
其中
Figure 818428DEST_PATH_IMAGE056
表示偏航轴上的角速率陀螺偏移值，
Figure 718251DEST_PATH_IMAGE057
表示滚动与俯仰轴上的角速率陀螺偏移值。
偏航轴上的角速率陀螺偏移
Figure 284361DEST_PATH_IMAGE056
的计算过程为：
Figure 117188DEST_PATH_IMAGE058
。
滚动与俯仰轴上的角速率陀螺偏移
Figure 477762DEST_PATH_IMAGE057
的计算过程为：
Figure 864881DEST_PATH_IMAGE059
其中
Figure 234683DEST_PATH_IMAGE060
为三轴加速度计芯片的测量值。
本发明所提出的姿态确定***由低成本的角速率陀螺芯片、三轴电子罗盘芯片、三轴加速度计芯片以及微控器芯片组成，与采用高精度惯性测量单元构成的姿态确定***相比，本发明***成本更低、质量更轻，更能够适合低成本低质量需求的小型无人机开发。
本发明所提出的姿态确定方法包括建立无人机本体坐标系与惯性坐标系之间初始方向余弦矩阵、更新姿态方向余弦矩阵、标准化方向余弦矩阵、计算无人机的修正三轴角速度和计算无人机三轴姿态角，设计简单的加减运算，与利用卡尔曼滤波等设计的姿态确定方法相比，本发明方法计算量小、结构简单，更能够有效提升姿态确定的速度。
本发明的有益效果在于：本发明通过使用廉价的姿态测量芯片进行姿态信息采集，降低了小型无人机姿态确定***的成本与重量。本发明提出的一种非基于滤波的传感器数据融合姿态确定方法，该算法结构简单、数据计算量小，解决了传统的姿态确定方法算法复杂、计算量大等问题，适用于小型低成本无人机***。
附图说明
图1是本发明的小型无人机姿态确定***硬件实现结构图；
图2是本发明的小型无人机姿态确定方法结构示意图；
图3是本发明的小型无人机姿态确定方法实验结果图。
具体实施方式
下面将结合附图和实施例对本发明作进一步详细的说明。
请参阅图1，本发明设计了一种小型无人机姿态确定***，其包括三个姿态测量器件：角速率陀螺芯片用于测量无人机角速度，三轴电子罗盘芯片用于测量无人机偏航角，三轴加速度计芯片用于测量重力在无人机本体坐标系下的分量。除此之外，该***还包括一个微控器芯片，该芯片用于采集姿态测量器件的测量数据，并执行本发明的姿态确定算法方法确定无人机姿态。微控器芯片与角速率陀螺芯片、三轴电子罗盘芯片以及三轴加速度计芯片通过I2C总线进行通讯。微控器芯片对各姿态测量芯片测量的数据采用多次采样进行平均的方式进行数据采集，以提高测量精度。本实施例中采用角速率陀螺芯片ITG-3200、三轴电子罗盘芯片HMC5883L、三轴加速度计芯片ADXL345以及微控器芯片ATemga2560，三种低成本姿态测量芯片与微控器总成本不超过500元人民币，总质量极轻，且总功耗相当小，十分符合低成本、小功耗的小型无人机。
令三轴电子罗盘芯片三轴测量值为
Figure 656437DEST_PATH_IMAGE039
，而加速度计芯片测量值为
Figure 453492DEST_PATH_IMAGE060
，且三轴角速率陀螺芯片测量值为
Figure 62327DEST_PATH_IMAGE061
，则本发明公开的一种小型无人机姿态确定方法如下：
根据姿态确定***硬件实现结构图，本发明的核心在于其姿态确定算法，该姿态确定方法结构示意图如图2所示，包含以下几个步骤：
步骤一：建立无人机本体坐标系与惯性坐标系之间初始方向余弦矩阵；
步骤二：更新姿态方向余弦矩阵；
步骤三：标准化方向余弦矩阵； 
步骤四：计算无人机的修正三轴角速度；
步骤五：计算无人机三轴姿态角。
步骤一中建立无人机本体坐标系与参考惯性坐标系之间的姿态方向余弦矩阵过程为： 
步骤I：为了描述无人机的姿态，首先需要定义相应的坐标系***。由于小型无人机的飞行航程短，因此通常将机体本体坐标系选S b -Ox b y b z b 为动坐标系，而将导航坐标系S n -Ox n y n z n 选为参考坐标系，定义为北东天参考***，即北向为S n 的x n 轴，东向为S n 的y n 轴，指向地心为S n 的z n 轴。
步骤II：建立无人机本体坐标系S b 与导航坐标系S n 之间的姿态。通过本体坐标系S b 绕z b 轴旋转
Figure 766978DEST_PATH_IMAGE030
角，然后绕y b 轴旋转
Figure 777660DEST_PATH_IMAGE029
角，最后绕z b 轴旋转
Figure 745616DEST_PATH_IMAGE028
角便可建立坐标系S b 与导航坐标系S n 之间的方向余弦阵即姿态矩阵
Figure 904064DEST_PATH_IMAGE062
：
Figure 615669DEST_PATH_IMAGE063
 （1）
其中、
Figure 619714DEST_PATH_IMAGE029
与
Figure 734300DEST_PATH_IMAGE030
分别称作滚动姿态角、俯仰姿态角与偏航姿态角。
步骤一中无人机姿态方向余弦矩阵的初始值建立过程为： 
步骤a、设定无人机初始的滚动与俯仰姿态角均为零度，即
Figure 984016DEST_PATH_IMAGE037
与
Figure 969289DEST_PATH_IMAGE038
，根据电子罗盘芯片输出的三轴测量值
Figure 810207DEST_PATH_IMAGE039
，计算该测量值在无人机本体坐标系x b 与y b 轴的投影分别为
Figure 880931DEST_PATH_IMAGE040
、
Figure 934337DEST_PATH_IMAGE041
。
步骤b、根据步骤a获得的
Figure 305276DEST_PATH_IMAGE042
与
Figure 785936DEST_PATH_IMAGE043
值计算初始偏航角
Figure 78377DEST_PATH_IMAGE044
的正余弦值分别为
Figure 466633DEST_PATH_IMAGE045
，。
步骤c、根据步骤a与b的姿态角以及欧拉角321坐标系旋转，由公式（1）建立无人机初始方向余弦矩阵为：
Figure 388955DEST_PATH_IMAGE047
       （2）
步骤二中无人机姿态方向余弦矩阵的更新过程为：
由欧拉定理，根据
Figure 784165DEST_PATH_IMAGE005
时刻的姿态矩阵
Figure 332958DEST_PATH_IMAGE062
、修正的三轴角速度信息以及微控器芯片采样时间
Figure 219191DEST_PATH_IMAGE004
，计算
Figure 418091DEST_PATH_IMAGE006
时刻无人机的方向余弦矩阵为：
                  （3）
Figure 365822DEST_PATH_IMAGE065
              （4）
当
Figure 633992DEST_PATH_IMAGE009
时，
Figure 157377DEST_PATH_IMAGE066
为三轴角速率陀螺芯片实际测量的角速率值。
由于公式（3）的获得使用了数值积分方法，因此存在一定的积分偏差。故无人机飞行一段时间后方向余弦阵
Figure 587222DEST_PATH_IMAGE007
可能不再满足正交性，为此步骤三中无人机姿态方向余弦矩阵的标准化过程为：
令
Figure 94426DEST_PATH_IMAGE011
、与分别表示姿态矩阵阵
Figure 613766DEST_PATH_IMAGE007
的第一、二与三列，采用如下的
Figure 873846DEST_PATH_IMAGE067
与
Figure 952661DEST_PATH_IMAGE068
来分别逼近
Figure 450638DEST_PATH_IMAGE011
、
Figure 487864DEST_PATH_IMAGE012
：
Figure 704082DEST_PATH_IMAGE018
            （5）
Figure 586587DEST_PATH_IMAGE019
           （6）
式中
Figure 470230DEST_PATH_IMAGE022
。令“
Figure 412778DEST_PATH_IMAGE021
”表示向量叉乘运算，则此时可根据正交性质，
Figure 850713DEST_PATH_IMAGE013
可用
Figure 802488DEST_PATH_IMAGE069
进行逼近：
Figure 275058DEST_PATH_IMAGE020
                   （7）
至此，由公式（5）-公式（7）可采用如下方法把姿态矩阵
Figure 122928DEST_PATH_IMAGE007
标准化、归一化如下的形式：
Figure 579317DEST_PATH_IMAGE014
  （8）
式中：
Figure 803625DEST_PATH_IMAGE015
           （9）
Figure 130701DEST_PATH_IMAGE016
              （10）
Figure 946211DEST_PATH_IMAGE017
             （11）
步骤四中无人机修正的三轴角速度
Figure 358737DEST_PATH_IMAGE025
的计算过程为：
由三轴角速率陀螺芯片的测量值
Figure 121157DEST_PATH_IMAGE070
与比例积分反馈输出修正项
Figure 833898DEST_PATH_IMAGE071
，计算修正的无人机三轴角速度
Figure 289150DEST_PATH_IMAGE025
为：
Figure 188973DEST_PATH_IMAGE026
               （12）
所述比例积分反馈输出修正项的计算过程为：
步骤1)：令
Figure 322331DEST_PATH_IMAGE056
为因角速率陀螺偏航轴上的偏移而引起的角速率陀螺偏移，分别表示方向余弦矩阵
Figure 866762DEST_PATH_IMAGE027
的第
Figure 970984DEST_PATH_IMAGE035
第
Figure 127159DEST_PATH_IMAGE036
的元素。由于根据三轴电子罗盘芯片测量值可获得无人机航向方向，为此
Figure 455372DEST_PATH_IMAGE056
可检测为：
Figure 64208DEST_PATH_IMAGE058
               （13）
步骤2) ：令
Figure 237700DEST_PATH_IMAGE057
为因角速率陀螺滚动-俯仰轴上的偏移而引起的角速率陀螺偏移，则根据三轴加速度计芯片的测量值
Figure 779540DEST_PATH_IMAGE072
可检测出该偏移值为：
Figure 747496DEST_PATH_IMAGE059
                       （14）
步骤3)：根据步骤1)与2)中检测的三轴角速率陀螺偏移值，则角速率陀螺总的偏移值可计算为：
Figure 820812DEST_PATH_IMAGE055
                      （15）
步骤4)：由控制理论中比例-积分-微分控制方法可知，经过一个比例-积分控制环节后消除一定的误差。为此在本步骤中将对所检测的总的偏移
Figure 951579DEST_PATH_IMAGE054
进行比例-积分控制处理，从而计算比例积分反馈输出修正
Figure 90436DEST_PATH_IMAGE071
为：
Figure 205022DEST_PATH_IMAGE048
           （16）
其中
Figure 454738DEST_PATH_IMAGE073
表示比例环节，而表示积分环节，且
Figure 280929DEST_PATH_IMAGE049
                                （17）
            （18）
式中
Figure 936218DEST_PATH_IMAGE051
与
Figure 775998DEST_PATH_IMAGE052
分别为比例、积分因子，且当
Figure 256658DEST_PATH_IMAGE009
时
Figure 80257DEST_PATH_IMAGE053
。
步骤五中无人机三轴姿态角的确定过程为：
根据步骤三标准化、归一化后的方向余弦矩阵
Figure 937355DEST_PATH_IMAGE027
，计算无人机姿态滚动
Figure 162800DEST_PATH_IMAGE028
、俯仰
Figure 814361DEST_PATH_IMAGE029
与偏航角
Figure 859678DEST_PATH_IMAGE030
分别为：
Figure 786045DEST_PATH_IMAGE075
，
Figure 334838DEST_PATH_IMAGE076
，
Figure 422880DEST_PATH_IMAGE077
        （19）
根据以上所述的小型无人机姿态确定***的硬件实现结构与软件算法方法，取微控器芯片在执行算法时其采样时间为
Figure 221072DEST_PATH_IMAGE078
秒，将本发明的姿态确定***与姿态确定方法应用于小型无人机，该无人机实际飞行的过程中由本发明姿态确定方法所确定的三轴姿态角如图3所示。从图3可知，本发明的姿态确定算法能够实现低成本的无人机姿态确定。
综上可见，本发明提出了一种由廉价的姿态测量芯片(角速率陀螺芯片、三轴电子罗盘芯片、三轴加速度计芯片)组成的无人机姿态确定***与方法。该***中的微控器芯片首先采集上述三个芯片的测量数据，然后融合采集数据，执行姿态确定算法确定了无人机的三轴姿态。本发明硬件设计简单、成本低廉，且姿态确定方法软件实现算法简单、计算量小。实际结果表明，本发明能准确地建立无人机飞行过程中的空间姿态，非常适用于低成本小型无人机。
这里本发明的描述和应用是说明性的，并非想将本发明的范围限制在上述实施例中。这里所披露的实施例的变形和改变是可能的，对于那些本领域的普通技术人员来说实施例的替换和等效的各种部件是公知的。本领域技术人员应该清楚的是，在不脱离本发明的精神或本质特征的情况下，本发明中姿态测量芯片以及微控器可以采用其它选型。

Claims (7)

1.一种小型无人机姿态确定***与方法，其特征在于其包括：
角角速率陀螺芯片，用于测量无人机的滚动、俯仰与偏航角速率；
三轴电子罗盘芯片，用于测量无人机飞行时的航迹方位角即偏航角；
三轴加速度计芯片量重力在无人机本体坐标轴的分量；
微控器芯片，分别通过I2C总线与角速率陀螺芯片、三轴电子罗盘芯片和三轴加速度计芯片连接，用于采集上述传感器的测量数据，并根据采集数据姿态确定方法确定无人机实时姿态。
2.根据权利要求1所述的一种小型无人机姿态确定***与方法，其特征在于所述姿态确定方法包括以下步骤：
步骤一、采用欧拉角321坐标系旋转建立无人机本体坐标系与参考惯性坐标系之间的无人机姿态，并建立两坐标系之间的初始方向余弦矩阵                                                ；
步骤二、更新姿态方向余弦矩阵
根据修正的三轴角速度值
Figure 2013107483751100001DEST_PATH_IMAGE002
，步骤一中建立的初始方向余弦值
Figure 2013107483751100001DEST_PATH_IMAGE003
，微控器芯片采样时间
Figure 2013107483751100001DEST_PATH_IMAGE004
，以及
Figure 2013107483751100001DEST_PATH_IMAGE005
时刻的方向余弦信息，计算
Figure 2013107483751100001DEST_PATH_IMAGE006
时刻的方向余弦矩阵
Figure DEST_PATH_IMAGE007
为：
当
Figure DEST_PATH_IMAGE009
时，修正的三轴角速度
Figure 2013107483751100001DEST_PATH_IMAGE010
直接采用三轴角速率陀螺芯片测量的角速率值；
步骤三、标准化方向余弦矩阵
根据方向余弦阵的第一列
Figure DEST_PATH_IMAGE011
、第二列
Figure 2013107483751100001DEST_PATH_IMAGE012
与第三列
Figure DEST_PATH_IMAGE013
，标准化方向余弦阵
Figure 868285DEST_PATH_IMAGE007
为
Figure DEST_PATH_IMAGE014
，
Figure DEST_PATH_IMAGE015
Figure DEST_PATH_IMAGE016
Figure DEST_PATH_IMAGE017
式中
Figure DEST_PATH_IMAGE018
Figure DEST_PATH_IMAGE019
Figure DEST_PATH_IMAGE020
其中“
Figure DEST_PATH_IMAGE021
”表示向量叉乘运算，且
Figure DEST_PATH_IMAGE022
；
步骤四、根据三轴角速率陀螺芯片测量的无人机角速度
Figure DEST_PATH_IMAGE023
以及比例积分反馈输出修正项
Figure DEST_PATH_IMAGE024
，计算无人机修正的三轴角速度
Figure DEST_PATH_IMAGE025
为：
Figure DEST_PATH_IMAGE026
；
步骤五、根据步骤三获得的标准化后的方向余弦矩阵
Figure DEST_PATH_IMAGE027
，计算无人机姿态滚动、俯仰
Figure DEST_PATH_IMAGE029
与偏航角
Figure DEST_PATH_IMAGE030
分别为：
Figure DEST_PATH_IMAGE032
Figure DEST_PATH_IMAGE033
其中
Figure DEST_PATH_IMAGE034
分别表示方向余弦矩阵
Figure 314179DEST_PATH_IMAGE027
的第第的元素。
3.根据权利要求2所述的一种小型无人机姿态确定方法，其特征在于步骤一中初始方向余弦矩阵
Figure 802929DEST_PATH_IMAGE003
的建立过程为：
步骤a、设定无人机初始的滚动与俯仰姿态角均为零度，即
Figure DEST_PATH_IMAGE037
与
Figure DEST_PATH_IMAGE038
，根据电子罗盘芯片输出的三轴测量值
Figure DEST_PATH_IMAGE039
，计算该测量值在无人机本体系x b 与y b 轴的投影分别为、
Figure DEST_PATH_IMAGE041
；
步骤b、根据步骤a获得的
Figure DEST_PATH_IMAGE042
与
Figure DEST_PATH_IMAGE043
值计算初始偏航角
Figure DEST_PATH_IMAGE044
的正余弦值分别为
Figure DEST_PATH_IMAGE045
，；
步骤c、根据步骤a与步骤b的姿态角以及欧拉角321坐标系旋转，建立无人机初始方向余弦矩阵为：
Figure DEST_PATH_IMAGE047
。
4.根据权利要求2所述的一种小型无人机姿态确定方法，其特征在于步骤四中获得比例积分反馈输出修正项
Figure 453539DEST_PATH_IMAGE024
为：
Figure DEST_PATH_IMAGE049
，
Figure DEST_PATH_IMAGE050
其中
Figure DEST_PATH_IMAGE051
与
Figure DEST_PATH_IMAGE052
分别为比例、积分因子，当
Figure 680121DEST_PATH_IMAGE009
时，
Figure DEST_PATH_IMAGE054
为检测出的角速。
5.根据权利要求4所述的一种小型无人机姿态确定方法，其特征在于计算角速率陀螺总的偏移值
Figure 921746DEST_PATH_IMAGE054
为：
Figure DEST_PATH_IMAGE055
其中
Figure DEST_PATH_IMAGE056
表示偏航轴上的角速率陀螺偏移值，
Figure DEST_PATH_IMAGE057
表示滚动与俯仰轴上的角速率陀螺偏移值。
6.根据权利要求5所述的一种小型无人机姿态确定***与方法，其特征在于偏航轴上的角速率陀螺偏移
Figure 993608DEST_PATH_IMAGE056
的计算过程为：
Figure DEST_PATH_IMAGE058
。
7.根据权利要求5所述的一种小型无人机姿态确定方法，其特征在于滚动与俯仰轴上的角速率陀螺偏移
Figure 902658DEST_PATH_IMAGE057
的计算过程为：
Figure DEST_PATH_IMAGE059
其中
Figure DEST_PATH_IMAGE060
为三轴加速度计芯片的测量值。
CN201310748375.1A 2013-12-31 2013-12-31 一种小型无人机姿态确定方法 Expired - Fee Related CN103712598B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201310748375.1A CN103712598B (zh) 2013-12-31 2013-12-31 一种小型无人机姿态确定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201310748375.1A CN103712598B (zh) 2013-12-31 2013-12-31 一种小型无人机姿态确定方法

Publications (2)

Publication Number Publication Date
CN103712598A true CN103712598A (zh) 2014-04-09
CN103712598B CN103712598B (zh) 2014-12-17

Family

ID=50405761

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201310748375.1A Expired - Fee Related CN103712598B (zh) 2013-12-31 2013-12-31 一种小型无人机姿态确定方法

Country Status (1)

Country Link
CN (1) CN103712598B (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106444809A (zh) * 2016-10-12 2017-02-22 湖南绿野航空科技有限公司 一种无人机飞行控制器
CN106468563A (zh) * 2016-10-20 2017-03-01 极翼机器人(上海)有限公司 一种机载磁传感器在线标定方法
CN107063173A (zh) * 2017-06-13 2017-08-18 广州辛群科技有限公司 角度检测方法和关节运动夹角检测***
CN107131865A (zh) * 2017-06-13 2017-09-05 广州辛群科技有限公司 角度检测装置
CN107346141A (zh) * 2016-05-06 2017-11-14 北京臻迪机器人有限公司 一种体感控制方法
CN107346140A (zh) * 2016-05-06 2017-11-14 北京臻迪机器人有限公司 一种无头控制的方法
CN108983795A (zh) * 2018-05-07 2018-12-11 长江大学 一种三轴姿态校正方法及设备
CN110174892A (zh) * 2019-04-08 2019-08-27 北京百度网讯科技有限公司 车辆朝向的处理方法、装置、设备及计算机可读存储介质
US11327477B2 (en) 2015-12-31 2022-05-10 Powervision Robot Inc. Somatosensory remote controller, somatosensory remote control flight system and method, and head-less control method

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004345435A (ja) * 2003-05-21 2004-12-09 Nagasaki Prefecture 飛翔体の位置姿勢計測装置
CN1669874A (zh) * 2004-03-16 2005-09-21 清华大学 一种用于飞行器的自动驾驶仪
CN1740746A (zh) * 2005-05-23 2006-03-01 清华大学 微小型动态载体姿态测量装置及其测量方法
CN102692225A (zh) * 2011-03-24 2012-09-26 北京理工大学 一种用于低成本小型无人机的姿态航向参考***
CN102854887A (zh) * 2012-09-06 2013-01-02 北京工业大学 一种无人机航迹规划和远程同步操控方法
CN103196445A (zh) * 2013-02-07 2013-07-10 哈尔滨工业大学 基于匹配技术的地磁辅助惯性的载体姿态测量方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004345435A (ja) * 2003-05-21 2004-12-09 Nagasaki Prefecture 飛翔体の位置姿勢計測装置
CN1669874A (zh) * 2004-03-16 2005-09-21 清华大学 一种用于飞行器的自动驾驶仪
CN1740746A (zh) * 2005-05-23 2006-03-01 清华大学 微小型动态载体姿态测量装置及其测量方法
CN102692225A (zh) * 2011-03-24 2012-09-26 北京理工大学 一种用于低成本小型无人机的姿态航向参考***
CN102854887A (zh) * 2012-09-06 2013-01-02 北京工业大学 一种无人机航迹规划和远程同步操控方法
CN103196445A (zh) * 2013-02-07 2013-07-10 哈尔滨工业大学 基于匹配技术的地磁辅助惯性的载体姿态测量方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
杨淑洁，等: "低成本无人机姿态测量***研究", 《传感器与微***》 *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11327477B2 (en) 2015-12-31 2022-05-10 Powervision Robot Inc. Somatosensory remote controller, somatosensory remote control flight system and method, and head-less control method
CN107346141A (zh) * 2016-05-06 2017-11-14 北京臻迪机器人有限公司 一种体感控制方法
CN107346140A (zh) * 2016-05-06 2017-11-14 北京臻迪机器人有限公司 一种无头控制的方法
CN106444809A (zh) * 2016-10-12 2017-02-22 湖南绿野航空科技有限公司 一种无人机飞行控制器
CN106444809B (zh) * 2016-10-12 2024-04-16 湖南绿野航空科技有限公司 一种无人机飞行控制器
CN106468563A (zh) * 2016-10-20 2017-03-01 极翼机器人(上海)有限公司 一种机载磁传感器在线标定方法
CN107063173A (zh) * 2017-06-13 2017-08-18 广州辛群科技有限公司 角度检测方法和关节运动夹角检测***
CN107131865A (zh) * 2017-06-13 2017-09-05 广州辛群科技有限公司 角度检测装置
CN108983795A (zh) * 2018-05-07 2018-12-11 长江大学 一种三轴姿态校正方法及设备
CN110174892A (zh) * 2019-04-08 2019-08-27 北京百度网讯科技有限公司 车辆朝向的处理方法、装置、设备及计算机可读存储介质

Also Published As

Publication number Publication date
CN103712598B (zh) 2014-12-17

Similar Documents

Publication Publication Date Title
CN103712598B (zh) 一种小型无人机姿态确定方法
CN106643737B (zh) 风力干扰环境下四旋翼飞行器姿态解算方法
CN104898681B (zh) 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法
CN105606094B (zh) 一种基于mems/gps组合***的信息条件匹配滤波估计方法
CN104698485B (zh) 基于bd、gps及mems的组合导航***及导航方法
CN108036785A (zh) 一种基于直接法与惯导融合的飞行器位姿估计方法
CN106482734A (zh) 一种用于imu多传感器数据融合的滤波方法
WO2018214227A1 (zh) 一种无人车实时姿态测量方法
CN108318038A (zh) 一种四元数高斯粒子滤波移动机器人姿态解算方法
CN109443349A (zh) 一种姿态航向测量***及其融合方法、存储介质
CN102937450B (zh) 一种基于陀螺测量信息的相对姿态确定方法
CN103363992A (zh) 基于梯度下降的四旋翼无人机姿态航向参考***解算方法
CN104075699A (zh) 三维固态电子罗盘及其传感器的零点和比例系数核正方法
CN103900559A (zh) 一种基于干扰估计的高精度姿态解算***
CN103994766A (zh) 一种抗gps失效固定翼无人机定向方法
CN111189474A (zh) 基于mems的marg传感器的自主校准方法
CN108592911A (zh) 一种四旋翼飞行器动力学模型/机载传感器组合导航方法
CN110779514B (zh) 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置
WO2018214226A1 (zh) 一种无人车实时姿态测量方法
CN104748734B (zh) 一种带倾角补偿的车载电子海拔罗盘仪
CN102607557B (zh) 一种基于gps/imu的飞行器姿态直接积分校正方法
CN111207734B (zh) 一种基于ekf的无人机组合导航方法
Blachuta et al. Attitude and heading reference system based on 3D complementary filter
Emran et al. A cascaded approach for quadrotor's attitude estimation
CN103471593B (zh) 一种基于gps信息的惯性导航***测量误差修正方法

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20141217

Termination date: 20151231

EXPY Termination of patent right or utility model