CN110873563A - Cloud deck attitude estimation method and device - Google Patents

Cloud deck attitude estimation method and device Download PDF

Info

Publication number
CN110873563A
CN110873563A CN201811003675.6A CN201811003675A CN110873563A CN 110873563 A CN110873563 A CN 110873563A CN 201811003675 A CN201811003675 A CN 201811003675A CN 110873563 A CN110873563 A CN 110873563A
Authority
CN
China
Prior art keywords
moment
base
ith
attitude
holder
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201811003675.6A
Other languages
Chinese (zh)
Other versions
CN110873563B (en
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.)
Hangzhou Hikvision Digital Technology Co Ltd
Hangzhou Hikrobot Co Ltd
Original Assignee
Hangzhou Hikrobot Technology 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 Hangzhou Hikrobot Technology Co Ltd filed Critical Hangzhou Hikrobot Technology Co Ltd
Priority to CN201811003675.6A priority Critical patent/CN110873563B/en
Publication of CN110873563A publication Critical patent/CN110873563A/en
Application granted granted Critical
Publication of CN110873563B publication Critical patent/CN110873563B/en
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/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

The invention discloses a holder attitude estimation method and device, and belongs to the technical field of holders. The method comprises the following steps: respectively acquiring the angular speed and linear acceleration of the image acquisition equipment in the holder at the three axes of the holder coordinate system at the (i + 1) th moment and the angular displacement of an output shaft of a course shaft motor in the holder; based on the angular speed and linear acceleration of the image acquisition equipment in the pan-tilt at the three axes of the pan-tilt coordinate system at the moment i +1 and the angular displacement of the output shaft of the course shaft motor in the pan-tilt, calculating the pan-tilt platform error angle at the moment i +1, acquiring the pan-tilt posture at the moment i, and updating the pan-tilt posture at the moment i based on the pan-tilt platform error angle at the moment i +1 to acquire the pan-tilt posture at the moment i + 1. The device comprises the following steps: the device comprises a first acquisition module, a calculation module, a second acquisition module and an updating module. The invention can improve the estimation precision of the posture of the holder.

Description

Cloud deck attitude estimation method and device
Technical Field
The invention relates to the technical field of cloud platforms, in particular to a cloud platform attitude estimation method and device.
Background
Aerial unmanned aerial vehicles typically carry a camera via a pan-tilt. At camera during operation, unstable condition such as vibrations, rocking can appear in the cloud platform receiving unmanned aerial vehicle flight environment's influence, at this moment, is unfavorable for the camera to shoot stable video or image. In order to ensure that the camera can acquire a stable video or image, the posture of the pan-tilt-zoom is required to be well followed by the target pan-tilt posture. When the good follow-up of the posture of the holder is realized, the posture of the holder needs to be accurately estimated.
The existing cradle head attitude estimation method is to install a gyroscope on a camera and estimate the cradle head attitude through the working parameters of the gyroscope. Specifically, an initial attitude of the cradle head is given, integration is performed on the angular velocity measured by the gyroscope to obtain an attitude increment of the current cradle head attitude relative to the previous cradle head attitude, and then the current cradle head attitude is obtained based on the previous cradle head attitude and the attitude increment.
In the process of implementing the invention, the inventor finds that the prior art has at least the following problems: the attitude increment obtained by integrating the angular velocity of the gyroscope has an accumulated error of integral operation, the accumulated error can increase along with the change of time, and the calculated attitude increment is inaccurate, so that the estimation precision of the attitude of the holder is reduced.
Disclosure of Invention
The embodiment of the invention provides a method and a device for estimating the posture of a holder, which can improve the estimation precision of the posture of the holder. The technical scheme is as follows:
in a first aspect, a method for estimating a pan/tilt head attitude is provided, the method comprising:
respectively acquiring the angular speed and linear acceleration of image acquisition equipment in a tripod head at the (i + 1) th moment on three axes of a tripod head coordinate system and the angular displacement of an output shaft of a course shaft motor in the tripod head;
calculating a platform error angle of the holder at the (i + 1) th moment based on the angular velocity and linear acceleration of the image acquisition equipment in the holder at the three axes of the holder coordinate system at the (i + 1) th moment and the angular displacement of an output shaft of a course shaft motor in the holder;
acquiring the posture of the holder at the ith moment;
and updating the cradle head posture at the ith moment based on the cradle head platform error angle at the ith +1 moment to obtain the cradle head posture at the ith +1 moment.
Optionally, the calculating an error angle of the platform of the pan/tilt head at the (i + 1) th moment based on the angular velocities and the linear accelerations of the image capturing device in the pan/tilt head at the three axes of the coordinate system of the pan/tilt head at the (i + 1) th moment and the angular displacement of the output shaft of the heading axis motor in the pan/tilt head includes:
calculating the holder attitude observed quantity at the (i + 1) th moment based on the linear acceleration of the image acquisition equipment at three axes of the holder coordinate system at the (i + 1) th moment and the angular displacement of the output shaft of the course shaft motor;
acquiring a platform error angle of the holder at the ith moment;
calculating an estimated tripod head platform error angle at the i +1 th moment based on the tripod head platform error angle at the i th moment and the angular velocity of the image acquisition equipment at the i +1 th moment on three axes of a tripod head coordinate system;
and correcting the estimated tripod head platform error angle at the (i + 1) th moment based on the tripod head posture observed quantity at the (i + 1) th moment to obtain the tripod head platform error angle at the (i + 1) th moment.
Optionally, the calculating a pan-tilt attitude observed quantity at the i +1 th moment based on linear accelerations of the image acquisition device at three axes of a pan-tilt coordinate system at the i +1 th moment and an angular displacement of an output shaft of the heading axis motor includes:
calculating the horizontal roll angle of the holder at the (i + 1) th moment based on the linear acceleration of the image acquisition equipment at the (i + 1) th moment on the three axes of the holder coordinate system;
when the horizontal roll angle of the holder is larger than a first threshold value, acquiring the base posture at the ith moment and the angular displacement of the output shaft of each shaft motor in the holder at the (i + 1) th moment, and calculating the holder posture observed quantity based on the base posture at the ith moment and the angular displacement of the output shaft of each shaft motor in the holder at the (i + 1) th moment; the holder is fixed on the carrier through the base, and the shaft motor comprises the course shaft motor;
and when the horizontal roll angle of the holder is smaller than the first threshold value, acquiring the carrier course angle at the (i + 1) th moment, and calculating the holder attitude observed quantity based on the carrier course angle at the (i + 1) th moment, the angular displacement of the output shaft of the course shaft motor at the (i + 1) th moment and the linear acceleration of the image acquisition equipment at the (i + 1) th moment on three axes of the holder coordinate system.
Optionally, the acquiring the posture of the base at the ith time includes:
and acquiring the posture of the carrier at the ith moment, and taking the posture of the carrier at the ith moment as the posture of the base at the ith moment.
Optionally, the acquiring the posture of the base at the ith time includes:
when i is 1, calculating the base posture at the ith moment based on the linear acceleration of the base on the three axes of the base coordinate system at the ith moment and the carrier course angle;
when the ith moment is not the 1 st moment, respectively acquiring the angular velocity and the linear acceleration of the base at the ith moment on three axes of a base coordinate system; calculating a base platform error angle at the ith moment based on the angular velocity and the linear acceleration of the base at the three axes of the base coordinate system at the ith moment; acquiring the posture of the base at the ith-1 moment; and updating the base posture at the ith moment based on the base platform error angle at the ith moment to obtain the base posture at the ith moment.
Optionally, the calculating a base posture at the ith moment based on the linear accelerations of the base on three axes of the base coordinate system at the ith moment and the carrier heading angle includes:
the attitude of the base at the ith time is calculated according to the following formula,
Figure BDA0001783537130000031
Figure BDA0001783537130000032
Figure BDA0001783537130000033
wherein the content of the first and second substances,
Figure BDA0001783537130000036
is the heading angle of the base at time i,
Figure BDA0001783537130000034
is the roll angle of the susceptor at time i,
Figure BDA0001783537130000037
is the pitch angle of the base at time i, psibody,iCarrier heading at i-th momentAn angle;
Figure BDA0001783537130000035
the linear acceleration of the base on three axes of the base coordinate system at the ith moment.
Optionally, the calculating a base platform error angle at the ith time based on the angular velocities and the linear accelerations of the base at the three axes of the base coordinate system at the ith time includes:
calculating the base posture observed quantity at the ith moment based on the linear acceleration of the base at the three axes of the base coordinate system at the ith moment;
acquiring a base platform error angle at the i-1 th moment;
calculating an estimated base platform error angle at the ith moment based on the base platform error angle at the ith-1 moment and the angular velocities of the base at the base coordinate system three axes at the ith moment;
and correcting the estimated base platform error angle at the ith moment based on the base attitude observed quantity at the ith moment to obtain the base platform error angle at the ith moment.
Optionally, the calculating a base posture observation at the ith time based on the linear accelerations of the base at the three axes of the base coordinate system at the ith time includes:
calculating a base roll angle at the ith moment based on the linear acceleration of the base at the three axes of the base coordinate system at the ith moment;
when the base roll angle is larger than a second threshold value, acquiring the carrier attitude at the ith moment, and taking the carrier attitude at the ith moment as the base attitude observed quantity at the ith moment;
and when the base roll angle is smaller than the second threshold value, acquiring a carrier course angle at the ith moment, and calculating the base attitude observed quantity at the ith moment based on the carrier course angle at the ith moment and the linear acceleration of the base at the base coordinate system triaxial at the ith moment.
Optionally, the calculating the pan/tilt attitude observed quantity based on the base attitude at the i-th time and the angular displacement of the output shaft of each shaft motor in the pan/tilt at the i + 1-th time includes:
when the axis motor further comprises a transverse axis motor and a pitching axis motor, the base posture at the ith moment is sequentially rotated according to the rotation sequence of an X axis-Y axis-Z axis, a Y axis-Z axis-X axis or a Z axis-X axis-Y axis to rotate the angular displacement of the output shaft of the corresponding axis motor at the (i + 1) th moment, so that the holder posture observed quantity at the (i + 1) th moment is obtained, the Z axis corresponds to the course axis motor, the X axis corresponds to the transverse axis motor, and the Y axis corresponds to the pitching axis motor.
Optionally, when i is 1, the acquiring the pan-tilt attitude at the ith time includes:
calculating the posture of the pan-tilt at the ith moment according to the following formula,
Figure BDA0001783537130000041
Figure BDA0001783537130000042
Figure BDA0001783537130000043
wherein the content of the first and second substances,
Figure BDA0001783537130000045
is the heading angle of the pan/tilt head at the ith moment,
Figure BDA0001783537130000046
the roll angle of the pan/tilt head at the ith moment,
Figure BDA0001783537130000047
is the pitch angle, psi, of the pan-tilt at moment ibody,iThe carrier course angle at the ith moment; psim,iIs the angular displacement of the output shaft of the course shaft motor at the ith moment,
Figure BDA0001783537130000044
for the ith momentAnd linear accelerations of three axes in the holder coordinate system.
Optionally, the updating the cradle head attitude at the ith time based on the cradle head platform error angle at the (i + 1) th time includes:
calculating the tripod head attitude increment of the ith +1 moment relative to the ith moment based on the tripod head platform error angle of the ith +1 moment;
and updating the cradle head attitude at the ith moment by adopting the cradle head attitude increment of the (i + 1) th moment relative to the ith moment.
In a second aspect, there is provided a pan-tilt attitude estimation apparatus, the apparatus comprising:
the first acquisition module is used for respectively acquiring the angular velocity and the linear acceleration of the image acquisition equipment in the tripod head at the (i + 1) th moment on three axes of a tripod head coordinate system and the angular displacement of an output shaft of a course shaft motor in the tripod head;
the computing module is used for computing a platform error angle of the holder at the (i + 1) th moment based on the angular velocity and the linear acceleration of the image acquisition equipment in the holder at the three axes of the holder coordinate system at the (i + 1) th moment and the angular displacement of the output shaft of the course shaft motor in the holder;
the second acquisition module is used for acquiring the posture of the holder at the ith moment;
and the updating module is used for updating the cradle head attitude at the ith moment based on the cradle head platform error angle at the ith +1 moment to obtain the cradle head attitude at the ith +1 moment.
Optionally, the calculation module is configured to,
calculating the holder attitude observed quantity at the (i + 1) th moment based on the linear acceleration of the image acquisition equipment at three axes of the holder coordinate system at the (i + 1) th moment and the angular displacement of the output shaft of the course shaft motor;
acquiring a platform error angle of the holder at the ith moment;
calculating an estimated tripod head platform error angle at the i +1 th moment based on the tripod head platform error angle at the i th moment and the angular velocity of the image acquisition equipment at the i +1 th moment on three axes of a tripod head coordinate system;
and correcting the estimated tripod head platform error angle at the (i + 1) th moment based on the tripod head posture observed quantity at the (i + 1) th moment to obtain the tripod head platform error angle at the (i + 1) th moment.
Optionally, the calculation module is configured to,
calculating the horizontal roll angle of the holder at the (i + 1) th moment based on the linear acceleration of the image acquisition equipment at the (i + 1) th moment on the three axes of the holder coordinate system;
when the horizontal roll angle of the holder is larger than a first threshold value, acquiring the base posture at the ith moment and the angular displacement of the output shaft of each shaft motor in the holder at the (i + 1) th moment, and calculating the holder posture observed quantity based on the base posture at the ith moment and the angular displacement of the output shaft of each shaft motor in the holder at the (i + 1) th moment; the holder is fixed on the carrier through the base, and the shaft motor comprises the course shaft motor;
and when the horizontal roll angle of the holder is smaller than the first threshold value, acquiring the carrier course angle at the (i + 1) th moment, and calculating the holder attitude observed quantity based on the carrier course angle at the (i + 1) th moment, the angular displacement of the output shaft of the course shaft motor at the (i + 1) th moment and the linear acceleration of the image acquisition equipment at the (i + 1) th moment on three axes of the holder coordinate system.
Optionally, the calculation module is configured to,
and acquiring the posture of the carrier at the ith moment, and taking the posture of the carrier at the ith moment as the posture of the base at the ith moment.
Optionally, the calculation module is configured to,
when the ith is 1, calculating the attitude of the base at the ith moment based on the linear acceleration of the base at the three axes of the base coordinate system at the ith moment and the heading angle of the carrier;
when the ith moment is not the 1 st moment, respectively acquiring the angular velocity and the linear acceleration of the base at the ith moment on three axes of a base coordinate system; calculating a base platform error angle at the ith moment based on the angular velocity and the linear acceleration of the base at the three axes of the base coordinate system at the ith moment; acquiring the posture of the base at the ith-1 moment; and updating the base posture at the ith moment based on the base platform error angle at the ith moment to obtain the base posture at the ith moment.
Optionally, the calculation module is configured to,
when the ith is 1, calculating the base posture at the ith moment according to the following formula,
Figure BDA0001783537130000061
Figure BDA0001783537130000062
Figure BDA0001783537130000063
wherein the content of the first and second substances,
Figure BDA0001783537130000065
is the heading angle of the base at time i,
Figure BDA0001783537130000066
is the roll angle of the susceptor at time i,
Figure BDA0001783537130000067
is the pitch angle of the base at time i, psibody,iThe carrier course angle at the ith moment;
Figure BDA0001783537130000064
the linear acceleration of the base on three axes of the base coordinate system at the ith moment.
Optionally, the calculation module is configured to,
when the ith moment is not the 1 st moment, calculating the base posture observed quantity at the ith moment based on the linear acceleration of the base at the three axes of the base coordinate system at the ith moment;
acquiring a base platform error angle at the i-1 th moment;
calculating an estimated base platform error angle at the ith moment based on the base platform error angle at the ith-1 moment and the angular velocities of the base at the base coordinate system three axes at the ith moment;
and correcting the estimated base platform error angle at the ith moment based on the base attitude observed quantity at the ith moment to obtain the base platform error angle at the ith moment.
Optionally, the calculation module is configured to,
calculating a base roll angle at the ith moment based on the linear acceleration of the base at the three axes of the base coordinate system at the ith moment;
when the base roll angle is larger than a second threshold value, acquiring the carrier attitude at the ith moment, and taking the carrier attitude at the ith moment as the base attitude observed quantity at the ith moment;
and when the base roll angle is smaller than the second threshold value, acquiring a carrier course angle at the ith moment, and calculating the base attitude observed quantity at the ith moment based on the carrier course angle at the ith moment and the linear acceleration of the base at the base coordinate system triaxial at the ith moment.
Optionally, the calculation module is configured to,
when the horizontal roll angle of the holder is larger than a first threshold value, and when the shaft motor further comprises a horizontal roll shaft motor and a pitch shaft motor, the base posture at the ith moment is sequentially rotated according to the rotation sequence of an X shaft-Y shaft-Z shaft, a Y shaft-Z shaft-X shaft or a Z shaft-X shaft-Y shaft to rotate the angular displacement of the output shaft of the corresponding shaft motor at the (i + 1) th moment, so that the holder posture observed quantity at the (i + 1) th moment is obtained, the Z shaft corresponds to the course shaft motor, the X shaft corresponds to the horizontal roll shaft motor, and the Y shaft corresponds to the pitch shaft motor.
Optionally, the calculation module is configured to,
when i is equal to 1, calculating the tripod head attitude at the ith moment according to the following formula,
Figure BDA0001783537130000071
Figure BDA0001783537130000072
Figure BDA0001783537130000073
wherein the content of the first and second substances,
Figure BDA0001783537130000075
is the heading angle of the pan/tilt head at the ith moment,
Figure BDA0001783537130000076
the roll angle of the pan/tilt head at the ith moment,
Figure BDA0001783537130000077
is the pitch angle, psi, of the pan-tilt at moment ibody,iThe carrier course angle at the ith moment; psim,iIs the angular displacement of the output shaft of the course shaft motor at the ith moment,
Figure BDA0001783537130000074
and the linear acceleration of the image acquisition equipment on three axes of the holder coordinate system at the ith moment.
Optionally, the update module is configured to,
calculating the tripod head attitude increment of the ith +1 moment relative to the ith moment based on the tripod head platform error angle of the ith +1 moment;
and updating the cradle head attitude at the ith moment by adopting the cradle head attitude increment of the (i + 1) th moment relative to the ith moment.
In a third aspect, a pan/tilt head attitude estimation apparatus is provided, where the pan/tilt head attitude estimation apparatus includes a processor and a memory, where the memory stores at least one instruction, and the instruction is loaded and executed by the processor to implement the operations performed by the pan/tilt head attitude estimation method.
In a fourth aspect, a computer-readable storage medium is provided, in which at least one instruction is stored, and the instruction is loaded and executed by a processor to implement the operations performed by the foregoing pan-tilt attitude estimation method.
The technical scheme provided by the embodiment of the invention has the following beneficial effects: calculating a platform error angle of the holder; the tripod head platform error angle is a relative angular displacement relation between a calculated tripod head platform coordinate system and a real tripod head platform coordinate system, the real tripod head platform coordinate system is a navigation coordinate system, the calculated tripod head platform coordinate system is a navigation coordinate system estimated by a computer, in particular to a navigation coordinate system containing attitude errors estimated in the process of calculating the tripod head attitude, the tripod head attitude refers to the relative angular displacement relation between the tripod head coordinate system and the navigation coordinate system, and the tripod head platform error angle and the tripod head attitude increment between two moments have a conversion relation, so that the tripod head attitude increment between the two moments can be estimated based on the tripod head platform error angle, thus, the tripod head attitude increment from the moment i to the moment i +1 can be estimated based on the tripod head platform error angle at the moment i +1, and the tripod head attitude at the moment i can be updated based on the tripod head attitude increment, the cradle head attitude at the (i + 1) th moment is obtained, and the cradle head attitude is not directly solved for the integral of the gyroscope when the cradle head attitude is calculated, so that the problem of integral error of the gyroscope is avoided, and the estimation precision of the cradle head attitude can be improved.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed to be used in the description of the embodiments will be briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
Fig. 1 is a schematic structural diagram of a pan-tilt head provided by an embodiment of the invention;
FIG. 2 is a schematic diagram of a carrier coordinate system and a navigation coordinate system according to an embodiment of the invention;
fig. 3 is a flowchart of a pan-tilt attitude estimation method according to an embodiment of the present invention;
fig. 4 is a flowchart of a pan-tilt attitude estimation method according to an embodiment of the present invention;
fig. 5 is a flowchart of a pan-tilt attitude observed quantity calculation method according to an embodiment of the present invention;
fig. 6 is a flowchart of a method for acquiring a platform error angle of a pan/tilt head according to an embodiment of the present invention;
fig. 7 is a flowchart of a pan-tilt attitude estimation method according to an embodiment of the present invention;
FIG. 8 is a flow chart of a manner of computing a base attitude observation provided by an embodiment of the present invention;
FIG. 9 is a flow chart of a method for obtaining a base platform error angle provided by an embodiment of the present invention;
fig. 10 is a flowchart for calculating the error angle of the base platform and the error angle of the pan/tilt platform in the same filtering process according to the embodiment of the present invention;
fig. 11 and 12 are schematic structural diagrams of a pan-tilt attitude estimation apparatus according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, embodiments of the present invention will be described in detail with reference to the accompanying drawings.
To facilitate understanding of the method and apparatus for estimating a pan-tilt attitude according to the embodiments of the present invention, some terms related to the embodiment are first explained.
Extended Kalman Filter (EKF) is a high-efficiency recursive Filter (autoregressive Filter). The basic idea of EKF is to linearize the nonlinear system and then perform kalman filtering. The EKF is able to estimate the state of a dynamic system from a series of measurements that do not completely contain noise.
Pan-tilt attitude, also known as pan-tilt camera attitude, refers to the relative angular displacement relationship between the pan-tilt coordinate system and the navigation coordinate system.
And the base posture refers to the relative angular displacement relation between the base coordinate system and the navigation coordinate system.
The platform error angle refers to a relative angular displacement relationship between a navigation coordinate system (also called a mathematical platform, a calculation system or a platform coordinate system) and a navigation coordinate system calculated by the strapdown inertial navigation system.
The pan-tilt platform error angle refers to a relative angular displacement relationship between a navigation coordinate system (also called pan-tilt platform coordinate system) and a navigation coordinate system calculated by a pan-tilt navigation system.
The error angle of the base platform refers to the relative angular displacement relationship between the navigation coordinate system (also called base platform coordinate system) and the navigation coordinate system calculated by the base navigation system.
The holder attitude observed quantity is the holder attitude obtained by calculating according to the parameters measured by the first sensor device when the error angle of the holder platform is estimated through the EKF. The parameters measured by the first sensor at least comprise linear acceleration of the image acquisition equipment in the holder on three axes of a holder coordinate system and angular displacement of a heading axis motor in the holder. The first sensor member includes, but is not limited to, an accelerometer and a code wheel.
The base attitude observed quantity is the base attitude obtained by calculation according to the parameters measured by the second sensing device when the base platform error angle is estimated through the EKF. The parameters measured by the second sensing device at least comprise linear accelerations of the base in three axes of the base coordinate system. The second sensing device includes, but is not limited to, an accelerometer.
In the above noun explanations, reference may be made to the following description of the pan/tilt and coordinate system, where not described in detail.
The structure of the pan/tilt head will be described with reference to fig. 1. As shown in fig. 1, the pan/tilt head includes a base 1, a shaft motor mechanism 2 fixed to the base 1, an image capturing device 3 mounted to the shaft motor mechanism 2, a pan/tilt head navigation system, and a pan/tilt head control system (not shown in fig. 1). The base 1 is mounted to a carrier (e.g., an aircraft such as an unmanned aerial vehicle) via a shock-absorbing mechanism (e.g., an elastic member). According to the number of the shaft motors, the shaft motor mechanism 2 can be a one-shaft motor mechanism comprising a course shaft motor, can also be a two-shaft motor mechanism comprising a course shaft motor and a roll shaft motor, or a course shaft motor and a pitch shaft motor, and can also be a three-shaft motor mechanism comprising a course shaft motor, a roll shaft motor and a pitch shaft motor. When the shaft motor mechanism 2 is a three-shaft motor mechanism, the shaft motor mechanism 2 comprises a support frame 21, a course shaft motor, a roll shaft motor and a pitch shaft motor. Three mounting cavities 22a, 22b and 22c (shown by a dotted line frame in fig. 1, one of the mounting cavities 22a is shielded by the image acquisition device 3) are provided in the support frame 21, and are respectively used for mounting a heading axis motor, a roll axis motor and a pitch axis motor.
The course axis motor is used for driving the image acquisition equipment 3 to rotate around an Oc-Z axis, namely, to do yaw motion; the transverse roller motor is used for driving the image acquisition equipment 3 to rotate around an Oc-X axis, namely, rolling motion is carried out; the pitching axis motor is used for driving the image acquisition equipment 3 to rotate around the Oc-Y axis, namely, to perform pitching motion.
The image capturing device 3 may be a camera or a video camera.
The cradle head navigation system can be a strapdown inertial navigation system and can comprise a plurality of sensors (such as an accelerometer and a gyroscope) and a first processor, wherein the sensors are directly fixed on the cradle head, the sensors are used for sensing information such as angular velocity of the cradle head, and the first processor is used for determining the attitude of the cradle head based on the information such as the angular velocity of the cradle head sensed by the sensors and an established mathematical platform. The holder control system may include a second processor for adjusting the holder attitude to a target holder attitude by each axis motor based on the holder attitude determined by the holder navigation system. The pan-tilt attitude may refer to a relative angular displacement relationship between a pan-tilt coordinate system and a navigation coordinate system. The first processor and the second processor may be the same processor.
To facilitate understanding of the attitude of the pan/tilt head, the aircraft is taken as an example of a pan/tilt head carrier, and the attitude of the aircraft expressed by a lower carrier coordinate system (also referred to as a b system), a navigation coordinate system (also referred to as an n system), and an euler angle is described below.
Fig. 2 shows a carrier coordinate system. Origin O of the carrier coordinate systembIs the center of mass, x, of the aircraftbThe axis (also called roll axis) is in the plane of symmetry of the aircraft and parallel to the design axis of the aircraft and points in the direction of advance of the aircraft nose, ybThe axis (also called pitch axis) is perpendicular to the plane of symmetry of the aircraft and points from the origin to the right of the aircraft, zbThe axis (also called course axis) is in the plane of symmetry of the aircraft and is aligned with xbAxis and ybThe plane formed by the shafts is vertical and points to the lower part of the fuselage.
The navigation coordinate system may be a North East Down (NED) coordinate system. The origin O of the NED coordinate system is the center of mass of the carrier and comprises three axes of n, e and d, wherein the n axis points to the direction of the geographical north arrow, the e axis is the direction of the earth rotation tangent east, and the d axis is the perpendicular line from the carrier to the ground plane and points downwards. For ease of understanding, see fig. 2, the NED coordinate system is shown shifted from the center of mass of the carrier to one side.
The Euler angle is the azimuth angle of a carrier coordinate system of the aircraft relative to a navigation coordinate system and is used for representing the attitude of the aircraft. The euler angles include heading angle ψ, roll angle γ, and pitch angle θ. Defining the carrier to rotate around the vertical line direction, the heading angle psi isbThe projection of the axis on the horizontal plane forms an included angle with the geographical north direction. The transverse roll angle gamma is, the carrier winds around xbThe angle of rotation of the axis relative to the vertical plane (shown by the triangular box filled by the vertical line in figure 1). Pitch angle theta is x generated by carrier rotating around e-axisbThe axis forms an angle with the horizontal plane formed by the n-e axis.
Based on the carrier coordinate system, it can be understood that the origin of the pan-tilt coordinate system is the centroid (Oc in fig. 1) of the image capturing device, and the three axes of the pan-tilt coordinate system may be consistent with the three axes of the carrier coordinate system. The euler angle represents the pan-tilt attitude as the azimuth of the pan-tilt coordinate system relative to the navigation coordinate system.
In this embodiment, the pan/tilt/zoom system first calculates a mathematical platform according to information of a sensor (e.g., a gyroscope), where the mathematical platform also refers to a calculated navigation coordinate system, and then estimates the attitude of the pan/tilt/zoom according to the mathematical platform. The mathematical platform is called a pan-tilt platform coordinate system (also called a computing system: p system). Because the solution of the mathematical platform has a solution error, the platform coordinate system (the solved navigation coordinate system) of the holder has an error with the real navigation coordinate system. Specifically, assuming a navigation coordinate system n, a platform coordinate system p, and a body coordinate system (the body may be a pan/tilt/pan/tilt carrier) b, the transformation relationship among the three is shown in formula (1).
Figure BDA0001783537130000111
Wherein the content of the first and second substances,
Figure BDA0001783537130000112
the coordinate transformation matrix from an n system to a b system is called as a real attitude matrix;
Figure BDA0001783537130000113
the coordinate transformation matrix from the p system to the b system is called as a calculation attitude matrix;
Figure BDA0001783537130000114
is a coordinate transformation matrix from an n system to a p system. Then assume that the plateau error angle between n and p is
Figure BDA0001783537130000115
The platform error angle is small, so represented by an angular vector,
Figure BDA0001783537130000116
the three axes of the navigation coordinate system n, e and d are error angles respectively. The small-angle attitude matrix formula comprises:
Figure BDA0001783537130000117
as can be seen, the pan-tilt platform coordinate system (the solved navigation coordinate system) has a deviation from the real navigation coordinate system, and then the pan-tilt attitude estimated according to the pan-tilt platform coordinate system has an attitude error from the real pan-tilt attitude.
The holder further comprises a base navigation system, and the base navigation system is used for estimating the attitude of the base. The base navigation system can also be a strapdown inertial navigation system, and is similar to a holder platform coordinate system, and the navigation coordinate system obtained by the base navigation system through calculation when the base attitude is estimated is called a base platform coordinate system. The origin of the base coordinate system is the center of mass of the base, and the three axes of the base coordinate system can be consistent with the three axes of the carrier coordinate system. The base attitude represented by the euler angle is the azimuth of the base coordinate system relative to the navigation coordinate system.
It should be noted that the structure of the pan/tilt head shown in fig. 1 is only an example, and the embodiment of the present invention does not limit the structure of the pan/tilt head. In addition, the embodiment of the invention does not limit the application scene of the holder, and the carrier of the holder comprises an aircraft, an automobile, a ship and a handheld device.
Fig. 3 illustrates a method for estimating a pan/tilt attitude according to an embodiment of the present invention, which is suitable for the pan/tilt shown in fig. 1. Referring to fig. 3, the process flow includes the following steps.
And 301, respectively acquiring the angular speed and linear acceleration of the image acquisition equipment in the cradle head at the (i + 1) th moment on three axes of a cradle head coordinate system and the angular displacement of an output shaft of a course shaft motor in the cradle head.
Angular velocity is measured through a gyroscope, linear acceleration is measured through an accelerometer, and angular displacement is measured through a code disc. i is a positive integer.
And step 302, calculating the error angle of the platform of the holder at the (i + 1) th moment based on the angular velocity and the linear acceleration of the image acquisition equipment in the holder at the three axes of the holder coordinate system at the (i + 1) th moment and the angular displacement of a course axis motor in the holder.
The error angle of the holder platform at the (i + 1) th moment can be calculated through the EKF. The cradle head platform error angle at the (i + 1) th moment is the cradle head platform error angle generated when the cradle head navigation system runs through the ith calculation period. The ith calculation cycle is a period of time from the ith time to the end of the (i + 1) th time. It should be noted that the calculation period is identical to the filtering period.
And 303, acquiring the posture of the holder at the ith moment.
And when i is equal to 1, calculating the attitude of the pan-tilt head at the ith moment based on the linear acceleration of the image acquisition equipment at the ith moment and the angular displacement of the heading axis motor.
And 304, updating the cradle head posture at the ith moment based on the cradle head platform error angle at the ith +1 moment to obtain the cradle head posture at the ith +1 moment.
The cloud platform attitude increment between two moments can be estimated through the cloud platform error angle, and the cloud platform attitude at the later moment in the two moments can be obtained through the cloud platform attitude increment and the cloud platform attitude at the earlier moment in the two moments.
In the embodiment of the invention, the error angle of the platform of the holder is calculated; the tripod head platform error angle is a relative angular displacement relation between a calculated tripod head platform coordinate system and a real tripod head platform coordinate system, the real tripod head platform coordinate system is a navigation coordinate system, the calculated tripod head platform coordinate system is a navigation coordinate system estimated by a computer, in particular to a navigation coordinate system containing attitude errors estimated in the process of calculating the tripod head attitude, the tripod head attitude refers to the relative angular displacement relation between the tripod head coordinate system and the navigation coordinate system, and the tripod head platform error angle and the tripod head attitude increment between two moments have a conversion relation, so that the tripod head attitude increment between the two moments can be estimated based on the tripod head platform error angle, thus, the tripod head attitude increment from the moment i to the moment i +1 can be estimated based on the tripod head platform error angle at the moment i +1, and the tripod head attitude at the moment i can be updated based on the tripod head attitude increment, the cradle head attitude at the (i + 1) th moment is obtained, and the cradle head attitude is not directly solved for the integral of the gyroscope when the cradle head attitude is calculated, so that the problem of integral error of the gyroscope is avoided, and the estimation precision of the cradle head attitude can be improved.
Fig. 4 illustrates a cradle head attitude estimation method provided by an embodiment of the present invention, which is suitable for the cradle head illustrated in fig. 1. In this embodiment, an unmanned aerial vehicle is taken as an example of a carrier of a pan/tilt head, and a pan/tilt head attitude estimation method is described in detail. Referring to fig. 4, the process flow includes the following steps.
Step 401, obtaining the angular speed and linear acceleration of the image acquisition device at the 1 st moment on three axes of the holder coordinate system, and obtaining the angular displacement of the heading axis motor.
The angular velocity and the linear acceleration may be obtained by an Inertial Measurement Unit (IMU for short). The IMU includes a gyroscope to measure angular velocity and an accelerometer to measure linear acceleration.
In this embodiment, the angular velocity and linear acceleration of the image capturing device on three axes of the holder coordinate system may be measured by the first IMU. The first IMU can be installed at the center of mass of the image acquisition equipment, the gyroscope in the first IMU measures the angular velocity of three axes of the holder coordinate system, and the accelerometer in the first IMU measures the linear acceleration of three axes of the holder coordinate system.
Wherein, when the tripod head axis motor mechanism is the triaxial motor mechanism, this step 401 still includes: and acquiring the angular displacement of the roll shaft motor and the pitch shaft motor at the 1 st moment. The angular displacement of each shaft motor in the three-shaft motor mechanism can be obtained through the code disc. The coded disc is arranged on an output shaft of the shaft motor.
And step 402, calculating the attitude of the holder at the 1 st moment based on the linear acceleration of the image acquisition equipment at the three axes of the holder coordinate system at the 1 st moment and the angular displacement of a course axis motor.
In this embodiment, the 1 st time may be a time when the pan/tilt navigation system starts to work to calculate the pan/tilt attitude for the first time. At time 1, the default pan-tilt platform error angle does not begin to contribute to the pan-tilt attitude.
The pan-tilt navigation system can calculate the pan-tilt attitude at the 1 st moment according to the formula (3). Wherein, what formula (3) calculated is the cloud platform gesture of euler's angle form.
Figure BDA0001783537130000141
Figure BDA0001783537130000142
Figure BDA0001783537130000143
Wherein the content of the first and second substances,
Figure BDA0001783537130000146
is the heading angle of the pan/tilt head at the ith moment,
Figure BDA0001783537130000147
the roll angle of the pan/tilt head at the ith moment,
Figure BDA0001783537130000148
is the pitch angle, psi, of the pan-tilt at moment ibody,iThe carrier course angle at the ith moment; psim,iIs the angular displacement of the output shaft of the course shaft motor at the ith moment,
Figure BDA0001783537130000144
the linear accelerations of the image capturing device at the three axes of the pan-tilt coordinate system X, Y, Z are respectively shown at the i-th moment.
When the carrier of cloud platform is unmanned aerial vehicle, unmanned aerial vehicle course angle can follow the aircraft control system and obtain. The pan-tilt navigation system is electrically connected with the aircraft control system through a Controller Area Network (CAN) bus.
The posture of the holder at the 1 st moment is obtained through calculation of a formula (3)
Figure BDA0001783537130000145
And step 403, calculating the platform error angle of the holder at the 2 nd moment through the EKF.
The cradle head platform error angle at the 2 nd moment is the cradle head platform error angle generated when the cradle head navigation system runs through the 1 st calculation period. The 1 st calculation cycle is a period of time from the 1 st time to the 2 nd time.
In this embodiment, the state quantity includes a pan-tilt platform error angle. Since the error angle of the platform is a small amount of error, the precision can be very high when the EKF is used for estimation. The principle of EKF is: firstly, assigning a value to the state quantity at the 1 st moment to obtain the state quantity at the 1 st moment; then, in the subsequent filtering process, calculating the estimated state quantity of the current moment based on the state quantity of the previous moment; and correcting the estimated state quantity of the current moment by utilizing the observed quantity to obtain the state quantity of the current moment.
Here, the observed quantity is calculated based on a parameter related to the state quantity sensed by the sensor device at the present time. In this embodiment, when the pan/tilt platform error angle is estimated through the EKF, the observed quantity includes a pan/tilt attitude calculated based on information measured by a first sensor device such as an accelerometer, and is hereinafter collectively referred to as a pan/tilt attitude observed quantity.
This step includes steps 4031-4033.
Step 4031, calculating the holder attitude observation quantity at the moment 2 based on the linear acceleration of the image acquisition equipment at the three axes of the holder coordinate system at the moment 2 and the angular displacement of a course axis motor.
Referring to fig. 5, present step 4031 includes steps 4031 a-4031 d as follows.
Step 4031a, calculating the horizontal roll angle of the holder at the 2 nd moment.
As shown in the formula for calculating the roll angle in formula (3), the roll angle of the pan/tilt head at the 2 nd moment
Figure DA00017835371338418
Figure BDA0001783537130000151
The linear accelerations of the image capturing device at three axes of the pan-tilt coordinate system X, Y, Z at time 2, respectively.
Step 4031b, comparing the pan-tilt roll angle at the moment 2 with a first threshold value.
When the horizontal roll angle of the tripod head at the moment 2 is smaller than a first threshold value, executing the step 4031 c; and when the pan-tilt roll angle at the moment 2 is larger than the first threshold, executing the step 4031 d. When the pan/tilt head roll angle at the time 2 is equal to the first threshold, step 4031c may be performed, and step 4031d may also be performed.
The first threshold value can be obtained through a large amount of experimental data, and the specific value of the first threshold value is related to the sensitivity of the holder accelerometer.
Step 4031c, calculating the holder attitude at the 2 nd moment through the formula (3), and taking the calculated holder attitude as the holder attitude observed quantity.
Calculating to obtain the observation quantity of the posture of the holder at the 2 nd moment through a formula (3)
Figure BDA0001783537130000152
When the roll angle of the holder is not greater than the first threshold value, the whole holder can be considered to be in a stop, uniform speed or low acceleration state, the horizontal attitude (the horizontal attitude comprises the roll angle and the pitch angle) of the holder can be accurately calculated through the linear acceleration of the image acquisition equipment sampled by the accelerometer, the horizontal attitude observed quantity of the holder is calculated by adopting the linear acceleration of the image acquisition equipment sampled by the accelerometer, and the estimation precision of the posture of the holder is improved.
Step 4031d, the base posture at the 1 st moment and the angular displacement of the output shaft of each shaft motor in the holder at the 2 nd moment are obtained, and the holder posture observed quantity at the 2 nd moment is calculated based on the base posture at the 1 st moment and the angular displacement of each shaft motor in the holder at the 2 nd moment.
In step 4031d, the carrier attitude at time 1 is acquired, and the carrier attitude at time 1 is taken as the base attitude at time 1. When the carrier of cloud platform is unmanned aerial vehicle, the unmanned aerial vehicle gesture can be followed aircraft control system and acquireed.
As mentioned above, when the axis motor mechanism is an axis motor mechanism, the axis motor in the pan/tilt head includes a heading axis motor. At the moment, the code wheel angle (namely angular displacement) of the course axis motor at the moment 2 is rotated around the Z axis according to the base posture at the moment 1, and the observed quantity of the holder posture at the moment 2 is obtained
Figure BDA0001783537130000153
When the axial motor mechanism is a two-axis motor mechanism and the central axis motor of the holder comprises a course axial motor and a transverse rolling shaft motor, at the moment, the base posture at the 1 st moment rotates the code wheel angle (namely angular displacement) of the corresponding axial motor at the 2 nd moment in sequence according to the rotation sequence of an X axis-Z axis or a Z axis-X axis, and the holder posture observed quantity at the 2 nd moment is obtained
Figure BDA0001783537130000154
When the shaft motor mechanism is a two-shaft motor mechanism and the central shaft motor of the holder comprises a course shaft motor and a pitch shaft motor, at the moment, the base posture at the 1 st moment rotates the code wheel angle (namely angular displacement) of the corresponding shaft motor at the 2 nd moment in sequence according to the rotation sequence of an X shaft-Y shaft or a Y shaft-X shaft, and the holder posture observed quantity at the 2 nd moment is obtained
Figure BDA0001783537130000161
When the axis motor mechanism is a three-axis motor mechanism, the center axis motor of the holder comprises a course axis motor, a roll axis motor and a pitch axis motor, and specifically, the base attitude at the 1 st moment rotates the code wheel angle (namely angular displacement) of the corresponding axis motor at the 2 nd moment according to the rotation sequence of an X axis-Y axis-Z axis, a Y axis-Z axis-X axis or a Z axis-X axis-Y axis in sequence to obtain the holder attitude observed quantity at the 2 nd moment
Figure BDA0001783537130000162
Wherein, the Z axis corresponds to a course axis motor, the X axis corresponds to a roll axis motor, and the Y axis corresponds to a pitch axis motor.
When the roll angle of the holder is larger than the first threshold value, the whole holder can be considered to be in an acceleration state, at this time, the linear acceleration of the image acquisition equipment sampled by the accelerometer carries the acceleration value of the whole holder, the attitude observed quantity of the holder cannot be accurately calculated, at this time, the attitude observed quantity of the holder is calculated by adopting the attitude of the base, and the estimation precision of the attitude of the holder is improved.
Step 4032, calculating the estimated platform error angle of the holder at the 2 nd moment based on the platform error angle of the holder at the 1 st moment and the angular velocities of the image acquisition equipment at the 2 nd moment on the three axes of the holder coordinate system.
Step 4032 includes: and calculating the estimated state quantity at the 2 nd moment through a pre-established state prediction equation, the state quantity at the 1 st moment and the angular velocity of the image acquisition equipment at the 2 nd moment on three axes of the holder coordinate system, wherein the state quantity comprises a holder platform error angle and a holder gyroscope offset, and the estimated state quantity comprises an estimated holder platform error angle and an estimated holder gyroscope offset.
The gyroscope bias may be angular velocities respectively output by the gyroscope at three axes of the holder coordinate system under a zero-input condition. Because the offset of the tripod head gyroscope is required to be used when the error angle of the tripod head platform is predicted and estimated, the offset of the tripod head gyroscope cannot be measured, and the tripod head gyroscope is also used as a state quantity to be calculated and output.
The pre-established state prediction equation is shown in equation (4).
Figure BDA0001783537130000163
Wherein the content of the first and second substances,
Figure BDA0001783537130000164
is the estimated state quantity at the (i + 1) th moment,
Figure DA00017835371338454
Figure BDA0001783537130000165
the estimated error angle of the platform of the pan/tilt head at the (i + 1) th moment,
Figure BDA0001783537130000166
and respectively estimating error angles of the platform of the holder in the north direction, the east direction and the ground direction at the (i + 1) th moment.
Figure BDA0001783537130000167
For the estimated tripod head gyroscope offset at the (i + 1) th moment,
Figure BDA0001783537130000168
and respectively estimating the gyroscope offset of the tripod head on the three axes of the tripod head coordinate system at the (i + 1) th moment.
Figure BDA0001783537130000169
And
Figure BDA00017835371300001610
the process noise is related to the precision of the used gyroscope, can be provided by a gyroscope manufacturer, and can also be obtained through test measurement;
Figure BDA0001783537130000171
is the angular velocity of the image acquisition equipment at the (i + 1) th moment on three axes of the holder coordinate system,
Figure BDA0001783537130000172
in order to calculate the period of the cycle,
Figure BDA0001783537130000173
equal to the length of the period from the start of the ith time instant to the end of the (i + 1) th time instant,
Figure BDA0001783537130000174
is the error angle of the platform of the pan/tilt head at the ith moment,
Figure BDA0001783537130000175
the error angles of the platform of the tripod head in the north direction, the east direction and the ground direction at the ith moment,
Figure BDA0001783537130000176
for the gyroscope bias of the image acquisition device at time i,
Figure BDA0001783537130000177
and respectively the gyroscope offsets of the image acquisition equipment at the ith moment on three axes of the holder coordinate system.
State quantity at time 1
Figure BDA0001783537130000178
Is a preset state quantity. That is to say that the position of the first electrode,
Figure BDA0001783537130000179
the value of each state quantity is a preset value. Based on the formula (4), the estimated state quantity of the 2 nd time is calculated and obtained
Figure BDA00017835371300001710
Since the EKF has a convergence characteristic, the state quantity value at the 1 st time has very little influence on the filtering effect in a general stable system, and therefore the magnitude of the preset state quantity is not limited, and for example, the preset state quantities may all be set to 0.
This step 4032 further includes: and calculating the estimated state quantity covariance at the 2 nd moment through a pre-established state quantity covariance prediction equation and the state quantity covariance at the 1 st moment.
The pre-established state quantity covariance prediction equation is shown in equation (5).
Figure BDA00017835371300001711
Wherein the content of the first and second substances,
Figure BDA00017835371300001712
is the estimated state quantity covariance, F, at the i +1 th momenti+1Is the state transition matrix at time i +1,
Figure BDA00017835371300001713
is Fi+1Transposed matrix of (2), QiIs the process noise covariance, P, at time iiIs the state quantity covariance at the ith time. State transition matrix F at time i +1i+1Can be found based on equation (4) and the state quantity at the ith time. Specifically, a jacobian matrix is solved in advance for formula (4), and a state transition matrix expression at the i +1 th time can be obtained, where the expression includes parameters such as the state quantity at the i th time. When the state transition matrix at the (i + 1) th moment is calculated, values of parameters such as the state quantity at the (i) th moment and the like contained in the expression are directly substituted into the expression to obtain the state transition matrix at the (i + 1) th moment.
The covariance of the state quantity at the 1 st time is a preset covariance. Since the EKF has a convergence characteristic, for a general stable system, the influence of the state quantity covariance value at the 1 st time on the filtering effect is very small, so the magnitude of the preset covariance is not limited, for example, the preset state quantity covariance may be set to 0.
In this embodiment, the covariance refers to a covariance matrix.
Step 4033, based on the cradle head posture observed quantity at the moment 2, correcting the estimated cradle head platform error angle at the moment 2 to obtain the cradle head platform error angle at the moment 2.
Referring to fig. 6, present step 4033 includes steps 4033 a-4033 c as follows.
Step 4033a, based on the pre-established measurement equation and the estimated state quantity at the 2 nd moment, calculating the estimated observed quantity at the 2 nd moment.
The measurement equation is used for describing the association between the observed quantity and the state quantity. The estimation observation quantity is a form of representing the estimation state quantity as the observation quantity, and is directly operated with the observation quantity in the correction process. Illustratively, the attitude can be expressed in three forms of an euler angle, a quaternion and a direction cosine matrix, and since the observed quantity is expressed in the form of the euler angle, the estimated observed quantity is also expressed in the form of the euler angle, and the measurement equation for calculating the estimated observed quantity is also expressed in the form of the euler angle. The measurement equation is
Figure BDA0001783537130000181
After expansion, the formula (6) shows.
Figure BDA0001783537130000182
Wherein, Z ″)i+1Represents the estimated observed quantity at the i +1 th time,
Figure BDA0001783537130000183
is a direction cosine matrix from the pan-tilt to the navigation coordinate system at the (i + 1) th moment,
Figure BDA0001783537130000184
is the 1 st row and 2 nd column element in the direction cosine matrix,
Figure BDA0001783537130000185
is the 2 nd row and 2 nd column element in the direction cosine matrix,
Figure BDA0001783537130000186
is the column 1 element of the 3 rd row in the direction cosine matrix,
Figure BDA0001783537130000187
is the 3 rd row and 2 nd column element in the direction cosine matrix,
Figure BDA0001783537130000188
is the 3 rd row and 3 rd column element in the direction cosine matrix. Wherein the content of the first and second substances,
Figure BDA0001783537130000189
the method can be obtained by the estimated error angle of the platform of the holder at the (i + 1) th moment. The process of calculating the estimated observation at time 2 includes:
firstly, based on the estimated tripod head platform error angle at the 2 nd moment and formulas (7) and (8), the estimated tripod head attitude at the 2 nd moment is calculated, and the estimated tripod head attitude at the 2 nd moment obtained by calculation is used as the estimated observed quantity.
Figure BDA00017835371300001810
Wherein the content of the first and second substances,
Figure BDA00017835371300001811
the estimated tripod head attitude increment of the ith +1 moment relative to the ith moment,
Figure BDA00017835371300001812
and the error angle of the platform of the cradle head is estimated at the (i + 1) th moment.
Figure BDA00017835371300001813
Wherein the content of the first and second substances,
Figure BDA00017835371300001814
the estimated tripod head attitude at the (i + 1) th moment, namely the estimated observed quantity represented by the quaternion,
Figure BDA00017835371300001815
the attitude of the pan/tilt at the ith time, i.e., the attitude of the pan/tilt at the ith time represented by the quaternion, represents a quaternion multiplication operation. Formula (7) represents the conversion relation between the platform error angle of the pan/tilt head and the attitude increment of the pan/tilt head, and in formula (7), the conversion relation is in the form of quaternion. The estimated cradle head posture at the (i + 1) th moment can be calculated through a formula (8).
It should be noted that the equations (7) and (8) can be derived according to an incremental algorithm of a quaternion differential equation (also called quaternion first-order incremental algorithm), an error angle error equation of a mathematical platform of a strapdown inertial navigation system, and the like, and the specific derivation process is well known to those skilled in the art and will not be described herein again.
Since the direction cosine method and the quaternion method are common algorithms in the attitude calculation, the above formula (7) may alternatively be expressed as a conversion relationship in the form of direction cosine, and accordingly, the formula (8) may also be expressed as a relationship between the attitudes in the form of direction cosine. The equations (7) and (8) in the form of direction cosine can be derived according to an incremental algorithm of a direction cosine differential equation (also called direction cosine first-order incremental algorithm), an error angle error equation of a mathematical platform of a strapdown inertial navigation system and the like.
Since the pan/tilt attitude at the 1 st time is represented by the euler angle, the pan/tilt attitude at the 1 st time represented by the euler angle is converted into a quaternion representation before the estimated pan/tilt attitude at the 2 nd time is calculated by the formula (8), and the conversion process is represented by the formula (9):
Figure BDA0001783537130000191
in formula (9), [ q ]0q1q2q3]TRepresenting quaternion, [ psi γ θ]TRepresenting the euler angle.
Secondly, estimating the quaternion of the observed quantity
Figure BDA0001783537130000192
Conversion of representation into directional cosine matrix
Figure BDA0001783537130000193
And (4) showing.
Then, the direction cosine matrix is
Figure BDA0001783537130000194
Substituting the corresponding specific elements into formula (9) to obtain the estimated observed quantity Z' represented by Euler anglei+1。
Step 4033b, based on the estimated covariance of the state quantities at the 2 nd time, calculating the Kalman filtering gain at the 2 nd time.
The kalman filter gain at the i +1 th time may be calculated according to the following equation (10).
Figure BDA0001783537130000195
Wherein, Ki+1The kalman filter gain at the i +1 th time,
Figure BDA0001783537130000196
is the estimated state quantity covariance, H, at the i +1 th momenti+1Is a measurement matrix at the i +1 th time, Ri+1Is the measured noise covariance at time i + 1. Measurement matrix H at the i +1 th timei+1May be obtained based on equation (6) and the estimated state quantity at the i +1 th time. Specifically, the Jacobian matrix is solved in advance for the formula (6), and a measurement matrix expression at the (i + 1) th time can be obtained, wherein the expression includes the estimated state quantity at the (i + 1) th time. When calculating the measurement matrix at the (i + 1) th moment, firstly, taking the estimated state quantity at the (i + 1) th moment as the state quantity at the (i + 1) th moment, and calculating a parameter related to the state quantity at the (i + 1) th moment; and substituting the calculated parameters related to the state quantity at the (i + 1) th moment into a measurement matrix expression to obtain a measurement matrix at the (i + 1) th moment. The measurement noise is noise generated in the process of obtaining the observed quantity, and can be set based on experience.
Step 4033c, estimating the state quantity at the 2 nd moment based on the estimated state quantity, the Kalman filtering gain, the holder attitude observed quantity and the estimated observed quantity at the 2 nd moment.
Wherein the state quantities comprise a holder platform error angle and a holder gyroscope offset.
The state quantity at the 2 nd time can be estimated according to a state estimation equation shown in equation (11).
Figure BDA0001783537130000201
Figure BDA0001783537130000202
Is the state quantity at the (i + 1) th moment,
Figure BDA0001783537130000203
Figure BDA0001783537130000204
is the error angle of the platform of the pan/tilt head at the moment of i +1,
Figure BDA0001783537130000205
the error angles of the platform of the holder in the north direction, the east direction and the ground direction at the moment i +1 are respectively.
Figure BDA0001783537130000206
Figure BDA0001783537130000207
For the pan tilt gyroscope bias at time i +1,
Figure BDA0001783537130000208
and (3) biasing the tripod head gyroscopes which are three axes of the tripod head coordinate system at the (i + 1) th moment respectively.
Figure BDA0001783537130000209
Is the estimated state quantity, K, at the i +1 th momenti+1Kalman filter gain, Z ", at time i +1i+1Is estimated observed quantity (estimated tripod head attitude) Z 'at the moment i + 1'i+1And (4) the observed quantity of the posture of the holder at the (i + 1) th moment.
Step 4033c further includes: and estimating the covariance of the state quantity at the 2 nd moment based on the covariance of the estimated state quantity at the 2 nd moment and the Kalman filtering gain.
The state quantity covariance at time 2 can be estimated according to the covariance estimation equation. The covariance estimation equation is shown in equation (12).
Figure BDA00017835371300002010
Wherein, Pi+1Is the state quantity covariance at the i +1 th instant,i is an identity matrix, Ki+1The kalman filter gain at the i +1 th time,
Figure BDA00017835371300002011
is the estimated state quantity covariance at the (i + 1) th moment.
And step 404, calculating the attitude increment of the pan-tilt head at the 2 nd moment relative to the 1 st moment based on the error angle of the pan-tilt head platform at the 2 nd moment.
The tripod head attitude increment at the 2 nd moment relative to the 1 st moment is the tripod head attitude increment after the tripod head attitude at the 1 st moment passes through the 1 st calculation period.
The pan-tilt navigation system can calculate the pan-tilt attitude increment of the 2 nd moment relative to the 1 st moment according to the updated formula (7)
Figure BDA00017835371300002012
The updated formula (7) is that,
Figure BDA00017835371300002013
the attitude increment of the pan/tilt head at the i +1 th moment relative to the i th moment,
Figure BDA00017835371300002014
the error angle of the platform of the tripod head at the (i + 1) th moment. And calculating the estimated tripod head attitude increment of the 2 nd time relative to the 1 st time
Figure BDA00017835371300002015
(see step 4033a) except that the pan head attitude increment at time 2 is calculated relative to time 1
Figure BDA00017835371300002016
Substituting the formula into the error angle of the platform of the pan/tilt head at the 2 nd moment
Figure BDA00017835371300002017
And 405, calculating the cradle head posture at the 2 nd moment based on the cradle head posture at the 1 st moment and the cradle head posture increment of the 2 nd moment relative to the 1 st moment.
The pan-tilt navigation system can calculate the pan-tilt attitude at the 2 nd moment according to the updated formula (8)
Figure BDA00017835371300002018
The updated formula (8) is that,
Figure BDA0001783537130000211
wherein the content of the first and second substances,
Figure BDA0001783537130000212
the posture of the pan/tilt head at the (i + 1) th moment represented by the quaternion,
Figure BDA0001783537130000213
the attitude of the pan/tilt at the ith time represents a quaternion multiplication operation.
Since the pan/tilt posture at the time 1 is represented by the euler angle, the pan/tilt posture at the time 1 represented by the euler angle is converted into a quaternion representation before the pan/tilt posture at the time 2 is calculated by the updated formula (8).
And step 406, calculating the cradle head attitude at the 3 rd moment and each moment after the 3 rd moment.
Specifically, steps 403 to 405 are repeatedly executed, that is, the pan/tilt attitude at the time point 3 is calculated according to the calculation process of calculating the pan/tilt attitude at the time point 2: first, the pan/tilt platform error angle at time 3 is calculated by EKF. And secondly, calculating the attitude increment of the tripod head at the 3 rd moment relative to the 2 nd moment based on the error angle of the tripod head platform at the 3 rd moment. Then, the pan/tilt attitude at time 3 is calculated based on the pan/tilt attitude at time 2 and the pan/tilt attitude increment at time 3 with respect to time 2.
Similarly, when the cradle head attitude at each time after the 3 rd time is calculated, the calculation is also performed according to the calculation process of calculating the cradle head attitude at the 3 rd time, which is not described herein again.
Fig. 7 illustrates a method for estimating a pan/tilt attitude according to an embodiment of the present invention, which is applicable to the pan/tilt shown in fig. 1. Compared with the embodiment shown in fig. 4, the EKF method provided by the embodiment of the present invention is different in the way of calculating the pan/tilt attitude observed quantity. Referring to fig. 7, the method flow includes the following steps.
Step 501, obtaining the angular velocity and linear acceleration of the image acquisition device at the 1 st moment on three axes of a holder coordinate system, obtaining the angular displacement of each axis motor at the 1 st moment, and obtaining the angular velocity and linear acceleration of the base at the 1 st moment on three axes of a base coordinate system.
The first IMU can be adopted to measure the angular velocity and the linear acceleration of the image acquisition equipment on the three axes of the holder coordinate system, and the second IMU can be adopted to measure the angular velocity and the linear acceleration of the base on the three axes of the base coordinate system. A second IMU may be mounted at the center of mass of the base, with the gyroscope measuring angular velocities of three axes of the base coordinate system and the accelerometer measuring linear accelerations of three axes of the base coordinate system.
It should be noted that the first IMU belongs to the pan/tilt/zoom (pan/tilt/zoom) navigation system, the second IMU belongs to the base navigation system, and the first IMU and the second IMU are two independent strapdown inertial navigation systems and have different mathematical platforms, respectively, so that a pan/tilt/zoom (pan/tilt/zoom) coordinate system based on the first IMU and a base coordinate system based on the second IMU are completely independent, and a pan/tilt/zoom (pan/tilt/zoom) coordinate system based on the first IMU and a base coordinate system based on the second IMU are also completely independent.
And 502, calculating the posture of the holder at the 1 st moment based on the linear acceleration of the image acquisition equipment at three axes of the holder coordinate system at the 1 st moment and the angular displacement of a course axis motor.
This step 502 is the same as step 402 in the embodiment shown in fig. 4, and is not described again here.
Step 503, calculating the base posture at the 1 st moment based on the linear acceleration of the base at the three axes of the base coordinate system at the 1 st moment and the carrier course angle.
At time 1, the default base platform error angle does not begin to contribute to the base attitude, at which time the base navigation system can calculate the base attitude at time 1 according to equation (13). Wherein, the formula (13) calculates the attitude of the base in the form of euler angles.
Figure BDA0001783537130000221
Figure BDA0001783537130000222
Figure BDA0001783537130000223
Wherein the content of the first and second substances,
Figure BDA0001783537130000226
is the heading angle of the base at time i,
Figure BDA0001783537130000227
is the roll angle of the susceptor at time i,
Figure BDA0001783537130000228
is the pitch angle of the base at time i, psibody,iThe carrier course angle at the ith moment;
Figure BDA0001783537130000224
the linear acceleration of the base on three axes of the base coordinate system at the ith moment.
Calculating the base posture at the 1 st moment through a formula (13)
Figure BDA0001783537130000225
And step 504, calculating the platform error angle of the holder at the 2 nd moment through the EKF.
This step 504 is similar to step 403 in the embodiment shown in fig. 4, and is not described again here.
It should be noted that, when the pan/tilt head roll angle at time 2 is greater than the first threshold, the acquired base posture at time 1 is the base posture at time 1 calculated in step 503, and then the pan/tilt head posture observed quantity at time 2 is calculated based on the base posture at time 1 and the angular displacement of each axis motor at time 2.
And 505, calculating the attitude increment of the holder at the 2 nd moment based on the error angle of the holder platform at the 2 nd moment.
This step 505 is similar to step 404 in the embodiment shown in fig. 4, and is not repeated here.
And step 506, calculating the cradle head posture at the 2 nd moment based on the cradle head posture at the 1 st moment and the cradle head posture increment at the 2 nd moment.
This step 506 is similar to step 405 in the embodiment shown in fig. 4, and is not described again here.
And step 507, calculating the error angle of the base platform at the 2 nd moment through the EKF.
The EKF process for calculating the error angle of the base platform at the time 2 is similar to the EKF process for calculating the error angle of the pan/tilt platform at the time 2, and the difference is that the specific parameters are different, and the process for calculating the error angle of the base platform at the time 2 through the EKF is briefly described below.
This step 507 includes steps 5071 to 5073 as follows.
Step 5071, calculating the attitude observation of the base at the 2 nd time based on the three-axis linear acceleration of the base in the base coordinate system at the 2 nd time.
In this embodiment, when estimating the base platform error angle through the EKF, the observed quantity includes a base attitude calculated based on information measured by the second sensor device of the accelerometer, and is hereinafter collectively referred to as a base attitude observed quantity. Referring to fig. 8, the present step 5071 includes steps 5071 a-5071 d as follows.
Step 5071a, calculating the base roll angle at time 2.
As shown in the formula for calculating the roll angle in the formula (13), the roll angle of the base at the 2 nd time
Figure BDA0001783537130000231
Figure BDA0001783537130000232
The linear acceleration of the base on three axes of the base coordinate system at the moment 2.
Step 5071b, comparing the base roll angle at time 2 to a second threshold.
When the base roll angle at the time 2 is smaller than the second threshold, executing step 5071 c; when the base roll angle at time 2 is greater than the second threshold, step 5071d is performed. When the base roll angle at time 2 is equal to the second threshold, step 5071c may be performed, and step 5071d may be performed.
The second threshold may be obtained from a large number of experimental data, and the specific value of the second threshold is related to the sensitivity of the base accelerometer.
Step 5071c, calculating the attitude of the base at the 2 nd moment according to the formula (13), and taking the calculated attitude of the base as the observed quantity of the attitude of the base.
Calculating to obtain the observation quantity of the base posture at the 2 nd moment through a formula (13)
Figure BDA0001783537130000233
And step 5071d, acquiring the carrier posture at the 2 nd time, and taking the carrier posture at the 2 nd time as the base posture observed quantity at the 2 nd time.
Step 5072, calculating an estimated base table error angle at time 2 based on the base table error angle at time 1 and the angular velocity of the base at time 2.
This step 5072 includes: and calculating the estimated state quantity at the 2 nd moment through a pre-established state prediction equation, the state quantity at the 1 st moment and the angular velocity of the base at the 2 nd moment, wherein the state quantity comprises a base platform error angle and a base gyroscope bias.
The pre-established state prediction equation is shown in equation (14).
Figure BDA0001783537130000234
Wherein the content of the first and second substances,
Figure BDA0001783537130000235
is the estimated state quantity at the (i + 1) th moment,
Figure DA00017835371338490
Figure BDA0001783537130000236
for the estimated base flatness at the i +1 th momentThe angle of the error of the stage is,
Figure BDA0001783537130000237
respectively, the estimated error angles of the base platform in the north direction, the east direction and the ground direction at the (i + 1) th moment.
Figure BDA0001783537130000238
For the estimated base gyroscope bias at time i +1,
Figure BDA0001783537130000241
and (4) respectively estimating the bias of the base gyroscope of the three axes of the base coordinate system at the (i + 1) th moment.
Figure BDA0001783537130000242
And
Figure BDA0001783537130000243
the process noise is related to the precision of the used gyroscope, can be provided by a gyroscope manufacturer, and can also be obtained through test measurement;
Figure BDA0001783537130000244
is the angular velocity of the base at the three axes of the base coordinate system at the moment i +1,
Figure BDA0001783537130000245
in order to calculate the period of the cycle,
Figure BDA0001783537130000246
equal to the period of time starting from the ith time instant and ending at the (i + 1) th time instant,
Figure BDA0001783537130000247
is the base platform error angle at time i,
Figure BDA0001783537130000248
north, east and ground error angles of the base platform at time i,
Figure BDA0001783537130000249
the gyro bias of the base at time i,
Figure BDA00017835371300002410
state quantity at time 1
Figure BDA00017835371300002411
Is a preset state quantity. That is to say that the position of the first electrode,
Figure BDA00017835371300002412
the value of each state quantity is a preset value. Based on the formula (4), the estimated state quantity of the 2 nd time is calculated and obtained
Figure BDA00017835371300002413
This step 5072 further includes: and calculating the estimated state quantity covariance at the 2 nd moment through a pre-established state quantity covariance prediction equation and the state quantity covariance at the 1 st moment.
The pre-established state quantity covariance prediction equation is shown in equation (15).
Figure BDA00017835371300002414
Wherein the content of the first and second substances,
Figure BDA00017835371300002415
the estimated state quantity covariance at the (i + 1) th time,
Figure BDA00017835371300002416
is the state transition matrix at time i +1,
Figure BDA00017835371300002417
is composed of
Figure BDA00017835371300002418
The transpose matrix of (a) is,
Figure BDA00017835371300002421
is the process noise covariance, P, at time ii bIs the state quantity covariance at the ith time. State transition matrix at the i +1 th time
Figure BDA00017835371300002419
Can be found based on equation (14) and the state quantity at the ith timing. Specifically, a jacobian matrix is solved in advance for formula (14), and a state transition matrix expression at the i +1 th time can be obtained, where the expression includes parameters such as the state quantity at the i th time. When the state transition matrix at the (i + 1) th moment is calculated, values of parameters such as the state quantity at the (i) th moment and the like contained in the expression are directly substituted into the expression to obtain the state transition matrix at the (i + 1) th moment.
In this embodiment, the covariance refers to a covariance matrix.
Step 5073, correcting the estimated base platform error angle at time 2 based on the base attitude observations at time 2 to obtain a base platform error angle at time 2.
Referring to fig. 9, the present step 5073 includes steps 5073 a-5073 c as follows.
Step 5073a, calculating the estimated observed quantity at the 2 nd time based on the pre-established measurement equation and the estimated state quantity at the 2 nd time.
The measurement equation is
Figure BDA00017835371300002420
After expansion, as shown in equation (16).
Figure BDA0001783537130000251
Wherein the content of the first and second substances,
Figure BDA0001783537130000252
represents the estimated observed quantity at the i +1 th time,
Figure BDA0001783537130000253
is the direction cosine matrix from the base to the navigation coordinate system at the (i + 1) th moment.
Figure BDA0001783537130000254
Is the direction cosine matrix
Figure BDA0001783537130000255
Row 1 and column 2 elements in the middle,
Figure BDA0001783537130000256
is the direction cosine matrix
Figure BDA0001783537130000257
Row 2 and column 2 elements of the middle,
Figure BDA0001783537130000258
is the direction cosine matrix
Figure BDA0001783537130000259
Row 3 and column 1 elements in,
Figure BDA00017835371300002510
is the direction cosine matrix
Figure BDA00017835371300002511
Row 3 and column 2 elements in the middle,
Figure BDA00017835371300002512
is the direction cosine matrix
Figure BDA00017835371300002513
Row 3 and column 3 elements. Wherein the content of the first and second substances,
Figure BDA00017835371300002514
can be obtained from the estimated error angle of the base platform at the (i + 1) th moment. The process of calculating the estimated observation at time 2 includes:
first, the estimated base attitude at the 2 nd time is calculated based on the estimated base platform error angle at the 2 nd time and equations (17) and (18), and the calculated estimated base attitude at the 2 nd time is used as the estimated observed quantity.
Figure BDA00017835371300002515
Wherein the content of the first and second substances,
Figure BDA00017835371300002516
the estimated attitude increment of the base at the (i + 1) th time relative to the (i) th time,
Figure BDA00017835371300002517
the estimated error angle of the base platform at the (i + 1) th moment.
Figure BDA00017835371300002518
Wherein the content of the first and second substances,
Figure BDA00017835371300002519
the estimated attitude of the base at the (i + 1) th moment, namely the estimated observed quantity expressed by the quaternion,
Figure BDA00017835371300002520
the base attitude at the i-th time represents a quaternion multiplication operation.
Secondly, estimating the quaternion of the observed quantity
Figure BDA00017835371300002521
Conversion of representation into directional cosine matrix
Figure BDA00017835371300002522
And (4) showing.
Then, the direction cosine matrix is
Figure BDA00017835371300002523
Substituting the corresponding specific elements into formula (16) to obtain the estimated observed quantity expressed by Euler angle
Figure BDA00017835371300002524
And 5073b, calculating a Kalman filter gain at the 2 nd time based on the estimated covariance of the state quantities at the 2 nd time.
The kalman filter gain at the i +1 th time may be calculated according to the following equation (19).
Figure BDA00017835371300002525
Wherein the content of the first and second substances,
Figure BDA00017835371300002526
the kalman filter gain at the i +1 th time,
Figure BDA00017835371300002527
the estimated state quantity covariance at the (i + 1) th time,
Figure BDA00017835371300002528
is the measurement matrix at the i +1 th time,
Figure BDA00017835371300002529
is the measured noise covariance at time i + 1. Measurement matrix at the i +1 th time
Figure BDA00017835371300002530
May be obtained based on equation (16) and the estimated state quantity at the i +1 th time. Specifically, the Jacobian matrix is solved in advance for the formula (16), and a measurement matrix expression at the i +1 th time can be obtained, wherein the expression includes the estimated state quantity at the i +1 th time. When calculating the measurement matrix at the (i + 1) th moment, firstly, taking the estimated state quantity at the (i + 1) th moment as the state quantity at the (i + 1) th moment, and calculating a parameter related to the state quantity at the (i + 1) th moment; and substituting the calculated parameters related to the state quantity at the (i + 1) th moment into an expression to obtain a measurement matrix at the (i + 1) th moment.
And step 5073c, estimating the state quantity at the 2 nd moment based on the estimated state quantity at the 2 nd moment, the Kalman filtering gain, the base attitude observed quantity and the estimated observed quantity.
Wherein the state quantities include a base platform error angle and a base gyroscope bias. The state quantity at the 2 nd time can be estimated according to a state estimation equation shown in equation (20).
Figure BDA0001783537130000261
Figure BDA0001783537130000262
Is the state quantity at the (i + 1) th moment,
Figure BDA0001783537130000263
Figure BDA0001783537130000264
the base platform error angle at time i +1,
Figure BDA0001783537130000265
the north, east and ground error angles of the base platform at time i +1, respectively.
Figure BDA0001783537130000266
Figure BDA0001783537130000267
For the base gyroscope bias at time i +1,
Figure BDA0001783537130000268
base gyroscope biases for three axes of the base coordinate system at time i +1, respectively.
Figure BDA0001783537130000269
Is the estimated state quantity at the (i + 1) th moment,
Figure BDA00017835371300002610
the estimated observation (estimated base attitude) at time i +1,
Figure BDA00017835371300002611
is the base attitude observation at time i + 1.
This step 5073c further includes: and estimating the covariance of the state quantity at the 2 nd moment based on the covariance of the estimated state quantity at the 2 nd moment and the Kalman filtering gain.
The state quantity covariance at time 2 can be estimated according to the covariance estimation equation. The covariance estimation equation is shown in equation (21).
Figure BDA00017835371300002612
Wherein the content of the first and second substances,
Figure BDA00017835371300002613
is the covariance of the state quantities at the I +1 th moment, I is the identity matrix,
Figure BDA00017835371300002614
the kalman filter gain at the i +1 th time,
Figure BDA00017835371300002615
is the estimated state quantity covariance at the (i + 1) th moment.
And step 508, calculating the attitude increment of the base at the 2 nd time relative to the 1 st time based on the error angle of the base platform at the 2 nd time.
The base navigation system may calculate the base attitude increment at time 2 relative to time 1 according to the updated equation (17)
Figure BDA00017835371300002616
The updated formula (17) is,
Figure BDA00017835371300002617
Figure BDA00017835371300002618
the incremental base attitude at time i +1 relative to time i,
Figure BDA00017835371300002619
the base platform error angle at time i + 1. And calculating the estimated observed quantity of the 2 nd time
Figure BDA00017835371300002620
(see step 5073a) except that the incremental base attitude at time 2 is calculated relative to time 1
Figure BDA00017835371300002621
Substituted into the formula is the error angle of the pedestal platform at time 2
Figure BDA00017835371300002622
Step 509, calculate the base attitude at time 2 based on the base attitude at time 1 and the base attitude increment at time 2 relative to time 1.
The base navigation system may calculate the base attitude at time 2 according to the updated equation (18)
Figure BDA0001783537130000271
The updated formula (18) is
Figure BDA0001783537130000272
The attitude of the base at time i +1 in quaternion representation,
Figure BDA0001783537130000273
the attitude of the base at the ith time.
And step 510, calculating the cradle head attitude at the 3 rd moment and each moment after the 3 rd moment.
Specifically, steps 504-506 are repeatedly executed, that is, the pan/tilt attitude at the time point 3 is calculated according to the calculation process of calculating the pan/tilt attitude at the time point 2: first, the pan/tilt platform error angle at time 3 is calculated by EKF. And secondly, calculating the attitude increment of the tripod head at the 3 rd moment relative to the 2 nd moment based on the error angle of the tripod head platform at the 3 rd moment. Then, the pan/tilt attitude at time 3 is calculated based on the pan/tilt attitude at time 2 and the pan/tilt attitude increment at time 3 with respect to time 2.
Similarly, when the pan/tilt attitude at each time after the 3 rd time is calculated, the calculation is also performed according to the calculation process of calculating the pan/tilt attitude at the 3 rd time.
And 511, calculating the base postures at the 3 rd time and each time after the 3 rd time.
Specifically, steps 507 to 509 are repeatedly executed, namely the base posture at the 3 rd time is calculated according to the calculation process of calculating the base posture at the 2 nd time: first, the base platform error angle at time 3 was calculated by EKF. Next, the base attitude increment at time 3 relative to time 2 is calculated based on the base platform error angle at time 3. Then, the base attitude at time 3 is calculated based on the base attitude at time 2 and the base attitude increment at time 3 with respect to time 2.
Similarly, when the posture of the base at each time after the 3 rd time is calculated, the calculation is also performed in accordance with the calculation process of calculating the posture of the base at the 3 rd time.
As an alternative first embodiment, the calculation of the base platform error angle by the EKF and the calculation of the pan platform error angle by the EKF are independent from each other, specifically, refer to steps 504 to 509. In implementation, two filters may be provided to implement respective EKFs. As an optional second embodiment, the base platform error angle and the pan/tilt platform error angle may be calculated simultaneously by EKF. In implementation, a filter may be provided to implement the EKF. The second embodiment will be described in detail below by taking an example of simultaneously calculating the pan/tilt platform error angle at the time 2 and the base platform error angle at the time 2 through the EKF, and referring to fig. 10, the second embodiment includes steps 601 to 603.
And 601, respectively calculating the observed quantities at the 2 nd moment, wherein the observed quantities comprise holder attitude observed quantity, base attitude observed quantity and motor angular displacement observed quantity.
The calculation method of the pan-tilt attitude observed quantity at the time 2 may refer to step 4031 in the embodiment shown in fig. 4, and is not described again here. When the pan/tilt roll angle at time 2 is greater than the first threshold, the acquired base posture at time 1 is the base posture at time 1 calculated in step 503, and the pan/tilt posture observed quantity at time 2 is calculated based on the base posture at time 1 and the angular displacement of each axis motor at time 2.
The manner of calculating the base attitude observed quantity at time 2 can refer to step 5071 in the embodiment shown in fig. 7, and is not described herein again.
The observed quantity of the motor angular displacement at the 2 nd moment is calculated in a mode that the angle measured by the motor code disc at the 2 nd moment can be directly read as the observed quantity of the motor angular displacement at the 2 nd moment.
Step 602, calculating the estimated state quantity at the 2 nd moment through a pre-established state prediction equation, the state quantity at the 1 st moment, the angular velocity of the image acquisition equipment at the 2 nd moment and the angular velocity of three axes of the base coordinate system at the 2 nd moment.
Wherein, the state quantity comprises a tripod head platform error angle, a tripod head gyroscope bias, a base platform error angle and a base gyroscope bias. The pre-established state prediction equation is shown in equation (22).
Figure BDA0001783537130000281
Step 602 further includes: and calculating the estimated state quantity covariance at the 2 nd moment through a pre-established state quantity covariance prediction equation and the state quantity covariance at the 1 st moment.
The state quantity covariance prediction equation is similar to equation (5) and will not be described in detail here. It should be noted that, due to the increase of the state quantity, the corresponding parameter related to the state quantity in the state quantity covariance prediction equation should be updated synchronously.
And 603, correcting the estimated state quantity at the 2 nd time based on the observed quantity at the 2 nd time to obtain the state quantity at the 2 nd time.
This step 603 includes the following steps 603a to 603 c.
Step 603a, calculating the estimated observed quantity at the 2 nd moment based on the pre-established measurement equation and the estimated state quantity at the 2 nd moment.
The pre-established measurement equation is shown in equation (23).
Figure BDA0001783537130000291
Wherein the content of the first and second substances,
Figure BDA0001783537130000292
and the direction cosine matrix from the holder coordinate system to the base coordinate system at the (i + 1) th moment.
Figure BDA0001783537130000293
Is the direction cosine matrix
Figure BDA0001783537130000294
Row 1 and column 2 elements in the middle,
Figure BDA0001783537130000295
is the direction cosine matrix
Figure BDA0001783537130000296
Row 2 and column 2 elements of the middle,
Figure BDA0001783537130000297
is the direction cosine matrix
Figure BDA0001783537130000298
Row 3 and column 1 elements in,
Figure BDA0001783537130000299
is the direction cosine matrix
Figure BDA00017835371300002910
Row 3 and column 2 elements in the middle,
Figure BDA00017835371300002911
is the direction cosine matrix
Figure BDA00017835371300002912
Row 3 and column 3 elements.
Figure BDA00017835371300002913
The calculation process of (2) includes: firstly, calculating the quaternion of the estimated tripod head attitude at the i +1 th moment
Figure BDA00017835371300002914
And calculating the quaternion of the estimated base attitude at the (i + 1) th moment
Figure BDA00017835371300002915
Secondly, using the formula
Figure BDA00017835371300002916
Calculating to obtain the quaternion of the estimated motor angular displacement
Figure BDA00017835371300002917
Finger-shaped
Figure BDA00017835371300002918
Conjugation of (1). Then, the angular displacement quaternion of the motor is estimated
Figure BDA00017835371300002919
Conversion to a directional cosine matrix
Figure BDA00017835371300002920
And (4) showing.
In calculating the estimated observed quantities, obtaining them separately
Figure BDA00017835371300002921
And
Figure BDA00017835371300002922
then, will
Figure BDA00017835371300002923
And
Figure BDA00017835371300002924
substituting the corresponding specific elements into a formula (23) to obtain the estimated observed quantity.
And step 603b, calculating the Kalman filtering gain at the 2 nd moment based on the estimated covariance of the state quantities at the 2 nd moment.
The kalman filter gain is calculated in a manner similar to that in step 4033b in the embodiment shown in fig. 4, and is not described herein again.
Step 603c, estimating the state quantity at the 2 nd moment based on the estimated state quantity at the 2 nd moment, the Kalman filtering gain, each attitude observed quantity and each estimated observed quantity.
Wherein, the state quantity comprises a tripod head platform error angle, a tripod head gyroscope bias, a base platform error angle and a base gyroscope bias.
This step 603c further includes: and estimating the covariance of the state quantity at the 2 nd moment based on the covariance of the estimated state quantity at the 2 nd moment and the Kalman filtering gain.
This step 603c is similar to step 4033c in the embodiment shown in fig. 4, and is not described again here.
Compared with the EKF provided by the first embodiment, the EKF provided by the second embodiment has the advantages that the observed quantity is increased by the motor displacement observed quantity, so that the range of the observed quantity is enlarged, and when the estimated state quantity is corrected through the observed quantity, because the range of the observed quantity is enlarged, more observed quantities are referred to during correction, compared with less observed quantities, on the premise of ensuring the accuracy of the observed quantity, the correction accuracy can be improved, and the accuracy of the tripod head platform error angle obtained by the EKF is improved. Therefore, when the cradle head attitude is calculated by adopting the cradle head platform error angle obtained by the EKF, the estimation precision of the cradle head attitude is correspondingly improved.
In the EKF process provided by the embodiment shown in fig. 4, when the pan-tilt roll angle exceeds the first threshold, the pan-tilt attitude observed quantity is obtained based on the carrier attitude. However, when the carrier is affected by vibration and shaking generated by an external environment, instability of the structure of the carrier and the like, the attitude of the carrier and the attitude of the base have a particularly large deviation, and at this time, the attitude of the pan/tilt unit is estimated by calculating the attitude observation quantity of the pan/tilt unit through the attitude of the carrier, and the angular displacement of the motor is adjusted by adopting the estimated attitude of the pan/tilt unit, so that the image acquired by the pan/tilt unit is skewed. To this end, the embodiment shown in fig. 7 provides an EKF process in which the pan-tilt attitude observation is derived based on the base attitude (see step 504) obtained by the EKF when the pan-tilt roll angle exceeds a first threshold. When the carrier is affected by vibration, shaking, instability of the structure of the carrier and the like caused by the external environment, the posture of the base does not change along with the violent change of the posture of the carrier under the buffer of a connecting piece (generally an elastic component) of the base and the carrier, and the cradle head is fixed on the carrier through the base, so that the posture of the cradle head can be measured more accurately by the posture of the base.
Fig. 11 shows a pan-tilt attitude estimation apparatus provided in an embodiment of the present invention, and referring to fig. 11, the apparatus includes a first obtaining module 701, a calculating module 702, a second obtaining module 703, and an updating module 704.
The first obtaining module 701 is configured to obtain angular velocities and linear accelerations of the image acquisition device in the pan-tilt coordinate system at the (i + 1) th moment on three axes, and an angular displacement of an output shaft of a heading axis motor in the pan-tilt, respectively.
And the calculating module 702 is configured to calculate an error angle of the platform of the pan-tilt at the (i + 1) th moment based on the angular velocity and the linear acceleration of the image acquisition device in the pan-tilt at three axes of the pan-tilt coordinate system at the (i + 1) th moment and the angular displacement of the output shaft of the heading axis motor in the pan-tilt.
The second obtaining module 703 is configured to obtain the cradle head posture at the ith time.
And the updating module 704 is configured to update the cradle head posture at the ith moment based on the cradle head platform error angle at the (i + 1) th moment, so as to obtain the cradle head posture at the (i + 1) th moment.
In a first optional embodiment, the calculating module 702 is configured to calculate a pan-tilt attitude observed quantity at a time i +1 based on linear accelerations of the image capturing device at three axes of a pan-tilt coordinate system at the time i +1 and an angular displacement of an output shaft of a heading axis motor; the holder attitude observed quantity is the holder attitude obtained by calculating according to the parameters measured by the first sensor device when the error angle of the holder platform is estimated through the extended Kalman filter EKF; acquiring a platform error angle of the holder at the ith moment; calculating an estimated tripod head platform error angle at the (i + 1) th moment based on the tripod head platform error angle at the (i) th moment and the angular velocity of the image acquisition equipment at the (i + 1) th moment on three axes of a tripod head coordinate system; and correcting the estimated tripod head platform error angle at the (i + 1) th moment based on the tripod head posture observed quantity at the (i + 1) th moment to obtain the tripod head platform error angle at the (i + 1) th moment.
With reference to the first optional embodiment, in a second optional embodiment, the calculating module 702 is configured to calculate, based on the linear accelerations of the image capturing device at the i +1 th time on three axes of the pan-tilt coordinate system, a pan-tilt roll angle at the i +1 th time; when the horizontal roll angle of the holder is larger than a first threshold value, acquiring the base posture at the ith moment and the angular displacement of the output shaft of each shaft motor in the holder at the (i + 1) th moment, and calculating the holder posture observed quantity based on the base posture at the ith moment and the angular displacement of the output shaft of each shaft motor in the holder at the (i + 1) th moment; the holder is fixed on the carrier through a base, and the shaft motor comprises a course shaft motor; and when the horizontal rolling angle of the holder is smaller than a first threshold value, acquiring the carrier course angle at the (i + 1) th moment, and calculating the holder attitude observed quantity based on the carrier course angle at the (i + 1) th moment, the angular displacement of the output shaft of the course shaft motor at the (i + 1) th moment and the linear acceleration of the image acquisition equipment at the (i + 1) th moment on three axes of the holder coordinate system.
With reference to the second optional embodiment, in a third optional embodiment, the calculation module 702 is configured to obtain a posture of the carrier at a time i, and use the posture of the carrier at the time i as a posture of the base at the time i.
With reference to the second optional embodiment, in a fourth optional embodiment, the calculating module 702 is configured to, when the ith is 1, calculate the attitude of the base at the ith time based on the linear accelerations of the base at the three axes of the base coordinate system at the ith time and the carrier heading angle; when the ith moment is not the 1 st moment, respectively acquiring the angular velocity and the linear acceleration of the base at the ith moment on the three axes of the base coordinate system; calculating a base platform error angle at the ith moment based on the angular velocity and linear acceleration of the base at the three axes of the base coordinate system at the ith moment; acquiring the posture of the base at the ith-1 moment; and updating the base posture at the ith moment based on the base platform error angle at the ith moment to obtain the base posture at the ith moment.
With reference to the fourth optional embodiment, in a fifth optional embodiment, the calculating module 702 is configured to calculate the base posture at the ith time according to equation (13) when i is equal to 1.
With reference to the fourth optional embodiment, in a sixth optional embodiment, the calculating module 702 is configured to, when the ith time is not the 1 st time, calculate the base posture observed quantity at the ith time based on the linear accelerations of the base at the three axes of the base coordinate system at the ith time; the base attitude observed quantity is the base attitude obtained by calculating according to the parameters measured by the second sensing device when the base platform error angle is estimated through the EKF; acquiring a base platform error angle at the i-1 th moment; calculating an estimated base platform error angle at the ith moment based on the base platform error angle at the ith-1 moment and the angular speed of the base at the ith moment on the three axes of the base coordinate system; and correcting the estimated base platform error angle at the ith moment based on the base attitude observed quantity at the ith moment to obtain the base platform error angle at the ith moment.
With reference to the sixth optional embodiment, in a seventh optional embodiment, the calculating module 702 is configured to calculate a base roll angle at an ith time based on linear accelerations of the base at three axes of the base coordinate system at the ith time; when the base roll angle is larger than a second threshold value, acquiring the carrier attitude at the ith moment, and taking the carrier attitude at the ith moment as the base attitude observed quantity at the ith moment; and when the base roll angle is smaller than a second threshold value, acquiring the carrier course angle at the ith moment, and calculating the base attitude observed quantity at the ith moment based on the carrier course angle at the ith moment and the linear acceleration of the base at the base coordinate system triaxial at the ith moment.
With reference to the second optional embodiment, in an eighth optional embodiment, the calculation module 702 is configured to, when the pan tilt roll angle is greater than the first threshold, and when the axis motor further includes a roll axis motor and a pitch axis motor, sequentially rotate the angular displacement of the output shaft of the corresponding axis motor at the i +1 th time according to the rotation sequence of the X axis-Y axis-Z axis, the Y axis-Z axis-X axis, or the Z axis-X axis-Y axis, so as to obtain the pan tilt attitude observed quantity at the i +1 th time, where the Z axis corresponds to the course axis motor, the X axis corresponds to the roll axis motor, and the Y axis corresponds to the pitch axis motor.
In a ninth optional embodiment, the calculating module 702 is configured to calculate the pan-tilt posture at the ith time according to formula (3) when i is equal to 1.
In a tenth optional embodiment, the updating module 704 is configured to calculate a pan/tilt attitude increment at a time i +1 with respect to a time i based on the pan/tilt platform error angle at the time i + 1; and updating the cradle head attitude at the ith moment by adopting the cradle head attitude increment of the (i + 1) th moment relative to the ith moment.
In the embodiment of the invention, the error angle of the platform of the holder is calculated; the tripod head platform error angle is a relative angular displacement relation between a calculated tripod head platform coordinate system and a real tripod head platform coordinate system, the real tripod head platform coordinate system is a navigation coordinate system, the calculated tripod head platform coordinate system is a navigation coordinate system estimated by a computer, in particular to a navigation coordinate system containing attitude errors estimated in the process of calculating the tripod head attitude, the tripod head attitude refers to the relative angular displacement relation between the tripod head coordinate system and the navigation coordinate system, and the tripod head platform error angle and the tripod head attitude increment between two moments have a conversion relation, so that the tripod head attitude increment between the two moments can be estimated based on the tripod head platform error angle, thus, the tripod head attitude increment from the moment i to the moment i +1 can be estimated based on the tripod head platform error angle at the moment i +1, and the tripod head attitude at the moment i can be updated based on the tripod head attitude increment, the cradle head attitude at the (i + 1) th moment is obtained, and the cradle head attitude is not directly solved for the integral of the gyroscope when the cradle head attitude is calculated, so that the problem of integral error of the gyroscope is avoided, and the estimation precision of the cradle head attitude can be improved.
It should be noted that: in the above-described embodiment, when estimating the attitude of the pan/tilt head, the device for estimating the attitude of the pan/tilt head according to the present invention is illustrated by only dividing the functional modules, and in practical applications, the function distribution may be completed by different functional modules according to needs, that is, the internal structure of the device may be divided into different functional modules, so as to complete all or part of the above-described functions. In addition, the holder attitude estimation device and the holder attitude estimation method provided by the above embodiments belong to the same concept, and specific implementation processes thereof are detailed in the method embodiments and are not described herein again.
Fig. 12 shows a pan-tilt attitude estimation apparatus according to an embodiment of the present invention. The cradle head attitude estimation device may have a relatively large difference due to a difference in configuration or performance, and specifically, the cradle head attitude estimation device may be the computer 1800. The computer 1800 may include a Central Processing Unit (CPU)1801, a system memory 1804 including a Random Access Memory (RAM)1802 and a Read Only Memory (ROM)1803, and a system bus 1805 that couples the system memory 1804 and the central processing unit 1801. The computer 1800 also includes mass storage devices 1807 for storing an operating system 1813, application programs 1814, and other program modules 1815.
The mass storage device 1807 is connected to the central processing unit 1801 through a mass storage controller (not shown) connected to the system bus 1805. The mass storage device 1807 and its associated computer-readable media provide non-volatile storage for the computer 1800. That is, the mass storage device 1807 may include a computer-readable medium (not shown) such as a hard disk or CD-ROM drive.
Without loss of generality, computer readable media may comprise computer storage media and communication media. Computer storage media includes volatile and nonvolatile, removable and non-removable media implemented in any method or technology for storage of information such as computer readable instructions, data structures, program modules or other data. Computer storage media includes RAM, ROM, EPROM, EEPROM, flash memory or other solid state memory technology, CD-ROM, DVD, or other optical storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices. Of course, those skilled in the art will appreciate that computer storage media is not limited to the foregoing. The system memory 1804 and mass storage device 1807 described above may be collectively referred to as memory.
According to various embodiments of the invention, the computer 1800 may also operate as a remote computer connected to a network, such as the Internet. That is, the computer 1800 may be connected to the network 1812 through the network interface unit 1811 connected to the system bus 1805, or the network interface unit 1811 may be used to connect to other types of networks or remote computer systems (not shown).
The memory also includes one or more programs, which are stored in the memory and configured to be executed by the CPU 1801. The methods illustrated in fig. 3 to 10 may be implemented when the CPU 1801 executes a program in the memory.
In an exemplary embodiment, a computer-readable storage medium comprising instructions, such as a memory comprising instructions, which are loadable and executable by the central processing unit 1801 of the computer 1800 to carry out the method illustrated in fig. 3-10 is also provided. For example, the computer readable storage medium may be a ROM, a Random Access Memory (RAM), a CD-ROM, a magnetic tape, a floppy disk, an optical data storage device, and the like.
The cradle head may further include other components for implementing functions of the apparatus, such as a base, a shaft motor mechanism, and an image capturing device, which are specifically shown in fig. 1 and are not described herein again.
It will be understood by those skilled in the art that all or part of the steps for implementing the above embodiments may be implemented by hardware, or may be implemented by a program instructing relevant hardware, where the program may be stored in a computer-readable storage medium, and the above-mentioned storage medium may be a read-only memory, a magnetic disk or an optical disk, etc.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.

Claims (24)

1. A pan-tilt attitude estimation method, the method comprising:
respectively acquiring the angular speed and linear acceleration of image acquisition equipment in a tripod head at the (i + 1) th moment on three axes of a tripod head coordinate system and the angular displacement of an output shaft of a course shaft motor in the tripod head;
calculating a platform error angle of the holder at the (i + 1) th moment based on the angular velocity and linear acceleration of the image acquisition equipment in the holder at the three axes of the holder coordinate system at the (i + 1) th moment and the angular displacement of an output shaft of a course shaft motor in the holder;
acquiring the posture of the holder at the ith moment;
and updating the cradle head posture at the ith moment based on the cradle head platform error angle at the ith +1 moment to obtain the cradle head posture at the ith +1 moment.
2. The method according to claim 1, wherein the calculating the error angle of the platform of the pan-tilt at the i +1 th moment based on the angular velocities and linear accelerations of the image capturing device in the pan-tilt at three axes of the pan-tilt coordinate system at the i +1 th moment and the angular displacement of the output shaft of the heading axis motor in the pan-tilt comprises:
calculating the holder attitude observed quantity at the (i + 1) th moment based on the linear acceleration of the image acquisition equipment at three axes of the holder coordinate system at the (i + 1) th moment and the angular displacement of the output shaft of the course shaft motor;
acquiring a platform error angle of the holder at the ith moment;
calculating an estimated tripod head platform error angle at the i +1 th moment based on the tripod head platform error angle at the i th moment and the angular velocity of the image acquisition equipment at the i +1 th moment on three axes of a tripod head coordinate system;
and correcting the estimated tripod head platform error angle at the (i + 1) th moment based on the tripod head posture observed quantity at the (i + 1) th moment to obtain the tripod head platform error angle at the (i + 1) th moment.
3. The method of claim 2, wherein the calculating of the pan-tilt attitude observation at the i +1 th time based on the linear accelerations of the image capture device at three axes of a pan-tilt coordinate system at the i +1 th time and the angular displacement of the output shaft of the course axis motor comprises:
calculating the horizontal roll angle of the holder at the (i + 1) th moment based on the linear acceleration of the image acquisition equipment at the (i + 1) th moment on the three axes of the holder coordinate system;
when the horizontal roll angle of the holder is larger than a first threshold value, acquiring the base posture at the ith moment and the angular displacement of the output shaft of each shaft motor in the holder at the (i + 1) th moment, and calculating the holder posture observed quantity based on the base posture at the ith moment and the angular displacement of the output shaft of each shaft motor in the holder at the (i + 1) th moment; the holder is fixed on the carrier through the base, and the shaft motor comprises the course shaft motor;
and when the horizontal roll angle of the holder is smaller than the first threshold value, acquiring the carrier course angle at the (i + 1) th moment, and calculating the holder attitude observed quantity based on the carrier course angle at the (i + 1) th moment, the angular displacement of the output shaft of the course shaft motor at the (i + 1) th moment and the linear acceleration of the image acquisition equipment at the (i + 1) th moment on three axes of the holder coordinate system.
4. The method of claim 3, wherein the obtaining the base pose at time i comprises:
and acquiring the posture of the carrier at the ith moment, and taking the posture of the carrier at the ith moment as the posture of the base at the ith moment.
5. The method of claim 3, wherein the obtaining the base pose at time i comprises:
when i is 1, calculating the base posture at the ith moment based on the linear acceleration of the base on the three axes of the base coordinate system at the ith moment and the carrier course angle;
when the ith moment is not the 1 st moment, respectively acquiring the angular velocity and the linear acceleration of the base at the ith moment on three axes of a base coordinate system; calculating a base platform error angle at the ith moment based on the angular velocity and the linear acceleration of the base at the three axes of the base coordinate system at the ith moment; acquiring the posture of the base at the ith-1 moment; and updating the base posture at the ith moment based on the base platform error angle at the ith moment to obtain the base posture at the ith moment.
6. The method of claim 5, wherein calculating the base attitude at time i based on the linear accelerations of the base at three axes of the base coordinate system at time i and the carrier heading angle comprises:
the attitude of the base at the ith time is calculated according to the following formula,
Figure FDA0001783537120000021
Figure FDA0001783537120000022
Figure FDA0001783537120000023
wherein the content of the first and second substances,
Figure FDA0001783537120000031
is the heading angle of the base at time i,
Figure FDA0001783537120000032
is the roll angle of the susceptor at time i,
Figure FDA0001783537120000034
is the pitch angle of the base at time i, psibody,iThe carrier course angle at the ith moment;
Figure FDA0001783537120000033
the linear acceleration of the base on three axes of the base coordinate system at the ith moment.
7. The method of claim 5, wherein calculating the base platform error angle at time i based on the angular velocities and linear accelerations of the base at three axes of the base coordinate system at time i comprises:
calculating the base posture observed quantity at the ith moment based on the linear acceleration of the base at the three axes of the base coordinate system at the ith moment;
acquiring a base platform error angle at the i-1 th moment;
calculating an estimated base platform error angle at the ith moment based on the base platform error angle at the ith-1 moment and the angular velocities of the base at the base coordinate system three axes at the ith moment;
and correcting the estimated base platform error angle at the ith moment based on the base attitude observed quantity at the ith moment to obtain the base platform error angle at the ith moment.
8. The method of claim 7, wherein calculating the base pose observation at time i based on the linear accelerations of the base at the three axes of the base coordinate system at time i comprises:
calculating a base roll angle at the ith moment based on the linear acceleration of the base at the three axes of the base coordinate system at the ith moment;
when the base roll angle is larger than a second threshold value, acquiring the carrier attitude at the ith moment, and taking the carrier attitude at the ith moment as the base attitude observed quantity at the ith moment;
and when the base roll angle is smaller than the second threshold value, acquiring a carrier course angle at the ith moment, and calculating the base attitude observed quantity at the ith moment based on the carrier course angle at the ith moment and the linear acceleration of the base at the base coordinate system triaxial at the ith moment.
9. The method of claim 3, wherein calculating the pan head attitude observations based on the base attitude at the i-th time and the angular displacement of the output shaft of each axis motor in the pan head at the i + 1-th time comprises:
when the axis motor further comprises a transverse axis motor and a pitching axis motor, the base posture at the ith moment is sequentially rotated according to the rotation sequence of an X axis-Y axis-Z axis, a Y axis-Z axis-X axis or a Z axis-X axis-Y axis to rotate the angular displacement of the output shaft of the corresponding axis motor at the (i + 1) th moment, so that the holder posture observed quantity at the (i + 1) th moment is obtained, the Z axis corresponds to the course axis motor, the X axis corresponds to the transverse axis motor, and the Y axis corresponds to the pitching axis motor.
10. The method according to any one of claims 1 to 9, wherein when i is 1, the acquiring the pan-tilt attitude at the ith time comprises:
calculating the posture of the pan-tilt at the ith moment according to the following formula,
Figure FDA0001783537120000041
Figure FDA0001783537120000042
Figure FDA0001783537120000043
wherein the content of the first and second substances,
Figure FDA0001783537120000044
is the heading angle of the pan/tilt head at the ith moment,
Figure FDA0001783537120000046
is the roll angle of the head at time ii cIs the pitch angle, psi, of the pan-tilt at moment ibody,iThe carrier course angle at the ith moment; psim,iIs the angular displacement of the output shaft of the course shaft motor at the ith moment,
Figure FDA0001783537120000045
and the linear acceleration of the image acquisition equipment on three axes of the holder coordinate system at the ith moment.
11. The method according to any one of claims 1-9, wherein updating the pan-tilt attitude at the i-th time based on the pan-tilt platform error angle at the i + 1-th time comprises:
calculating the tripod head attitude increment of the ith +1 moment relative to the ith moment based on the tripod head platform error angle of the ith +1 moment;
and updating the cradle head attitude at the ith moment by adopting the cradle head attitude increment of the (i + 1) th moment relative to the ith moment.
12. A pan-tilt attitude estimation apparatus, characterized in that the apparatus comprises:
the first acquisition module is used for respectively acquiring the angular velocity and the linear acceleration of the image acquisition equipment in the tripod head at the (i + 1) th moment on three axes of a tripod head coordinate system and the angular displacement of an output shaft of a course shaft motor in the tripod head;
the computing module is used for computing a platform error angle of the holder at the (i + 1) th moment based on the angular velocity and the linear acceleration of the image acquisition equipment in the holder at the three axes of the holder coordinate system at the (i + 1) th moment and the angular displacement of the output shaft of the course shaft motor in the holder;
the second acquisition module is used for acquiring the posture of the holder at the ith moment;
and the updating module is used for updating the cradle head attitude at the ith moment based on the cradle head platform error angle at the ith +1 moment to obtain the cradle head attitude at the ith +1 moment.
13. The apparatus of claim 12, wherein the computing module is configured to,
calculating the holder attitude observed quantity at the (i + 1) th moment based on the linear acceleration of the image acquisition equipment at three axes of the holder coordinate system at the (i + 1) th moment and the angular displacement of the output shaft of the course shaft motor;
acquiring a platform error angle of the holder at the ith moment;
calculating an estimated tripod head platform error angle at the i +1 th moment based on the tripod head platform error angle at the i th moment and the angular velocity of the image acquisition equipment at the i +1 th moment on three axes of a tripod head coordinate system;
and correcting the estimated tripod head platform error angle at the (i + 1) th moment based on the tripod head posture observed quantity at the (i + 1) th moment to obtain the tripod head platform error angle at the (i + 1) th moment.
14. The apparatus of claim 13, wherein the computing module is configured to,
calculating the horizontal roll angle of the holder at the (i + 1) th moment based on the linear acceleration of the image acquisition equipment at the (i + 1) th moment on the three axes of the holder coordinate system;
when the horizontal roll angle of the holder is larger than a first threshold value, acquiring the base posture at the ith moment and the angular displacement of the output shaft of each shaft motor in the holder at the (i + 1) th moment, and calculating the holder posture observed quantity based on the base posture at the ith moment and the angular displacement of the output shaft of each shaft motor in the holder at the (i + 1) th moment; the holder is fixed on the carrier through the base, and the shaft motor comprises the course shaft motor;
and when the horizontal roll angle of the holder is smaller than the first threshold value, acquiring the carrier course angle at the (i + 1) th moment, and calculating the holder attitude observed quantity based on the carrier course angle at the (i + 1) th moment, the angular displacement of the output shaft of the course shaft motor at the (i + 1) th moment and the linear acceleration of the image acquisition equipment at the (i + 1) th moment on three axes of the holder coordinate system.
15. The apparatus of claim 14, wherein the computing module is configured to,
and acquiring the posture of the carrier at the ith moment, and taking the posture of the carrier at the ith moment as the posture of the base at the ith moment.
16. The apparatus of claim 14, wherein the computing module is configured to,
when i is 1, calculating the base posture at the ith moment based on the linear acceleration of the base on the three axes of the base coordinate system at the ith moment and the carrier course angle;
when the ith moment is not the 1 st moment, respectively acquiring the angular velocity and the linear acceleration of the base at the ith moment on three axes of a base coordinate system; calculating a base platform error angle at the ith moment based on the angular velocity and the linear acceleration of the base at the three axes of the base coordinate system at the ith moment; acquiring the posture of the base at the ith-1 moment; and updating the base posture at the ith moment based on the base platform error angle at the ith moment to obtain the base posture at the ith moment.
17. The apparatus of claim 16, wherein the computing module is configured to,
when i is 1, calculating the base posture at the ith moment according to the following formula,
Figure FDA0001783537120000061
Figure FDA0001783537120000062
Figure FDA0001783537120000063
wherein the content of the first and second substances,
Figure FDA0001783537120000065
is the heading angle of the base at time i,
Figure FDA0001783537120000066
is the roll angle of the base at time i, thetai bIs the pitch angle of the base at time i, psibody,iThe carrier course angle at the ith moment;
Figure FDA0001783537120000064
the linear acceleration of the base on three axes of the base coordinate system at the ith moment.
18. The apparatus of claim 16, wherein the computing module is configured to,
when the ith moment is not the 1 st moment, calculating the base posture observed quantity at the ith moment based on the linear acceleration of the base at the three axes of the base coordinate system at the ith moment;
acquiring a base platform error angle at the i-1 th moment;
calculating an estimated base platform error angle at the ith moment based on the base platform error angle at the ith-1 moment and the angular velocities of the base at the base coordinate system three axes at the ith moment;
and correcting the estimated base platform error angle at the ith moment based on the base attitude observed quantity at the ith moment to obtain the base platform error angle at the ith moment.
19. The apparatus of claim 18, wherein the computing module is configured to,
calculating a base roll angle at the ith moment based on the linear acceleration of the base at the three axes of the base coordinate system at the ith moment;
when the base roll angle is larger than a second threshold value, acquiring the carrier attitude at the ith moment, and taking the carrier attitude at the ith moment as the base attitude observed quantity at the ith moment;
and when the base roll angle is smaller than the second threshold value, acquiring a carrier course angle at the ith moment, and calculating the base attitude observed quantity at the ith moment based on the carrier course angle at the ith moment and the linear acceleration of the base at the base coordinate system triaxial at the ith moment.
20. The apparatus of claim 14, wherein the computing module is configured to,
when the horizontal roll angle of the holder is larger than a first threshold value, and when the shaft motor further comprises a horizontal roll shaft motor and a pitch shaft motor, the base posture at the ith moment is sequentially rotated according to the rotation sequence of an X shaft-Y shaft-Z shaft, a Y shaft-Z shaft-X shaft or a Z shaft-X shaft-Y shaft to rotate the angular displacement of the output shaft of the corresponding shaft motor at the (i + 1) th moment, so that the holder posture observed quantity at the (i + 1) th moment is obtained, the Z shaft corresponds to the course shaft motor, the X shaft corresponds to the horizontal roll shaft motor, and the Y shaft corresponds to the pitch shaft motor.
21. The apparatus according to any of claims 12-20, wherein the computing module is configured to,
when i is equal to 1, calculating the tripod head attitude at the ith moment according to the following formula,
Figure FDA0001783537120000071
Figure FDA0001783537120000072
Figure FDA0001783537120000073
wherein the content of the first and second substances,
Figure FDA0001783537120000074
is the heading angle of the pan/tilt head at the ith moment,
Figure FDA0001783537120000076
is the roll angle of the head at time ii cIs the pitch angle, psi, of the pan-tilt at moment ibody,iThe carrier course angle at the ith moment; psim,iIs the angular displacement of the output shaft of the course shaft motor at the ith moment,
Figure FDA0001783537120000075
and the linear acceleration of the image acquisition equipment on three axes of the holder coordinate system at the ith moment.
22. The apparatus according to any of claims 12-20, wherein the update module is configured to,
calculating the tripod head attitude increment of the ith +1 moment relative to the ith moment based on the tripod head platform error angle of the ith +1 moment;
and updating the cradle head attitude at the ith moment by adopting the cradle head attitude increment of the (i + 1) th moment relative to the ith moment.
23. A pan-tilt attitude estimation apparatus, characterized in that it comprises a processor and a memory, in which is stored at least one instruction, which is loaded and executed by the processor to carry out the operations performed by the pan-tilt attitude estimation method according to any one of claims 1 to 11.
24. A computer-readable storage medium having stored therein at least one instruction, which is loaded and executed by a processor to perform operations performed by a pan-tilt attitude estimation method according to any one of claims 1 to 11.
CN201811003675.6A 2018-08-30 2018-08-30 Cloud deck attitude estimation method and device Active CN110873563B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811003675.6A CN110873563B (en) 2018-08-30 2018-08-30 Cloud deck attitude estimation method and device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811003675.6A CN110873563B (en) 2018-08-30 2018-08-30 Cloud deck attitude estimation method and device

Publications (2)

Publication Number Publication Date
CN110873563A true CN110873563A (en) 2020-03-10
CN110873563B CN110873563B (en) 2022-03-08

Family

ID=69715213

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811003675.6A Active CN110873563B (en) 2018-08-30 2018-08-30 Cloud deck attitude estimation method and device

Country Status (1)

Country Link
CN (1) CN110873563B (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111625768A (en) * 2020-05-19 2020-09-04 西安因诺航空科技有限公司 Handheld holder attitude estimation method based on extended Kalman filtering
CN111654634A (en) * 2020-06-24 2020-09-11 杭州海康威视数字技术股份有限公司 Method for determining inclination of engine core assembly and pan-tilt assembly in camera and camera
CN113272755A (en) * 2020-07-01 2021-08-17 深圳市大疆创新科技有限公司 Control method and device of holder, holder and computer readable storage medium
CN114370872A (en) * 2022-01-14 2022-04-19 苏州挚途科技有限公司 Vehicle attitude determination method and vehicle

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101216321A (en) * 2008-01-04 2008-07-09 南京航空航天大学 Rapid fine alignment method for SINS
CN101893445A (en) * 2010-07-09 2010-11-24 哈尔滨工程大学 Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition
CN102706366A (en) * 2012-06-19 2012-10-03 北京航空航天大学 SINS (strapdown inertial navigation system) initial alignment method based on earth rotation angular rate constraint
CN104296747A (en) * 2014-10-27 2015-01-21 北京航天控制仪器研究所 One-dimensional positioning method for inertia measurement system based on rocket sledge orbital coordinate system
CN104634348A (en) * 2015-03-12 2015-05-20 北京华力创通科技股份有限公司 Attitude angle calculation method in integrated navigation
CN105513072A (en) * 2015-12-05 2016-04-20 中国航空工业集团公司洛阳电光设备研究所 PTZ correction method
CN106200692A (en) * 2016-08-11 2016-12-07 零度智控（北京）智能科技有限公司 Ground cloud platform control method, device and ground The Cloud Terrace
CN106959110A (en) * 2017-04-06 2017-07-18 亿航智能设备（广州）有限公司 A kind of head attitude detecting method and device
DE102017102269A1 (en) * 2016-02-12 2017-08-17 Gm Global Technology Operations, Llc TILT AND MISSING EQUALIZATION FOR 6-DOF IMU USING GNSS / INS DATA
CN107710283A (en) * 2016-12-02 2018-02-16 深圳市大疆创新科技有限公司 A kind of filming control method, device and control device
WO2018040906A1 (en) * 2016-08-31 2018-03-08 纳恩博（北京）科技有限公司 Pan-tilt control method and device, and computer storage medium
CN108088443A (en) * 2016-11-23 2018-05-29 北京自动化控制设备研究所 A kind of positioning and directing device rate compensation method

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101216321A (en) * 2008-01-04 2008-07-09 南京航空航天大学 Rapid fine alignment method for SINS
CN101893445A (en) * 2010-07-09 2010-11-24 哈尔滨工程大学 Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition
CN102706366A (en) * 2012-06-19 2012-10-03 北京航空航天大学 SINS (strapdown inertial navigation system) initial alignment method based on earth rotation angular rate constraint
CN104296747A (en) * 2014-10-27 2015-01-21 北京航天控制仪器研究所 One-dimensional positioning method for inertia measurement system based on rocket sledge orbital coordinate system
CN104634348A (en) * 2015-03-12 2015-05-20 北京华力创通科技股份有限公司 Attitude angle calculation method in integrated navigation
CN105513072A (en) * 2015-12-05 2016-04-20 中国航空工业集团公司洛阳电光设备研究所 PTZ correction method
DE102017102269A1 (en) * 2016-02-12 2017-08-17 Gm Global Technology Operations, Llc TILT AND MISSING EQUALIZATION FOR 6-DOF IMU USING GNSS / INS DATA
CN106200692A (en) * 2016-08-11 2016-12-07 零度智控（北京）智能科技有限公司 Ground cloud platform control method, device and ground The Cloud Terrace
WO2018040906A1 (en) * 2016-08-31 2018-03-08 纳恩博（北京）科技有限公司 Pan-tilt control method and device, and computer storage medium
CN108088443A (en) * 2016-11-23 2018-05-29 北京自动化控制设备研究所 A kind of positioning and directing device rate compensation method
CN107710283A (en) * 2016-12-02 2018-02-16 深圳市大疆创新科技有限公司 A kind of filming control method, device and control device
CN106959110A (en) * 2017-04-06 2017-07-18 亿航智能设备（广州）有限公司 A kind of head attitude detecting method and device

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
LUBIN CHANG，等: "Initial Alignment by Attitude Estimation for", 《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》 *
丁涛，等: "捷联惯导***的误差角关系与推导", 《自动测量与控制》 *
乔相伟，等: "用四元数状态切换无迹卡尔曼滤波器估计的飞行器姿态", 《控制理论与应用》 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111625768A (en) * 2020-05-19 2020-09-04 西安因诺航空科技有限公司 Handheld holder attitude estimation method based on extended Kalman filtering
CN111625768B (en) * 2020-05-19 2023-05-30 西安因诺航空科技有限公司 Hand-held cradle head posture estimation method based on extended Kalman filtering
CN111654634A (en) * 2020-06-24 2020-09-11 杭州海康威视数字技术股份有限公司 Method for determining inclination of engine core assembly and pan-tilt assembly in camera and camera
CN111654634B (en) * 2020-06-24 2022-02-08 杭州海康威视数字技术股份有限公司 Method for determining inclination of engine core assembly and pan-tilt assembly in camera and camera
CN113272755A (en) * 2020-07-01 2021-08-17 深圳市大疆创新科技有限公司 Control method and device of holder, holder and computer readable storage medium
WO2022000386A1 (en) * 2020-07-01 2022-01-06 深圳市大疆创新科技有限公司 Gimbal control method and apparatus, gimbal, and computer-readable storage medium
CN114370872A (en) * 2022-01-14 2022-04-19 苏州挚途科技有限公司 Vehicle attitude determination method and vehicle
CN114370872B (en) * 2022-01-14 2024-04-09 苏州挚途科技有限公司 Vehicle attitude determination method and vehicle

Also Published As

Publication number Publication date
CN110873563B (en) 2022-03-08

Similar Documents

Publication Publication Date Title
CN110873563B (en) Cloud deck attitude estimation method and device
CN109000612B (en) Equipment angle estimation method and device, camera shooting assembly and aircraft
CN106814753B (en) Target position correction method, device and system
JP4199553B2 (en) Hybrid navigation device
CN111380514A (en) Robot position and posture estimation method and device, terminal and computer storage medium
CN110887480B (en) Flight attitude estimation method and system based on MEMS sensor
CN105116926A (en) Holder control method and device
US20220155800A1 (en) Method and apparatus for yaw fusion and aircraft
CN106370178B (en) Attitude measurement method and device of mobile terminal equipment
CN109682377A (en) A kind of Attitude estimation method based on the decline of dynamic step length gradient
US11669109B2 (en) Method and apparatus for yaw fusion and aircraft
JP2012173190A (en) Positioning system and positioning method
CN105910606A (en) Direction adjustment method based on angular velocity difference
JP2007232443A (en) Inertia navigation system and its error correction method
CN108627152A (en) A kind of air navigation aid of the miniature drone based on Fusion
CN112055804A (en) Information processing apparatus, information processing method, and program
CN113959462A (en) Quaternion-based inertial navigation system self-alignment method
CN114526731A (en) Inertia combination navigation direction positioning method based on moped
CN113566850B (en) Method and device for calibrating installation angle of inertial measurement unit and computer equipment
CN109506674B (en) Acceleration correction method and device
CN111207734B (en) EKF-based unmanned aerial vehicle integrated navigation method
CN111141285B (en) Aviation gravity measuring device
CN110940357B (en) Inner rod arm calibration method for self-alignment of rotary inertial navigation single shaft
CN115727842A (en) Rapid alignment method and system for unmanned aerial vehicle, computer equipment and storage medium
CN110637266A (en) Cloud deck control method and cloud deck

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
CP03 Change of name, title or address

Address after: 310051 room 304, B / F, building 2, 399 Danfeng Road, Binjiang District, Hangzhou City, Zhejiang Province

Patentee after: Hangzhou Hikvision Robot Co.,Ltd.

Address before: 310051 5th floor, building 1, building 2, no.700 Dongliu Road, Binjiang District, Hangzhou City, Zhejiang Province

Patentee before: HANGZHOU HIKROBOT TECHNOLOGY Co.,Ltd.

CP03 Change of name, title or address
TR01 Transfer of patent right

Effective date of registration: 20230615

Address after: No.555, Qianmo Road, Binjiang District, Hangzhou City, Zhejiang Province

Patentee after: Hangzhou Hikvision Digital Technology Co.,Ltd.

Address before: 310051 room 304, B / F, building 2, 399 Danfeng Road, Binjiang District, Hangzhou City, Zhejiang Province

Patentee before: Hangzhou Hikvision Robot Co.,Ltd.

TR01 Transfer of patent right