CN108318035A - 一种基于欧拉角的姿态测量算法中避免万向节死锁的方法 - Google Patents

一种基于欧拉角的姿态测量算法中避免万向节死锁的方法 Download PDF

Info

Publication number
CN108318035A
CN108318035A CN201810026385.7A CN201810026385A CN108318035A CN 108318035 A CN108318035 A CN 108318035A CN 201810026385 A CN201810026385 A CN 201810026385A CN 108318035 A CN108318035 A CN 108318035A
Authority
CN
China
Prior art keywords
axis
eulerian angles
attitude
universal joint
coordinate system
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
CN201810026385.7A
Other languages
English (en)
Other versions
CN108318035B (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.)
Shandong University
Original Assignee
Shandong 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 Shandong University filed Critical Shandong University
Priority to CN201810026385.7A priority Critical patent/CN108318035B/zh
Publication of CN108318035A publication Critical patent/CN108318035A/zh
Application granted granted Critical
Publication of CN108318035B publication Critical patent/CN108318035B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F3/00—Input arrangements for transferring data to be processed into a form capable of being handled by the computer; Output arrangements for transferring data from processing unit to output unit, e.g. interface arrangements
    • G06F3/01—Input arrangements or combined input and output arrangements for interaction between user and computer
    • G06F3/011—Arrangements for interaction with the human body, e.g. for user immersion in virtual reality

Abstract

本发明公开了一种基于欧拉角的姿态测量算法中避免万向节死锁的方法，确定导航坐标系和机体坐标系；利用姿态检测单元中的加速度传感器与磁力传感器测量值计算姿态检测算法中的欧拉角；设定姿态测量单元的加速度传感器在X轴上的分量的阈值和在Y轴上的分量的阈值；判断姿态检测单元的加速度传感器的输出是否达到设定的阈值，并按照判断结果修正姿态检测算法中的欧拉角计算方法；按照修正后的欧拉角计算方法计算被检测物体的当前姿态。本发明避免了万向节死锁问题，可以实现被测物体的全姿态测量。

Description

一种基于欧拉角的姿态测量算法中避免万向节死锁的方法
技术领域
本发明涉及一种基于欧拉角的姿态测量算法中避免万向节死锁的方法。
背景技术
目前基于欧拉角的姿态解算算法中，由于万向节死锁问题，无法实现全姿态解析。被测物体的横滚角范围可以达到(-π，π]，俯仰角的范围可以达到[-π/2，π/2]，偏航角的范围可以达到(-π，π]，无法满足被测物体全姿态测量。为避免发生万向节死锁的问题，在进行姿态检测时，需要人为的控制被检测物体在工作过程中的姿态角在一定范围内，即绕X轴旋转的角度不超过±180度，绕Y轴旋转的角度不超过±90度，绕Z轴旋转的角度不超过±180度。这种操作需要较高的技巧，而且无法实现被测物体的全姿态检测。在被测物体的旋转姿态为全姿态时，使用基于欧拉角的姿态检测算法必然发生万向节死锁问题，无法满足实际检测要求。
在被检测物体实际工作过程中，其绕某个轴的转动角度超出规定范围这一问题无法避免。因此，使用基于欧拉角的姿态解算算法实现被测物体的全姿态解析，是一个重要的问题。
发明内容
本发明为了解决上述问题，提出了一种基于欧拉角的姿态测量算法中避免万向节死锁的方法，本发明能够实现被检测物体的全姿态解析。
为了实现上述目的，本发明采用如下技术方案：
一种基于欧拉角的姿态测量算法中避免万向节死锁的方法，包括以下步骤：
(1)确定导航坐标系和机体坐标系；
(2)利用姿态测量单元中的加速度传感器与磁力传感器测量值计算姿态检测算法中的欧拉角；
(3)设定姿态测量单元的加速度传感器在X轴上的分量的阈值和在Y轴上的分量的阈值；
(4)判断姿态测量单元的加速度传感器的输出是否达到设定的阈值，并按照判断结果修正姿态检测算法中的欧拉角计算方法；
(5)按照修正后的欧拉角计算方法计算被检测物体的当前姿态。
进一步的，所述步骤(1)中，选择北东地(NED)坐标系作为导航坐标系，机体坐标系遵循右手法则，机体坐标系原点与机体质心重合，X轴平行于机体轴线并指向机体前方，Y轴垂直于X轴并指向机体右方，Z轴垂直于XY平面并指向机体下方。
进一步的，所述步骤(2)中，利用姿态测量单元中的加速度传感器与磁力传感器测量值计算姿态检测算法中的欧拉角，得到横滚角、俯仰角和航向角。
进一步的，所述步骤(4)中，当姿态测量单元的加速度传感器的X轴分量大于设定的阈值时，认为被检测物体的俯仰角到达或者接近±90度，将造成万向节死锁问题；当姿态测量单元的加速度传感器的Y轴分量大于设定的阈值时，认为被检测物体的横滚角到达或者接近±90度，将造成万向节死锁问题。
所述步骤(4)中，当姿态检测单元在t时刻时加速度传感器测量值在机体坐标系X轴或Y轴的分量大于设定的阈值时，以前一时刻的欧拉角中的横滚角和俯仰角作为当前时刻的横滚角和俯仰角。
本发明的基本原理是：基于欧拉角的姿态检测算法中，为避免出现万向节死锁问题，通过设定姿态测量单元中加速度传感器在X轴和Y轴上的测量值阈值，比较姿态测量过程中加速度传感器在X轴和Y轴上的测量值与所设定的阈值之间的关系，修正姿态检测算法中的欧拉角计算公式，按照新的欧拉角计算公式进行姿态测量，进而避免万向节死锁问题的发生，实现被测物体的全姿态测量。
本发明的有益效果是：根据姿态检测单元的加速度传感器的输出值，修正基于欧拉角的姿态检测算法中的欧拉角计算公式，可以避免万向节死锁问题；实现被测物体的全姿态解析；避免在进行姿态测量时人为控制被测物体的姿态范围在允许范围之内。
附图说明
构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。
图1为本发明提供的一个示意性例子的框架示意图；
图2为本发明的方法流程图；
图3(a)和图3(b)为本发明提供的一种测量坐标系选取方式示意图；
图4为本发明提供的一个示意性例子在具体应用场景下的发生万向节死锁的曲线示意图；
图5为本发明提供的一个示意性例子在具体应用场景下的使用本方法后的欧拉角曲线示意图。
具体实施方式：
下面结合附图与实施例对本发明作进一步说明。
应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
在本发明中，术语如“上”、“下”、“左”、“右”、“前”、“后”、“竖直”、“水平”、“侧”、“底”等指示的方位或位置关系为基于附图所示的方位或位置关系，只是为了便于叙述本发明各部件或元件结构关系而确定的关系词，并非特指本发明中任一部件或元件，不能理解为对本发明的限制。
本发明中，术语如“固接”、“相连”、“连接”等应做广义理解，表示可以是固定连接，也可以是一体地连接或可拆卸连接；可以是直接相连，也可以通过中间媒介间接相连。对于本领域的相关科研或技术人员，可以根据具体情况确定上述术语在本发明中的具体含义，不能理解为对本发明的限制。
正如背景技术中所提到，现有技术中在使用姿态测量单元来进行物体姿态检测的基于欧拉角的姿态检测算法中，由于欧拉角的万向节死锁问题，需要人为的控制被检测物体在工作过程中的姿态角在一定范围内，无法实现被测物体的全姿态检测，无法满足实际应用的需求。
鉴于此，本发明利用姿态检测单元的加速度传感器的测量值，通过设定合理的阈值，修正姿态检测算法中的欧拉角计算公式，避免万向节死锁问题，实现被测物体的全姿态检测。下面结合附图与实施实例对本发明做进一步说明。
图1为本发明提供的一个示意性例子的框架示意图，图2为本发明的方法流程图。图1和图2中，本发明为避免基于欧拉角的姿态检测算法中存在的万向节死锁问题，实现被测物体全姿态检测的方法为：
第一步：选取合适的导航坐标系和机体坐标系；
第二步：利用姿态检测单元中的加速度传感器与磁力传感器测量值计算姿态检测算法中的欧拉角；
第三步：设定姿态测量单元的加速度传感器在X轴上的分量的阈值axl和在Y轴上的分量的阈值ayl；
第四步：判断姿态检测单元的加速度传感器的输出是否达到设定的阈值；
第五步：修正姿态检测算法中的欧拉角计算方法；
第六步：按照修正后的欧拉角计算方法计算被检测物体的当前姿态。
第一步中，选取合适的导航坐标系和载体坐标系，由于坐标系的选取是多种多样的，图3(a)为世界坐标系，图3(b)为载体坐标系。导航坐标系选择北东地(NED)坐标系，机体坐标系遵循右手法则，机体坐标系原点与机体质心重合，X轴平行于机体轴线并指向机体前方，Y轴垂直于X轴并指向机体右方，Z轴垂直于XY平面并指向机体下方。
第二步中，利用姿态检测单元中的加速度传感器与磁力传感器测量值计算姿态检测算法中的欧拉角，计算公式形式多样，本方法选择其中一种计算方法：
其中，bax，bay，baz分别为姿态检测单元的加速度传感器测量值在机体坐标系X轴，Y轴，Z轴上的分量；bmx，bmy，bmz分别为姿态检测单元的磁力传感器测量值在机体坐标系X轴，Y轴，Z轴上的分量；分别为欧拉角中的横滚角，俯仰角和航向角。
第四步中，判断姿态测量单元的加速度传感器的输出是否达到设定的阈值，当姿态测量单元的加速度传感器的X轴分量大于设定的阈值axl时，认为被检测物体的俯仰角到达或者接近±90度左右，将造成万向节死锁问题；当姿态测量单元的加速度传感器的Y轴分量大于设定的阈值ayl时，认为被检测物体的横滚角到达或者接近±90度左右，将造成万向节死锁问题。
第五步中，修正姿态检测算法中的欧拉角计算方法，根据所述第四步中的判断，修正欧拉角计算公式：
其中，baxt，bayt，bazt分别为t时刻时姿态检测单元加速度传感器测量值在机体坐标系X轴，Y轴，Z轴上的分量；bmxt，bmyt，bmzt分别为t时刻时姿态检测单元磁力传感器测量值在机体坐标系X轴，Y轴，Z轴上的分量；分别为t时刻时欧拉角中的横滚角，俯仰角和航向角；分别为上一时刻(t-1)的横滚角和俯仰角；axl，ayl分别为设定的加速度传感器在X轴和Y轴上分量的阈值。
为了使本领域技术人员更清楚的明白本发明的技术方案，还提供了具体的实例，具体的可为：
操作者手持姿态检测单元，开启姿态检测单元的电源，将姿态检测单元绕机体坐标系X轴旋转大约正90度，然后绕着X轴旋转大约负90度；再将姿态检测单元绕机体坐标系Y轴旋转大约正90度，然后绕着Y轴旋转大约负90度；最后将姿态检测单元绕机体坐标系Z轴旋转大约正90度，然后绕着Z轴旋转大约负90度。图4为在进行上述操作时姿态检测单元的欧拉角输出值，可以明显的看到在虚线圆圈1、2、3和4的位置处，由于万向节死锁，横滚角、俯仰角和偏航角发生了畸变以及大幅度波动等现象。图5为使用本方法后的欧拉角曲线示意图，可以明显的看出，经过本方法后，在1、2、3和4处成功避免了万向节死锁问题，扩大了基于欧拉角的姿态检测范围，可以实现被测物体的全姿态测量，突显出本发明的实际应用价值。
本发明的技术方案不局限于所给出的参考坐标系以及应用场景，在实际过程中，由于姿态检测单元的应用场景及参考坐标系的选择是多种多样的，但根据姿态检测单元中加速度传感器的测量值进行判断是否发生万向节死锁问题，然后修正姿态检测算法中的欧拉角计算公式，这个避免万向节死锁问题的思路完全相同，因此本领域技术人员在本发明思路下针对具体应用场景以及参考坐标系的选取所做的修正仍属于本发明的技术方案。
以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (5)

1.一种基于欧拉角的姿态测量算法中避免万向节死锁的方法，其特征是：包括以下步骤：
(1)确定导航坐标系和机体坐标系；
(2)利用姿态检测单元中的加速度传感器与磁力传感器测量值计算姿态检测算法中的欧拉角；
(3)设定姿态测量单元的加速度传感器在X轴上的分量的阈值和在Y轴上的分量的阈值；
(4)判断姿态检测单元的加速度传感器的输出是否达到设定的阈值，并按照判断结果修正姿态检测算法中的欧拉角计算方法；
(5)按照修正后的欧拉角计算方法计算被检测物体的当前姿态。
2.如权利要求1所述的一种基于欧拉角的姿态测量算法中避免万向节死锁的方法，其特征是：所述步骤(1)中，选择北东地坐标系作为导航坐标系，机体坐标系遵循右手法则，机体坐标系原点与机体质心重合，X轴平行于机体轴线并指向机体前方，Y轴垂直于X轴并指向机体右方，Z轴垂直于XY平面并指向机体下方。
3.如权利要求1所述的一种基于欧拉角的姿态测量算法中避免万向节死锁的方法，其特征是：所述步骤(2)中，利用姿态检测单元中的加速度传感器与磁力传感器测量值计算姿态检测算法中的欧拉角，得到横滚角、俯仰角和航向角。
4.如权利要求1所述的一种基于欧拉角的姿态测量算法中避免万向节死锁的方法，其特征是：所述步骤(4)中，当姿态测量单元的加速度传感器的X轴分量大于设定的阈值时，认为被检测物体的俯仰角到达或者接近±90度时，将造成万向节死锁问题；当姿态测量单元的加速度传感器的Y轴大于设定的阈值时，认为被检测物体的横滚角到达或者接近±90度时，将造成万向节死锁问题。
5.如权利要求1所述的一种基于欧拉角的姿态测量算法中避免万向节死锁的方法，其特征是：所述步骤(4)中，当姿态检测单元在t时刻时加速度传感器测量值在机体坐标系X轴或Y轴的分量大于设定的阈值时，以前一时刻的欧拉角中的横滚角和俯仰角为当前时刻的横滚角和俯仰角。
CN201810026385.7A 2018-01-08 2018-01-08 一种基于欧拉角的姿态测量算法中避免万向节死锁的方法 Active CN108318035B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810026385.7A CN108318035B (zh) 2018-01-08 2018-01-08 一种基于欧拉角的姿态测量算法中避免万向节死锁的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810026385.7A CN108318035B (zh) 2018-01-08 2018-01-08 一种基于欧拉角的姿态测量算法中避免万向节死锁的方法

Publications (2)

Publication Number Publication Date
CN108318035A true CN108318035A (zh) 2018-07-24
CN108318035B CN108318035B (zh) 2020-09-08

Family

ID=62893968

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810026385.7A Active CN108318035B (zh) 2018-01-08 2018-01-08 一种基于欧拉角的姿态测量算法中避免万向节死锁的方法

Country Status (1)

Country Link
CN (1) CN108318035B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109115212A (zh) * 2018-10-30 2019-01-01 中国船舶重工集团公司第七0七研究所 一种惯导系统全范围姿态角提取方法
CN109945889A (zh) * 2019-04-17 2019-06-28 合肥工业大学 一种基于双姿态传感器的关节角度测量方法
WO2021134645A1 (zh) * 2019-12-31 2021-07-08 深圳市大疆创新科技有限公司 云台的控制方法和云台
CN113515201A (zh) * 2021-07-27 2021-10-19 北京字节跳动网络技术有限公司 光标位置更新方法、装置和电子设备
WO2022110034A1 (zh) * 2020-11-27 2022-06-02 深圳市大疆创新科技有限公司 云台控制方法、装置、云台及可移动平台
CN117091596A (zh) * 2023-10-09 2023-11-21 腾讯科技（深圳）有限公司 一种姿态信息获取方法以及相关设备

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101915580A (zh) * 2010-07-14 2010-12-15 中国科学院自动化研究所 一种基于微惯性和地磁技术的自适应三维姿态定位方法
US20120150294A1 (en) * 2005-08-09 2012-06-14 Charles Stark Draper Laboratory Multi-axis Tilt Estimation and Fall Remediation
CN103512584A (zh) * 2012-06-26 2014-01-15 北京赛佰特科技有限公司 导航姿态信息输出方法、装置及捷联航姿参考系统
CN104677358A (zh) * 2013-11-29 2015-06-03 哈尔滨恒誉名翔科技有限公司 一种微捷联航姿系统全姿态控制器
CN105403900A (zh) * 2015-11-17 2016-03-16 广州南方卫星导航仪器有限公司 一种快速自动测量方法
CN107014386A (zh) * 2017-06-02 2017-08-04 武汉云衡智能科技有限公司 一种飞行器姿态解算的干扰加速度测量方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120150294A1 (en) * 2005-08-09 2012-06-14 Charles Stark Draper Laboratory Multi-axis Tilt Estimation and Fall Remediation
CN101915580A (zh) * 2010-07-14 2010-12-15 中国科学院自动化研究所 一种基于微惯性和地磁技术的自适应三维姿态定位方法
CN103512584A (zh) * 2012-06-26 2014-01-15 北京赛佰特科技有限公司 导航姿态信息输出方法、装置及捷联航姿参考系统
CN104677358A (zh) * 2013-11-29 2015-06-03 哈尔滨恒誉名翔科技有限公司 一种微捷联航姿系统全姿态控制器
CN105403900A (zh) * 2015-11-17 2016-03-16 广州南方卫星导航仪器有限公司 一种快速自动测量方法
CN107014386A (zh) * 2017-06-02 2017-08-04 武汉云衡智能科技有限公司 一种飞行器姿态解算的干扰加速度测量方法

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109115212A (zh) * 2018-10-30 2019-01-01 中国船舶重工集团公司第七0七研究所 一种惯导系统全范围姿态角提取方法
CN109945889A (zh) * 2019-04-17 2019-06-28 合肥工业大学 一种基于双姿态传感器的关节角度测量方法
CN109945889B (zh) * 2019-04-17 2023-07-25 合肥工业大学 一种基于双姿态传感器的关节角度测量方法
WO2021134645A1 (zh) * 2019-12-31 2021-07-08 深圳市大疆创新科技有限公司 云台的控制方法和云台
WO2022110034A1 (zh) * 2020-11-27 2022-06-02 深圳市大疆创新科技有限公司 云台控制方法、装置、云台及可移动平台
CN113515201A (zh) * 2021-07-27 2021-10-19 北京字节跳动网络技术有限公司 光标位置更新方法、装置和电子设备
CN113515201B (zh) * 2021-07-27 2024-03-19 北京字节跳动网络技术有限公司 光标位置更新方法、装置和电子设备
CN117091596A (zh) * 2023-10-09 2023-11-21 腾讯科技（深圳）有限公司 一种姿态信息获取方法以及相关设备

Also Published As

Publication number Publication date
CN108318035B (zh) 2020-09-08

Similar Documents

Publication Publication Date Title
CN108318035A (zh) 一种基于欧拉角的姿态测量算法中避免万向节死锁的方法
CN107000839B (zh) 无人机的控制方法、装置、设备和无人机的控制系统
CN104097791B (zh) 一种基于磁敏感器和星敏感器的全姿态捕获方法及其装置
CN104635730B (zh) 一种机器人自主充电方法
CN103776446A (zh) 一种基于双mems-imu的行人自主导航解算算法
CN105718727B (zh) 一种估计平流层飞艇飞行性能参数的方法和系统
CN106595578A (zh) 一种基于多传感器信息融合的无人机高度测量方法及系统
CN206417213U (zh) 云台和无人飞行器
CN108170297A (zh) 实时六自由度vr/ar/mr设备定位方法
CN103862465B (zh) 多关节机械臂坐标校正方法和装置
CN107390704A (zh) 一种基于imu姿态补偿的多旋翼无人机光流悬停方法
CN106842905A (zh) Pid控制方法及装置
CN106933223A (zh) 一种机器人自主导航方法及系统
US7616130B2 (en) Method and a device for processing and displaying aircraft piloting information
CN106095103A (zh) 虚拟现实的显示控制方法、装置及相关设备
CN106443062B (zh) 无人机速度测量方法、装置及无人机
CN106990836A (zh) 一种头戴式人体学输入设备空间位置及姿态测量方法
CN105539883A (zh) 一种基于矢量匹配的多敏感器星上自主互校验方法
CN106468563A (zh) 一种机载磁传感器在线标定方法
CN107782304A (zh) 移动机器人的定位方法及装置、移动机器人及存储介质
CN105004368B (zh) 一种自主机器人的碰撞检测方法、装置及系统
CN106217374B (zh) 一种智能机械臂的控制方法、装置及系统
CN105459116A (zh) 一种基于磁强计的机器人遥操作装置及方法
CN107702710A (zh) 一种多陀螺表头常值漂移实时估计方法
CN113093296B (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