WO2019205034A1

WO2019205034A1 - Camera stabilizer position correction method and device

Info

Publication number: WO2019205034A1
Application number: PCT/CN2018/084499
Authority: WO
Inventors: 张翔; 李兵; 周游
Original assignee: 深圳市大疆创新科技有限公司
Priority date: 2018-04-25
Filing date: 2018-04-25
Publication date: 2019-10-31
Also published as: EP3786757A1; US20210033242A1; CN110325822B; EP3786757B1; EP3786757A4; CN110325822A; US11274788B2

Abstract

A camera stabilizer position correction method and device, the device comprising a vertical compensation device connected to the camera stabilizer, a visual module (2) disposed on the vertical compensation device, and an inertial measurement unit (1) disposed on the vertical compensation device. The vertical compensation device is used to compensate for movement of the camera stabilizer in a vertical direction, and the visual module (2) and the inertial measurement unit (1) are both electrically connected to the vertical compensation device; the vertical compensation device is used to acquire a first position of the camera stabilizer based on the inertial measurement unit (1), and to acquire, based on the visual module (2), a second position of the vertical compensation device, and to execute correction of the first position in accordance with the second position.

Description

PTZ attitude correction method and device

Technical field

The present invention relates to the field of pan/tilt control, and in particular, to a pan/tilt pose correction method and apparatus.

Background technique

One of the key problems to be solved by pose estimation robot control is to obtain position, velocity, attitude and heading information that meets control bandwidth, dynamic performance, stability and accuracy requirements based on data of various motion state sensors. The system of instant pose information is called a navigation system. Common navigation systems include: inertial navigation systems, Global Navigation Satellite System (GNSS), Doppler navigation systems, visual navigation systems, and more. The technique of measuring the same information source using a variety of different navigation systems, extracting and correcting the error of each navigation system from the measured values is called integrated navigation technology, and integrated navigation technology is one of the important applications in the field of multi-sensor information fusion state estimation. .

Inertial-GNSS integrated navigation is one of the commonly used integrated navigation. The traditional inertial-GNSS integrated navigation uses the NED coordinate system as the navigation coordinate system. It needs to refer to the north heading observation. Generally, the geomagnetic sensor is used to provide the reference heading, while the geomagnetic sensor is susceptible to current and magnetic field interference; and the traditional inertial-GNSS combined navigation uses The latitude and longitude indicates the position, so GNSS is required to provide position measurement in the form of latitude and longitude. When the use is indoor environment, GNSS navigation cannot work; conventional single-point GNSS has m-level positioning and speed measurement error, and in some cases speed control Accuracy requirements are up to the mm level, so the use of inertial-GNSS combined navigation alone does not meet the accuracy requirements.

Summary of the invention

The invention provides a pan/tilt posture correction method and device.

Specifically, the present invention is achieved by the following technical solutions:

According to a first aspect of the present invention, a pan/tilt posture correction method is provided, wherein a pan/tilt head is connected to a vertical compensating device, and the vertical compensating device compensates for movement of the gimbal in a vertical direction, the vertical The direct compensation device is provided with a vision module and an inertial measurement unit, and the method comprises:

Acquiring the first pose of the pan/tilt based on the inertial measurement unit;

Acquiring a second pose of the vertical compensation device based on the vision module;

The first pose is corrected according to the second pose.

According to a second aspect of the present invention, a pan/tilt posture correcting apparatus is provided, comprising: a vertical compensating device connected to the pan/tilt head, a visual module mounted on the vertical compensating device, and being mounted on the vertical An inertial measurement unit on the compensation device, the vertical compensation device is configured to compensate movement of the pan/tilt in a vertical direction, and the visual module and the inertial measurement unit are electrically connected to the vertical compensation device;

The vertical compensation device is configured to acquire a first pose of the pan/tilt based on the inertial measurement unit; acquire a second pose of the vertical compensation device based on the vision module; and according to the second Position, correcting the first pose.

According to the technical solution provided by the embodiment of the present invention, the present invention adopts an inertial-visual combined navigation method, and corrects the first posture obtained by the inertial measurement unit based on the second posture obtained by the vision module, and obtains the control bandwidth and accuracy requirements. In the pose, the inertial-visual combined navigation method of the present invention is free from current and magnetic field interference, and is suitable for various environments indoors and outdoors.

DRAWINGS

In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings used in the description of the embodiments will be briefly described below. It is obvious that the drawings in the following description are only some embodiments of the present invention. Other drawings may also be obtained from those of ordinary skill in the art in view of the drawings.

1 is a schematic structural view of a pan/tilt posture correcting device according to an embodiment of the present invention;

2 is a schematic structural diagram of a pan/tilt posture correcting device according to an embodiment of the present invention;

3 is a schematic structural diagram of a pan/tilt posture correcting device according to another embodiment of the present invention;

4 is a flow chart of a method for a pan/tilt pose correction method according to an embodiment of the present invention;

FIG. 5 is a flow chart of a method for a pan/tilt pose correction method according to an embodiment of the present invention.

LIST OF REFERENCE NUMERALS 1: Inertial measurement unit; 2: Vision module; 3: Body; 31: Body; 32: Base; 33: Hand-held; 4: Axle; 5: Motor;

detailed description

The technical solutions in the embodiments of the present invention are clearly and completely described in the following with reference to the accompanying drawings in the embodiments of the present invention. It is obvious that the described embodiments are only a part of the embodiments of the present invention, but not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative efforts are within the scope of the present invention.

The pan/tilt pose correction method and apparatus of the present invention will be described in detail below with reference to the accompanying drawings. The features of the embodiments and embodiments described below may be combined with each other without conflict.

1 is a schematic structural diagram of a pan/tilt posture correction device according to an embodiment of the present invention, wherein a pan/tilt is connected to a vertical compensation device, and the vertical compensation device compensates for movement of the gimbal in a vertical direction. In this embodiment, the pan/tilt is mounted on the movable object (such as a user, a drone, a robot) by the vertical compensation device, and when the movable object moves, there is a vertical motion, the vertical direction The motion on the top makes the camera picture on the pan/tilt unstable. Therefore, the vertical compensation device compensates the movement of the pan/tilt in the vertical direction, thereby improving the pan/tilt directly mounted on the movable object and along with the movable object. The vertical movement caused by the movement ensures the camera picture is stable.

Further, the vertical compensation device of the embodiment is provided with a vision module 2 and an inertial measurement unit (IMU). 2 and 3, the vertical compensating device comprises a body 3 and a shaft arm 4 for connecting the pan/tilt, the shaft arm 4 rotating to compensate for the movement of the gimbal in the vertical direction. Optionally, the body 3 is provided with a motor 5 for driving the shaft arm 4 to rotate, and in other embodiments, the shaft arm 4 can also be driven to rotate by other driving means. The inertial measurement unit 1 is disposed on the shaft arm 4, and the inertial measurement unit 1 can be installed at one end of the shaft arm 4 connected to the pan/tilt. Of course, the inertial measurement can also be installed in the Any other position on the axle arm 4 is described.

The vision module 2 is disposed on the body 3. The detection direction of the vision module 2 of the embodiment may be upward or downward. Specifically, when the pan-tilt posture correction system is located in an outdoor environment, the vision module 2 faces downward; when the pan-tilt posture correction system is located indoors, the vision module 2 may face upward or downward. The detection direction of the vision module 2 of the present embodiment is substantially parallel to the vertical direction, and the detection direction of the vision module 2 allows a smaller inclination angle relative to the vertical direction (the angle range of the inclination angle can be set according to an empirical value). Preferably, the vision module 2 is monitored vertically upwards or vertically downwards. Further, in conjunction with FIG. 2 and FIG. 3, the body 3 of the present embodiment may include a body 31 and a base 32 fixedly coupled to the body 31, and the vision module 2 is disposed on the base 32.

In some examples, the gimbal is mounted on a drone, mobile robot, or other mobile device via a cradle 32. During the movement of the drone, mobile robot or other mobile device, there is vertical motion affecting the camera (camera on the pan/tilt), and the vertical compensation device compensates for the vertical motion to cancel the vertical The effect of the direction of motion on the camera picture.

In other examples, in conjunction with FIGS. 2 and 3, the vertical compensating device is a handheld device, and the compensating device can include a hand portion 33 that is fixedly coupled to the body 31. The user holds the hand-held portion 33 to drive the vertical compensation device to move integrally. During the walking process, the user influences the camera picture in the vertical direction along with the step frequency, and the vertical compensation device affects the vertical direction. The motion is compensated to counteract the effects of vertical motion on the camera's picture.

It should be noted that, in the following embodiments, the body coordinate system {b}-O _b x _b y _b z _b is defined as follows: the coordinate system origin O _b is the plane geometry of the axial end of the axial arm 4 connected to the pan/tilt head. Center; the x _b axis is directed in the vertical symmetry plane of the fuselage 31 and parallel to the bottom surface of the pedestal 32; the y _b axis is perpendicular to the vertical symmetry plane of the fuselage 31 to the right of the fuselage 31; the z _b axis is in the fuselage 31 In the vertical symmetry plane, perpendicular to the x _b axis and pointing below the fuselage 31.

Base 32 defines a coordinate system _{_{{p} -O p x p y}} p z p as follows: the origin of the coordinate system center axis O _p arm 4, i.e. the intersection of shaft 31 rotates the arm 4 vertical symmetry plane of the center line of the fuselage; X _p-axis in the vertical plane of symmetry of the body 31 and parallel to the bottom surface of the base 32 is directed forward; _p Y axis perpendicular to the vertical plane of symmetry of the fuselage 31 of the body 31 pointing to the right; _p vertical symmetry axis Z of the body 31 In-plane, perpendicular to the x _p axis and pointing below the fuselage 31.

The camera coordinate system is {c}-O _c x _c y _c z _c , and the navigation coordinate system is {n}-O _n x _n y _n z _n . The navigation coordinate system origin O _is determined by the vertical projection of the camera coordinate system origin _Oc on the ground when the system starts working, and the navigation coordinate system coordinate axis is determined by the output of the vision module 2. The output camera coordinate system {c} of the vision module 2 is relative to the pose of the navigation coordinate system {n}, in some examples, the vision module 2 outputs the reference position of the vertical compensation device

Reference speed

And reference pose

In other examples, the vision module 2 outputs a reference position of the vertical compensation device

And reference speed

FIG. 4 is a flowchart of a method for a pan/tilt posture correction method according to an embodiment of the present invention. The execution body of the method may be a processor in a vertical compensation device or a separately provided control unit, and the control unit is communicatively coupled to a processor in the vertical compensation device. As shown in FIG. 4, the method may include the following steps:

Step S401: Acquire a first pose of the pan/tilt based on the inertial measurement unit 1;

Wherein, the first pose may include a speed, a position and a posture of the pan/tilt.

In this embodiment, the inertial measurement unit 1 may include a gyroscope and an accelerometer. Optionally, the gyroscope is a three-axis gyroscope, and the accelerometer is a three-axis accelerometer. Step S401 specifically includes: acquiring an angular velocity of the pan-tilt based on the gyroscope, acquiring a specific force of the pan-tilt based on the accelerometer, and then calculating, according to the angular velocity and the specific force, the pan-tilt Gesture, speed and position.

Specifically, the updating process of the pan-tilt attitude includes: designing a posture update formula according to the angular velocity and the specific force; and updating the posture of the pan-tilt according to the posture update formula.

In this embodiment, the design process of the posture update formula is as follows:

The ideal output of the gyroscope is the projection of the angular coordinate rate of the body coordinate system {b} relative to the inertial system {i} in the {b} system, recorded as

The actual output of the gyroscope is recorded as

The ideal output of the accelerometer is the projection of the specific force in the {b} system, denoted as f ^b , and the actual output of the accelerometer is recorded as

Quaternion

As a representation of the {b} system's attitude to the {b} system, the error-free ideal quaternion differential equation is determined by:

Attitude angular rate in formula (1)

Determined by:

In formula (2)

Determined by the latest posture update value,

with

They are the angular rate of the Earth's rotation and the angular velocity of the position. The embodiment of the present invention is suitable for low speed, short distance, near ground motion shooting, therefore

with

Can be ignored, so there is

In the actual system, due to the existence of gyroscope measurement error and navigation solution error, the actual solution of the quaternion differential equation is performed by:

The determined actual body coordinate system is denoted as {b'}. The quaternion differential equation shown in equation (1) is discretized and the first-order approximation is obtained. The quaternion update formula shown below can be obtained:

Perform attitude quaternion update according to formula (4) to obtain attitude matrix

In fact, the mathematical platform of SINS is established.

Further, the updating process of the pan/tilt speed comprises: designing a speed update formula according to the angular velocity and the specific force; and updating the speed of the pan/tilt according to the speed update formula. Specifically, the embodiment of the present invention adopts the following formula as an approximate speed update formula:

Further, the updating process of the pan/tilt position comprises: designing a location update formula according to the angular velocity and the specific force; and updating the location of the pan-tilt according to the location update formula. Specifically, the embodiment of the present invention adopts the following formula as an approximate location update formula:

It should be noted that the posture update formula, the speed update formula, and the position update formula in the embodiment of the present invention are not limited to the design manner of the above embodiment.

Step S402: Acquire a second pose of the vertical compensation device based on the vision module 2;

The vision module can include a visual odometer or a visual inertia odometer. For example, in one embodiment, the vision module includes a visual odometer that includes the speed and position of the vertical compensation device. In another embodiment, referring to FIG. 5, the vision module includes a visual inertia odometer that includes the speed, position, and attitude of the vertical compensation device.

In some examples, the vertical compensation device may further be equipped with a TOF ranging module (Time of Flight Measurement), and the detection of the visual module 2 by the detection result of the TOF ranging module in this embodiment. The result is corrected. Specifically, the vertical compensation device detects the position of the vertical compensation device by the TOF ranging module, and corrects the position of the compensation device acquired by the vision module 2 to obtain accurate verticality. The position of the compensation device.

In some examples, the visual module can be replaced with a UWB positioning device (Ultra Wideband), and the posture of the vertical compensating device is measured by the UWB positioning device, and the inertial-UWB integrated navigation method of the present invention is also free from current and Magnetic field interference is suitable for a variety of environments indoors and outdoors.

Since the vision module 2 of the embodiment is fixed on the base 32, the coordinate system of the reference speed and position of the vertical compensation device directly output by the vision module 2 is different from the coordinate system of the first pose, and the vision module cannot be used. 2 directly outputting the reference speed and the reference position of the vertical compensation device as a reference for the first pose, and correcting the first pose, the embodiment requires the vertical compensation device directly output to the vision module 2 The reference speed and the reference position are coordinate-converted to obtain a second pose, and the second pose is in the same coordinate system as the first pose.

Specifically, in the present embodiment, the axle arm 4 or the upper side is provided with an angular velocity sensor 6 for acquiring the joint angle of the axle arm 4. Before the step S202, the method may further comprise obtaining the joint angle of the axle arm 4 based on the angular velocity sensor 6. In other embodiments, the joint angle of the axle arm 4 can also be determined based on the joint angle of the motor 5 that drives the axle arm 4. It should be noted that, in this embodiment, the type of the angular velocity sensor 6 is not limited, and any existing angular velocity sensor 6 may be selected.

Step S402 of the embodiment includes: performing coordinate conversion on the reference speed of the vertical compensation device output by the vision module 2 according to the joint angle, obtaining the speed of the vertical compensation device, and then performing vertical compensation according to the vertical compensation The speed of the device can be corrected for the speed of the gimbal. Further, step S402 further includes: performing coordinate conversion on a reference position of the vertical compensation device output by the vision module 2 according to the joint angle, obtaining a position of the vertical compensation device, and then performing vertical compensation The position of the device corrects the position of the gimbal.

The visual module 2 includes a visual inertial odometer. The step S402 of the embodiment further includes: constructing a reference direction cosine matrix of the reference pose based on the reference pose output by the visual inertia odometer; and according to the reference direction cosine matrix, The attitude of the vertical compensation device is obtained. Obtaining, according to the direction cosine matrix, the posture of the vertical compensation device comprises: obtaining an attitude correction value of the vertical compensation device according to the reference direction cosine matrix; and obtaining the The posture of the vertical compensation device. At this point, the attitude of the pan/tilt can be corrected by the attitude of the vertical device.

Step S403: Correct the first pose according to the second pose.

According to the second pose obtained in the above step S402, the first pose obtained in the above step S401 is corrected, and the pose estimation value of the gimbal can be obtained. According to the pose estimation, the posture of the gimbal can be controlled to ensure the accuracy of the posture of the gimbal. It should be noted that, in the embodiment of the present invention, for the three-axis (yaw axis, pitch axis, roll axis) pan/tilt, the first pose is corrected, or the pan-tilt pose is corrected, which actually means correction. The attitude of the gimbal in the yaw axis, pitch axis and/or roll axis direction.

Wherein, step S403 can combine the first pose and the second pose by loop feedback, optimal estimation or other algorithms to realize inertial-visual combined navigation. In the present embodiment, Kalman filtering (one of the optimal estimation algorithms) is employed to fuse the first pose and the second pose. The following embodiment will specifically illustrate the implementation process of merging the first pose and the second pose using Kalman filtering.

In this embodiment, step S401 further includes: acquiring an angular velocity of the pan-tilt based on the gyroscope, acquiring a specific force of the pan-tilt based on the accelerometer; and then, according to the angular velocity and the specific force, Calculating the error of the first pose. Specifically, calculating the error of the first pose according to the angular velocity and the specific force comprises: constructing an attitude error, a velocity error, and the first pose according to the angular velocity and the specific force The position error is calculated based on the attitude error, the velocity error, and the position error, and the error of the first pose is calculated.

Further, step S403 specifically includes: approximating the error of the first pose, obtaining a Kalman filter; using the second pose as an observation value, obtaining a correction value through the Kalman filter wave; The correction value is corrected, and the first pose is corrected to correct the pose of the gimbal in the vertical direction. In this embodiment, the error approximation processing for the first pose is to remove the error term having less influence among the errors of the first pose.

In a specific embodiment, referring to FIG. 5, the application of the pan/tilt in this embodiment is a low speed, short distance, near ground motion photography. The measurement error model of the gyroscope is:

Where n _r is the gyro measurement noise, and assumes that n _r is Gaussian white noise; b is the gyro bias, and is assumed to be

A form of random walk, where n _w is Gaussian white noise. use

Indicates the gyro bias estimation, think

Is a constant zero offset, then

According to the measurement error model of the gyroscope, it is obtained:

Further, the gyro zero offset error is defined as:

then

Define the state quantity of the attitude solution as

According to the quaternion differential equation and the gyroscope measurement error model are:

For state estimators:

Combined with the above formula, the calculation process of the attitude error equation is as follows:

use

Express

The resulting error quaternion is obtained from the quaternion multiplication:

Equation (9) derives the time and obtains the state equation of the system according to the attitude:

Considering the gyroscope measurement error model (7), the formula (10) can be written as:

Note that φ is the {b′} system relative to the {b} system's attitude angle deviation and that φ is a small angle, then

The approximate expression is

Bring into formula (11) to get:

Then the equation of state of the attitude error is:

Further, the calculation process of the speed error:

According to the force equation, the error-free ideal velocity value is determined according to the following differential equation:

Where g ⁿ represents the representation of the gravitational acceleration in the navigational coordinate system. Since the embodiment of the present invention is suitable for low speed, short distance, near ground motion shooting,

with

Can be ignored, so the approximate speed error is calculated as follows:

among them,

Indicates the projection of the accelerometer zero offset in the navigation coordinate system.

Further, the calculation process of the position error:

Different from the conventional combined navigation using the latitude and longitude description position, the embodiment of the present invention uses visual navigation for position measurement, and the applicable occasion is near-ground short-distance motion, so the position error equation of the distance form shown by the formula (15) can be used:

The calculation formula of the integrated attitude error, velocity error and position error can obtain the error of the first pose (ie the combined navigation system error equation):

Among them, the system state quantity X is:

The state transition matrix F is:

among them,

for

Opposition matrix

for

Opposition matrix.

The system noise vector w is:

w=[n _r n _w n _a ] ^T

Where n _r is the gyro noise, n _w is the gyro random walk noise, and n _a is the accelerometer noise.

The noise distribution matrix G is:

The equation (16) is discretized and the first-order approximation is obtained, and the discretized first pose error calculation formula is obtained. The Kalman filter is designed by using the discretized first pose error calculation formula.

Further, in the embodiment, the vision module 2 includes a visual inertia odometer. In this embodiment, the observation value of the Kalman filter is designed according to the output result of the visual inertia odometer. The specific design process is as follows:

The reference attitude of the visual inertia odometer output is

The reference direction cosine matrix is

In this embodiment, the reference heading outputted by the visual inertial odometer is used as the heading observation of the integrated navigation system, and it is considered that the {b} system and the {c} system are completely aligned.

Let the three coordinate positive unit vectors of the navigation coordinate system {n} be:

Then the projection of the heading reference vector in the {b} system (ie, {b} is the reference vector in the x direction)

for:

According to the force equation, the unity projection of the gravity reference vector in the {b} system when it is completely at rest (ie, {b} is the reference vector in the z direction)

for:

According to the orthogonal relationship of the coordinate system,

with

Can get the {b} y direction reference vector

by

The direction cosine matrix of the reference pose is constructed as follows:

by

Reference attitude quaternion

Then the posture correction quaternion is:

In formula (20)

Estimate the quaternion for the current state of the pose. Under small angle conditions, the observation of the attitude correction according to the above formula is as follows:

among them,

with

All are error quaternions.

The observation equation of the attitude correction of this embodiment is:

Where v _φ is the attitude observation noise.

Where H _φ = [I _{3 × 3} 0 _{3 × 12} ], v _φ = [v _φx v _φy v _φz ] ^T .

Equation (22) is used as the attitude observation equation of the Kalman filter, and the attitude correction value is output through the Kalman filter.

use

By correcting the attitude update value of the pan/tilt obtained in equation (4), the corrected attitude output can be obtained, and the posture of the pan/tilt can be corrected.

Speed, position vector of visual inertia odometer output

For the speed and position of the camera coordinate system {c} relative to the {n} system, it is necessary to obtain the speed and position observation of the {b} system. This embodiment does not consider mechanical errors. During the rotation of the axle arm 4, the parallelogram mechanism of the axle arm 4 ensures that the plane of the shaft end is always parallel to the bottom surface of the base 32, so that only between the {b} system and the {p} system There is a translational motion.

According to the output of the visual inertia odometer and the geometric and dynamic transmission relationship of the mechanical structure, the reference velocity V _r ^{n of the} axle arm 4 and the reference position P _r ^{n are} solved. The specific formula is as follows:

among them,

Is {p} line to {n} direction based cosine matrix, ΔP ^p is O _b relative position vector to O _c projected {p} lines, ΔV ^p is O _b to O _c relative velocity vector { p} Projection in the system. The definition [O _x O _y O _z ] ^T is the positional offset vector of O _c to O _p represented in the {p} system; the joint angle defining the axial arm 4 is α, and when the axial arm 4 is parallel to the ground of the base 32 α =0, the counterclockwise direction is defined as the positive direction; the length L of the axial arm 4 is defined as the length from the rotation center line of the shaft arm 4 to the shaft end (ie, the end of the shaft arm 4 connected to the pan/tilt), and ΔP ^p is calculated as follows:

ΔV ^p is calculated as follows:

According to the reference velocity vector V _r ⁿ obtained by the equations (23) and (24) and the reference position vector P _r ⁿ , the velocity observation equation and the position observation equation of the integrated navigation system are obtained, respectively:

Where: H _φ = [0 _{3 × 3} I _{3 × 3} 0 _{3 × 9} ], v _V = [v _Vx v _Vy v _Vz ] ^T ;

H _P =[0 _3×6 I _3×3 0 _3×6 ], v _P =[v _Px v _Py v _Pz ] ^T ;

v _V is the velocity observation noise, and H _P is the position observation noise.

The formula (27) is used as the velocity observation equation of the Kalman filter. After the Kalman filter output speed correction value is used, the speed update value of the pan/tilt obtained by the formula (5) is corrected by the speed correction value, and the corrected value can be obtained. The speed output is used to correct the speed of the gimbal. Further, the equation (28) is used as the position observation equation of the Kalman filter, and the position correction value of the pan/tilt obtained by the formula (6) is corrected by the position correction value through the Kalman filter output position correction value. Corrected position output to correct the position of the gimbal.

The embodiment of the present invention adopts an inertial-visual combined navigation method, and corrects the first posture obtained by the inertial measurement unit 1 based on the second posture obtained by the visual module 2, and obtains a posture satisfying the control bandwidth and the accuracy requirement, and the inertia of the present invention. - Vision combined navigation is immune to current and magnetic fields and is suitable for indoor and outdoor environments.

With reference to FIG. 1 to FIG. 3, an embodiment of the present invention further provides a pan/tilt posture correction device, which may include a vertical compensation device connected to the pan/tilt head, and a vision module installed on the vertical compensation device. 2 and an inertial measurement unit 1 mounted on the vertical compensation device, the vertical compensation device is for compensating for movement of the pan/tilt in a vertical direction, and the visual module 2 and the inertial measurement unit 1 are both It is electrically connected to the vertical compensation device.

The vertical compensation device is configured to acquire a first pose of the pan/tilt based on the inertial measurement unit 1; acquire a second pose of the vertical compensation device based on the vision module 2; In the second pose, the first pose is corrected.

Further, the vertical compensation device includes a body 3 and a shaft arm 4 for connecting the pan/tilt, the shaft arm 4 is rotated to compensate movement of the pan/tilt in a vertical direction; the inertial measurement unit 1 is disposed on the axle arm 4, and the vision module 2 is disposed on the body 3.

Further, the vision module 2 includes a visual odometer that includes the speed and position of the vertical compensation device.

Further, the vision module 2 includes a visual inertia odometer that includes the speed, position, and attitude of the vertical compensation device.

Further, the vertical compensation device includes a shaft arm 4 for connecting the pan/tilt, the shaft arm 4 rotates to compensate for movement of the pan/tilt head in a vertical direction, and the arm arm 4 is provided with an angular velocity a sensor 6; the vertical compensating means for obtaining an articulation angle of the axle arm 4 based on the angular velocity sensor 6.

Further, the first pose includes a speed of the pan/tilt; the vertical compensation device is configured to perform a reference speed of the vertical compensation device output by the vision module 2 according to the joint angle Coordinate transformation to obtain the speed of the vertical compensation device.

Further, the first pose includes a position of the pan/tilt; the vertical compensation device is configured to perform, according to the joint angle, a reference position of the vertical compensation device output by the vision module 2 Coordinate transformation to obtain the position of the vertical compensation device.

Further, the vertical compensation device is configured to construct a reference direction cosine matrix of the reference posture based on a reference posture output by the visual inertia odometer; and obtain the vertical compensation device according to the reference direction cosine matrix Gesture.

Further, the vertical compensation device is configured to obtain an attitude correction value of the vertical compensation device according to the reference direction cosine matrix; and obtain a posture of the vertical compensation device according to the posture correction value.

Further, the first pose includes a speed, a position, and a posture of the pan/tilt.

Further, the inertial measurement unit 1 includes a gyroscope and an accelerometer; the vertical compensation device is configured to acquire an angular velocity of the pan/tilt based on the gyroscope; and acquire a ratio of the gimbal based on the accelerometer a force; calculating a posture, a speed, and a position of the pan/tilt according to the angular velocity and the specific force.

Further, the vertical compensation device is configured to design a posture update formula according to the angular velocity and the specific force; and update the posture of the pan/tilt according to the posture update formula.

Further, the vertical compensation device is configured to design a speed update formula according to the angular velocity and the specific force; and update the speed of the pan/tilt according to the speed update formula.

Further, the vertical compensation device is configured to design a position update formula according to the angular velocity and the specific force; and update the position of the pan/tilt according to the position update formula.

Further, the inertial measurement unit 1 includes a gyroscope and an accelerometer; the vertical compensation device is configured to acquire an angular velocity of the pan/tilt based on the gyroscope; and acquire a ratio of the gimbal based on the accelerometer a force; calculating an error of the first pose based on the angular velocity and the specific force.

Further, the vertical compensation device is configured to construct an attitude error, a speed error, and a position error of the first pose according to the angular velocity and the specific force; according to the attitude error, the velocity error, and the position error Calculating the error of the first pose.

Further, the vertical compensation device is configured to approximate the error of the first pose, obtain a Kalman filter, and use the second pose as an observation value to be corrected by the Kalman filter wave. a value; the first pose is corrected according to the correction value.

For the device embodiment, since it basically corresponds to the method embodiment, reference may be made to the partial description of the method embodiment. The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separate, and the components displayed as units may or may not be physical units, ie may be located A place, or it can be distributed to multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the embodiment. Those of ordinary skill in the art can understand and implement without any creative effort.

It should be noted that, in this context, relational terms such as first and second are used merely to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply such entities or operations. There is any such actual relationship or order between them. The terms "including", "comprising" or "comprising" or "comprising" are intended to include a non-exclusive inclusion, such that a process, method, article, or device that comprises a plurality of elements includes not only those elements but also other items not specifically listed Elements, or elements that are inherent to such a process, method, item, or device. An element that is defined by the phrase "comprising a ..." does not exclude the presence of additional equivalent elements in the process, method, item, or device that comprises the element.

The method and apparatus for correcting the posture of the pan-tilt provided by the embodiments of the present invention are described in detail above. The principles and embodiments of the present invention are described in the following. The description of the above embodiments is only used to help understand the present invention. The method of the invention and its core idea; at the same time, for the person of ordinary skill in the art, according to the idea of the present invention, there are some changes in the specific embodiment and the scope of application. In summary, the content of the specification should not be understood. To limit the invention.

Claims

A pan/tilt posture correction method, characterized in that a pan/tilt head is connected to a vertical compensation device, the vertical compensation device compensates movement of the pan/tilt head in a vertical direction, and the vertical compensation device is provided with a vision a module and an inertial measurement unit, the method comprising:

Acquiring the first pose of the pan/tilt based on the inertial measurement unit;

Acquiring a second pose of the vertical compensation device based on the vision module;

The first pose is corrected according to the second pose.
The method according to claim 1, wherein said vertical compensating means comprises a body and a shaft arm for connecting said pan/tilt, said arm arm rotating to compensate said pan/tilt in a vertical direction mobile;

The inertial measurement unit is disposed on the axle arm, and the vision module is disposed on the body.
The method of claim 1 wherein said vision module comprises a visual odometer, said second pose comprising a speed and a position of said vertical compensation device.
The method of claim 1 wherein said vision module comprises a visual inertia odometer, said second pose comprising a speed, a position and a pose of said vertical compensation device.
The method according to claim 3 or 4, wherein said vertical compensating means comprises a shaft arm for connecting said pan/tilt, said arm arm rotating to compensate for movement of said pan/tilt head in a vertical direction An angular velocity sensor is mounted on the axle arm;

Before the obtaining, by the vision module, the second pose of the vertical compensation device, the method includes:

Based on the angular velocity sensor, the joint angle of the axle arm is obtained.
The method of claim 5 wherein said first pose comprises a speed of said pan/tilt;

The acquiring the second pose of the vertical compensation device based on the vision module includes:

And performing coordinate conversion on a reference speed of the vertical compensation device output by the vision module according to the joint angle to obtain a speed of the vertical compensation device.
The method according to claim 5, wherein said first pose comprises a position of said pan/tilt;

The acquiring the second pose of the vertical compensation device based on the vision module includes:

And performing coordinate conversion on a reference position of the vertical compensation device output by the vision module according to the joint angle to obtain a position of the vertical compensation device.
The method according to claim 4, wherein the acquiring the second pose of the vertical compensation device based on the vision module comprises:

Constructing a reference direction cosine matrix of the reference pose based on a reference pose output by the visual inertia odometer;

A posture of the vertical compensation device is obtained according to the reference direction cosine matrix.
The method according to claim 8, wherein the obtaining the posture of the vertical compensation device according to the direction cosine matrix comprises:

Obtaining an attitude correction value of the vertical compensation device according to the reference direction cosine matrix;

Based on the attitude correction value, the attitude of the vertical compensation device is obtained.
The method of claim 1 wherein said first pose comprises a speed, a position and a pose of said pan/tilt.
The method of claim 10 wherein said inertial measurement unit comprises a gyroscope and an accelerometer;

The acquiring the first pose of the pan/tilt based on the inertial measurement unit comprises:

Obtaining an angular velocity of the pan/tilt based on the gyroscope;

Obtaining a specific force of the pan/tilt based on the accelerometer;

The attitude, speed, and position of the pan/tilt are calculated based on the angular velocity and the specific force.
The method according to claim 11, wherein the calculating the attitude, speed and position of the pan/tilt according to the angular velocity comprises:

Designing a posture update formula according to the angular velocity and the specific force;

The posture of the pan/tilt is updated according to the gesture update formula.
The method according to claim 11, wherein the calculating the attitude, speed and position of the gimbal according to the angular velocity and the specific force comprises:

Designing a speed update formula according to the angular velocity and the specific force;

The speed of the pan/tilt is updated according to the speed update formula.
The method according to claim 11, wherein the calculating the attitude, speed and position of the gimbal according to the angular velocity and the specific force comprises:

Designing a position update formula according to the angular velocity and the specific force;

The location of the pan/tilt is updated according to the location update formula.
The method of claim 10 wherein said inertial measurement unit comprises a gyroscope and an accelerometer;

The acquiring the first pose of the pan/tilt based on the inertial measurement unit comprises:

Obtaining an angular velocity of the pan/tilt based on the gyroscope;

Obtaining a specific force of the pan/tilt based on the accelerometer;

An error of the first pose is calculated based on the angular velocity and the specific force.
The method according to claim 15, wherein said calculating an error of said first pose based on said angular velocity and said specific force comprises:

Constructing an attitude error, a velocity error, and a position error of the first pose according to the angular velocity and the specific force;

An error of the first pose is calculated based on the attitude error, the velocity error, and the position error.
The method according to claim 15, wherein the correcting the first pose according to the second pose comprises:

Approximating the error of the first pose to obtain a Kalman filter;

Using the second pose as an observation value, obtaining a correction value via the Kalman filter wave;

The first pose is corrected based on the correction value.
A pan/tilt posture correction device, comprising: a vertical compensation device connected to the pan/tilt head, a vision module mounted on the vertical compensation device, and an inertia mounted on the vertical compensation device a measuring unit, the vertical compensating device is configured to compensate a movement of the gimbal in a vertical direction, and the visual module and the inertial measuring unit are electrically connected to the vertical compensating device;

The vertical compensation device is configured to acquire a first pose of the pan/tilt based on the inertial measurement unit; acquire a second pose of the vertical compensation device based on the vision module; and according to the second Position, correcting the first pose.
The apparatus according to claim 18, wherein said vertical compensating means comprises a body and a shaft arm for connecting said pan/tilt, said arm arm rotating to compensate said pan/tilt in a vertical direction mobile;

The inertial measurement unit is disposed on the axle arm, and the vision module is disposed on the body.
The device of claim 18 wherein said vision module comprises a visual odometer, said second pose comprising a speed and a position of said vertical compensation device.
The apparatus of claim 18 wherein said vision module comprises a visual inertia odometer, said second pose comprising a speed, a position and a pose of said vertical compensation means.
The apparatus according to claim 20 or 21, wherein said vertical compensating means comprises a shaft arm for connecting said pan/tilt, said arm arm rotating to compensate for movement of said gimbal in a vertical direction An angular velocity sensor is mounted on the axle arm;

The vertical compensation device is configured to obtain a joint angle of the axle arm based on the angular velocity sensor.
The apparatus according to claim 22, wherein said first pose comprises a speed of said pan/tilt;

The vertical compensation device is configured to perform coordinate conversion on a reference speed of the vertical compensation device output by the vision module according to the joint angle to obtain a speed of the vertical compensation device.
The apparatus according to claim 22, wherein said first pose comprises a position of said pan/tilt;

The vertical compensation device is configured to perform coordinate conversion on a reference position of the vertical compensation device output by the vision module according to the joint angle to obtain a position of the vertical compensation device.
The apparatus according to claim 21, wherein said vertical compensation means is configured to construct a reference direction cosine matrix of said reference pose based on a reference pose output by said visual inertia odometer;

A posture of the vertical compensation device is obtained according to the reference direction cosine matrix.
The apparatus according to claim 25, wherein said vertical compensation means is configured to obtain an attitude correction value of said vertical compensation means based on said reference direction cosine matrix;

Based on the attitude correction value, the attitude of the vertical compensation device is obtained.
The apparatus of claim 18 wherein said first pose comprises a speed, a position and a pose of said head.
The apparatus according to claim 27, wherein said inertial measurement unit comprises a gyroscope and an accelerometer;

The vertical compensation device is configured to acquire an angular velocity of the pan/tilt based on the gyroscope;

Obtaining a specific force of the pan/tilt based on the accelerometer;

The attitude, speed, and position of the pan/tilt are calculated based on the angular velocity and the specific force.
The apparatus according to claim 28, wherein said vertical compensation means is configured to design a posture update formula based on said angular velocity and said specific force;

The posture of the pan/tilt is updated according to the gesture update formula.
The apparatus according to claim 28, wherein said vertical compensating means is configured to design a speed update formula based on said angular velocity and said specific force;

The speed of the pan/tilt is updated according to the speed update formula.
The apparatus according to claim 28, wherein said vertical compensating means is configured to design a position update formula based on said angular velocity and said specific force;

The location of the pan/tilt is updated according to the location update formula.
The apparatus according to claim 27, wherein said inertial measurement unit comprises a gyroscope and an accelerometer;

The vertical compensation device is configured to acquire an angular velocity of the pan/tilt based on the gyroscope;

Obtaining a specific force of the pan/tilt based on the accelerometer;

An error of the first pose is calculated based on the angular velocity and the specific force.
The apparatus according to claim 32, wherein said vertical compensating means is configured to construct an attitude error, a velocity error and a position error of said first pose according to said angular velocity and said specific force;

An error of the first pose is calculated based on the attitude error, the velocity error, and the position error.
The apparatus according to claim 32, wherein said vertical compensating means is configured to approximate the error of said first pose to obtain a Kalman filter;

Using the second pose as an observation value, obtaining a correction value via the Kalman filter wave;

The first pose is corrected based on the correction value.