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

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; and calculating the error angle of the holder platform at the moment i +1 based on 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 moment i +1 and the angular displacement of the output shaft of the course shaft motor in the holder, acquiring the holder attitude at the moment i, and acquiring the holder attitude at the moment i +1 based on the holder platform error angle at the moment i +1 and the holder attitude at the moment i. 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 obtaining the cradle head attitude at the (i + 1) th moment based on the cradle head platform error angle at the (i + 1) th moment and the cradle head attitude at the (i) th 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 obtaining the base posture at the ith moment based on the base platform error angle at the ith moment and the base posture at the ith-1 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,

wherein,

is the heading angle of the base at time i,

is the roll angle of the susceptor at time i,

is the pitch angle of the base at time i, psi_body,iThe carrier course angle at the ith moment;

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,

wherein,

is the heading angle of the pan/tilt head at the ith moment,

the roll angle of the pan/tilt head at the ith moment,

is the pitch angle, psi, of the pan-tilt at moment i_body,iThe carrier course angle at the ith moment; psi_m,iIs the angular displacement of the output shaft of the course shaft motor at the ith moment,

and the linear acceleration of the image acquisition equipment on three axes of the holder coordinate system at the ith moment.

Optionally, the obtaining the pan/tilt head attitude at the i +1 th moment based on the pan/tilt head platform error angle at the i +1 th moment and the pan/tilt head attitude at the i +1 th moment 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 obtaining the cradle head attitude at the (i + 1) th moment by adopting the cradle head attitude increment at the (i + 1) th moment relative to the (i) th moment and the cradle head attitude at the i th 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 obtaining the cradle head attitude at the i +1 th moment based on the cradle head platform error angle at the i +1 th moment and the cradle head attitude at the i th 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 obtaining the base posture at the ith moment based on the base platform error angle at the ith moment and the base posture at the ith-1 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,

wherein,

is the heading angle of the base at time i,

is the roll angle of the susceptor at time i,

is the pitch angle of the base at time i, psi_body,iThe carrier course angle at the ith moment;

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,

wherein,

is the heading angle of the pan/tilt head at the ith moment,

the roll angle of the pan/tilt head at the ith moment,

is the pitch angle, psi, of the pan-tilt at moment i_body,iThe carrier course angle at the ith moment; psi_m,iIs the angular displacement of the output shaft of the course shaft motor at the ith moment,

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 obtaining the cradle head attitude at the (i + 1) th moment by adopting the cradle head attitude increment at the (i + 1) th moment relative to the (i) th moment and the cradle head attitude at the i th 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 is 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 between the moment i +1, and the tripod head attitude at the moment i is obtained based on the tripod head attitude increment and the tripod head attitude at the moment i, the cloud deck attitude is not directly solved for the integral of the gyroscope when the cloud deck attitude is calculated, so that the problem of integral error of the gyroscope is avoided, and the estimation precision of the cloud deck 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 system_bIs the center of mass, x, of the aircraft_bThe 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, y_bThe 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, z_bThe axis (also called course axis) is in the plane of symmetry of the aircraft and is aligned with x_bAxis and y_bThe 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 is_bShaft in waterThe angle between the projection on the plane and the north direction of the geography. The transverse roll angle gamma is, the carrier winds around x_bThe 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-axis_bThe 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).

Wherein,

the coordinate transformation matrix from an n system to a b system is called as a real attitude matrix;

the coordinate transformation matrix from the p system to the b system is called as a calculation attitude matrix;

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

The platform error angle is small, so represented by an angular vector,

the three axes of the navigation coordinate system n, e and d are error angles respectively. The small-angle attitude matrix formula comprises:

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, obtaining the cradle head posture at the ith +1 moment based on the cradle head platform error angle at the ith +1 moment and the cradle head posture at the ith 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 is 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 between the moment i +1, and the tripod head attitude at the moment i is obtained based on the tripod head attitude increment and the tripod head attitude at the moment i, the cloud deck attitude is not directly solved for the integral of the gyroscope when the cloud deck attitude is calculated, so that the problem of integral error of the gyroscope is avoided, and the estimation precision of the cloud deck 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.

Wherein,

is the heading angle of the pan/tilt head at the ith moment,

the roll angle of the pan/tilt head at the ith moment,

is the pitch angle, psi, of the pan-tilt at moment i_body,iThe carrier course angle at the ith moment; psi_m,iIs the angular displacement of the output shaft of the course shaft motor at the ith moment,

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)

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

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)

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

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

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

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

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).

Wherein,

is the estimated state quantity at the (i + 1) th moment,

the estimated error angle of the platform of the pan/tilt head at the (i + 1) th moment,

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.

For the estimated tripod head gyroscope offset at the (i + 1) th moment,

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.

And

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;

is the angular velocity of the image acquisition equipment at the (i + 1) th moment on three axes of the holder coordinate system,

in order to calculate the period of the cycle,

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,

is the error angle of the platform of the pan/tilt head at the ith moment,

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,

for the gyroscope bias of the image acquisition device at time i,

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

Is a preset state quantity.That is to say that the position of the first electrode,

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

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).

Wherein,

is the estimated state quantity covariance, F, at the i +1 th moment_i+1Is the state transition matrix at time i +1,

is F_i+1Transposed matrix of (2), Q_iIs the process noise covariance, P, at time i_iIs the state quantity covariance at the ith time. State transition matrix F at time i +1_i+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, directly substituting the values of parameters such as the state quantity at the (i) th moment contained in the expression into the expression to obtain the state at the (i + 1) th momentA state transition matrix.

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

After expansion, the formula (6) shows.

Wherein, Z ″)_i+1Represents the estimated observed quantity at the i +1 th time,

is a direction cosine matrix from the pan-tilt to the navigation coordinate system at the (i + 1) th moment,

is the 1 st row and 2 nd column element in the direction cosine matrix,

is the 2 nd row and 2 nd column element in the direction cosine matrix,

is the column 1 element of the 3 rd row in the direction cosine matrix,

is the 3 rd row and 2 nd column element in the direction cosine matrix,

is the 3 rd row and 3 rd column element in the direction cosine matrix. Wherein,

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.

Wherein,

the estimated tripod head attitude increment of the ith +1 moment relative to the ith moment,

and the error angle of the platform of the cradle head is estimated at the (i + 1) th moment.

Wherein,

the estimated tripod head attitude at the (i + 1) th moment, namely the estimated observed quantity represented by the quaternion,

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):

in formula (9), [ q ]₀ q₁ q₂ q₃]^TRepresenting quaternion, [ psi γ θ]^TRepresenting the euler angle.

Secondly, estimating the quaternion of the observed quantity

Conversion of representation into directional cosine matrix

And (4) showing.

Then, the direction cosine matrix is

Substituting the corresponding specific elements into formula (9) to obtain the estimated observed quantity Z' represented by Euler angle_i+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).

Wherein, K_i+1The kalman filter gain at the i +1 th time,

is the estimated state quantity covariance, H, at the i +1 th moment_i+1Is a measurement matrix at the i +1 th time, R_i+1Is the measured noise covariance at time i + 1. Measurement matrix H at the i +1 th time_i+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 time, first, the estimate of the i +1 th time is estimatedCalculating a parameter related to the state quantity at the (i + 1) th moment by taking the state quantity as 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).

Is the state quantity at the (i + 1) th moment,

is the error angle of the platform of the pan/tilt head at the moment of i +1,

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.

For the pan tilt gyroscope bias at time i +1,

and (3) biasing the tripod head gyroscopes which are three axes of the tripod head coordinate system at the (i + 1) th moment respectively.

Is the estimated state quantity, K, at the i +1 th moment_i+1Kalman filter gain, Z ", at time i +1_i+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).

Wherein, P_i+1Is the covariance of the state quantity at the I +1 th moment, I is the identity matrix, K_i+1The kalman filter gain at the i +1 th time,

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)

The updated formula (7) is that,

the attitude increment of the pan/tilt head at the i +1 th moment relative to the i th moment,

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

(see step 4033a) except that the pan head attitude increment at time 2 is calculated relative to time 1

Substituting the formula into the error angle of the platform of the pan/tilt head at the 2 nd moment

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)

The updated formula (8) is that,

wherein,

the posture of the pan/tilt head at the (i + 1) th moment represented by the quaternion,

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.

Wherein,

is the heading angle of the base at time i,

is the roll angle of the susceptor at time i,

is the pitch angle of the base at time i, psi_body,iThe carrier course angle at the ith moment;

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)

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

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)

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).

Wherein,

is the estimated state quantity at the (i + 1) th moment,

the estimated error angle of the base platform at the (i + 1) th moment,

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.

For the estimated base gyroscope bias at time i +1,

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.

And

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;

is the angular velocity of the base at the three axes of the base coordinate system at the moment i +1,

in order to calculate the period of the cycle,

equal to the period of time starting from the ith time instant and ending at the (i + 1) th time instant,

is the base platform error angle at time i,

north, east and ground error angles of the base platform at time i,

the gyro bias of the base at time i,

state quantity at time 1

Is a preset state quantity. That is to say that the position of the first electrode,

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

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).

Wherein,

the estimated state quantity covariance at the (i + 1) th time,

is the state transition matrix at time i +1,

is composed of

The transpose matrix of (a) is,

is the process noise covariance, P, at time i_i ^bIs the state quantity covariance at the ith time. State transition matrix at the i +1 th time

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

After expansion, as shown in equation (16).

Wherein,

represents the estimated observed quantity at the i +1 th time,

is the direction cosine matrix from the base to the navigation coordinate system at the (i + 1) th moment.

Is the direction cosine matrix

Row 1 and column 2 elements in the middle,

is the direction cosine matrix

Row 2 and column 2 elements of the middle,

is the direction cosine matrix

Row 3 and column 1 elements in,

is the direction cosine matrix

Row 3 and column 2 elements in the middle,

is the direction cosine matrix

Row 3 and column 3 elements. Wherein,

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.

Wherein,

the estimated attitude increment of the base at the (i + 1) th time relative to the (i) th time,

the estimated error angle of the base platform at the (i + 1) th moment.

Wherein,

the estimated attitude of the base at the (i + 1) th moment, namely the estimated observed quantity expressed by the quaternion,

the base attitude at the i-th time represents a quaternion multiplication operation.

Secondly, willEstimation of observed quaternion

Conversion of representation into directional cosine matrix

And (4) showing.

Then, the direction cosine matrix is

Substituting the corresponding specific elements into formula (16) to obtain the estimated observed quantity expressed by Euler angle

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).

Wherein,

the kalman filter gain at the i +1 th time,

the estimated state quantity covariance at the (i + 1) th time,

is the measurement matrix at the i +1 th time,

is the measured noise covariance at time i + 1. Measurement matrix at the i +1 th time

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).

Is the state quantity at the (i + 1) th moment,

the base platform error angle at time i +1,

the north, east and ground error angles of the base platform at time i +1, respectively.

For the base gyroscope bias at time i +1,

base gyroscope biases for three axes of the base coordinate system at time i +1, respectively.

Is the estimated state quantity at the (i + 1) th moment,

the estimated observation (estimated base attitude) at time i +1,

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).

Wherein,

is the covariance of the state quantities at the I +1 th moment, I is the identity matrix,

the kalman filter gain at the i +1 th time,

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)

The updated formula (17) is,

the incremental base attitude at time i +1 relative to time i,

the base platform error angle at time i + 1. And calculating the estimated observed quantity of the 2 nd time

(see step 5073a) except that the incremental base attitude at time 2 is calculated relative to time 1

Substituted into the formula is the error angle of the pedestal platform at time 2

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)

The updated formula (18) is

At the i +1 th time represented by quaternionThe attitude of the base of (a) is,

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).

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).

Wherein,

and the direction cosine matrix from the holder coordinate system to the base coordinate system at the (i + 1) th moment.

Is the direction cosine matrix

Row 1 and column 2 elements in the middle,

is the direction cosine matrix

Row 2 and column 2 elements of the middle,

is the direction cosine matrix

Row 3 and column 1 elements in,

is the direction cosine matrix

Row 3 and column 2 elements in the middle,

is the direction cosine matrix

Row 3 and column 3 elements.

The calculation process of (2) includes: firstly, calculating the quaternion of the estimated tripod head attitude at the i +1 th moment

And calculating the quaternion of the estimated base attitude at the (i + 1) th moment

Secondly, using the formula

Calculating to obtain the quaternion of the estimated motor angular displacement

Finger-shaped

Conjugation of (1). Then, the angular displacement quaternion of the motor is estimated

Conversion to a directional cosine matrix

And (4) showing.

In calculating the estimated observed quantities, obtaining them separately

And

then, will

And

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 obtain the cradle head posture at the i +1 th moment based on the cradle head platform error angle at the i +1 th moment and the cradle head posture at the i 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 obtaining the base posture at the ith moment based on the base platform error angle at the ith moment and the base posture at the ith-1 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 adopting the cradle head attitude increment of the ith +1 moment relative to the ith moment and the cradle head attitude of the ith moment to obtain the cradle head attitude of the ith +1 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 is 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 between the moment i +1, and the tripod head attitude at the moment i is obtained based on the tripod head attitude increment and the tripod head attitude at the moment i, the cloud deck attitude is not directly solved for the integral of the gyroscope when the cloud deck attitude is calculated, so that the problem of integral error of the gyroscope is avoided, and the estimation precision of the cloud deck 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 (22)

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;

calculating the cradle head attitude increment of the ith moment relative to the ith moment based on the cradle head platform error angle of the ith +1 moment, and obtaining the cradle head attitude of the ith +1 moment based on the cradle head attitude increment of the ith moment relative to the ith moment and the cradle head attitude of the ith moment;

the calculating the cradle head attitude increment of the ith moment relative to the ith moment based on the cradle head platform error angle of the ith +1 moment comprises:

calculating the attitude increment of the pan-tilt at the i +1 th moment relative to the i-th moment according to the following formula based on the error angle of the pan-tilt platform at the i +1 th moment,

wherein, the

Is the attitude increment of the pan-tilt head at the i +1 th moment relative to the i th moment, the

And the error angle of the platform of the holder at the (i + 1) th 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 calculating the base attitude increment of the ith moment relative to the ith-1 moment based on the base platform error angle of the ith moment, and obtaining the base attitude of the ith moment based on the base attitude increment of the ith moment relative to the ith-1 moment and the base attitude of the ith-1 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,

wherein,

is the heading angle of the base at time i,

is the roll angle of the susceptor at time i,

is the pitch angle of the base at time i, psi_body,iThe carrier course angle at the ith moment;

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,

wherein,

is the heading angle of the pan/tilt head at the ith moment,

the roll angle of the pan/tilt head at the ith moment,

is the pitch angle, psi, of the pan-tilt at moment i_body,iThe carrier course angle at the ith moment; psi_m,iIs the angular displacement of the output shaft of the course shaft motor at the ith moment,

and the linear acceleration of the image acquisition equipment on three axes of the holder coordinate system at the ith moment.

11. 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;

the updating module is used for calculating the tripod head attitude increment of the ith moment relative to the ith moment based on the tripod head platform error angle of the ith +1 moment, and obtaining the tripod head attitude of the ith +1 moment based on the tripod head attitude increment of the ith moment relative to the ith moment and the tripod head attitude of the ith moment;

the update module is configured to:

the calculating the cradle head attitude increment of the ith moment relative to the ith moment based on the cradle head platform error angle of the ith +1 moment comprises:

calculating the attitude increment of the pan-tilt at the i +1 th moment relative to the i-th moment according to the following formula based on the error angle of the pan-tilt platform at the i +1 th moment,

wherein, the

Is the ithA pan tilt attitude increment at +1 time relative to the ith time, said

And the error angle of the platform of the holder at the (i + 1) th moment.

12. The apparatus of claim 11, 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.

13. The apparatus of claim 12, 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.

14. The apparatus of claim 13, 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.

15. The apparatus of claim 13, 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 calculating the base attitude increment of the ith moment relative to the ith-1 moment based on the base platform error angle of the ith moment, and obtaining the base attitude of the ith moment based on the base attitude increment of the ith moment relative to the ith-1 moment and the base attitude of the ith-1 moment.

16. The apparatus of claim 15, wherein the computing module is configured to,

when i is 1, calculating the base posture at the ith moment according to the following formula,

wherein,

is the heading angle of the base at time i,

is the roll angle of the susceptor at time i,

is the pitch angle of the base at time i, psi_body,iThe carrier course angle at the ith moment;

the linear acceleration of the base on three axes of the base coordinate system at the ith moment.

17. The apparatus of claim 15, 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.

18. The apparatus of claim 17, 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.

19. The apparatus of claim 13, 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.

20. The apparatus according to any of claims 11-19, 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,

wherein,

is the heading angle of the pan/tilt head at the ith moment,

the roll angle of the pan/tilt head at the ith moment,

is the pitch angle, psi, of the pan-tilt at moment i_body,iThe carrier course angle at the ith moment; psi_m,iIs the angular displacement of the output shaft of the course shaft motor at the ith moment,

and the linear acceleration of the image acquisition equipment on three axes of the holder coordinate system at the ith moment.

21. 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 10.

22. 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 10.

Images