CN106959110B - 一种云台姿态检测方法及装置 - Google Patents

一种云台姿态检测方法及装置 Download PDF

Info

Publication number
CN106959110B
CN106959110B CN201710220939.2A CN201710220939A CN106959110B CN 106959110 B CN106959110 B CN 106959110B CN 201710220939 A CN201710220939 A CN 201710220939A CN 106959110 B CN106959110 B CN 106959110B
Authority
CN
China
Prior art keywords
data
expression
holder
measurement unit
unmanned aerial
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
CN201710220939.2A
Other languages
English (en)
Other versions
CN106959110A (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.)
Ehang Intelligent Equipment Guangzhou Co Ltd
Original Assignee
Ehang Intelligent Equipment Guangzhou 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 Ehang Intelligent Equipment Guangzhou Co Ltd filed Critical Ehang Intelligent Equipment Guangzhou Co Ltd
Priority to CN201710220939.2A priority Critical patent/CN106959110B/zh
Publication of CN106959110A publication Critical patent/CN106959110A/zh
Priority to PCT/CN2018/079517 priority patent/WO2018184467A1/zh
Application granted granted Critical
Publication of CN106959110B publication Critical patent/CN106959110B/zh
Active 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
    • 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
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

本发明涉及无人机技术领域，公开了一种云台姿态检测方法及装置，该方法包括：获取云台的惯性测量单元的数据，所述惯性测量单元的数据包括陀螺仪的角速度数据和加速度计数据；对所述惯性测量单元的数据进行四元数积分运算；将所述惯性测量单元的数据由四元数表述转换成旋转矩阵表述；将所述惯性测量单元的数据由旋转矩阵表述转换成欧拉角表述；其中，所述对所述惯性测量单元的数据进行四元数积分运算包括：以所述旋转矩阵表述为反馈，对所述四元数表述进行反馈校正，通过姿态航向系统获取云台姿态数据，采用不同的算法进行云台姿态表述，并对云台姿态进行补偿和校正，提高了云台姿态检测的准确性。

Description

一种云台姿态检测方法及装置
技术领域
本发明涉及无人机技术领域，尤其涉及一种云台姿态检测方法及装置。
背景技术
随着控制理论的不断发展，无人飞行器也受到各国研究者的关注，世界上产生了各式各样的飞行器，对于其中机械结构简单、占地体积小旋翼类飞行器尤为引人关注。目前机载云台的姿态表述方法可分为：欧拉角、四元数和旋转矩阵，四元数避免了欧拉角所带有的奇异性问题，但欧拉角表述姿态更直观；四元数与旋转矩阵相比，旋转矩阵需要9个变量来表述姿态，四元数只需要更少的4个变量来表述姿态，四元数的积分运算及归一化运算比旋转矩阵更方便，但四元数在进行向量旋转的时候没有旋转矩阵方便。
发明内容
本发明的主要目的在于提出一种云台姿态检测方法及装置，能够通过姿态航向系统获取云台姿态数据，采用不同的算法进行云台姿态表述，并对云台姿态进行补偿和校正，提高了云台姿态检测的准确性。
为实现上述目的，本发明提供的一种云台姿态检测方法，包括：
获取云台的惯性测量单元的数据，所述惯性测量单元的数据包括陀螺仪的角速度数据和加速度计数据；
对所述惯性测量单元的数据进行四元数积分运算；
将所述惯性测量单元的数据由四元数表述转换成旋转矩阵表述；
将所述惯性测量单元的数据由旋转矩阵表述转换成欧拉角表述；
其中，所述对所述惯性测量单元的数据进行四元数积分运算包括：
以所述旋转矩阵表述为反馈，对所述四元数表述进行反馈校正。
可选地，所述陀螺仪的角速度数据包括机体三轴角速度。
可选地，所述以所述旋转矩阵表述为反馈，对所述四元数表述进行反馈校正包括：
将所述旋转矩阵表述与大地坐标系中的重力加速度进行乘积运算，得到机体坐标系下的加速度数据；
将所述机体坐标系下的加速度数据与所述加速度计数据进行交叉乘积运算，得到旋转差向量；
对所述旋转差向量进行放大，作为校准角速度参与云台姿态数据的四元数积分运算。
可选地，所述将所述旋转矩阵表述与大地坐标系中的重力加速度进行乘积运算，得到机体坐标系下的加速度数据包括：
通过无人机导航系统获取实时加速度数据，将所述实时加速度数据与所述大地坐标系中的重力加速度进行加运算，得到补偿后的加速度数据；
将所述旋转矩阵表述与所述补偿后的加速度数据进行乘积运算，得到机体坐标系下的加速度数据。
可选地，对所述惯性测量单元的数据进行四元数积分运算之前包括：
获取无人机偏航角，通过磁编码器计算无人机偏航角与云台偏航角的偏差，从而得到云台偏航角；
以所述欧拉角表述为反馈，对所述云台偏航角进行差值运算，以对所述云台偏航角进行纠偏。
作为本发明的另一方面，提供的一种云台姿态检测装置，其特征在于，包括：
姿态数据获取模块，用于获取云台的惯性测量单元的数据，所述惯性测量单元的数据包括陀螺仪的角速度数据和加速度计数据；
四元数积分模块，用于对所述惯性测量单元的数据进行四元数积分运算；
旋转矩阵模块，用于将所述惯性测量单元的数据由四元数表述转换成旋转矩阵表述；
欧拉角模块，用于将所述惯性测量单元的数据由旋转矩阵表述转换成欧拉角表述；
反馈校正模块，用于以所述旋转矩阵表述为反馈，对所述四元数表述进行反馈校正。
可选地，所述陀螺仪的角速度数据包括机体三轴角速度。
可选地，所述反馈校正模块包括：
坐标系转换单元，用于将所述旋转矩阵表述与大地坐标系中的重力加速度进行乘积运算，得到机体坐标系下的加速度数据；
校正单元，用于将所述机体坐标系下的加速度数据与所述加速度计数据进行交叉乘积运算，得到旋转差向量；
放大更新单元，用于对所述旋转差向量进行放大，作为校准角速度参与云台姿态数据的四元数积分运算。
可选地，所述坐标系转换单元包括：
补偿单元，用于通过无人机导航系统获取实时加速度数据，将所述实时加速度数据与所述大地坐标系中的重力加速度进行加运算，得到补偿后的加速度数据；
乘积运算单元，用于将所述旋转矩阵表述与所述补偿后的加速度数据进行乘积运算，得到机体坐标系下的加速度数据。
可选地，还包括：
偏航角纠偏模块，用于获取无人机偏航角，通过磁编码器计算无人机偏航角与云台偏航角的偏差，从而得到云台偏航角；以所述欧拉角表述为反馈，对所述云台偏航角进行差值运算，以对所述云台偏航角进行纠偏。
本发明提出的一种云台姿态检测方法及装置，该方法包括：获取云台的惯性测量单元的数据，所述惯性测量单元的数据包括陀螺仪的角速度数据和加速度计数据；对所述惯性测量单元的数据进行四元数积分运算；将所述惯性测量单元的数据由四元数表述转换成旋转矩阵表述；将所述惯性测量单元的数据由旋转矩阵表述转换成欧拉角表述；其中，所述对所述惯性测量单元的数据进行四元数积分运算包括：以所述旋转矩阵表述为反馈，对所述四元数表述进行反馈校正，通过姿态航向系统获取云台姿态数据，采用不同的算法进行云台姿态表述，并对云台姿态进行补偿和校正，提高了云台姿态检测的准确性，相比于卡尔曼滤波或其他姿态融合算法，该算法具有运算量少的优点，适合于运算资源不多的云台系统，有助于云台的小型化；相比于只利用陀螺仪和加速度计进行姿态解算的云台姿态解算方法，该算法充分利用了云台系统所具有的的硬件资源，包括利用了云台电机板的磁编码器数据、飞控的航向角数据、飞控的加速度数据，可获得更精确的姿态数据。
附图说明
图1为本发明实施例一提供的一种云台姿态检测方法流程图；
图2为本发明实施例一提供的机载云台系统硬件结构示意图；
图3为图1中步骤S21的方法流程图；
图4为本发明实施例一提供的云台姿态检测方法的数据处理流程图；
图5为本发明实施例一提供的加速度补偿的数据处理流程图；
图6为本发明实施例一提供的偏航角校正的数据处理流程图；
图7为本发明实施例二提供的一种云台姿态检测装置示范性结构框图；
图8为图7中反馈校正模块的示范性结构框图；
图9为本发明实施例二提供的另一种云台姿态检测装置示范性结构框图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
应当理解，此处所表述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
在后续的表述中，使用用于表示元件的诸如“模块”、“部件”或“单元”的后缀仅为了有利于本发明的说明，其本身并没有特定的意义。因此，"模块"与"部件"可以混合地使用。
实施例一
如图1所示，在本实施例中，一种云台姿态检测方法，包括：
S10、获取云台的惯性测量单元的数据，所述惯性测量单元的数据包括陀螺仪的角速度数据和加速度计数据；
S20、对所述惯性测量单元的数据进行四元数积分运算；
S30、将所述惯性测量单元的数据由四元数表述转换成旋转矩阵表述；
S40、将所述惯性测量单元的数据由旋转矩阵表述转换成欧拉角表述；
其中，所述方法还包括：
S21、以所述旋转矩阵表述为反馈，对所述四元数表述进行反馈校正。
在本实施例中，通过姿态航向系统获取云台姿态数据，所述云台姿态数据包括惯性测量单元的数据，采用不同的算法进行云台姿态表述，并对云台姿态进行补偿和校正，提高了云台姿态检测的准确性，相比于卡尔曼滤波或其他姿态融合算法，该算法具有运算量少的优点，适合于运算资源不多的云台系统，有助于云台的小型化；相比于只利用陀螺仪和加速度计进行姿态解算的云台姿态解算方法，该算法充分利用了云台系统所具有的的硬件资源，包括利用了云台电机板的磁编码器数据、飞控的航向角数据、飞控的加速度数据，可获得更精确的姿态数据。
在本实施例中，主要适用于机载云台系统，该系统硬件图如图2所示，包括：云台、飞控系统、无人机、磁编码器和电机M，其中，云台姿态检测设备选用MPU6000；飞控系统中设置有姿态航向系统AHRS(Attitude and Heading Reference System)，所述姿态航向系统包括陀螺仪和加速度计，用于获取机体三轴角速度及加速度数据；所述云台通过控制器局域网络CAN(Controller Area Network)与无人机及飞控系统进行通信；所述云台通过串行外设接口SPI(Serial Peripheral Interface)与磁编码器及MPU6000进行通信。
在本实施例中，四元数算法只需要四个变量就能表述云台姿态，比如：三轴角速度，适用于云台姿态的短期表述；而旋转矩阵的表述方法在进行向量旋转时比较方便；但欧拉角的姿态表述更为直观，适用于云台姿态的对外输出，如输出给姿态控制器及上位机。
在本实施例中，陀螺仪检测到的是机体角速度信息，响应速度快，但是会受到零点随温度漂移产生的积分干扰和积分漂移，在陀螺仪检测到机体坐标系三轴的角速度之后，可以利用四元数积分法简单快速的计算出实时姿态。
在本实施例中，四元数与旋转矩阵的转换公式为：
Figure GDA0002501708860000051
其中，qx、qy、qz、qw为四元数算法的四个变量，R(q)为旋转矩阵。
在本实施例中，加速度传感器用于测量支持力向量，在静止时，测量得到的支持力矢量与重力加速度矢量方向相反，大小相等，响应速度也非常高，但是会受到机体振动等引起的高频干扰；结合两个姿态传感器的特点进行考虑，姿态解算的总体思想为：陀螺仪积分，以获得高响应性能；长时间内利用加速度计融合校正，以纠正积分漂移。
如图3所示，在本实施例中，所述步骤S21包括：
S211、将所述旋转矩阵表述与大地坐标系中的重力加速度进行乘积运算，得到机体坐标系下的加速度数据；
S212、将所述机体坐标系下的加速度数据与所述加速度计数据进行交叉乘积运算，得到旋转差向量；
S213、对所述旋转差向量进行放大，作为校准角速度参与云台姿态数据的四元数积分运算。
在本实施例中，所述云台姿态检测方法的数据处理流程图如图4所示，其中，模块1表示陀螺仪的角速度数据，模块2表示加速度计数据，Gef为大地坐标系中的重力加速度。
在本实施例中，所述步骤S211包括：
通过无人机导航系统获取实时加速度数据，将所述实时加速度数据与所述大地坐标系中的重力加速度进行加运算，得到补偿后的加速度数据；
将所述旋转矩阵表述与所述补偿后的加速度数据进行乘积运算，得到机体坐标系下的加速度数据。
在本实施例中，大地坐标系是地球坐标系，机体坐标系是云台本身的坐标系，加速度计数据为机体坐标系下测量到的云台加速度值，为Gbf+abf，此时的加速度数据除了带有重力加速度Gbf外，还带有线加速度abf；而大地坐标系中的重力加速度是一个常数，表示为Gef＝g＝9.8m/s^2。
在步骤S211中，因为云台加速度计实际测量本身存在偏差，为了得云台加速度计实际测量不断的趋近正确的值，把云台姿态的旋转矩阵当作一个负反馈，以将云台最后输出来的姿态更加准确。
在本实施例中，云台在进行线性加速度运动时，加速度计检测值为Gbf+abf，此时的加速度数据除了带有重力加速度Gbf外，还带有线加速度abf，此时仍然使用加速度数据进行姿态融合的话，将会导致姿态检测出现较大偏差，普遍的方法是调整加速度的融合权重值到足够小(过小将影响校正陀螺仪的效果)，以减少高频的线加速度abf(比如震动带来的高频线加速度)对姿态检测的影响。但当无人机进行低频的线加速运动时(长时间的向某一方向加速运动或刷锅动作等)，以上方法将不能过滤掉线加速度abf导致的姿态检测误差。针对以上问题，本方案一方面通过CAN把无人机机导航系统的解算的真实加速度数据auav发送给云台进行加速度补偿，无人机具有定位传感器包括GPS、气压计、光流传感器以及超声波等，所解算的加速度数据auav误差更小。另一方面，动态计算补偿后的加速度模长，并与重力加速度权重对比，评估补偿后的加速度数据的可信度，并根据可信度动态调整加速度融合权重，以提高对加速度计噪音的过滤效果。
在本实施例中，增加了加速度补偿后的云台姿态检测方法的数据处理流程图如图5所示，其中，模块3表示无人机导航系统获取实时加速度数据。
在本实施例中，所述云台姿态数据还包括偏航角，相应地，对所述惯性测量单元的数据进行四元数积分运算之前包括：
获取无人机偏航角，通过磁编码器计算无人机偏航角与云台偏航角的偏差，从而得到云台偏航角；
以所述欧拉角表述为反馈，对所述云台偏航角进行差值运算，以对所述云台偏航角进行纠偏。
机载云台由于大小与成本的限制，除了陀螺仪外并不装配磁罗盘等其他可直接测量云台偏航角的传感器器件。因此以上方法搭建的AHRS系统所解算出来的偏航角不指北，并且由于陀螺仪的温漂及积分误差的原因，将会导致偏航角yaw发生缓慢漂移，最终导致云台在锁头状态时(云台稳定在大地上的某一偏航角)，所拍摄的画面将会发生持续缓慢的偏移。针对以上问题，本方案采用现有传感器，包括陀螺仪、加速度计、磁编码器以及无人机的传感器信息，进行偏航角的融合校正。
在本实施例中，增加了偏航角融合校正后的云台姿态检测方法的数据处理流程图如图6所示，其中，模块4表示通过磁编码器计算得到的云台偏航角。
其中，无人机与云台的偏差角yawoffset_from_UAV_to_gimbal的计算方法是：云台与无人机的姿态关系为：
Figure GDA0002501708860000071
因为我们只需要计算无人机相对于云台的航向偏差，故可认为云台的航向为0，因此令：
Rgimbal＝RψRθ
无人机的对于云台的相对航向为：
yawoffset_from_UAV_to_gimbal＝atan2(-RUAV12，RUAV22)
各轴的旋转矩阵为：
Figure GDA0002501708860000081
Figure GDA0002501708860000082
Figure GDA0002501708860000083
Figure GDA0002501708860000084
实施例二
如图7所示，在本实施例中，一种云台姿态检测装置，包括：
姿态数据获取模块10，用于获取云台的惯性测量单元的数据，所述惯性测量单元的数据包括陀螺仪的角速度数据和加速度计数据；
四元数积分模块20，用于对所述惯性测量单元的数据进行四元数积分运算；
旋转矩阵模块30，用于将所述惯性测量单元的数据由四元数表述转换成旋转矩阵表述；
欧拉角模块40，用于将所述惯性测量单元的数据由旋转矩阵表述转换成欧拉角表述；
反馈校正模块50，用于以所述旋转矩阵表述为反馈，对所述四元数表述进行反馈校正。
在本实施例中，通过姿态航向系统获取云台姿态数据，所述云台姿态数据包括惯性测量单元的数据，采用不同的算法进行云台姿态表述，并对云台姿态进行补偿和校正，提高了云台姿态检测的准确性，相比于卡尔曼滤波或其他姿态融合算法，该算法具有运算量少的优点，适合于运算资源不多的云台系统，有助于云台的小型化；相比于只利用陀螺仪和加速度计进行姿态解算的云台姿态解算方法，该算法充分利用了云台系统所具有的的硬件资源，包括利用了云台电机板的磁编码器数据、飞控的航向角数据、飞控的加速度数据，可获得更精确的姿态数据。
在本实施例中，主要适用于机载云台系统，该系统硬件图如图2所示，包括：云台、飞控系统、无人机、磁编码器和电机M，其中，云台姿态检测设备选用MPU6000；飞控系统中设置有姿态航向系统AHRS(Attitude and Heading Reference System)，所述姿态航向系统包括陀螺仪和加速度计，用于获取机体三轴角速度及加速度数据；所述云台通过控制器局域网络CAN(Controller Area Network)与无人机及飞控系统进行通信；所述云台通过串行外设接口SPI(Serial Peripheral Interface)与磁编码器及MPU6000进行通信。
在本实施例中，四元数算法只需要四个变量就能表述云台姿态，比如：三轴角速度，适用于云台姿态的短期表述；而旋转矩阵的表述方法在进行向量旋转时比较方便；但欧拉角的姿态表述更为直观，适用于云台姿态的对外输出，如输出给姿态控制器及上位机。
在本实施例中，陀螺仪检测到的是机体角速度信息，响应速度快，但是会受到零点随温度漂移产生的积分干扰和积分漂移，在陀螺仪检测到机体坐标系三轴的角速度之后，可以利用四元数积分法简单快速的计算出实时姿态。
在本实施例中，四元数与旋转矩阵的转换公式为：
Figure GDA0002501708860000091
其中，qx、qy、qz、qw为四元数算法的四个变量，R(q)为旋转矩阵。
在本实施例中，加速度传感器用于测量支持力向量，在静止时，测量得到的支持力矢量与重力加速度矢量方向相反，大小相等，响应速度也非常高，但是会受到机体振动等引起的高频干扰；结合两个姿态传感器的特点进行考虑，姿态解算的总体思想为：陀螺仪积分，以获得高响应性能；长时间内利用加速度计融合校正，以纠正积分漂移。
如图8所示，在本实施例中，所述反馈校正模块包括：
坐标系转换单元51，用于将所述旋转矩阵表述与大地坐标系中的重力加速度进行乘积运算，得到机体坐标系下的加速度数据；
校正单元52，用于将所述机体坐标系下的加速度数据与所述加速度计数据进行交叉乘积运算，得到校正后的加速度计数据；
放大更新单元53，用于对所述校正后的加速度计数据进行放大，作为更新后的加速度计数据参与云台姿态数据的四元数积分运算。
在本实施例中，所述云台姿态检测方法的数据处理流程图如图4所示，其中，模块1表示陀螺仪的角速度数据，模块2表示加速度计数据，Gef为大地坐标系中的重力加速度。
在本实施例中，所述坐标系转换单元包括：
补偿单元，用于通过无人机导航系统获取实时加速度数据，将所述实时加速度数据与所述大地坐标系中的重力加速度进行加运算，得到补偿后的加速度数据；
乘积运算单元，用于将所述旋转矩阵表述与所述补偿后的加速度数据进行乘积运算，得到机体坐标系下的加速度数据。
在本实施例中，大地坐标系是地球坐标系，机体坐标系是云台本身的坐标系，加速度计数据为机体坐标系下测量到的云台加速度值，为Gbf+abf，此时的加速度数据除了带有重力加速度Gbf外，还带有线加速度abf；而大地坐标系中的重力加速度是一个常数，表示为Gef＝g＝9.8m/s^2。
因为云台加速度计实际测量本身存在偏差，为了得云台加速度计实际测量不断的趋近正确的值，把云台姿态的旋转矩阵当作一个负反馈，以将云台最后输出来的姿态更加准确。
在本实施例中，云台在进行线性加速度运动时，加速度计检测值为Gbf+abf，此时的加速度数据除了带有重力加速度Gbf外，还带有线加速度abf，此时仍然使用加速度数据进行姿态融合的话，将会导致姿态检测出现较大偏差，普遍的方法是调整加速度的融合权重值到足够小(过小将影响校正陀螺仪的效果)，以减少高频的线加速度abf(比如震动带来的高频线加速度)对姿态检测的影响。但当无人机进行低频的线加速运动时(长时间的向某一方向加速运动或刷锅动作等)，以上方法将不能过滤掉线加速度abf导致的姿态检测误差。针对以上问题，本方案一方面通过CAN把无人机机导航系统的解算的真实加速度数据auav发送给云台进行加速度补偿，无人机具有定位传感器包括GPS、气压计、光流传感器以及超声波等，所解算的加速度数据auav误差更小。另一方面，动态计算补偿后的加速度模长，并与重力加速度权重对比，评估补偿后的加速度数据的可信度，并根据可信度动态调整加速度融合权重，以提高对加速度计噪音的过滤效果。
在本实施例中，增加了加速度补偿后的云台姿态检测方法的数据处理流程图如图5所示，其中，模块3表示无人机导航系统获取实时加速度数据。
如图9所示，在本实施例中，所述云台姿态数据还包括偏航角，相应地，所述云台姿态检测装置还包括：
偏航角纠偏模块60，用于获取无人机偏航角，通过磁编码器计算无人机偏航角与云台偏航角的偏差，从而得到云台偏航角；以所述欧拉角表述为反馈，对所述云台偏航角进行差值运算，以对所述云台偏航角进行纠偏。
机载云台由于大小与成本的限制，除了陀螺仪外并不装配磁罗盘等其他可直接测量云台偏航角的传感器器件。因此以上方法搭建的AHRS系统所解算出来的偏航角不指北，并且由于陀螺仪的温漂及积分误差的原因，将会导致偏航角yaw发生缓慢漂移，最终导致云台在锁头状态时(云台稳定在大地上的某一偏航角)，所拍摄的画面将会发生持续缓慢的偏移。针对以上问题，本方案采用现有传感器，包括陀螺仪、加速度计、磁编码器以及无人机的传感器信息，进行偏航角的融合校正。
在本实施例中，增加了偏航角融合校正后的云台姿态检测方法的数据处理流程图如图6所示，其中，模块4表示通过磁编码器计算得到的云台偏航角。
其中，无人机与云台的偏差角yawoffset_from_UAV_to_gimbal的计算方法是：云台与无人机的姿态关系为：
Figure GDA0002501708860000111
因为我们只需要计算无人机相对于云台的航向偏差，故可认为云台的航向为0，因此令：
Rgimbal＝RψRθ
无人机的对于云台的相对航向为：
yawoffset-from_UAV_to_gimbal＝atan2(-RUAV12，RUAV22)
各轴的旋转矩阵为：
Figure GDA0002501708860000112
Figure GDA0002501708860000121
Figure GDA0002501708860000122
Figure GDA0002501708860000123
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
上述本发明实施例序号仅仅为了表述，不代表实施例的优劣。
通过以上的实施方式的表述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (4)

1.一种云台姿态检测方法，其特征在于，包括：
获取云台的惯性测量单元的数据，所述惯性测量单元的数据包括陀螺仪的角速度数据和加速度计数据；
对所述惯性测量单元的数据进行四元数积分运算；
将所述惯性测量单元的数据由四元数表述转换成旋转矩阵表述；
将所述惯性测量单元的数据由旋转矩阵表述转换成欧拉角表述；
其中，所述对所述惯性测量单元的数据进行四元数积分运算包括：
以所述旋转矩阵表述为反馈，对所述四元数表述进行反馈校正；
其中，对所述惯性测量单元的数据进行四元数积分运算之前包括：
获取无人机偏航角，通过磁编码器计算无人机偏航角与云台偏航角的偏差，从而得到云台偏航角；
以所述欧拉角表述为反馈，对所述云台偏航角进行差值运算，以对所述云台偏航角进行纠偏；
其中，所述以所述旋转矩阵表述为反馈，对所述四元数表述进行反馈校正包括：
将所述旋转矩阵表述与大地坐标系中的重力加速度进行乘积运算，得到无人机机体坐标系下的加速度数据；
将所述无人机机体坐标系下的加速度数据与所述加速度计数据进行交叉乘积运算，得到旋转差向量；
对所述旋转差向量进行放大，作为校准角速度参与云台姿态数据的四元数积分运算；
其中，所述将所述旋转矩阵表述与大地坐标系中的重力加速度进行乘积运算，得到无人机机体坐标系下的加速度数据包括：
通过无人机导航系统获取实时加速度数据，将所述实时加速度数据与所述大地坐标系中的重力加速度进行加运算，得到补偿后的加速度数据；
将所述旋转矩阵表述与所述补偿后的加速度数据进行乘积运算，得到无人机机体坐标系下的加速度数据。
2.根据权利要求1所述的一种云台姿态检测方法，其特征在于，所述陀螺仪的角速度数据包括无人机机体三轴角速度。
3.一种云台姿态检测装置，其特征在于，包括：
姿态数据获取模块，用于获取云台的惯性测量单元的数据，所述惯性测量单元的数据包括陀螺仪的角速度数据和加速度计数据；
四元数积分模块，用于对所述惯性测量单元的数据进行四元数积分运算；
旋转矩阵模块，用于将所述惯性测量单元的数据由四元数表述转换成旋转矩阵表述；
欧拉角模块，用于将所述惯性测量单元的数据由旋转矩阵表述转换成欧拉角表述；
反馈校正模块，用于以所述旋转矩阵表述为反馈，对所述四元数表述进行反馈校正；
偏航角纠偏模块，用于获取无人机偏航角，通过磁编码器计算无人机偏航角与云台偏航角的偏差，从而得到云台偏航角；以所述欧拉角表述为反馈，对所述云台偏航角进行差值运算，以对所述云台偏航角进行纠偏；
其中，所述反馈校正模块包括：
坐标系转换单元，用于将所述旋转矩阵表述与大地坐标系中的重力加速度进行乘积运算，得到无人机机体坐标系下的加速度数据；
校正单元，用于将所述无人机机体坐标系下的加速度数据与所述加速度计数据进行交叉乘积运算，得到旋转差向量；
放大更新单元，用于对所述旋转差向量进行放大，作为校准角速度参与云台姿态数据的四元数积分运算；
其中，所述坐标系转换单元包括：
补偿单元，用于通过无人机导航系统获取实时加速度数据，将所述实时加速度数据与所述大地坐标系中的重力加速度进行加运算，得到补偿后的加速度数据；
乘积运算单元，用于将所述旋转矩阵表述与所述补偿后的加速度数据进行乘积运算，得到无人机机体坐标系下的加速度数据。
4.根据权利要求3所述的一种云台姿态检测装置，其特征在于，所述陀螺仪的角速度数据包括无人机机体三轴角速度。
CN201710220939.2A 2017-04-06 2017-04-06 一种云台姿态检测方法及装置 Active CN106959110B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201710220939.2A CN106959110B (zh) 2017-04-06 2017-04-06 一种云台姿态检测方法及装置
PCT/CN2018/079517 WO2018184467A1 (zh) 2017-04-06 2018-03-20 一种云台姿态检测方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710220939.2A CN106959110B (zh) 2017-04-06 2017-04-06 一种云台姿态检测方法及装置

Publications (2)

Publication Number Publication Date
CN106959110A CN106959110A (zh) 2017-07-18
CN106959110B true CN106959110B (zh) 2020-08-11

Family

ID=59483992

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710220939.2A Active CN106959110B (zh) 2017-04-06 2017-04-06 一种云台姿态检测方法及装置

Country Status (2)

Country Link
CN (1) CN106959110B (zh)
WO (1) WO2018184467A1 (zh)

Families Citing this family (33)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106959110B (zh) * 2017-04-06 2020-08-11 亿航智能设备（广州）有限公司 一种云台姿态检测方法及装置
CN108521814B (zh) 2017-09-12 2021-10-15 深圳市大疆灵眸科技有限公司 云台的控制方法、控制器和云台
CN107807680B (zh) * 2017-09-21 2020-06-12 中国科学院长春光学精密机械与物理研究所 一种云台漂移补偿方法
CN109313455B (zh) * 2017-11-16 2021-09-28 深圳市大疆创新科技有限公司 智能眼镜及其控制云台的方法、云台、控制方法和无人机
CN108061855B (zh) * 2017-11-30 2020-05-08 天津大学 一种基于mems传感器的球形电机转子位置检测方法
WO2019119441A1 (zh) 2017-12-22 2019-06-27 深圳市大疆创新科技有限公司 体感控制器控制云台的方法、云台、体感控制器和系统
CN109074103B (zh) * 2017-12-23 2021-12-24 深圳市大疆创新科技有限公司 一种云台校准方法及云台设备
WO2019126958A1 (zh) * 2017-12-25 2019-07-04 深圳市大疆创新科技有限公司 偏航姿态控制方法、无人机、计算机可读存储介质
CN108491001A (zh) * 2018-03-21 2018-09-04 深圳臻迪信息技术有限公司 增稳云台、增稳云台实现方法及无人机系统
CN108762324A (zh) * 2018-05-23 2018-11-06 深圳市道通智能航空技术有限公司 云台电机角度和角速度估算方法、装置、云台及飞行器
CN110337624A (zh) * 2018-05-31 2019-10-15 深圳市大疆创新科技有限公司 姿态转换方法、姿态显示方法及云台系统
CN110622090A (zh) * 2018-06-05 2019-12-27 深圳市大疆创新科技有限公司 云台及其校准方法、无人机和计算设备
CN109000612B (zh) * 2018-06-19 2020-11-27 深圳市道通智能航空技术有限公司 设备的角度估算方法、装置、摄像组件及飞行器
CN110793515A (zh) * 2018-08-02 2020-02-14 哈尔滨工业大学 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
CN110873563B (zh) * 2018-08-30 2022-03-08 杭州海康机器人技术有限公司 一种云台姿态估计方法及装置
CN114578841A (zh) * 2018-10-31 2022-06-03 深圳市道通智能航空技术股份有限公司 无人机及其云台控制方法
CN109691992A (zh) * 2019-03-04 2019-04-30 深圳星脉医疗仪器有限公司 一种血压检测信号的修正方法和血压检测装置
CN111279165A (zh) * 2019-03-13 2020-06-12 深圳市大疆创新科技有限公司 云台振动调节的方法、云台以及客户端
CN110086973B (zh) * 2019-05-10 2020-11-27 中国计量大学 一种基于光流相机的云台稳像系统
CN110553669B (zh) * 2019-09-30 2022-03-29 睿魔智能科技（深圳）有限公司 云台校准方法及校准系统
CN110674888B (zh) * 2019-10-11 2022-04-05 中国人民解放军海军航空大学青岛校区 一种基于数据融合的头部姿态识别方法
CN111654634B (zh) * 2020-06-24 2022-02-08 杭州海康威视数字技术股份有限公司 确定摄像机中机芯组件和云台组件倾斜的方法、摄像机
CN111770270B (zh) * 2020-06-24 2021-06-25 杭州海康威视数字技术股份有限公司 一种相机姿态的校正方法、摄像机
CN113795798A (zh) * 2020-07-20 2021-12-14 深圳市大疆创新科技有限公司 云台及其性能的评估方法及装置、可移动平台
CN112188037B (zh) * 2020-09-24 2023-03-24 影石创新科技股份有限公司 生成陀螺仪旋转方向的方法及计算机设备
CN112414365B (zh) * 2020-12-14 2022-08-16 广州昂宝电子有限公司 位移补偿方法和设备及速度补偿方法和设备
CN114789439B (zh) * 2021-01-26 2024-03-19 深圳市普渡科技有限公司 斜坡定位校正方法、装置、机器人及可读存储介质
CN113325865B (zh) * 2021-05-10 2024-05-28 哈尔滨理工大学 一种无人机控制方法、控制装置及控制系统
CN113406964B (zh) * 2021-05-19 2022-11-18 浙江华飞智能科技有限公司 运动参数调节方法、装置、存储介质及电子装置
CN113359867B (zh) * 2021-06-07 2022-01-28 合肥工业大学 一种无人机自稳云台及控制方法
CN113568438B (zh) * 2021-07-02 2024-08-23 杭州海康威视数字技术股份有限公司 一种姿态角的恢复方法、装置及设备
CN114545017A (zh) * 2022-01-31 2022-05-27 深圳市云鼠科技开发有限公司 基于光流和加速度计的速度融合方法、装置和计算机设备
CN115293299B (zh) * 2022-10-08 2023-01-24 中科物栖（北京）科技有限责任公司 人体姿态特征实时检测方法、装置、设备及介质

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101956186B1 (ko) * 2011-04-27 2019-03-11 삼성전자주식회사 가속도 센서를 이용한 자세 추정 장치 및 방법
CN104567931B (zh) * 2015-01-14 2017-04-05 华侨大学 一种室内惯性导航定位的航向漂移误差消除方法
CN106155105A (zh) * 2015-04-08 2016-11-23 优利科技有限公司 控制云台的装置及云台系统
CN104914864B (zh) * 2015-05-22 2018-06-08 深圳市大疆灵眸科技有限公司 一种移动装置、移动装置控制系统及控制方法
CN105116926B (zh) * 2015-08-20 2018-05-04 深圳一电航空技术有限公司 云台控制方法和装置
CN106292741A (zh) * 2016-09-27 2017-01-04 成都普诺思博科技有限公司 一种基于无刷电机的移动机器人云台系统
CN106959110B (zh) * 2017-04-06 2020-08-11 亿航智能设备（广州）有限公司 一种云台姿态检测方法及装置

Also Published As

Publication number Publication date
WO2018184467A1 (zh) 2018-10-11
CN106959110A (zh) 2017-07-18

Similar Documents

Publication Publication Date Title
CN106959110B (zh) 一种云台姿态检测方法及装置
JP5061264B1 (ja) 小型姿勢センサ
JP4782111B2 (ja) 輸送手段の位置、姿勢、および／または飛行方向を推定するシステムおよび方法
CN107014371A (zh) 基于扩展自适应区间卡尔曼的无人机组合导航方法与装置
US20170153650A1 (en) Multiple rotors aircraft and control method
CN110081878B (zh) 一种多旋翼无人机的姿态及位置确定方法
US20220155800A1 (en) Method and apparatus for yaw fusion and aircraft
CN110325822B (zh) 云台位姿修正方法和装置
CN106774378B (zh) 一种无人机飞行控制及定位方法
CN112066985B (zh) 一种组合导航系统初始化方法、装置、介质及电子设备
JP2012173190A (ja) 測位システム、測位方法
CN106813679B (zh) 运动物体的姿态估计的方法及装置
US11408735B2 (en) Positioning system and positioning method
CN106352897B (zh) 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
JP2007232443A (ja) 慣性航法装置およびその誤差補正方法
CN110793515A (zh) 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
CN109143303B (zh) 飞行定位方法、装置及固定翼无人机
CN112055804A (zh) 信息处理装置、信息处理方法和程序
TWI578124B (zh) 多旋翼飛行器及其控制方法
KR102090615B1 (ko) 모델 예측 제어를 이용한 드론 제어 시스템
Du et al. A low-cost attitude estimation system for UAV application
KR20130079881A (ko) 고정 표적을 이용한 무인항공기의 표적 위치 보정 방법 및 컴퓨터 판독 가능한 기록매체
Ryan et al. MEMS based AHRS with adaptive bias estimation for high performance rate sensor replacement
CN114018241A (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