CN108549399B - 飞行器偏航角修正方法、装置及飞行器 - Google Patents

飞行器偏航角修正方法、装置及飞行器 Download PDF

Info

Publication number
CN108549399B
CN108549399B CN201810501521.3A CN201810501521A CN108549399B CN 108549399 B CN108549399 B CN 108549399B CN 201810501521 A CN201810501521 A CN 201810501521A CN 108549399 B CN108549399 B CN 108549399B
Authority
CN
China
Prior art keywords
angle
aircraft
yaw
angular velocity
motor
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
CN201810501521.3A
Other languages
English (en)
Other versions
CN108549399A (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.)
Shenzhen Autel Intelligent Aviation Technology Co Ltd
Original Assignee
Autel Robotics 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 Autel Robotics Co Ltd filed Critical Autel Robotics Co Ltd
Priority to CN201810501521.3A priority Critical patent/CN108549399B/zh
Publication of CN108549399A publication Critical patent/CN108549399A/zh
Priority to PCT/CN2018/116719 priority patent/WO2019223271A1/zh
Application granted granted Critical
Publication of CN108549399B publication Critical patent/CN108549399B/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)
  • Studio Devices (AREA)
  • Electric Propulsion And Braking For Vehicles (AREA)

Abstract

本发明实施例涉及飞行器技术领域，公开了一种飞行器偏航角修正方法、装置及飞行器。其中，该方法包括：获取飞行器的初始姿态信息，飞行器的初始姿态信息包括飞行器的初始角速度及飞行器的初始角度；获取云台基座的角速度；将第一偏航角速度与第二偏航角速度进行比较，第一偏航角速度为飞行器的初始角速度中的偏航角速度，第二偏航角速度为云台基座的角速度中的偏航角速度；当第一偏航角速度与第二偏航角速度的差值大于或等于预设阈值时，根据第一偏航角速度与第二偏航角速度，对飞行器的初始角度中的偏航角进行修正，以得到修正后的飞行器的偏航角。通过该飞行器偏航角修正方法，可以有效的提高估算飞行器偏航角的准确度。

Description

飞行器偏航角修正方法、装置及飞行器
技术领域
本发明实施例涉及飞行器技术领域，尤其涉及一种飞行器偏航角修正方法、飞行器偏航角修正装置，以及飞行器。
背景技术
近年来，飞行器，如无人飞行器(Unmanned Aerial Vehicle，UAV)，也称无人机得到了越来越广泛的应用。UAV是一种处在迅速发展中的新概念装备，其具有体积小、重量轻、机动灵活、反应快速、无人驾驶、操作要求低的优点。UAV的各个动作(或姿态)通常是通过控制UAV的动力系统中的多个驱动电机不同转速实现的。其中，UAV偏航角是对UAV的姿态(如前、后、左、右、上、下等飞行姿态)进行控制中的重要参数，也即UAV的偏航角正确估算对UAV的姿态控制尤其重要，若UAV的偏航角估算错误，UAV轻则无法按照预设的方向或轨迹飞行，重则可能失稳以致炸机，因此，如何提高偏航角的估算准确度具有十分重要的意义。
目前通常是基于磁力计采集的数据得到飞行器(如UAV等)的偏航角，但采用该方法得到偏航角很容易受到外界因素的影响，尤其是当磁力计处于强磁干扰环境中时，磁力计的数据可能严重错误，导致偏航角的估算出现较大的偏差，飞行器的偏航角的估算准确度低。
发明内容
本发明实施例提供一种飞行器偏航角修正方法、装置及飞行器，可以有效的提高估算飞行器偏航角的准确度。
本发明实施例公开了如下技术方案：
第一方面，本发明实施例提供了一种飞行器偏航角修正方法，所述飞行器连接有云台，所述云台包括云台电机及云台基座，所述飞行器设置有姿态传感器组件，所述方法包括：
获取所述飞行器的初始姿态信息，所述飞行器的初始姿态信息由所述姿态传感器组件采集得到，所述飞行器的初始姿态信息包括飞行器的初始角速度及飞行器的初始角度；
获取所述云台基座的角速度；
将第一偏航角速度与第二偏航角速度进行比较，所述第一偏航角速度为所述飞行器的初始角速度中的偏航角速度，所述第二偏航角速度为所述云台基座的角速度中的偏航角速度；
当所述第一偏航角速度与所述第二偏航角速度的差值大于或等于预设阈值时，根据所述第一偏航角速度与所述第二偏航角速度，对所述飞行器的初始角度中的偏航角进行修正，以得到修正后的所述飞行器的偏航角。
在一些实施例中，所述预设阈值由是否存在偏航操作所确定。
在一些实施例中，所述预设阈值由是否存在偏航操作所确定包括：
当存在偏航操作时，所述预设阈值为第一预设阈值；
当不存在偏航操作时，所述预设阈值为第二预设阈值，所述第二预设阈值小于所述第一预设阈值。
在一些实施例中，所述根据所述第一偏航角速度与所述第二偏航角速度，对所述飞行器的初始角度中的偏航角进行修正，包括：
根据所述第一偏航角速度与所述第二偏航角速度的差值，得到偏航角补偿值；
根据所述偏航角补偿值对所述飞行器的初始角度中的偏航角进行修正。
在一些实施例中，根据所述第一偏航角速度与所述第二偏航角速度的差值得到偏航角补偿值的计算公式为：
Figure BDA0001670494520000031
其中，Δψp为偏航角补偿值，t0为获取所述飞行器的初始姿态信息所对应的时间，ΔT为对所述飞行器的初始角度中的偏航角进行修正的周期，
Figure BDA0001670494520000032
为第一偏航角速度，
Figure BDA0001670494520000033
为第二偏航角速度。
在一些实施例中，得到修正后的所述飞行器的偏航角的计算公式为：
ψ′p＝ψp+Δψp
其中，ψ′p为修正后的偏航角，ψp为所述飞行器的初始角度中的偏航角，Δψp为偏航角补偿值。
在一些实施例中，所述云台上搭载有拍摄装置，所述拍摄装置设置有惯性测量单元；
所述获取所述云台基座的角速度，包括：
获取所述云台电机的角度；
根据所述云台电机的角度，确定所述云台电机的角速度；
获取所述拍摄装置的角速度，所述拍摄装置的角速度由所述惯性测量单元采集得到；
根据所述云台电机的角速度、所述云台电机的角度及所述拍摄装置的角速度，确定所述云台基座的角速度。
在一些实施例中，根据所述云台电机的角度，确定所述云台电机的角速度，包括：
将所述云台电机的角度作为输入，通过二阶离散非线性跟踪微分器，计算得到所述云台电机的角速度。
在一些实施例中，所述二阶离散非线性跟踪微分器的表达式为：
r1(k+1)＝r1(k)+T·r2(k)
r2(k+1)＝r2(k)+T·fst(r1(k)-Ρ(k),r2(k),δ,h)
其中，T为获取所述云台电机的角度的采样周期，Ρ(k)＝[φ(k) θ(k) ψ(k)]T为第k时刻所述云台电机的角度，r1(k)为通过所述二阶离散非线性跟踪微分器对Ρ(k)进行跟踪由Ρ(k)所确定的值，r2(k)为Ρ(k)的导数，k+1为第k+1时刻所对应的值，fst()为最速控制函数，δ为位于最速控制函数的第三位的参数，h为位于最速控制函数的第四位的参数。
在一些实施例中，所述根据所述云台电机的角速度、所述云台电机的角度及所述拍摄装置的角速度，确定所述云台基座的角速度，包括：
根据所述云台电机的角度确定旋转变换矩阵，所述旋转变换矩阵为云台基座坐标系到云台电机坐标系的旋转矩阵；
根据所述云台电机的角速度、所述旋转变换矩阵及所述拍摄装置的角速度，确定所述云台基座的角速度。
在一些实施例中，根据所述云台电机的角度确定旋转变换矩阵的计算公式为：
Figure BDA0001670494520000041
其中，D为旋转变换矩阵；(φ,θ,ψ)为所述云台电机的角度，φ为所述云台电机的角度中的翻滚角，θ为所述云台电机的角度中的俯仰角，ψ为所述云台电机的角度中的偏航角。
在一些实施例中，根据所述云台电机的角速度、所述旋转变换矩阵及所述拍摄装置的角速度确定所述云台基座的角速度的计算公式为：
Figure BDA0001670494520000042
其中，
Figure BDA0001670494520000043
为所述云台基座的角速度，
Figure BDA0001670494520000044
为所述拍摄装置的角速度，D为旋转变换矩阵，r2为所述云台电机的角速度。
第二方面，本发明实施例提供了一种飞行器偏航角修正装置，所述飞行器连接有云台，所述云台包括云台电机及云台基座，所述飞行器设置有姿态传感器组件，所述装置包括：
初始姿态信息获取模块，用于获取所述飞行器的初始姿态信息，所述飞行器的初始姿态信息由所述姿态传感器组件采集得到，所述飞行器的初始姿态信息包括飞行器的初始角速度及飞行器的初始角度；
基座角速度获取模块，用于获取所述云台基座的角速度；
比较模块，用于将第一偏航角速度与第二偏航角速度进行比较，所述第一偏航角速度为所述飞行器的初始角速度中的偏航角速度，所述第二偏航角速度为所述云台基座的角速度中的偏航角速度；
修正模块，用于当比较模块确定到所述第一偏航角速度与所述第二偏航角速度的差值大于或等于预设阈值时，根据所述第一偏航角速度与所述第二偏航角速度，对所述飞行器的初始角度中的偏航角进行修正，以得到修正后的所述飞行器的偏航角。
在一些实施例中，所述装置还包括预设阈值确定模块，用于根据判断是否存在偏航操作，确定所述预设阈值。
在一些实施例中，所述预设阈值确定模块具体用于：
当存在偏航操作时，确定所述预设阈值为第一预设阈值；
当不存在偏航操作时，确定所述预设阈值为第二预设阈值，所述第二预设阈值小于所述第一预设阈值。
在一些实施例中，所述修正模块根据所述第一偏航角速度与所述第二偏航角速度，对所述飞行器的初始角度中的偏航角进行修正，包括：
根据所述第一偏航角速度与所述第二偏航角速度的差值，得到偏航角补偿值；
根据所述偏航角补偿值对所述飞行器的初始角度中的偏航角进行修正。
在一些实施例中，所述云台上搭载有拍摄装置，所述拍摄装置设置有惯性测量单元；
所述基座角速度获取模块具体用于：
获取所述云台电机的角度；
根据所述云台电机的角度，确定所述云台电机的角速度；
获取所述拍摄装置的角速度，所述拍摄装置的角速度由所述惯性测量单元采集得到；
根据所述云台电机的角速度、所述云台电机的角度及所述拍摄装置的角速度，确定所述云台基座的角速度。
在一些实施例中，所述基座角速度获取模块根据所述云台电机的角度，确定所述云台电机的角速度，包括：
将所述云台电机的角度作为输入，通过二阶离散非线性跟踪微分器，计算得到所述云台电机的角速度。
在一些实施例中，所述基座角速度获取模块根据所述云台电机的角速度、所述云台电机的角度及所述拍摄装置的角速度，确定所述云台基座的角速度，包括：
根据所述云台电机的角度确定旋转变换矩阵，所述旋转变换矩阵为云台基座坐标系到云台电机坐标系的旋转矩阵；
根据所述云台电机的角速度、所述旋转变换矩阵及所述拍摄装置的角速度，确定所述云台基座的角速度。
第三方面，本发明实施例提供了一种飞行器，包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如上所述的飞行器偏航角修正方法。
第四方面，本发明实施例提供了一种计算机程序产品，所述计算机程序产品包括存储在非易失性计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，使计算机执行如上所述的飞行器偏航角修正方法。
第五方面，本发明实施例还提供了一种非易失性计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行如上所述的飞行器偏航角修正方法。
本发明实施例通过飞行器的初始角速度中的偏航角速度与云台基座的角速度中的偏航角速度的差值，对飞行器的初始角度中的偏航角进行修正，可以有效的提高估算飞行器偏航角的准确度，该估算方法可避免外界因素的干扰，也即在GPS信号弱与强磁干扰环境下，也具有较高的估算准确度，从而有效提高飞行器飞行的安全性与稳定性。
附图说明
一个或多个实施例通过与之对应的附图中的图片进行示例性说明，这些示例性说明并不构成对实施例的限定，附图中具有相同参考数字标号的元件表示为类似的元件，除非有特别申明，附图中的图不构成比例限制。
图1是本发明实施例提供的一种飞行器偏航角修正方法的流程示意图；
图2是本发明实施例提供的姿态传感器组件的位置设置示意图；
图3是本发明实施例提供的飞行器偏航角修正装置的示意图；
图4是本发明实施例提供的飞行器硬件结构示意图；
图5是本发明实施例提供的飞行器的连接框图；
图6是图5中的动力系统的示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
本发明实施例提供的飞行器偏航角修正方法可以应用到各种通过电机或马达驱动的可移动物体上，包括但不限于飞行器、机器人等。其中飞行器可包括无人飞行器(unmanned aerial vehicle，UAV)，无人飞船等。现以UAV为例进行说明。UAV包括机身、控制器和动力系统。控制器与动力系统连接，动力系统安装在所述机身上，用于为所述飞行器提供飞行动力。具体的，控制器用于执行上述飞行器偏航角修正方法以修正飞行器的偏航角，并根据修正后的飞行器的偏航角生成控制指令，并将该控制指令发送给动力系统的电调，电调通过该控制指令控制动力系统的驱动电机。或者，控制器用于执行飞行器偏航角修正方法以修正飞行器的偏航角，并将修正后的飞行器的偏航角发送至电调，电调根据修正后的飞行器的偏航角生成控制指令，并通过该控制指令控制动力系统的驱动电机。
机身包括：中心壳体以及与中心壳体连接的一个或多个机臂，一个或多个机臂呈辐射状从中心壳体延伸出。机臂与中心壳体的连接可以是一体连接或者固定连接。动力系统安装于机臂上。
控制器用于执行上述飞行器偏航角修正方法以修正飞行器的偏航角，并根据修正后的飞行器的偏航角生成控制指令，并将该控制指令发送给动力系统的电调，以便电调通过该控制指令控制动力系统的驱动电机。控制器为具有一定逻辑处理能力的器件，如控制芯片、单片机、微控制单元(Microcontroller Unit，MCU)等。
动力系统包括：电调，驱动电机和螺旋桨。电调位于机臂或中心壳体所形成的空腔内。电调分别与控制器及驱动电机连接。具体的，电调与驱动电机电连接，用于控制所述驱动电机。驱动电机安装在机臂上，驱动电机的转动轴连接螺旋桨。螺旋桨在驱动电机的驱动下产生使得UAV移动的力，例如，使得UAV移动的升力或者推力。
UAV完成各个规定速度、动作(或姿态)是通过电调控制驱动电机以实现的。电调全称电子调速器，根据控制信号调节UAV的驱动电机的转速。其中，控制器为执行飞行器偏航角修正方法的执行主体，电调基于修正后的飞行器的偏航角所生成控制指令来控制驱动电机。电调控制驱动电机的原理大致为：驱动电机是将电脉冲信号转变为角位移或线位移的开环控制元件。在非超载的情况下，驱动电机的转速、停止的位置只取决于脉冲信号的频率和脉冲数，而不受负载变化的影响，当驱动器接收到一个脉冲信号，它就驱动动力系统的驱动电机按设定的方向转动一个固定的角度，它的旋转是以固定的角度运行的。因此，电调可以通过控制脉冲个数来控制角位移量，从而达到准确定位的目的；同时可以通过控制脉冲频率来控制驱动电机转动的速度和加速度，从而达到调速的目的。
目前UAV主要功能为航拍、影像实时传输、高危地区探测等。为了实现航拍、影像实时传输、高危地区探测等功能，UAV上会连接有摄像组件。具体的，UAV和摄像组件通过连接结构，如减振球等进行连接。该摄像组件用于在UAV进行航拍的过程中，获取拍摄画面。
具体的，摄像组件包括：云台及拍摄装置。云台与UAV连接。其中，拍摄装置搭载于所述云台上，拍摄装置可以为图像采集装置，用于采集图像，该拍摄装置包括但不限于：相机、摄影机、摄像头、扫描仪、拍照手机等。云台用于搭载拍摄装置，以实现拍摄装置的固定、或随意调节拍摄装置的姿态(例如，改变拍摄装置的高度、倾角和/或方向)以及使所述拍摄装置稳定保持在设定的姿态上。例如，当UAV进行航拍时，云台主要用于使所述拍摄装置稳定保持在设定的姿态上，防止拍摄装置拍摄画面抖动，保证拍摄画面的稳定。
云台包括：云台电机及云台基座。其中，云台电机安装于云台基座。飞行器的控制器也可通过动力系统的电调来控制云台电机，具体的，飞行器的控制器与电调连接，电调与云台电机电连接，飞行器的控制器生成云台电机控制指令，电调通过云台电机控制指令以控制云台电机。
云台基座与UAV的机身连接，用于将摄像组件固定安装于UAV的机身上。
云台电机分别与云台基座及拍摄装置连接。该云台可以为多轴云台，与之适应的，云台电机为多个，也即每个轴设置有一个云台电机。云台电机一方面可带动拍摄装置的转动，从而满足拍摄转轴的水平旋转和俯仰角度的调节，通过手动远程控制云台电机旋转或利用程序让电机自动旋转，从而达到全方位扫描监控的作用；另一方面，在UAV进行航拍的过程中，通过云台电机的转动实时抵消拍摄装置受到的扰动，防止拍摄装置抖动，保证拍摄画面的稳定。
拍摄装置搭载于云台上，拍摄装置上设置有惯性测量单元，该惯性测量单元用于获取拍摄装置的姿态信息。
在对UAV的姿态进行控制的过程中，UAV的偏航角是对UAV的姿态进行控制中的重要参数，需要基于UAV偏航角，来控制驱动电机。通过飞行器的控制器实时获取UAV的偏航角，为UAV的姿态控制提供必要的姿态信息。也即UAV的偏航角正确估算对UAV的姿态控制尤其重要，若UAV的偏航角估算错误，UAV轻则无法按照预设的方向或轨迹飞行，重则可能失稳以致炸机。
目前通常是基于磁力计采集的数据得到UAV的偏航角，但采用该方法得到偏航角很容易受到外界因素的影响，尤其是当磁力计处于强磁干扰环境中时，磁力计的数据可能严重错误，导致偏航角的估算出现较大的偏差，飞行器的偏航角的估算准确度低。
为了提高UAV的偏航角估算的准确性，目前常见的是利用外置的GPS模组，依靠GPS估算出一个偏航角，用以对基于磁力计的偏航角值进行修正。但GPS信号有时可能不稳定，使得在某些情况下，磁力计估算的偏航角即使出现了偏差却得不到有效修正。也即利用外置的GPS模组修正UAV的偏航角虽然一定程度上可以提高飞行器的偏航角的估算准确度，但效果并不好，特别是当GPS信号较弱，利用外置的GPS模组未能起到有效的航向修正作用。
因此，基于上述问题，本发明实施例主要目的在于提供一种飞行器偏航角修正方法、装置及飞行器，可以基于云台提供的姿态信息修正飞行器的偏航角，有效的提高估算飞行器偏航角的准确度，从而提高飞行器飞行的安全性与稳定性。
其中，本发明的思路是：首先，飞行器上设置有姿态传感器组件，通过该姿态传感器组件，采集得到飞行器的初始姿态信息，该姿态传感器组件并将该姿态传感器组件发送给飞行器的控制器，以使飞行器的控制器获取得到飞行器的初始姿态信息，其中，所述飞行器的初始姿态信息包括飞行器的初始角速度及飞行器的初始角度；然后，飞行器的控制器获取所述云台基座的角速度；接着，飞行器的控制器便可将获取得到的飞行器的初始角速度中的偏航角速度与云台基座的角速度中的偏航角速度进行比较；最后，基于比较的结果，飞行器的控制器对飞行器的偏航角进行修正，具体的，飞行器的初始角速度中的偏航角速度与云台基座的角速度中的偏航角速度的差值大于或等于预设阈值时，根据该差值，对飞行器的初始角度中的偏航角进行修正，以得到修正后的飞行器的偏航角。
本发明实施例通过飞行器的初始角速度中的偏航角速度与云台基座的角速度中的偏航角速度的差值，对飞行器的初始角度中的偏航角进行修正，可以有效的提高估算飞行器偏航角的准确度，该估算方法可避免外界因素的干扰，也即在GPS信号弱与强磁干扰环境下，也具有较高的估算准确度，从而有效提高飞行器飞行的安全性与稳定性。
下面结合附图，对本发明实施例作进一步阐述。
实施例1：
图1为本发明实施例提供的一种飞行器偏航角修正方法的流程示意图。其中，该飞行器偏航角修正方法可由各种具有一定逻辑处理能力的电子设备执行，如飞行器、控制芯片等，该飞行器可以包括无人机、无人船等。以下电子设备以飞行器为例进行说明。其中，飞行器连接有云台，云台包括云台电机及云台基座，其中，云台可以为多轴云台，如两轴云台、三轴云台，以下三轴云台为例进行说明。飞行器设置有姿态传感器组件。对于该飞行器及云台的具体结构的描述可以参考上述描述，因此，在此处不作赘述。
请参照图1，所述飞行器偏航角修正方法包括：
101：获取所述飞行器的初始姿态信息。
飞行器获取所述飞行器的初始姿态信息具体包括：首先由设置于飞行器上的姿态传感器组件采集得到初始姿态信息，并将该初始姿态信息发送至飞行器，以使飞行器获取得到该初始姿态信息。其中，飞行器的初始姿态信息包括飞行器的初始角速度及飞行器的初始角度。
其中，姿态传感器组件包括：磁力计、第一惯性测量单元(Inertial measurementunit，IMU)、GPS(Global Positioning System)等。磁力计可用于采集飞行器的初始角度，第一惯性测量单元可用于采集飞行器的初始角速度。
其中，姿态传感器组件的具体设置位置可参考图2所示。其中，图2中包括有4个坐标系：飞行器坐标系、云台基座坐标系、云台电机坐标系及拍摄装置坐标系。在图2中，处于最上端的是飞行器的机身，姿态传感器组件设置于机身上，位于飞行器的机身下端并最接近机身的是云台基座，云台基座与机身通过减振球(图未示)相连，位于云台基座下端的是云台电机，位于最下端的是拍摄装置，拍摄装置上设置有第二惯性测量单元，用于采集拍摄装置的角速度。云台基座与拍摄装置之间通过Z-X-Y欧拉角轴序的三轴电机相连，即从上到下依次是偏航轴Yaw(Z轴)、翻滚轴Roll(X轴)、俯仰轴Pitch(Y轴)三个电机轴。
其中，惯性测量单元(Inertial measurement unit，IMU)为一种测量物体三轴姿态角(或角速率)以及加速度的装置。通常的，IMU为六轴的IMU。其中，六轴的IMU中，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。
通过六轴的IMU可以检测自身在惯性系中的角度信息，具体的，设置于飞行器上的第一惯性测量单元采集得到飞行器的初始角速度，该飞行器的初始角速度可用向量
Figure BDA0001670494520000131
表示，也即该飞行器的初始角速度
Figure BDA0001670494520000132
表示为飞行器相对于惯性系的角速度在飞行器坐标系下的坐标向量，飞行器的初始角速度包括：飞行器的初始翻滚角速度、飞行器的初始俯仰角速度及飞行器的初始偏航角速度。其中，
Figure BDA0001670494520000133
表示飞行器的初始翻滚角速度，
Figure BDA0001670494520000134
表示飞行器的初始俯仰角速度，
Figure BDA0001670494520000135
表示为飞行器的初始偏航角速度。类似的，设置于拍摄装置上的第二惯性测量单元采集得到拍摄装置的角速度，该拍摄装置的角速度可用向量
Figure BDA0001670494520000136
表示，也即该拍摄装置的角速度
Figure BDA0001670494520000137
表示为拍摄装置相对于惯性系的角速度在拍摄装置坐标系下的坐标向量。其中，
Figure BDA0001670494520000138
表示拍摄装置的翻滚角速度，
Figure BDA0001670494520000139
表示拍摄装置的俯仰角速度，
Figure BDA00016704945200001310
表示为拍摄装置的偏航角速度。其中，惯性系，又称惯性坐标系、惯性参照系、大地坐标系或世界坐标系，由于无人飞行器可安放在任意位置,在环境中选择一个基准坐标来描述无人飞行器及无人飞行器的各个部件的位置,并用它描述环境中任何物体的位置,该坐标系称为惯性系。
磁力计，也称地磁、磁感器，可用于测试磁场强度和方向，定位设备的方位。基于磁力计价格低、质轻、结构紧凑等优点，被广泛应用于飞行器的角度的采集。通过磁力计采集得到飞行器的初始角度，该飞行器的初始角度可用向量(φp,θp,ψp)表示，也即该飞行器的初始角度(φp,θp,ψp)表示为飞行器相对于惯性系的角度在飞行器坐标系下的坐标向量，其中，φp表示飞行器的初始翻滚角度，θp表示飞行器的初始俯仰角速度，ψp表示为飞行器的初始偏航角度。
102：获取所述云台基座的角速度。
为了避免姿态求解过程中出现万向节锁现象，使用四元数对飞行器、云台基座、云台电机、拍摄装置的姿态进行描述。其中，产生万向节锁现象的根本原因是，旋转矩阵是依次进行的，假设先围绕x轴旋转，再围绕y轴旋转，最后围绕z轴旋转，这就导致物体其实是围绕自己的坐标系的x轴旋转，而不是惯性系的x轴旋转。表现就是，在一个欧拉角(x1,y1,z1)下，改变x1的值，物体会围绕物体自己的坐标系的x轴进行旋转，而不是世界惯性系的x轴进行旋转,最后，当把物体的x轴旋转到与惯性系的z轴重合时，欧垃角的x1和z1旋转结果就都一样了，也就丢失了一个维度,这便是万向节锁现象。概括起来可以这么说，绕着物体坐标系中某一个轴，比如y轴的+(-)90度的某次旋转，使得这次旋转的前一次绕物体坐标系x轴的旋转和这次旋转的后一次绕物体坐标系z轴的旋转的两个旋转轴是一样(一样的意思是指在惯性系中，两次旋转轴是共轴的但方向相反),从而造成一个旋转自由度丢失，也就是万向节锁现象。
使用三个量来表示三维空间的朝向的系统都会出现万向节锁现象这个问题，而通过四元数进行描述可以有效的避免万向节锁现象。具体的，可以假设飞行器相对于惯性系的姿态四元数为qip＝[qip0 qip1 qip2 qip3]T，云台基座相对于惯性坐标系的姿态四元数为qib＝[qib0 qib1 qib2 qib3]T，云台电机相对于惯性坐标系的姿态四元数为qbc＝[qbc0 qbc1 qbc2qbc3]T，拍摄装置相对于惯性坐标系的姿态四元数为qic＝[qic0 qic1 qic2 qic3]T。
飞行器获取所述云台基座的角速度具体包括：获取所述云台电机的角度；根据所述云台电机的角度，确定所述云台电机的角速度；获取所述拍摄装置的角速度，所述拍摄装置的角速度由所述惯性测量单元采集得到；根据所述云台电机的角速度、所述云台电机的角度及所述拍摄装置的角速度，确定所述云台基座的角速度。
其中，飞行器根据所述云台电机的角度具体过程可以为：在云台电机上设置线性霍尔传感器，通过线性霍尔传感器采集得到云台电机的角度，并将该云台电机的角度发送至飞行器，以使飞行器可获取得到该云台电机的角度。
为了节约成本及降低控制云台电机算法的复杂程度，通常云台的云台电机上不会直接设置测速元件，云台电机的角速度无法直接测量。因此，在本发明实施例中，为了得到云台电机的角速度，由于云台电机的角度可以通过线性霍尔传感器采集得到，因此，可以再借助微分器，得到云台电机的角速度。具体的，根据所述云台电机的角度，确定所述云台电机的角速度包括：将所述云台电机的角度作为输入，通过二阶离散非线性跟踪微分器，计算得到所述云台电机的角速度。
其中，所述二阶离散非线性跟踪微分器的表达式为：
r1(k+1)＝r1(k)+T·r2(k)
r2(k+1)＝r2(k)+T·fst(r1(k)-Ρ(k),r2(k),δ,h)
其中，T为获取所述云台电机的角度的采样周期，也即线性霍尔传感器的采样周期，通常T＝0.001s，Ρ(k)＝[φ(k) θ(k) ψ(k)]T为第k时刻所述云台电机的角度，r1(k)为通过所述二阶离散非线性跟踪微分器对Ρ(k)进行跟踪由Ρ(k)所确定的值，r2(k)为Ρ(k)的导数，k+1为第k+1时刻所对应的值，fst()为最速控制函数，δ为位于最速控制函数的第三位的参数，δ用于确定跟踪速度，h为位于最速控制函数的第四位的参数。其中，最速控制函数，又称快速控制函数，是能以最短时间完成规定控制作用的最优控制函数。
最速控制函数fst()的表达式为：
Figure BDA0001670494520000151
Figure BDA0001670494520000161
其中，x1为位于最速控制函数的第一位的参数，x2为位于最速控制函数的第四位的参数，δ为位于最速控制函数的第三位的参数，δ用于确定跟踪速度，基于在追踪速度和噪声放大的矛盾中取折中，并经过试验分析，取δ＝20，h为位于最速控制函数的第四位的参数。
sgn()为阶跃函数，a为阶跃函数的运算参数，d＝δ·T为最速控制函数的第一运算参数，根据线性霍尔传感器的噪声特性，通常取d＝0.5，d0＝T·d为最速控制函数的第二运算参数，y＝x1+T·x2为最速控制函数的第三运算参数，
Figure BDA0001670494520000162
为最速控制函数的第四运算参数。
通过二阶离散非线性跟踪微分器，可实现的功能为：
r1(k)→Ρ(k)
Figure BDA0001670494520000163
也即，二阶离散非线性跟踪微分器实现的功能为：通过r1(k)去跟踪Ρ(k)，获得Ρ(k)的微分r2(k)，Ρ(k)的微分就是Ρ(k)的导数
Figure BDA0001670494520000164
也即云台电机的角速度
Figure BDA0001670494520000165
飞行器根据所述云台电机的角速度、所述云台电机的角度及所述拍摄装置的角速度，确定所述云台基座的角速度，具体包括：根据所述云台电机的角度确定旋转变换矩阵，所述旋转变换矩阵为云台基座坐标系到云台电机坐标系的旋转矩阵；根据所述云台电机的角速度、所述旋转变换矩阵及所述拍摄装置的角速度，确定所述云台基座的角速度。
首先，根据电机的角度(φ,θ,ψ)，确定旋转变换矩阵D。具体的，设Rz(ψ)、Rx(φ)、Ry(θ)分别是绕Z、X、Y轴旋转的单位旋转阵，依据惯性导航基本原理，其Rz(ψ)、Rx(φ)、Ry(θ)的值分别如下：
Figure BDA0001670494520000166
Figure BDA0001670494520000171
Figure BDA0001670494520000172
拍摄装置的角速度、云台电机的角速度与云台基座的角速度三者之间的联系可用如下姿态动力学方程描述：
Figure BDA0001670494520000173
其中，
Figure BDA0001670494520000174
为所述云台基座的角速度，
Figure BDA0001670494520000175
为所述拍摄装置的角速度，
Figure BDA0001670494520000176
为所述云台电机的角速度。
基于上述等式，得到根据所述云台电机的角度确定旋转变换矩阵的计算公式为：
Figure BDA0001670494520000177
其中，D表示为旋转变换矩阵；(φ,θ,ψ)表示为所述电机的角度，φ为所述云台电机的角度中的翻滚角，θ为所述云台电机的角度中的俯仰角，ψ为所述云台电机的角度中的偏航角。
然后，根据所述云台电机的角速度r2、所述旋转变换矩阵D及所述拍摄装置的角速度
Figure BDA0001670494520000178
确定所述云台基座的角速度
Figure BDA0001670494520000179
具体的，计算所述云台基座的角速度的公式为：
Figure BDA00016704945200001710
其中，
Figure BDA00016704945200001711
表示云台基座的角速度，云台基座的角速度包括：云台基座的翻滚角速度、云台基座的俯仰角速度及云台基座的偏航角速度。例如，
Figure BDA00016704945200001712
则，
Figure BDA00016704945200001713
表示飞行器的初始翻滚角速度，
Figure BDA00016704945200001714
表示飞行器的初始俯仰角速度，
Figure BDA00016704945200001715
表示为飞行器的初始偏航角速度。
103：将第一偏航角速度与第二偏航角速度进行比较，所述第一偏航角速度为所述飞行器的初始角速度中的偏航角速度，所述第二偏航角速度为所述云台基座的角速度中的偏航角速度。
例如，飞行器的初始角速度为
Figure BDA0001670494520000181
则所述第一偏航角速度为
Figure BDA0001670494520000182
云台基座的角速度为
Figure BDA0001670494520000183
则所述第二偏航角速度为
Figure BDA0001670494520000184
通过将第一偏航角速度
Figure BDA0001670494520000185
与第二偏航角速度
Figure BDA0001670494520000186
进行比较，可以确定两者的大小关系、两者的差值等。
104：当所述第一偏航角速度与所述第二偏航角速度的差值大于或等于预设阈值时，根据所述第一偏航角速度与所述第二偏航角速度，对所述飞行器的初始角度中的偏航角进行修正，以得到修正后的所述飞行器的偏航角。
其中，该预设阈值由是否存在偏航操作所确定。具体的，所述预设阈值由是否存在偏航操作所确定包括：当存在偏航操作时，所述预设阈值为第一预设阈值；当不存在偏航操作时，所述预设阈值为第二预设阈值，所述第二预设阈值小于所述第一预设阈值。
通常在飞行器飞行的过程中，云台基座的偏航角是实时跟随飞行器的偏航角运动而运动的，即第二偏航角速度
Figure BDA0001670494520000187
与第一偏航角速度
Figure BDA0001670494520000188
是几近同步的，考虑飞行器飞行过程中由于飞控不稳导致的偏航角误转动，
Figure BDA0001670494520000189
与
Figure BDA00016704945200001810
满足关系：
Figure BDA00016704945200001811
其中，ζ表示预设阈值。当用户的转航向杆时，也即存在偏航操作时，如飞行器接收到由用户进行偏航操作而生成偏航操作指令时，由于飞行器在航向方向有一个加速运动，云台基座虽然试图及时跟上飞行器第偏航角的运动，但总有一个小滞后，因而此时，所述预设阈值为第一预设阈值，如ζ＝ζ1＝5。其中，ζ1表示第一预设阈值。当用户不转航向杆时，也即不存在偏航操作时，飞行器在航向方向没有加速运动，此时云台基座可较为及时的跟随飞行器的航向运动，也即所述预设阈值为第二预设阈值，如ζ＝ζ1＝2。其中，ζ2表示第二预设阈值。并且，所述第二预设阈值ζ2小于所述第一预设阈值ζ1。
如果在某一时刻，飞行器的磁力计受到强干扰且GPS信号较弱，GPS未能起到有效的偏航角修正作用，则飞行器的偏航角估算会突然出错，结果是使得飞行器产生一个相对云台基座的快速转动，产生一个角速度差值
Figure BDA0001670494520000191
其中，η表示所述第一偏航角速度与所述第二偏航角速度的差值，η≥ζ，可判断磁力计受到干扰。此时需要对飞行器进行修正。
具体的，当所述第一偏航角速度
Figure BDA0001670494520000192
与所述第二偏航角速度
Figure BDA0001670494520000193
的差值η大于或等于预设阈值ζ时，飞行器根据所述第一偏航角速度
Figure BDA0001670494520000194
与所述第二偏航角速度
Figure BDA0001670494520000195
对所述飞行器的初始角度中的偏航角ψp进行修正，以得到修正后的所述飞行器的偏航角ψ′p。
其中，飞行器根据所述第一偏航角速度
Figure BDA0001670494520000196
与所述第二偏航角速度
Figure BDA0001670494520000197
对所述飞行器的初始角度中的偏航角ψp进行修正，包括：根据所述第一偏航角速度
Figure BDA0001670494520000198
与所述第二偏航角速度
Figure BDA0001670494520000199
的差值η，得到偏航角补偿值Δψp；根据所述偏航角补偿值Δψp对所述飞行器的初始角度中的偏航角ψp进行修正。
飞行器根据所述第一偏航角速度与所述第二偏航角速度的差值得到偏航角补偿值的计算公式为：
Figure BDA00016704945200001910
其中，Δψp为偏航角补偿值，t0为获取所述飞行器的初始姿态信息所对应的时间，ΔT为对所述飞行器的初始角度中的偏航角进行修正的周期，根据经验，通常ΔT＝5T，也即，代表每5个采样间隔的时长进行一次偏航角修正，
Figure BDA00016704945200001911
为第一偏航角速度，
Figure BDA00016704945200001912
为第二偏航角速度。
飞行器得到修正后的所述飞行器的偏航角的计算公式为：
ψ′p＝ψp+Δψp
其中，ψ′p为修正后的偏航角，ψp为所述飞行器的初始角度中的偏航角，Δψp为偏航角补偿值。
在本发明实施例中，通过飞行器的初始角速度中的偏航角速度与云台基座的角速度中的偏航角速度的差值，对飞行器的初始角度中的偏航角进行修正，可以有效的提高估算飞行器偏航角的准确度，该估算方法可避免外界因素的干扰，也即在GPS信号弱与强磁干扰环境下，也具有较高的估算准确度，从而有效提高飞行器飞行的安全性与稳定性。
实施例2：
图3为本发明实施例提供的一种飞行器偏航角修正装置示意图。其中，所述飞行器偏航角修正装置30可配置于各种电子设备中，如飞行器、控制芯片等，该飞行器可以包括无人机、无人船等。以下电子设备以飞行器为例进行说明。其中，飞行器连接有云台，云台包括云台电机及云台基座，其中，云台可以为多轴云台，如两轴云台、三轴云台，以下三轴云台为例进行说明。飞行器设置有姿态传感器组件。对于该飞行器及云台的具体结构的描述可以参考上述描述，因此，在此处不作赘述。
请参照图3，所述飞行器偏航角修正装置30包括：
初始姿态信息获取模块301，用于获取所述飞行器的初始姿态信息。
初始姿态信息获取模块301获取所述飞行器的初始姿态信息具体包括：首先由设置于飞行器上的姿态传感器组件采集得到初始姿态信息，并将该初始姿态信息发送至初始姿态信息获取模块301，以使初始姿态信息获取模块301获取得到该初始姿态信息。其中，飞行器的初始姿态信息包括飞行器的初始角速度及飞行器的初始角度。
基座角速度获取模块302，用于获取所述云台基座的角速度。
基座角速度获取模块302获取所述云台基座的角速度具体包括：获取所述云台电机的角度；根据所述云台电机的角度，确定所述云台电机的角速度；获取所述拍摄装置的角速度，所述拍摄装置的角速度由所述惯性测量单元采集得到；根据所述云台电机的角速度、所述云台电机的角度及所述拍摄装置的角速度，确定所述云台基座的角速度。
其中，基座角速度获取模块302根据所述云台电机的角度具体过程可以为：在云台电机上设置线性霍尔传感器，通过线性霍尔传感器采集得到云台电机的角度，并将该云台电机的角度发送至基座角速度获取模块302，以使基座角速度获取模块302可获取得到该云台电机的角度。
为了节约成本及降低控制云台电机算法的复杂程度，通常云台的云台电机上不会直接设置测速元件，云台电机的角速度无法直接测量。因此，在本发明实施例中，为了得到云台电机的角速度，由于云台电机的角度可以通过线性霍尔传感器采集得到，因此，可以再借助微分器，得到云台电机的角速度。具体的，基座角速度获取模块302根据所述云台电机的角度，确定所述云台电机的角速度包括：将所述云台电机的角度作为输入，通过二阶离散非线性跟踪微分器，计算得到所述云台电机的角速度。
其中，所述二阶离散非线性跟踪微分器的表达式为：
r1(k+1)＝r1(k)+T·r2(k)
r2(k+1)＝r2(k)+T·fst(r1(k)-Ρ(k),r2(k),δ,h)
其中，T为获取所述云台电机的角度的采样周期，也即线性霍尔传感器的采样周期，通常T＝0.001s，Ρ(k)＝[φ(k) θ(k) ψ(k)]T为第k时刻所述云台电机的角度，r1(k)为通过所述二阶离散非线性跟踪微分器对Ρ(k)进行跟踪由Ρ(k)所确定的值，r2(k)为Ρ(k)的导数，k+1为第k+1时刻所对应的值，fst()为最速控制函数，δ为位于最速控制函数的第三位的参数，δ用于确定跟踪速度，h为位于最速控制函数的第四位的参数。
通过二阶离散非线性跟踪微分器，可实现的功能为：
r1(k)→Ρ(k)
Figure BDA0001670494520000211
也即，二阶离散非线性跟踪微分器实现的功能为：通过r1(k)去跟踪Ρ(k)，获得Ρ(k)的微分r2(k)，Ρ(k)的微分就是Ρ(k)的导数
Figure BDA0001670494520000212
也即云台电机的角速度
Figure BDA0001670494520000213
基座角速度获取模块302根据所述云台电机的角速度、所述云台电机的角度及所述拍摄装置的角速度，确定所述云台基座的角速度，具体包括：根据所述云台电机的角度确定旋转变换矩阵，所述旋转变换矩阵为云台基座坐标系到云台电机坐标系的旋转矩阵；根据所述云台电机的角速度、所述旋转变换矩阵及所述拍摄装置的角速度，确定所述云台基座的角速度。
其中，基座角速度获取模块302根据所述云台电机的角度确定旋转变换矩阵的计算公式为：
Figure BDA0001670494520000221
其中，D表示为旋转变换矩阵；(φ,θ,ψ)表示为所述电机的角度，φ为所述云台电机的角度中的翻滚角，θ为所述云台电机的角度中的俯仰角，ψ为所述云台电机的角度中的偏航角。
然后，基座角速度获取模块302根据所述云台电机的角速度r2、所述旋转变换矩阵D及所述拍摄装置的角速度
Figure BDA0001670494520000222
确定所述云台基座的角速度
Figure BDA0001670494520000223
具体的，基座角速度获取模块302计算所述云台基座的角速度的公式为：
Figure BDA0001670494520000224
比较模块303，用于将第一偏航角速度与第二偏航角速度进行比较。
其中，所述第一偏航角速度为所述飞行器的初始角速度中的偏航角速度，所述第一偏航角速度为所述云台基座的角速度中的偏航角速度。例如，飞行器的初始角速度为
Figure BDA0001670494520000225
则所述第一偏航角速度为
Figure BDA0001670494520000226
云台基座的角速度为
Figure BDA0001670494520000227
则所述第二偏航角速度为
Figure BDA0001670494520000228
通过比较模块303将第一偏航角速度
Figure BDA0001670494520000229
与第二偏航角速度
Figure BDA00016704945200002210
进行比较，可以确定两者的大小关系、两者的差值等。
修正模块304，用于当所述第一偏航角速度与所述第二偏航角速度的差值大于或等于预设阈值时，根据所述第一偏航角速度与所述第二偏航角速度，对所述飞行器的初始角度中的偏航角进行修正，以得到修正后的所述飞行器的偏航角。
预设阈值确定模块305，用于根据判断是否存在偏航操作，确定所述预设阈值。
其中，预设阈值确定模块305具体用于：当存在偏航操作时，确定所述预设阈值为第一预设阈值；当不存在偏航操作时，确定所述预设阈值为第二预设阈值，所述第二预设阈值小于所述第一预设阈值。
通常在飞行器飞行的过程中，云台基座的偏航角是实时跟随飞行器的偏航角运动而运动的，即第二偏航角速度
Figure BDA0001670494520000231
与第一偏航角速度
Figure BDA0001670494520000232
是几近同步的，考虑飞行器飞行过程中由于飞控不稳导致的偏航角误转动，
Figure BDA0001670494520000233
与
Figure BDA0001670494520000234
满足关系：
Figure BDA0001670494520000235
其中，ζ表示预设阈值。当用户的转航向杆时，也即存在偏航操作时，由于飞行器在航向方向有一个加速运动，云台基座虽然试图及时跟上飞行器第偏航角的运动，但总有一个小滞后，因而此时，预设阈值确定模块305将所述预设阈值确定为第一预设阈值，如ζ＝ζ1＝5。其中，ζ1表示第一预设阈值。当用户不转航向杆时，也即不存在偏航操作时，飞行器在航向方向没有加速运动，此时云台基座可较为及时的跟随飞行器的航向运动，也即预设阈值确定模块305将所述预设阈值确定为第二预设阈值，如ζ＝ζ1＝2。其中，ζ2表示第二预设阈值。并且，所述第二预设阈值ζ2小于所述第一预设阈值ζ1。
如果在某一时刻，飞行器的磁力计受到强干扰且GPS信号较弱，GPS未能起到有效的偏航角修正作用，则飞行器的偏航角估算会突然出错，结果是使得飞行器产生一个相对云台基座的快速转动，产生一个角速度差值
Figure BDA0001670494520000236
其中，η表示所述第一偏航角速度与所述第二偏航角速度的差值，η≥ζ，可判断磁力计受到干扰。此时需要修正模块304对飞行器的偏航角进行修正。
具体的，当所述第一偏航角速度
Figure BDA0001670494520000237
与所述第二偏航角速度
Figure BDA0001670494520000238
的差值η大于或等于预设阈值ζ时，修正模块304根据所述第一偏航角速度
Figure BDA0001670494520000239
与所述第二偏航角速度
Figure BDA00016704945200002310
对所述飞行器的初始角度中的偏航角ψp进行修正，以得到修正后的所述飞行器的偏航角ψ′p。
其中，修正模块304根据所述第一偏航角速度
Figure BDA00016704945200002311
与所述第二偏航角速度
Figure BDA00016704945200002312
对所述飞行器的初始角度中的偏航角ψp进行修正，包括：根据所述第一偏航角速度
Figure BDA0001670494520000241
与所述第二偏航角速度
Figure BDA0001670494520000242
的差值η，得到偏航角补偿值Δψp；根据所述偏航角补偿值Δψp对所述飞行器的初始角度中的偏航角ψp进行修正。
修正模块304根据所述第一偏航角速度与所述第二偏航角速度的差值得到偏航角补偿值的计算公式为：
Figure BDA0001670494520000243
其中，Δψp为偏航角补偿值，t0为获取所述飞行器的初始姿态信息所对应的时间，ΔT为对所述飞行器的初始角度中的偏航角进行修正的周期，根据经验，通常ΔT＝5T，也即，代表每5个采样间隔的时长进行一次偏航角修正，
Figure BDA0001670494520000244
为第一偏航角速度，
Figure BDA0001670494520000245
为第二偏航角速度。
修正模块304得到修正后的所述飞行器的偏航角的计算公式为：
ψ′p＝ψp+Δψp
其中，ψ′p为修正后的偏航角，ψp为所述飞行器的初始角度中的偏航角，Δψp为偏航角补偿值。
需要说明的是，在本发明实施例中，所述飞行器偏航角修正装置30可执行方法实施例所提供的飞行器偏航角修正方法，具备执行方法相应的功能模块和有益效果。未在飞行器偏航角修正装置30的实施例中详尽描述的技术细节，可参见方法发明实施例所提供的飞行器偏航角修正方法。
实施例3：
图4是本发明实施例提供的飞行器硬件结构示意图，如图4所示，所述飞行器40包括：
一个或多个处理器401以及存储器402，图4中以一个处理器401为例。
处理器401和存储器402可以通过总线或者其他方式连接，图4中以通过总线连接为例。
存储器402作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块，如本发明实施例中的飞行器偏航角修正方法对应的程序指令/模块(例如，附图3所示的初始姿态信息获取模块301、基座角速度获取模块302、比较模块303、修正模块304以及预设阈值确定模块405)。处理器401通过运行存储在存储器402中的非易失性软件程序、指令以及单元，从而执行飞行器的各种功能应用以及数据处理，即实现所述方法实施例的飞行器偏航角修正方法。
存储器402可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据飞行器使用所创建的数据等。此外，存储器402可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器402可选包括相对于处理器401远程设置的存储器，这些远程存储器可以通过网络连接至飞行器。所述网络的实施例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
所述一个或者多个单元存储在所述存储器402中，当被所述一个或者多个处理器401执行时，执行所述方法实施例中的飞行器偏航角修正方法，例如，执行以上描述的图1中的方法步骤101至步骤104，实现图3中的301-305模块的功能。
请参阅图5和图6，所述飞行器40还包括动力系统403，所述动力系统403用于飞行器提供飞行动力，所述动力系统403与处理器401连接。所述动力系统403包括：驱动电机4031及电调4032，所述电调4032与驱动电机4031电连接，用于控制所述驱动电机4031。具体的，所述电调4032基于处理器401执行上述飞行器偏航角修正方法后得到的修正后的所述飞行器的偏航角，生成控制指令，通过控制指令控制该驱动电机4032。
所述飞行器40可执行本发明实施例1所提供的飞行器偏航角修正方法，具备执行方法相应的功能模块和有益效果。未在飞行器实施例中详尽描述的技术细节，可参见本发明实施例1所提供的飞行器偏航角修正方法。
本发明实施例提供了一种计算机程序产品，所述计算机程序产品包括存储在非易失性计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，使所述计算机执行如上所述的飞行器偏航角修正方法。例如，执行以上描述的图1中的方法步骤101至步骤104，实现图3中的301-305模块的功能。
本发明实施例提供了一种非易失性计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行如上所述的飞行器偏航角修正方法。例如，执行以上描述的图1中的方法步骤101至步骤104，实现图3中的301-305模块的功能。
需要说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理模块，即可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。
通过以上的实施例的描述，本领域普通技术人员可以清楚地了解到各实施例可借助软件加通用硬件平台的方式来实现，当然也可以通过硬件。本领域普通技术人员可以理解实现所述实施例方法中的全部或部分流程是可以通过计算机程序指令相关的硬件来完成，所述的程序可存储于计算机可读取存储介质中，该程序在执行时，可包括如所述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-OnlyMemory,ROM)或随机存储记忆体(RandomAccessMemory,RAM)等。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明的不同方面的许多其它变化，为了简明，它们没有在细节中提供；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (20)

1.一种飞行器偏航角修正方法，其特征在于，所述飞行器连接有云台，所述云台包括云台电机及云台基座，所述飞行器设置有姿态传感器组件，所述方法包括：
获取所述飞行器的初始姿态信息，所述飞行器的初始姿态信息由所述姿态传感器组件采集得到，所述飞行器的初始姿态信息包括飞行器的初始角速度及飞行器的初始角度；
获取所述云台基座的角速度；
将第一偏航角速度与第二偏航角速度进行比较，所述第一偏航角速度为所述飞行器的初始角速度中的偏航角速度，所述第二偏航角速度为所述云台基座的角速度中的偏航角速度；
当所述第一偏航角速度与所述第二偏航角速度的差值大于或等于预设阈值时，根据所述第一偏航角速度与所述第二偏航角速度，对所述飞行器的初始角度中的偏航角进行修正，以得到修正后的所述飞行器的偏航角。
2.根据权利要求1所述的方法，其特征在于，所述预设阈值由是否存在偏航操作所确定。
3.根据权利要求2所述的方法，其特征在于，所述预设阈值由是否存在偏航操作所确定包括：
当存在偏航操作时，所述预设阈值为第一预设阈值；
当不存在偏航操作时，所述预设阈值为第二预设阈值，所述第二预设阈值小于所述第一预设阈值。
4.根据权利要求1-3任一项所述的方法，其特征在于，所述根据所述第一偏航角速度与所述第二偏航角速度，对所述飞行器的初始角度中的偏航角进行修正，包括：
根据所述第一偏航角速度与所述第二偏航角速度的差值，得到偏航角补偿值；
根据所述偏航角补偿值对所述飞行器的初始角度中的偏航角进行修正。
5.根据权利要求4所述的方法，其特征在于，根据所述第一偏航角速度与所述第二偏航角速度的差值得到偏航角补偿值的计算公式为：
Figure FDA0002467893170000021
其中，Δψp为偏航角补偿值，t0为获取所述飞行器的初始姿态信息所对应的时间，ΔT为对所述飞行器的初始角度中的偏航角进行修正的周期，
Figure FDA0002467893170000022
为第一偏航角速度，
Figure FDA0002467893170000023
为第二偏航角速度。
6.根据权利要求5所述的方法，其特征在于，得到修正后的所述飞行器的偏航角的计算公式为：
ψ′p＝ψp+Δψp
其中，ψ′p为修正后的偏航角，ψp为所述飞行器的初始角度中的偏航角，Δψp为偏航角补偿值。
7.根据权利要求1所述的方法，其特征在于，所述云台上搭载有拍摄装置，所述拍摄装置设置有惯性测量单元；
所述获取所述云台基座的角速度，包括：
获取所述云台电机的角度；
根据所述云台电机的角度，确定所述云台电机的角速度；
获取所述拍摄装置的角速度，所述拍摄装置的角速度由所述惯性测量单元采集得到；
根据所述云台电机的角速度、所述云台电机的角度及所述拍摄装置的角速度，确定所述云台基座的角速度。
8.根据权利要求7所述的方法，其特征在于，根据所述云台电机的角度，确定所述云台电机的角速度，包括：
将所述云台电机的角度作为输入，通过二阶离散非线性跟踪微分器，计算得到所述云台电机的角速度。
9.根据权利要求8所述的方法，其特征在于，所述二阶离散非线性跟踪微分器的表达式为：
r1(k+1)＝r1(k)+T·r2(k)
r2(k+1)＝r2(k)+T·fst(r1(k)-Ρ(k),r2(k),δ,h)
其中，T为获取所述云台电机的角度的采样周期，Ρ(k)＝[φ(k) θ(k) ψ(k)]T为第k时刻所述云台电机的角度，φ(k)为第k时刻所述云台电机的角度中的翻滚角，θ(k)为第k时刻所述云台电机的角度中的俯仰角，ψ(k)为第k时刻所述云台电机的角度中的偏航角，r1(k)为通过所述二阶离散非线性跟踪微分器对Ρ(k)进行跟踪由Ρ(k)所确定的值，r2(k)为Ρ(k)的导数，r1(k+1)为通过所述二阶离散非线性跟踪微分器对第k+1时刻所述云台电机的角度进行跟踪由第k+1时刻所述云台电机的角度所确定的值，r2(k+1)为第k+1时刻所述云台电机的角度的导数，fst()为最速控制函数，δ为位于最速控制函数的第三位的参数，h为位于最速控制函数的第四位的参数。
10.根据权利要求9所述的方法，其特征在于，所述根据所述云台电机的角速度、所述云台电机的角度及所述拍摄装置的角速度，确定所述云台基座的角速度，包括：
根据所述云台电机的角度确定旋转变换矩阵，所述旋转变换矩阵为云台基座坐标系到云台电机坐标系的旋转矩阵；
根据所述云台电机的角速度、所述旋转变换矩阵及所述拍摄装置的角速度，确定所述云台基座的角速度。
11.根据权利要求10所述的方法，其特征在于，根据所述云台电机的角度确定旋转变换矩阵的计算公式为：
Figure FDA0002467893170000041
其中，D为旋转变换矩阵；(φ,θ,ψ)为所述云台电机的角度，φ为所述云台电机的角度中的翻滚角，θ为所述云台电机的角度中的俯仰角，ψ为所述云台电机的角度中的偏航角。
12.根据权利要求11所述的方法，其特征在于，根据所述云台电机的角速度、所述旋转变换矩阵及所述拍摄装置的角速度确定所述云台基座的角速度的计算公式为：
Figure FDA0002467893170000042
其中，
Figure FDA0002467893170000043
为所述云台基座的角速度，
Figure FDA0002467893170000044
为所述拍摄装置的角速度，D为旋转变换矩阵，r2为所述云台电机的角速度。
13.一种飞行器偏航角修正装置，其特征在于，所述飞行器连接有云台，所述云台包括云台电机及云台基座，所述飞行器设置有姿态传感器组件，所述装置包括：
初始姿态信息获取模块，用于获取所述飞行器的初始姿态信息，所述飞行器的初始姿态信息由所述姿态传感器组件采集得到，所述飞行器的初始姿态信息包括飞行器的初始角速度及飞行器的初始角度；
基座角速度获取模块，用于获取所述云台基座的角速度；
比较模块，用于将第一偏航角速度与第二偏航角速度进行比较，所述第一偏航角速度为所述飞行器的初始角速度中的偏航角速度，所述第二偏航角速度为所述云台基座的角速度中的偏航角速度；
修正模块，用于当比较模块确定到所述第一偏航角速度与所述第二偏航角速度的差值大于或等于预设阈值时，根据所述第一偏航角速度与所述第二偏航角速度，对所述飞行器的初始角度中的偏航角进行修正，以得到修正后的所述飞行器的偏航角。
14.根据权利要求13所述的装置，其特征在于，所述装置还包括预设阈值确定模块，用于根据判断是否存在偏航操作，确定所述预设阈值。
15.根据权利要求14所述的装置，其特征在于，所述预设阈值确定模块具体用于：
当存在偏航操作时，确定所述预设阈值为第一预设阈值；
当不存在偏航操作时，确定所述预设阈值为第二预设阈值，所述第二预设阈值小于所述第一预设阈值。
16.根据权利要求13-15任一项所述的装置，其特征在于，所述修正模块根据所述第一偏航角速度与所述第二偏航角速度，对所述飞行器的初始角度中的偏航角进行修正，包括：
根据所述第一偏航角速度与所述第二偏航角速度的差值，得到偏航角补偿值；
根据所述偏航角补偿值对所述飞行器的初始角度中的偏航角进行修正。
17.根据权利要求13所述的装置，其特征在于，所述云台上搭载有拍摄装置，所述拍摄装置设置有惯性测量单元；
所述基座角速度获取模块具体用于：
获取所述云台电机的角度；
根据所述云台电机的角度，确定所述云台电机的角速度；
获取所述拍摄装置的角速度，所述拍摄装置的角速度由所述惯性测量单元采集得到；
根据所述云台电机的角速度、所述云台电机的角度及所述拍摄装置的角速度，确定所述云台基座的角速度。
18.根据权利要求17所述的装置，其特征在于，所述基座角速度获取模块根据所述云台电机的角度，确定所述云台电机的角速度，包括：
将所述云台电机的角度作为输入，通过二阶离散非线性跟踪微分器，计算得到所述云台电机的角速度。
19.根据权利要求17或18所述的装置，其特征在于，所述基座角速度获取模块根据所述云台电机的角速度、所述云台电机的角度及所述拍摄装置的角速度，确定所述云台基座的角速度，包括：
根据所述云台电机的角度确定旋转变换矩阵，所述旋转变换矩阵为云台基座坐标系到云台电机坐标系的旋转矩阵；
根据所述云台电机的角速度、所述旋转变换矩阵及所述拍摄装置的角速度，确定所述云台基座的角速度。
20.一种飞行器，其特征在于，包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-12的任一项所述的方法。
CN201810501521.3A 2018-05-23 2018-05-23 飞行器偏航角修正方法、装置及飞行器 Active CN108549399B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201810501521.3A CN108549399B (zh) 2018-05-23 2018-05-23 飞行器偏航角修正方法、装置及飞行器
PCT/CN2018/116719 WO2019223271A1 (zh) 2018-05-23 2018-11-21 飞行器偏航角修正方法、装置及飞行器

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810501521.3A CN108549399B (zh) 2018-05-23 2018-05-23 飞行器偏航角修正方法、装置及飞行器

Publications (2)

Publication Number Publication Date
CN108549399A CN108549399A (zh) 2018-09-18
CN108549399B true CN108549399B (zh) 2020-08-21

Family

ID=63495639

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810501521.3A Active CN108549399B (zh) 2018-05-23 2018-05-23 飞行器偏航角修正方法、装置及飞行器

Country Status (2)

Country Link
CN (1) CN108549399B (zh)
WO (1) WO2019223271A1 (zh)

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108549399B (zh) * 2018-05-23 2020-08-21 深圳市道通智能航空技术有限公司 飞行器偏航角修正方法、装置及飞行器
CN110770671A (zh) * 2018-09-30 2020-02-07 深圳市大疆创新科技有限公司 云台及其控制方法、可移动平台
WO2020062281A1 (zh) * 2018-09-30 2020-04-02 深圳市大疆创新科技有限公司 云台的控制方法、云台、可移动平台及可读存储介质
CN111433702B (zh) * 2018-10-31 2022-04-15 深圳市道通智能航空技术股份有限公司 无人机及其云台控制方法
CN109557484B (zh) * 2018-11-14 2021-07-20 深圳增强现实技术有限公司 磁场检测方法及系统、终端
CN109725652B (zh) * 2018-11-16 2022-04-01 广州昂宝电子有限公司 用于无人机云台航向解算与控制的方法
CN111656299A (zh) * 2019-04-28 2020-09-11 深圳市大疆创新科技有限公司 一种云台控制方法、设备、可移动平台及存储介质
CN110304270B (zh) * 2019-06-03 2021-01-05 宁波天擎航天科技有限公司 用于运载火箭的全方位发射控制方法、装置、计算机设备
CN110260888B (zh) * 2019-06-06 2021-10-15 航天科工仿真技术有限责任公司 一种摆动角测量方法、装置及系统
CN110440805B (zh) * 2019-08-09 2021-09-21 深圳市道通智能航空技术股份有限公司 一种偏航角的融合方法、装置及飞行器
CN110377056B (zh) * 2019-08-19 2022-09-20 深圳市道通智能航空技术股份有限公司 无人机航向角初值选取方法及无人机
CN110487277B (zh) 2019-08-21 2021-07-30 深圳市道通智能航空技术股份有限公司 一种偏航角的融合方法、装置及飞行器
CN110377058B (zh) * 2019-08-30 2021-11-09 深圳市道通智能航空技术股份有限公司 一种飞行器的偏航角修正方法、装置及飞行器
CN114077719A (zh) * 2020-08-13 2022-02-22 华为技术有限公司 一种自运动估计方法及相关装置
CN116197891B (zh) * 2021-12-01 2024-04-02 北京小米机器人技术有限公司 控制机器人运动的方法、装置、机器人及存储介质
CN116630374B (zh) * 2023-07-24 2023-09-19 贵州翰凯斯智能技术有限公司 目标对象的视觉跟踪方法、装置、存储介质及设备

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5331558A (en) * 1991-11-19 1994-07-19 Raytheon Company Autopilot having an adaptive deadband feature

Family Cites Families (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100102173A1 (en) * 2008-10-21 2010-04-29 Everett Michael L Light Aircraft Stabilization System
US8352099B1 (en) * 2009-07-09 2013-01-08 The Boeing Company Varying engine thrust for directional control of an aircraft experiencing engine thrust asymmetry
TW201643579A (zh) * 2015-06-15 2016-12-16 鴻海精密工業股份有限公司 無人飛行載具自動啟動系統及方法
CN105988474A (zh) * 2015-07-06 2016-10-05 深圳市前海疆域智能科技股份有限公司 一种飞行器的偏差补偿方法和飞行器
CN105468014B (zh) * 2016-01-18 2018-07-31 中国人民解放军国防科学技术大学 一种单自驾仪一体化的飞行器系统及其二维云台控制方法
CN105857595A (zh) * 2016-04-23 2016-08-17 北京工业大学 一种基于云台的小型飞行器系统
CN106020220B (zh) * 2016-05-24 2023-12-08 零度智控（北京）智能科技有限公司 无人机、无人机飞行控制方法及装置
CN106249745B (zh) * 2016-07-07 2019-02-19 苏州大学 四轴无人机的控制方法
WO2018023492A1 (zh) * 2016-08-03 2018-02-08 深圳市大疆灵眸科技有限公司 一种云台控制方法及系统
CN106774390A (zh) * 2016-12-09 2017-05-31 深圳市道通智能航空技术有限公司 一种飞行器航向控制方法、装置和电子设备
WO2018120059A1 (zh) * 2016-12-30 2018-07-05 深圳市大疆灵眸科技有限公司 用于云台的控制方法、控制系统、云台和无人飞行器
CN107074348B (zh) * 2016-12-30 2021-06-15 深圳市大疆创新科技有限公司 控制方法、装置、设备及无人飞行器
CN206417213U (zh) * 2016-12-30 2017-08-18 深圳市大疆灵眸科技有限公司 云台和无人飞行器
CN107656532A (zh) * 2017-09-12 2018-02-02 歌尔科技有限公司 减小无人机偏航数据漂移的方法、装置、系统和无人机
CN107831791B (zh) * 2017-11-17 2020-12-15 深圳意动航空科技有限公司 一种无人机的操控方法、装置、操控设备及存储介质
CN108549399B (zh) * 2018-05-23 2020-08-21 深圳市道通智能航空技术有限公司 飞行器偏航角修正方法、装置及飞行器

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5331558A (en) * 1991-11-19 1994-07-19 Raytheon Company Autopilot having an adaptive deadband feature

Also Published As

Publication number Publication date
CN108549399A (zh) 2018-09-18
WO2019223271A1 (zh) 2019-11-28

Similar Documents

Publication Publication Date Title
CN108549399B (zh) 飞行器偏航角修正方法、装置及飞行器
WO2019223270A1 (zh) 云台电机角度和角速度估算方法、装置、云台及飞行器
CN109000612B (zh) 设备的角度估算方法、装置、摄像组件及飞行器
CN110440805B (zh) 一种偏航角的融合方法、装置及飞行器
CN110347171B (zh) 一种飞行器控制方法及飞行器
CN110377058B (zh) 一种飞行器的偏航角修正方法、装置及飞行器
CN110487277B (zh) 一种偏航角的融合方法、装置及飞行器
WO2018053877A1 (zh) 控制方法、控制设备和运载系统
CN105045293B (zh) 云台控制方法、外部载体控制方法及云台
WO2021052334A1 (zh) 一种无人飞行器的返航方法、装置及无人飞行器
WO2018099260A1 (zh) 一种云台姿态控制方法及系统
CN108780331B (zh) 云台控制方法和设备、云台以及无人机
Bhargavapuri et al. Vision-based autonomous tracking and landing of a fully-actuated rotorcraft
CN111977006B (zh) 一种关节角的初始化方法、装置及飞行器
CN104811588A (zh) 一种基于陀螺仪的船载稳像控制方法
WO2020042159A1 (zh) 一种云台的转动控制方法、装置及控制设备、移动平台
CN114578841A (zh) 无人机及其云台控制方法
CN114604439B (zh) 一种面向扑翼飞行机器人的航拍视频稳像系统
WO2021168821A1 (zh) 可移动平台的控制方法和设备
CN116540753A (zh) 一种无人机着陆控制方法
WO2022134036A1 (zh) 可移动平台及其控制方法、装置
CN113110542A (zh) 基于单轴云台的四旋翼无人机目标追踪系统
Cabecinhas et al. Vision-based quadrotor stabilization using a pan and tilt camera
WO2018103192A1 (zh) 一种无人机姿态保持方法及装置
WO2022040883A1 (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
CP01 Change in the name or title of a patent holder

Address after: 518055 Guangdong city of Shenzhen province Nanshan District Xili Street Xueyuan Road No. 1001 Chi Yuen Building 9 layer B1

Patentee after: Shenzhen daotong intelligent Aviation Technology Co.,Ltd.

Address before: 518055 Guangdong city of Shenzhen province Nanshan District Xili Street Xueyuan Road No. 1001 Chi Yuen Building 9 layer B1

Patentee before: AUTEL ROBOTICS Co.,Ltd.

CP01 Change in the name or title of a patent holder