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

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

Info

Publication number
WO2018184467A1
WO2018184467A1 PCT/CN2018/079517 CN2018079517W WO2018184467A1 WO 2018184467 A1 WO2018184467 A1 WO 2018184467A1 CN 2018079517 W CN2018079517 W CN 2018079517W WO 2018184467 A1 WO2018184467 A1 WO 2018184467A1
Authority
WO
WIPO (PCT)
Prior art keywords
data
pan
measurement unit
coordinate system
acceleration
Prior art date
Application number
PCT/CN2018/079517
Other languages
English (en)
French (fr)
Inventor
胡华智
孙海洋
Original Assignee
亿航智能设备（广州）有限公司
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 亿航智能设备（广州）有限公司 filed Critical 亿航智能设备（广州）有限公司
Publication of WO2018184467A1 publication Critical patent/WO2018184467A1/zh

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

Definitions

  • the invention relates to the technical field of drones, and in particular to a method and device for detecting attitude of a gimbal.
  • attitude representation method of airborne gimbal can be divided into: Euler angle, quaternion and rotation matrix.
  • the quaternion avoids the singularity problem of Euler angle, but the expression angle of Euler angle is more intuitive;
  • the rotation matrix requires 9 variables to represent the pose.
  • the quaternion only needs less 4 variables to express the pose.
  • the integral operation and normalization of the quaternion are more convenient than the rotation matrix. However, it is convenient for the quaternion to have no rotation matrix when performing vector rotation.
  • the main object of the present invention is to provide a method and device for detecting attitude of a gimbal, which can acquire the attitude data of the gimbal through the attitude and heading system, adopt different algorithms to perform the posture representation of the gimbal, and compensate and correct the posture of the gimbal, thereby improving the posture.
  • the accuracy of the attitude detection of the gimbal is to provide a method and device for detecting attitude of a gimbal, which can acquire the attitude data of the gimbal through the attitude and heading system, adopt different algorithms to perform the posture representation of the gimbal, and compensate and correct the posture of the gimbal, thereby improving the posture.
  • the accuracy of the attitude detection of the gimbal is to provide a method and device for detecting attitude of a gimbal, which can acquire the attitude data of the gimbal through the attitude and heading system, adopt different algorithms to perform the posture representation of the gimbal, and compensate and correct the posture of the gimbal, thereby improving the posture.
  • the present invention provides a pan/tilt attitude detection method, including:
  • the data of the inertial measurement unit including angular velocity data of the gyroscope and accelerometer data;
  • the performing quaternion integral operation on the data of the inertial measurement unit includes:
  • the rotation matrix is expressed as feedback, and feedback correction is performed on the quaternion expression.
  • the angular velocity data of the gyroscope includes a three-axis angular velocity of the body.
  • the expressing, by using the rotation matrix as feedback, performing feedback correction on the quaternion expression includes:
  • the rotation matrix expression is multiplied by the gravity acceleration in the geodetic coordinate system to obtain acceleration data in the body coordinate system;
  • the rotation difference vector is amplified to participate in a quaternion integral operation of the pan-tilt attitude data as a calibration angular velocity.
  • the multiplying the rotation matrix representation with the gravity acceleration in the geodetic coordinate system to obtain the acceleration data in the body coordinate system comprises:
  • Real-time acceleration data is acquired by the UAV navigation system, and the real-time acceleration data is added to the gravity acceleration in the geodetic coordinate system to obtain the compensated acceleration data;
  • the rotation matrix expression is multiplied by the compensated acceleration data to obtain acceleration data in the body coordinate system.
  • the method before performing the quaternion integral operation on the data of the inertial measurement unit, the method includes:
  • the Euler angle is expressed as feedback, and a difference operation is performed on the yaw angle of the pan-tilt to correct the yaw angle of the pan-tilt.
  • a cloud platform attitude detecting apparatus comprising:
  • An attitude data acquiring module configured to acquire data of an inertial measurement unit of the pan/tilt, wherein the data of the inertial measurement unit includes angular velocity data of the gyroscope and accelerometer data;
  • a quaternion integral module configured to perform quaternion integral operation on data of the inertial measurement unit
  • a rotation matrix module configured to convert data of the inertial measurement unit into a rotation matrix representation by a quaternion expression
  • An Euler angle module for converting data of the inertial measurement unit from a rotation matrix representation to an Euler angle representation
  • the feedback correction module is configured to perform feedback correction on the quaternion expression by using the rotation matrix as feedback.
  • the angular velocity data of the gyroscope includes a three-axis angular velocity of the body.
  • the feedback correction module includes:
  • a coordinate system conversion unit configured to perform a product of the rotation matrix representation and the gravity acceleration in the geodetic coordinate system to obtain acceleration data in the body coordinate system
  • a correction unit configured to cross-product multiply the acceleration data in the body coordinate system with the accelerometer data to obtain a rotation difference vector
  • the enlargement updating unit is configured to amplify the rotation difference vector and participate in a quaternion integral operation of the pan-tilt attitude data as a calibration angular velocity.
  • the coordinate system conversion unit includes:
  • a compensation unit configured to acquire real-time acceleration data by using a UAV navigation system, and add the real-time acceleration data to the gravity acceleration in the geodetic coordinate system to obtain compensated acceleration data;
  • the product operation unit is configured to perform a product operation on the rotation matrix expression and the compensated acceleration data to obtain acceleration data in a body coordinate system.
  • it also includes:
  • a yaw angle correction module for acquiring a yaw angle of the drone, and calculating a deviation of the yaw angle of the drone from the yaw angle of the gimbal by a magnetic encoder, thereby obtaining a yaw angle of the gimbal;
  • the difference calculation is performed on the yaw angle of the gimbal to correct the yaw angle of the gimbal.
  • a method and apparatus for detecting a posture of a pan/tilt head comprising: acquiring data of an inertial measurement unit of a gimbal, the data of the inertial measurement unit comprising angular velocity data of the gyroscope and accelerometer data; The data of the measuring unit is subjected to a quaternion integral operation; the data of the inertial measurement unit is converted into a rotation matrix representation by a quaternion expression; and the data of the inertial measurement unit is converted into a Euler angle expression by a rotation matrix representation; Performing the quaternion integral operation on the data of the inertial measurement unit includes: using the rotation matrix to express feedback, performing feedback correction on the quaternion expression, and acquiring pan-tilt attitude data through the attitude heading system, Different algorithms perform the attitude representation of the gimbal and compensate and correct the attitude of the gimbal, which improves the accuracy of the attitude detection of the gimbal.
  • the algorithm Compared with Kalman filtering or other attitude fusion algorithms, the algorithm has the advantage of less computation.
  • a pan/tilt system suitable for computing resources is small, which contributes to the miniaturization of the gimbal; compared to using only gyroscopes and
  • the attitude meter solves the attitude calculation method of the attitude of the gimbal.
  • the algorithm makes full use of the hardware resources of the PTZ system, including the magnetic encoder data of the PTZ motor board, the heading angle data of the flight control, and the fly. Controlled acceleration data for more accurate attitude data.
  • FIG. 1 is a flowchart of a PTZ attitude detection method according to Embodiment 1 of the present invention
  • FIG. 2 is a schematic structural diagram of hardware of an airborne PTZ system according to Embodiment 1 of the present invention
  • FIG. 3 is a flow chart of the method of step S21 of Figure 1;
  • FIG. 4 is a flowchart of data processing of a pan/tilt attitude detecting method according to Embodiment 1 of the present invention.
  • FIG. 5 is a flowchart of data processing for acceleration compensation according to Embodiment 1 of the present invention.
  • FIG. 6 is a flowchart of data processing for yaw angle correction according to Embodiment 1 of the present invention.
  • FIG. 7 is a block diagram showing an exemplary structure of a pan/tilt attitude detecting apparatus according to Embodiment 2 of the present invention.
  • FIG. 8 is a block diagram showing an exemplary structure of the feedback correction module of FIG. 7;
  • FIG. 9 is a block diagram showing an exemplary structure of another pan/tilt attitude detecting apparatus according to Embodiment 2 of the present invention.
  • a pan/tilt attitude detection method includes:
  • the method further includes:
  • the attitude of the pan-tilt is obtained by the attitude heading system, and the attitude data of the pan-tilt includes the data of the inertial measurement unit, and the gesture representation of the gimbal is performed by using different algorithms, and the posture of the gimbal is compensated and corrected, and the posture is improved.
  • the algorithm has the advantage of less computational complexity, and is suitable for the PTZ system with few computing resources, which is helpful for the miniaturization of the PTZ;
  • the algorithm makes full use of the hardware resources of the PTZ system, including the use of the magnetic encoder data of the PTZ motor board.
  • the flight control angle data of the flight control and the acceleration data of the flight control can obtain more accurate attitude data.
  • FIG. 2 The hardware diagram of the system is shown in FIG. 2, including: a pan/tilt, a flight control system, a drone, a magnetic encoder, and a motor M, wherein the attitude of the gimbal
  • the detection device selects the MPU6000;
  • the flight control system is provided with an attitude and heading system AHRS (Attitude and Heading Reference System), and the attitude heading system includes a gyroscope and an accelerometer for acquiring the three-axis angular velocity and acceleration data of the body;
  • the controller communicates with the drone and the flight control system through a controller area network CAN (Controller Area Network);
  • the pan/tilt communicates with the magnetic encoder and the MPU 6000 through a serial peripheral interface SPI (Serial Peripheral Interface).
  • SPI Serial Peripheral Interface
  • the quaternion algorithm only needs four variables to express the attitude of the gimbal, for example: three-axis angular velocity, which is suitable for short-term expression of the attitude of the gimbal; and the representation method of the rotation matrix is convenient when performing vector rotation.
  • the posture of Euler's angle is more intuitive, and it is suitable for the external output of the attitude of the gimbal, such as output to the attitude controller and the host computer.
  • the gyroscope detects the angular velocity information of the body, and the response speed is fast, but the integral interference and the integral drift caused by the zero point drift with the temperature are obtained. After the gyroscope detects the angular velocity of the three axes of the body coordinate system, The real-time pose is calculated simply and quickly using the quaternion integral method.
  • the conversion formula of the quaternion and the rotation matrix is:
  • the acceleration sensor is used to measure the support force vector.
  • the measured support force vector is opposite to the gravity acceleration vector, the magnitude is equal, and the response speed is also very high, but it is caused by the vibration of the body.
  • Frequency interference combined with the characteristics of two attitude sensors, the overall idea of attitude calculation is: gyroscope integration to obtain high response performance; use accelerometer fusion correction for a long time to correct integral drift.
  • the step S21 includes:
  • the data processing flowchart of the pan/tilt attitude detecting method is as shown in FIG. 4, wherein the module 1 represents angular velocity data of the gyroscope, the module 2 represents accelerometer data, and the Gef is gravity in the geodetic coordinate system. Acceleration.
  • step S211 includes:
  • Real-time acceleration data is acquired by the UAV navigation system, and the real-time acceleration data is added to the gravity acceleration in the geodetic coordinate system to obtain the compensated acceleration data;
  • the rotation matrix expression is multiplied by the compensated acceleration data to obtain acceleration data in the body coordinate system.
  • the geodetic coordinate system is the earth coordinate system
  • the body coordinate system is the coordinate system of the gimbal itself
  • the accelerometer data is the acceleration value of the gimbal measured under the body coordinate system, which is G bf +a bf .
  • step S211 because the actual measurement of the PTZ accelerometer itself has a deviation, in order to obtain the actual value of the PTZ accelerometer continuously approaching the correct value, the rotation matrix of the PTZ attitude is regarded as a negative feedback to The resulting gesture is more accurate.
  • the accelerometer detection value is G bf + a bf
  • the acceleration data at this time has a linear acceleration a bf in addition to the gravitational acceleration G bf , and still If the acceleration data is used for attitude fusion, the attitude detection will be greatly deviated.
  • the common method is to adjust the acceleration weight value of the acceleration to be small enough (too small will affect the effect of the correction gyroscope) to reduce the linear acceleration of the high frequency. Bf (such as the high-frequency line acceleration caused by vibration) on the attitude detection.
  • the scheme transmits the real acceleration data a uav of the unmanned aerial vehicle navigation system to the cloud platform for acceleration compensation through CAN, and the drone has a positioning sensor including a GPS, a barometer, an optical flow sensor, and Ultrasonic waves, etc., the calculated acceleration data a uav error is smaller.
  • the compensated acceleration mode length is dynamically calculated, and compared with the gravity acceleration weight, the reliability of the compensated acceleration data is evaluated, and the acceleration fusion weight is dynamically adjusted according to the credibility to improve the filtering of the accelerometer noise. effect.
  • FIG. 5 a data processing flowchart of the pan-tilt attitude detecting method after the acceleration compensation is added is shown in FIG. 5, wherein the module 3 indicates that the drone navigation system acquires real-time acceleration data.
  • the pan/tilt attitude data further includes a yaw angle, and correspondingly, before performing quaternion integral operation on the data of the inertial measurement unit, the method includes:
  • the Euler angle is expressed as feedback, and a difference operation is performed on the yaw angle of the pan-tilt to correct the yaw angle of the pan-tilt.
  • the yaw angle calculated by the AHRS system built by the above method does not refer to the north, and due to the temperature drift of the gyroscope and the integral error, the yaw angle yaw will slowly drift, eventually leading to the lock of the gimbal. In the state (the pylon is stable at a certain yaw angle on the ground), the captured picture will continue to shift slowly.
  • the solution uses existing sensors, including gyroscopes, accelerometers, magnetic encoders, and sensor information of the drone to perform fused correction of the yaw angle.
  • FIG. 6 a data processing flowchart of the pan-tilt attitude detection method after the yaw angle fusion correction is added is shown in FIG. 6, wherein the module 4 represents the pan-tilt angle calculated by the magnetic encoder.
  • the calculation method of the deviation angle yaw offset_from_UAV_to_gimbal of the drone and the gimbal is: the attitude relationship between the gimbal and the drone is:
  • R gimbal R ⁇ R ⁇
  • yaw offset_from_UAV_to_gimbal atan2(-R UAV 12, R UAV 22
  • the rotation matrix of each axis is:
  • a pan/tilt attitude detecting apparatus includes:
  • the attitude data acquiring module 10 is configured to acquire data of an inertial measurement unit of the pan/tilt, and the data of the inertial measurement unit includes angular velocity data of the gyroscope and accelerometer data;
  • the quaternion integral module 20 is configured to perform quaternion integral operation on the data of the inertial measurement unit;
  • a rotation matrix module 30 configured to convert data of the inertial measurement unit into a rotation matrix representation by a quaternion expression
  • the Euler angle module 40 is configured to convert data of the inertial measurement unit into a Euler angle expression from a rotation matrix representation
  • the feedback correction module 50 is configured to perform feedback correction on the quaternion expression by using the rotation matrix as feedback.
  • the attitude of the pan-tilt is obtained by the attitude heading system, and the attitude data of the pan-tilt includes the data of the inertial measurement unit, and the gesture representation of the gimbal is performed by using different algorithms, and the posture of the gimbal is compensated and corrected, and the posture is improved.
  • the algorithm has the advantage of less computational complexity, and is suitable for the PTZ system with few computing resources, which is helpful for the miniaturization of the PTZ;
  • the algorithm makes full use of the hardware resources of the PTZ system, including the use of the magnetic encoder data of the PTZ motor board.
  • the flight control angle data of the flight control and the acceleration data of the flight control can obtain more accurate attitude data.
  • FIG. 2 The hardware diagram of the system is shown in FIG. 2, including: a pan/tilt, a flight control system, a drone, a magnetic encoder, and a motor M, wherein the attitude of the gimbal
  • the detection device selects the MPU6000;
  • the flight control system is provided with an attitude and heading system AHRS (Attitude and Heading Reference System), and the attitude heading system includes a gyroscope and an accelerometer for acquiring the three-axis angular velocity and acceleration data of the body;
  • the controller communicates with the drone and the flight control system through a controller area network CAN (Controller Area Network);
  • the pan/tilt communicates with the magnetic encoder and the MPU 6000 through a serial peripheral interface SPI (Serial Peripheral Interface).
  • SPI Serial Peripheral Interface
  • the quaternion algorithm only needs four variables to express the attitude of the gimbal, for example: three-axis angular velocity, which is suitable for short-term expression of the attitude of the gimbal; and the representation method of the rotation matrix is convenient when performing vector rotation.
  • the posture of Euler's angle is more intuitive, and it is suitable for the external output of the attitude of the gimbal, such as output to the attitude controller and the host computer.
  • the gyroscope detects the angular velocity information of the body, and the response speed is fast, but the integral interference and the integral drift caused by the zero point drift with the temperature are obtained. After the gyroscope detects the angular velocity of the three axes of the body coordinate system, The real-time pose is calculated simply and quickly using the quaternion integral method.
  • the conversion formula of the quaternion and the rotation matrix is:
  • the acceleration sensor is used to measure the support force vector.
  • the measured support force vector is opposite to the gravity acceleration vector, the magnitude is equal, and the response speed is also very high, but it is caused by the vibration of the body.
  • Frequency interference combined with the characteristics of two attitude sensors, the overall idea of attitude calculation is: gyroscope integration to obtain high response performance; use accelerometer fusion correction for a long time to correct integral drift.
  • the feedback correction module includes:
  • the coordinate system conversion unit 51 is configured to perform a product operation on the rotation matrix expression and the gravity acceleration in the geodetic coordinate system to obtain acceleration data in the body coordinate system;
  • the correcting unit 52 is configured to cross-product the acceleration data in the body coordinate system and the accelerometer data to obtain the corrected accelerometer data;
  • the enlargement updating unit 53 is configured to amplify the corrected accelerometer data and participate in the quaternion integral operation of the pan-tilt attitude data as the updated accelerometer data.
  • the data processing flowchart of the pan/tilt attitude detecting method is as shown in FIG. 4, wherein the module 1 represents angular velocity data of the gyroscope, the module 2 represents accelerometer data, and the Gef is gravity in the geodetic coordinate system. Acceleration.
  • the coordinate system conversion unit includes:
  • a compensation unit configured to acquire real-time acceleration data by using a UAV navigation system, and add the real-time acceleration data to the gravity acceleration in the geodetic coordinate system to obtain compensated acceleration data;
  • the product operation unit is configured to perform a product operation on the rotation matrix expression and the compensated acceleration data to obtain acceleration data in a body coordinate system.
  • the geodetic coordinate system is the earth coordinate system
  • the body coordinate system is the coordinate system of the gimbal itself
  • the accelerometer data is the acceleration value of the gimbal measured under the body coordinate system, which is G bf +a bf .
  • the rotation matrix of the PTZ attitude is regarded as a negative feedback to make the attitude of the PTZ final output more accurate.
  • the accelerometer detection value is G bf + a bf
  • the acceleration data at this time has a linear acceleration a bf in addition to the gravitational acceleration G bf , and still If the acceleration data is used for attitude fusion, the attitude detection will be greatly deviated.
  • the common method is to adjust the acceleration weight value of the acceleration to be small enough (too small will affect the effect of the correction gyroscope) to reduce the linear acceleration of the high frequency. Bf (such as the high-frequency line acceleration caused by vibration) on the attitude detection.
  • the scheme transmits the real acceleration data a uav of the unmanned aerial vehicle navigation system to the cloud platform for acceleration compensation through CAN, and the drone has a positioning sensor including a GPS, a barometer, an optical flow sensor, and Ultrasonic waves, etc., the calculated acceleration data a uav error is smaller.
  • the compensated acceleration mode length is dynamically calculated, and compared with the gravity acceleration weight, the reliability of the compensated acceleration data is evaluated, and the acceleration fusion weight is dynamically adjusted according to the credibility to improve the filtering of the accelerometer noise. effect.
  • FIG. 5 a data processing flowchart of the pan-tilt attitude detecting method after the acceleration compensation is added is shown in FIG. 5, wherein the module 3 indicates that the drone navigation system acquires real-time acceleration data.
  • the pan/tilt attitude data further includes a yaw angle
  • the pan/tilt attitude detecting apparatus further includes:
  • the yaw angle correction module 60 is configured to acquire a yaw angle of the drone, and calculate a deviation of the yaw angle of the drone from the yaw angle of the gimbal by a magnetic encoder, thereby obtaining a yaw angle of the gimbal;
  • the angle is expressed as feedback, and the difference calculation is performed on the yaw angle of the gimbal to correct the yaw angle of the gimbal.
  • the yaw angle calculated by the AHRS system built by the above method does not refer to the north, and due to the temperature drift of the gyroscope and the integral error, the yaw angle yaw will slowly drift, eventually leading to the lock of the gimbal. In the state (the pylon is stable at a certain yaw angle on the ground), the captured picture will continue to shift slowly.
  • the solution uses existing sensors, including gyroscopes, accelerometers, magnetic encoders, and sensor information of the drone to perform fused correction of the yaw angle.
  • FIG. 6 a data processing flowchart of the pan-tilt attitude detection method after the yaw angle fusion correction is added is shown in FIG. 6, wherein the module 4 represents the pan-tilt angle calculated by the magnetic encoder.
  • the calculation method of the deviation angle yaw offset_from_UAV_to_gimbal of the drone and the gimbal is: the attitude relationship between the gimbal and the drone is:
  • R gimbal R ⁇ R ⁇
  • the relative heading of the drone to the gimbal is:
  • the rotation matrix of each axis is:
  • the method of the foregoing embodiment can be implemented by means of software plus a necessary general hardware platform, and of course, can also be through hardware, but in many cases, the former is better.
  • Implementation Based on such understanding, the technical solution of the present invention, which is essential or contributes to the prior art, may be embodied in the form of a software product stored in a storage medium (such as ROM/RAM, disk,
  • the optical disc includes a number of instructions for causing a terminal device (which may be a cell phone, a computer, a server, an air conditioner, or a network device, etc.) to perform the methods described in various embodiments of the present invention.
  • the method and device for detecting attitude of a gimbal proposed by the invention obtains the attitude data of the gimbal through the attitude heading system, adopts different algorithms to perform the posture representation of the gimbal, compensates and corrects the attitude of the gimbal, and improves the attitude detection of the gimbal.
  • the algorithm Compared with Kalman filter or other attitude fusion algorithm, the algorithm has the advantage of less computational complexity, and is suitable for the PTZ system with few computing resources, which is helpful for the miniaturization of the PTZ;
  • the attitude of the gyroscope and the accelerometer for attitude calculation the algorithm makes full use of the hardware resources of the PTZ system, including the use of the magnetic encoder data of the PTZ motor board, the heading angle of the flight control Data, flight control acceleration data, can obtain more accurate attitude data.

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

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

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 PCTCN2018079517-appb-000001
其中，q x、q y、q z、q w为四元数算法的四个变量，R(q)为旋转矩阵。
在本实施例中，加速度传感器用于测量支持力向量，在静止时，测量得到的支持力矢量与重力加速度矢量方向相反，大小相等，响应速度也非常高，但是会受到机体振动等引起的高频干扰；结合两个姿态传感器的特点进行考虑，姿态解算的总体思想为：陀螺仪积分，以获得高响应性能；长时间内利用加速度计融合校正，以纠正积分漂移。
如图3所示，在本实施例中，所述步骤S21包括：
S211、将所述旋转矩阵表述与大地坐标系中的重力加速度进行乘积运算，得到机体坐标系下的加速度数据；
S212、将所述机体坐标系下的加速度数据与所述加速度计数据进行交叉乘积运算，得到旋转差向量；
S213、对所述旋转差向量进行放大，作为校准角速度参与云台姿态数据的四元数积分运算。
在本实施例中，所述云台姿态检测方法的数据处理流程图如图4所示，其中，模块1表示陀螺仪的角速度数据，模块2表示加速度计数据，Gef为大地坐标系中的重力加速度。
在本实施例中，所述步骤S211包括：
通过无人机导航系统获取实时加速度数据，将所述实时加速度数据与所述大地坐标系中的重力加速度进行加运算，得到补偿后的加速度数据；
将所述旋转矩阵表述与所述补偿后的加速度数据进行乘积运算，得到机体坐标系下的加速度数据。
在本实施例中，大地坐标系是地球坐标系，机体坐标系是云台本身的坐标系，加速度计数据为机体坐标系下测量到的云台加速度值，为G bf+a bf，此时的加速度数据除了带有重力加速度G bf外，还带有线加速度a bf；而大地坐标系中的重力加速度是一个常数，表示为G ef＝g＝9.8m/s^2。
在步骤S211中，因为云台加速度计实际测量本身存在偏差，为了得云台加速度计实际测量不断的趋近正确的值，把云台姿态的旋转矩阵当作一个负反馈，以将云台最后输出来的姿态更加准确。
在本实施例中，云台在进行线性加速度运动时，加速度计检测值为G bf+a bf，此时的加速度数据除了带有重力加速度G bf外，还带有线加速度a bf，此时仍然使用加速度数据进行姿态融合的话，将会导致姿态检测出现较大偏差，普遍的方法是调整加速度的融合权重值到足够小(过小将影响校正陀螺仪的效果)，以减少高频的线加速度a bf(比如震动带来的高频线加速度)对姿态检测的影响。但当无人机进行低频的线加速运动时(长时间的向某一方向加速运动或刷锅动作等)，以上方法将不能过滤掉线加速度a bf导致的姿态检测误差。针对以上问题，本方案一方面通过CAN把无人机机导航系统的解算的真实加速度数据a uav发送给云台进行加速度补偿，无人机具有定位传感 器包括GPS、气压计、光流传感器以及超声波等，所解算的加速度数据a uav误差更小。另一方面，动态计算补偿后的加速度模长，并与重力加速度权重对比，评估补偿后的加速度数据的可信度，并根据可信度动态调整加速度融合权重，以提高对加速度计噪音的过滤效果。
在本实施例中，增加了加速度补偿后的云台姿态检测方法的数据处理流程图如图5所示，其中，模块3表示无人机导航系统获取实时加速度数据。
在本实施例中，所述云台姿态数据还包括偏航角，相应地，对所述惯性测量单元的数据进行四元数积分运算之前包括：
获取无人机偏航角，通过磁编码器计算无人机偏航角与云台偏航角的偏差，从而得到云台偏航角；
以所述欧拉角表述为反馈，对所述云台偏航角进行差值运算，以对所述云台偏航角进行纠偏。
机载云台由于大小与成本的限制，除了陀螺仪外并不装配磁罗盘等其他可直接测量云台偏航角的传感器器件。因此以上方法搭建的AHRS系统所解算出来的偏航角不指北，并且由于陀螺仪的温漂及积分误差的原因，将会导致偏航角yaw发生缓慢漂移，最终导致云台在锁头状态时(云台稳定在大地上的某一偏航角)，所拍摄的画面将会发生持续缓慢的偏移。针对以上问题，本方案采用现有传感器，包括陀螺仪、加速度计、磁编码器以及无人机的传感器信息，进行偏航角的融合校正。
在本实施例中，增加了偏航角融合校正后的云台姿态检测方法的数据处理流程图如图6所示，其中，模块4表示通过磁编码器计算得到的云台偏航角。
其中，无人机与云台的偏差角yaw offset_from_UAV_to_gimbal的计算方法是：云台与无人机的姿态关系为：
Figure PCTCN2018079517-appb-000002
因为我们只需要计算无人机相对于云台的航向偏差，故可认为云台的航向为0，因此令：
R gimbal＝R ψR θ
无人机的对于云台的相对航向为：yaw offset_from_UAV_to_gimbal＝atan2(-R UAV12，R UAV22
各轴的旋转矩阵为：
Figure PCTCN2018079517-appb-000003
Figure PCTCN2018079517-appb-000004
Figure PCTCN2018079517-appb-000005
Figure PCTCN2018079517-appb-000006
实施例二
如图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 PCTCN2018079517-appb-000007
其中，q x、q y、q z、q w为四元数算法的四个变量，R(q)为旋转矩阵。
在本实施例中，加速度传感器用于测量支持力向量，在静止时，测量得到的支持力矢量与重力加速度矢量方向相反，大小相等，响应速度也非常高，但是会受到机体振动等引起的高频干扰；结合两个姿态传感器的特点进行考虑，姿态解算的总体思想为：陀螺仪积分，以获得高响应性能；长时间内利用加速度计融合校正，以纠正积分漂移。
如图8所示，在本实施例中，所述反馈校正模块包括：
坐标系转换单元51，用于将所述旋转矩阵表述与大地坐标系中的重力加速度进行乘积运算，得到机体坐标系下的加速度数据；
校正单元52，用于将所述机体坐标系下的加速度数据与所述加速度计数据进行交叉乘积运算，得到校正后的加速度计数据；
放大更新单元53，用于对所述校正后的加速度计数据进行放大，作为更新后的加速度计数据参与云台姿态数据的四元数积分运算。
在本实施例中，所述云台姿态检测方法的数据处理流程图如图4所示，其中，模块1表示陀螺仪的角速度数据，模块2表示加速度计数据，Gef为大地坐标系中的重力加速度。
在本实施例中，所述坐标系转换单元包括：
补偿单元，用于通过无人机导航系统获取实时加速度数据，将所述实时加速度数据与所述大地坐标系中的重力加速度进行加运算，得到补偿后的加速度数据；
乘积运算单元，用于将所述旋转矩阵表述与所述补偿后的加速度数据进行乘积运算，得到机体坐标系下的加速度数据。
在本实施例中，大地坐标系是地球坐标系，机体坐标系是云台本身的坐标系，加速度计数据为机体坐标系下测量到的云台加速度值，为G bf+a bf，此时的加速度数据除了带有重力加速度G bf外，还带有线加速度a bf；而大地坐标系中的重力加速度是一个常数，表示为G ef＝g＝9.8m/s^2。
因为云台加速度计实际测量本身存在偏差，为了得云台加速度计实际测量不断的趋近正确的值，把云台姿态的旋转矩阵当作一个负反馈，以将云台最后输出来的姿态更加准确。
在本实施例中，云台在进行线性加速度运动时，加速度计检测值为G bf+a bf，此时的加速度数据除了带有重力加速度G bf外，还带有线加速度a bf，此时仍然使用加速度数据进行姿态融合的话，将会导致姿态检测出现较大偏差，普遍的方法是调整加速度的融合权重值到足够小(过小将影响校正陀螺仪的效果)，以减少高频的线加速度a bf(比如震动带来的高频线加速度)对姿态检测的影响。但当无人机进行低频的线加速运动时(长时间的向某一方向加速运动或刷锅动作等)，以上方法将不能过滤掉线加速度a bf导致的姿态检测误差。针对以上问题，本方案一方面通过CAN把无人机机导航系统的解算的真实加速度数据a uav发送给云台进行加速度补偿，无人机具有定位传感器包括GPS、气压计、光流传感器以及超声波等，所解算的加速度数据a uav误 差更小。另一方面，动态计算补偿后的加速度模长，并与重力加速度权重对比，评估补偿后的加速度数据的可信度，并根据可信度动态调整加速度融合权重，以提高对加速度计噪音的过滤效果。
在本实施例中，增加了加速度补偿后的云台姿态检测方法的数据处理流程图如图5所示，其中，模块3表示无人机导航系统获取实时加速度数据。
如图9所示，在本实施例中，所述云台姿态数据还包括偏航角，相应地，所述云台姿态检测装置还包括：
偏航角纠偏模块60，用于获取无人机偏航角，通过磁编码器计算无人机偏航角与云台偏航角的偏差，从而得到云台偏航角；以所述欧拉角表述为反馈，对所述云台偏航角进行差值运算，以对所述云台偏航角进行纠偏。
机载云台由于大小与成本的限制，除了陀螺仪外并不装配磁罗盘等其他可直接测量云台偏航角的传感器器件。因此以上方法搭建的AHRS系统所解算出来的偏航角不指北，并且由于陀螺仪的温漂及积分误差的原因，将会导致偏航角yaw发生缓慢漂移，最终导致云台在锁头状态时(云台稳定在大地上的某一偏航角)，所拍摄的画面将会发生持续缓慢的偏移。针对以上问题，本方案采用现有传感器，包括陀螺仪、加速度计、磁编码器以及无人机的传感器信息，进行偏航角的融合校正。
在本实施例中，增加了偏航角融合校正后的云台姿态检测方法的数据处理流程图如图6所示，其中，模块4表示通过磁编码器计算得到的云台偏航角。
其中，无人机与云台的偏差角yaw offset_from_UAV_to_gimbal的计算方法是：云台与无人机的姿态关系为：
Figure PCTCN2018079517-appb-000008
因为我们只需要计算无人机相对于云台的航向偏差，故可认为云台的航向为0，因此令：
R gimbal＝R ψR θ
无人机的对于云台的相对航向为：
yaw offset_from_UAV_to_gimbal＝atan2(-R UAV12，R UAV22)
各轴的旋转矩阵为：
Figure PCTCN2018079517-appb-000009
Figure PCTCN2018079517-appb-000010
Figure PCTCN2018079517-appb-000011
Figure PCTCN2018079517-appb-000012
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
上述本发明实施例序号仅仅为了表述，不代表实施例的优劣。
通过以上的实施方式的表述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间 接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。
工业实用性
本发明提出的一种云台姿态检测方法及装置，通过姿态航向系统获取云台姿态数据，采用不同的算法进行云台姿态表述，并对云台姿态进行补偿和校正，提高了云台姿态检测的准确性，相比于卡尔曼滤波或其他姿态融合算法，该算法具有运算量少的优点，适合于运算资源不多的云台系统，有助于云台的小型化；相比于只利用陀螺仪和加速度计进行姿态解算的云台姿态解算方法，该算法充分利用了云台系统所具有的的硬件资源，包括利用了云台电机板的磁编码器数据、飞控的航向角数据、飞控的加速度数据，可获得更精确的姿态数据。

Claims (10)

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

Applications Claiming Priority (2)

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

Publications (1)

Publication Number Publication Date
WO2018184467A1 true WO2018184467A1 (zh) 2018-10-11

Family

ID=59483992

Family Applications (1)

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

Country Status (2)

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

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110674888A (zh) * 2019-10-11 2020-01-10 中国人民解放军海军航空大学青岛校区 一种基于数据融合的头部姿态识别方法
CN114545017A (zh) * 2022-01-31 2022-05-27 深圳市云鼠科技开发有限公司 基于光流和加速度计的速度融合方法、装置和计算机设备
CN115293299A (zh) * 2022-10-08 2022-11-04 中科物栖（北京）科技有限责任公司 人体姿态特征实时检测方法、装置、设备及介质

Families Citing this family (30)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106959110B (zh) * 2017-04-06 2020-08-11 亿航智能设备（广州）有限公司 一种云台姿态检测方法及装置
CN113985928A (zh) * 2017-09-12 2022-01-28 深圳市大疆灵眸科技有限公司 云台的控制方法、控制器和云台
CN107807680B (zh) * 2017-09-21 2020-06-12 中国科学院长春光学精密机械与物理研究所 一种云台漂移补偿方法
WO2019095210A1 (zh) * 2017-11-16 2019-05-23 深圳市大疆创新科技有限公司 智能眼镜及其控制云台的方法、云台、控制方法和无人机
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 深圳市大疆创新科技有限公司 一种云台校准方法及云台设备
CN109074087A (zh) * 2017-12-25 2018-12-21 深圳市大疆创新科技有限公司 偏航姿态控制方法、无人机、计算机可读存储介质
CN108491001A (zh) * 2018-03-21 2018-09-04 深圳臻迪信息技术有限公司 增稳云台、增稳云台实现方法及无人机系统
CN108762324A (zh) * 2018-05-23 2018-11-06 深圳市道通智能航空技术有限公司 云台电机角度和角速度估算方法、装置、云台及飞行器
WO2019227410A1 (zh) * 2018-05-31 2019-12-05 深圳市大疆创新科技有限公司 姿态转换方法、姿态显示方法及云台系统
WO2019232697A1 (zh) * 2018-06-05 2019-12-12 深圳市大疆创新科技有限公司 云台及其校准方法、无人机和计算设备
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 杭州海康机器人技术有限公司 一种云台姿态估计方法及装置
CN111433702B (zh) * 2018-10-31 2022-04-15 深圳市道通智能航空技术股份有限公司 无人机及其云台控制方法
CN109691992A (zh) * 2019-03-04 2019-04-30 深圳星脉医疗仪器有限公司 一种血压检测信号的修正方法和血压检测装置
WO2020181530A1 (zh) * 2019-03-13 2020-09-17 深圳市大疆创新科技有限公司 云台振动调节的方法、云台以及客户端
CN110086973B (zh) * 2019-05-10 2020-11-27 中国计量大学 一种基于光流相机的云台稳像系统
CN110553669B (zh) * 2019-09-30 2022-03-29 睿魔智能科技（深圳）有限公司 云台校准方法及校准系统
CN111654634B (zh) * 2020-06-24 2022-02-08 杭州海康威视数字技术股份有限公司 确定摄像机中机芯组件和云台组件倾斜的方法、摄像机
CN111770270B (zh) * 2020-06-24 2021-06-25 杭州海康威视数字技术股份有限公司 一种相机姿态的校正方法、摄像机
WO2022016322A1 (zh) * 2020-07-20 2022-01-27 深圳市大疆创新科技有限公司 云台及其性能的评估方法及装置、可移动平台
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 合肥工业大学 一种无人机自稳云台及控制方法
CN113568438A (zh) * 2021-07-02 2021-10-29 杭州海康威视数字技术股份有限公司 一种姿态角的恢复方法、装置及设备

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120278024A1 (en) * 2011-04-27 2012-11-01 Samsung Electronics Co., Ltd. Position estimation apparatus and method using acceleration sensor
CN104567931A (zh) * 2015-01-14 2015-04-29 华侨大学 一种室内惯性导航定位的航向漂移误差消除方法
CN105116926A (zh) * 2015-08-20 2015-12-02 深圳一电科技有限公司 云台控制方法和装置
CN106155105A (zh) * 2015-04-08 2016-11-23 优利科技有限公司 控制云台的装置及云台系统
CN106959110A (zh) * 2017-04-06 2017-07-18 亿航智能设备（广州）有限公司 一种云台姿态检测方法及装置

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108443680B (zh) * 2015-05-22 2020-06-05 深圳市大疆灵眸科技有限公司 一种移动装置、移动装置控制系统及控制方法
CN106292741A (zh) * 2016-09-27 2017-01-04 成都普诺思博科技有限公司 一种基于无刷电机的移动机器人云台系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120278024A1 (en) * 2011-04-27 2012-11-01 Samsung Electronics Co., Ltd. Position estimation apparatus and method using acceleration sensor
CN104567931A (zh) * 2015-01-14 2015-04-29 华侨大学 一种室内惯性导航定位的航向漂移误差消除方法
CN106155105A (zh) * 2015-04-08 2016-11-23 优利科技有限公司 控制云台的装置及云台系统
CN105116926A (zh) * 2015-08-20 2015-12-02 深圳一电科技有限公司 云台控制方法和装置
CN106959110A (zh) * 2017-04-06 2017-07-18 亿航智能设备（广州）有限公司 一种云台姿态检测方法及装置

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110674888A (zh) * 2019-10-11 2020-01-10 中国人民解放军海军航空大学青岛校区 一种基于数据融合的头部姿态识别方法
CN110674888B (zh) * 2019-10-11 2022-04-05 中国人民解放军海军航空大学青岛校区 一种基于数据融合的头部姿态识别方法
CN114545017A (zh) * 2022-01-31 2022-05-27 深圳市云鼠科技开发有限公司 基于光流和加速度计的速度融合方法、装置和计算机设备
CN115293299A (zh) * 2022-10-08 2022-11-04 中科物栖（北京）科技有限责任公司 人体姿态特征实时检测方法、装置、设备及介质
CN115293299B (zh) * 2022-10-08 2023-01-24 中科物栖（北京）科技有限责任公司 人体姿态特征实时检测方法、装置、设备及介质

Also Published As

Publication number Publication date
CN106959110A (zh) 2017-07-18
CN106959110B (zh) 2020-08-11

Similar Documents

Publication Publication Date Title
WO2018184467A1 (zh) 一种云台姿态检测方法及装置
CN109001787B (zh) 一种姿态角解算与定位的方法及其融合传感器
JP5061264B1 (ja) 小型姿勢センサ
CN112630813B (zh) 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN110081878B (zh) 一种多旋翼无人机的姿态及位置确定方法
WO2019071916A1 (zh) 天线波束姿态控制方法和系统
US20220155800A1 (en) Method and apparatus for yaw fusion and aircraft
CN110325822B (zh) 云台位姿修正方法和装置
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及系统
JP2012173190A (ja) 測位システム、測位方法
WO2022063120A1 (zh) 组合导航系统初始化方法、装置、介质及电子设备
CN113551665B (zh) 一种用于运动载体的高动态运动状态感知系统及感知方法
US11408735B2 (en) Positioning system and positioning method
CN112197765B (zh) 一种实现水下机器人精细导航的方法
CN106813679B (zh) 运动物体的姿态估计的方法及装置
WO2019016930A1 (ja) データ処理装置、駆動制御装置、移動体、データ処理方法、駆動制御方法、および記憶媒体
WO2021037047A1 (zh) 一种飞行器的偏航角修正方法、装置及飞行器
US20210108923A1 (en) Information processing apparatus, information processing method, and program
CN110793515A (zh) 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
Magnussen et al. Experimental validation of a quaternion-based attitude estimation with direct input to a quadcopter control system
CN108444468B (zh) 一种融合下视视觉与惯导信息的定向罗盘
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法
CN113129377A (zh) 一种三维激光雷达快速鲁棒slam方法和装置
TWI805141B (zh) 用於無人機的定位方法和設備
Ryan et al. MEMS based AHRS with adaptive bias estimation for high performance rate sensor replacement

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 18781270

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

32PN Ep: public notification in the ep bulletin as address of the adressee cannot be established

Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205N DATED 27/11/2020)

122 Ep: pct application non-entry in european phase

Ref document number: 18781270

Country of ref document: EP

Kind code of ref document: A1