CN111556309A - Control method of binocular tripod head with variable base line - Google Patents

Control method of binocular tripod head with variable base line Download PDF

Info

Publication number
CN111556309A
CN111556309A CN202010257293.7A CN202010257293A CN111556309A CN 111556309 A CN111556309 A CN 111556309A CN 202010257293 A CN202010257293 A CN 202010257293A CN 111556309 A CN111556309 A CN 111556309A
Authority
CN
China
Prior art keywords
binocular
coordinate system
head
angle
camera
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010257293.7A
Other languages
Chinese (zh)
Other versions
CN111556309B (en
Inventor
纪刚
柴琦
蒋云飞
朱慧
杨丰拓
安帅
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Qingdao Lianhe Chuangzhi Technology Co ltd
Original Assignee
Qingdao Lianhe Chuangzhi Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Qingdao Lianhe Chuangzhi Technology Co ltd filed Critical Qingdao Lianhe Chuangzhi Technology Co ltd
Priority to CN202010257293.7A priority Critical patent/CN111556309B/en
Publication of CN111556309A publication Critical patent/CN111556309A/en
Application granted granted Critical
Publication of CN111556309B publication Critical patent/CN111556309B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04N—PICTORIAL COMMUNICATION, e.g. TELEVISION
    • H04N13/00—Stereoscopic video systems; Multi-view video systems; Details thereof
    • H04N13/20—Image signal generators
    • H04N13/296—Synchronisation thereof; Control thereof
    • F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
    • F16—ENGINEERING ELEMENTS AND UNITS; GENERAL MEASURES FOR PRODUCING AND MAINTAINING EFFECTIVE FUNCTIONING OF MACHINES OR INSTALLATIONS; THERMAL INSULATION IN GENERAL
    • F16M—FRAMES, CASINGS OR BEDS OF ENGINES, MACHINES OR APPARATUS, NOT SPECIFIC TO ENGINES, MACHINES OR APPARATUS PROVIDED FOR ELSEWHERE; STANDS; SUPPORTS
    • F16M11/00—Stands or trestles as supports for apparatus or articles placed thereon ; Stands for scientific apparatus such as gravitational force meters
    • F16M11/02—Heads
    • F16M11/04—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand
    • F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
    • F16—ENGINEERING ELEMENTS AND UNITS; GENERAL MEASURES FOR PRODUCING AND MAINTAINING EFFECTIVE FUNCTIONING OF MACHINES OR INSTALLATIONS; THERMAL INSULATION IN GENERAL
    • F16M—FRAMES, CASINGS OR BEDS OF ENGINES, MACHINES OR APPARATUS, NOT SPECIFIC TO ENGINES, MACHINES OR APPARATUS PROVIDED FOR ELSEWHERE; STANDS; SUPPORTS
    • F16M11/00—Stands or trestles as supports for apparatus or articles placed thereon ; Stands for scientific apparatus such as gravitational force meters
    • F16M11/02—Heads
    • F16M11/04—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand
    • F16M11/06—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand allowing pivoting
    • F16M11/12—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand allowing pivoting in more than one direction
    • F16M11/121—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand allowing pivoting in more than one direction constituted of several dependent joints
    • F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
    • F16—ENGINEERING ELEMENTS AND UNITS; GENERAL MEASURES FOR PRODUCING AND MAINTAINING EFFECTIVE FUNCTIONING OF MACHINES OR INSTALLATIONS; THERMAL INSULATION IN GENERAL
    • F16M—FRAMES, CASINGS OR BEDS OF ENGINES, MACHINES OR APPARATUS, NOT SPECIFIC TO ENGINES, MACHINES OR APPARATUS PROVIDED FOR ELSEWHERE; STANDS; SUPPORTS
    • F16M11/00—Stands or trestles as supports for apparatus or articles placed thereon ; Stands for scientific apparatus such as gravitational force meters
    • F16M11/02—Heads
    • F16M11/18—Heads with mechanism for moving the apparatus relatively to the stand
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/292—Multi-camera tracking
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04N—PICTORIAL COMMUNICATION, e.g. TELEVISION
    • H04N13/00—Stereoscopic video systems; Multi-view video systems; Details thereof
    • H04N13/20—Image signal generators
    • H04N13/204—Image signal generators using stereoscopic image cameras
    • H04N13/239—Image signal generators using stereoscopic image cameras using two 2D image sensors having a relative position equal to or related to the interocular distance

Landscapes

  • Engineering & Computer Science (AREA)
  • General Engineering & Computer Science (AREA)
  • Mechanical Engineering (AREA)
  • Multimedia (AREA)
  • Signal Processing (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Control Of Position Or Direction (AREA)

Abstract

The invention discloses a control method of a binocular tripod head with a variable baseline, which comprises the following steps: acquiring the posture of a machine body where a binocular pan-tilt is located; carrying out image comparison to obtain the pixel offset of a target point, thereby obtaining the absolute adjustment angle of the binocular head in the pitching and horizontal rotating directions; solving and obtaining the adjustment quantity of the binocular head by forward and inverse kinematics; establishing a system equation of motion with respect to a baseline; and driving the binocular head to adjust the attitude through fuzzy self-adaptive PID control. According to the control method disclosed by the invention, when the base line changes, the course angle and the pitch angle of the binocular head can be quickly and accurately adjusted through the fuzzy self-adaptive PID control method, and the accurate tracking of the moving target is realized.

Description

Control method of binocular tripod head with variable base line
Technical Field
The invention relates to a control method of a binocular tripod head with a variable baseline.
Background
The binocular head can obtain three-dimensional information from two plane images by using a parallax principle to construct a three-dimensional view. In order to construct a complete three-dimensional map, the binocular head is often fixed on a moving body, and the movement of the binocular head is adjusted according to the movement of the body, so that the moving target is followed in the moving process.
However, the existing binocular head generally adopts a mode of fixing a base line, the distance between two cameras is fixed, the imaging mode has a great defect, and the generated stereo image can obtain a clear stereo effect only in a partial area.
At present, the binocular camera with the variable base line can solve the problem that only a local area image is clear, and the base line length is adjusted, so that a shooting object in a large distance range can show a good three-dimensional effect.
However, as the baseline changes, the distance between the two cameras changes, and the rotational inertia of the binocular head also changes, and for the conventional PID control method, the method for setting the PID parameters at one time cannot be well adapted to the situation that the system motion parameters change, the dynamic characteristics are not ideal, the motion of the head may be unstable, and the head cannot be adjusted in place quickly, so that the quality of the obtained image is poor, or a clear and correct three-dimensional image cannot be obtained quickly.
Disclosure of Invention
In order to solve the technical problem, the invention provides a control method of a binocular tripod head with a variable base line, so as to achieve the purpose of ensuring the accuracy and rapidity of the movement of the binocular tripod head when the length of the base line is changed.
In order to achieve the purpose, the technical scheme of the invention is as follows:
a control method of a binocular tripod head with a variable baseline comprises the following steps:
acquiring the posture of a machine body where a binocular pan-tilt head is located;
step two, carrying out image comparison to obtain the pixel offset of a target point, thereby obtaining the absolute adjustment angle of the binocular tripod head in the pitching and horizontal rotating directions;
step three, solving and obtaining the adjustment quantity of the binocular tripod head by forward and reverse kinematics;
step four, establishing a system motion equation related to the base line;
and step five, driving the binocular tripod head to adjust the posture through fuzzy self-adaptive PID control.
In the scheme, the first specific method comprises the following steps:
(1) the binocular head is arranged on a movable body, and the quaternion Q ═ Q j k of the current posture of the body is obtained through a sensor]TThe quaternion consists of a real number q and three imaginary numbers i, j, k, which respectively represent the rotation around three axes;
(2) obtaining a rotation matrix of the body according to the posture quaternion of the body
Figure BDA0002437853620000021
Will rotate the matrix RdIs shown as
Figure BDA0002437853620000022
(3) Obtaining an Euler angle omega of the current machine body according to the rotation matrix to obtain a controlled attitude;
the Euler angle of the current state of the body is
Figure BDA0002437853620000023
Figure BDA0002437853620000024
Is the body heading angle, βdAt the pitch angle of the body, αdRoll for machine bodyAn angle;
Figure BDA0002437853620000025
Figure BDA0002437853620000026
Figure BDA0002437853620000027
in the scheme, the second step is as follows:
identifying the pixel position of the target in the camera image by a target identification algorithm, monitoring the pixel position change of the target point, generating no binocular head motion instruction when the target point is in the camera tracking frame, and obtaining the absolute adjustment angles delta β and delta v of the binocular head in the pitching and horizontal rotation directions respectively according to the pixel difference (delta u, delta v) between the pixel position of the target point and the pixel position of the picture frame central point when the target point exceeds the camera tracking frame
Figure BDA0002437853620000028
Figure BDA0002437853620000029
Figure BDA00024378536200000210
Where f is the focal length of the camera.
In the scheme, the third specific method comprises the following steps:
(1) coordinate x to world coordinate systemw，yw，zwTransforming to obtain coordinate x of body coordinate systemd，yd，zd；
(2) Coordinate x of body coordinate systemd，yd，zdTransforming to obtain coordinate x of camera coordinate systemc，yc，zc；
(3) Coordinate x of camera coordinate systemc，yc，zcTransforming to obtain coordinates x and y of a physical coordinate system of the image;
(4) transforming the coordinates x, y of the image physical coordinate system to obtain the coordinates u, v of the image pixel coordinate system, and thus obtaining the coordinate x of the world coordinate systemw，yw，zwA positive kinematic transformation formula to the coordinates u, v of the image pixel coordinate system;
(5) inverting two ends of the positive kinematics transformation formula to obtain coordinates u, v of the image pixel coordinate system to coordinates x of the world coordinate systemw，yw，zwThe inverse kinematics transformation formula of (a);
(6) the pixel difference (delta u, delta v) of the pixel position of the target point and the pixel position between the center points of the frames and the absolute adjusting angles delta β and delta v of the binocular head in the pitch and horizontal rotating directions respectively
Figure BDA0002437853620000031
Substituting the inverse kinematics transformation formula to obtain the angle change values of the relative pitch angle and the course angle of the binocular pan-tilt head of β'c，
Figure BDA0002437853620000032
Thereby the target returns to the center of the camera frame and the tracking is finished.
In the scheme, the third specific method comprises the following steps:
(1) coordinate x to world coordinate systemw，yw，zwThe transformation is formulated as follows:
Figure BDA0002437853620000033
wherein the content of the first and second substances,
Figure BDA0002437853620000034
Figure BDA0002437853620000035
Figure BDA0002437853620000036
the coordinate system of the machine body is a rectangular coordinate system ZdThe shaft is used as a transverse rolling shaft of the machine body and represents the transverse rolling motion of the machine body; xdThe axis is used as a pitching axis of the machine body and represents pitching motion of the machine body; y isdThe axis is used as a heading axis of the machine body and represents the horizontal rotation motion of the machine body; rdIs a rotation matrix from the world coordinate system to the body coordinate system, tdRepresenting translation, tx, of the world coordinate system to the body coordinate systemd，tyd，tzdRespectively representing the origin of the coordinate system of the body in x relative to the origin of the world coordinate systemw，yw，zwThe amount of translation in the direction of the axis,
Figure BDA0002437853620000041
βd，θdthe heading angle, the pitch angle and the roll angle of the machine body are represented and obtained by a displacement sensor arranged on the machine body;
(2) coordinate x of body coordinate systemd，yd，zdThe transformation is formulated as follows:
Figure BDA0002437853620000042
wherein the content of the first and second substances,
Figure BDA0002437853620000043
Figure BDA0002437853620000044
Rcfor a body coordinate system to camera coordinate system rotation matrix, tcRepresenting translation of the body coordinate system to the camera coordinate system, txc，tyc，tzcRespectively representing the origin of the camera coordinate systemThe origin of the coordinate system of the body is xd，yd，zdThe translation amount in the direction is obtained from the relative installation position of the binocular head on the machine body;
Figure BDA0002437853620000045
βc，θcrespectively representing a course angle, a pitch angle and a roll angle of the camera, and obtaining the course angle, the pitch angle and the roll angle through an angle sensor; here, since the binocular head cannot be turned over, the roll angle θ with respect to the machine bodyc＝0；
(3) Coordinate x of camera coordinate systemc，yc，zcThe transformation is formulated as follows:
Figure BDA0002437853620000046
wherein z iscIs the vertical distance from the target to the camera, and f is the focal length of the camera;
(4) the formula for transforming the coordinates x, y of the physical coordinate system of the image is as follows:
Figure BDA0002437853620000051
wherein the content of the first and second substances,
Figure BDA0002437853620000052
dx and dy are the physical sizes of image pixel points on x and y axes of an image physical coordinate system respectively, and gamma is a distortion factor of the camera;
the coordinates x from the world coordinate system are obtained by integrating the equations (8), (9), (10) and (11)w，yw，zwPositive kinematic transformation formula to coordinates u, v of the image pixel coordinate system:
Figure BDA0002437853620000053
inverting both ends of the formula (12) to obtain coordinates u, v of the image pixel coordinate system to coordinate x of the world coordinate systemw，yw，zwThe inverse kinematics variant of (a):
Figure BDA0002437853620000054
(5) the pixel difference (delta u, delta v) between the pixel position of the target point and the pixel position between the center points of the frames and the absolute adjustment angles obtained by the formulas (6) and (7)
Figure BDA0002437853620000055
When Δ β is substituted into the equation (13), a system of equations can be obtained
Figure BDA0002437853620000056
Figure BDA0002437853620000057
By solving the system of equations (14), a matrix is obtained
Figure BDA0002437853620000061
Thus, when the translation amount of the camera pixel coordinate system is delta u, delta v, the angle change amount corresponding to the relative course angle and the pitch angle of the binocular head is obtained as
Figure BDA0002437853620000062
β′cThe target can be returned to the center of the camera frame to complete the tracking.
In the scheme, the system motion equation is established in the fourth step as follows:
Figure BDA0002437853620000063
wherein M is1+ mL is the moment of inertia of the binocular pan/tilt for horizontal rotation, M is the mass of a single camera, L is the length of the base line of the binocular pan/tilt, M1For removing the horizontal moment of inertia of the binocular head behind the camera, M2The moment of inertia of pitching motion of the binocular head and the camera; c1，C2Equivalent damping, Q, when changing course and pitch, respectively, for a binocular pan-tilt1，Q2Respectively changing course and pitching equivalent elastic coefficients for the binocular head;
Figure BDA0002437853620000064
βcrespectively a course angle and a pitch angle of the binocular pan-tilt,
Figure BDA0002437853620000065
respectively a course angular velocity and a pitch angular velocity of the binocular head,
Figure BDA0002437853620000066
respectively acquiring course angular acceleration and pitch angular acceleration of the binocular pan-tilt; t is1，T2Two input moments for driving the binocular head to change course and pitch respectively.
In the scheme, the concrete method of the step five is as follows:
(1) comparing the calculated angle change quantity of the relative course angle and the pitch angle of the binocular head with the actual change quantity of the binocular head in the course angle and the pitch angle obtained by the sensor to obtain a difference value between the two, and solving the change rate of the difference value along with time;
(2) inputting the difference value and the change rate of the difference value into a fuzzy adaptive parameter PID controller, performing fuzzification processing on input quantity by the controller, inquiring a fuzzy matrix table to perform parameter adjustment, then performing deblurring processing on output quantity, and generating the optimal control quantity for the rotation of the motor when the length of a basic line of a binocular head is changed;
(3) the optimal control quantity is amplified and converted by the driver to generate a driving signal, so that the motor is driven to rotate, and the binocular head reaches a preset course angle and a preset pitch angle.
In the above scheme, the calculation formula of the optimal control quantity d (t) of the motor rotation is as follows:
Figure BDA0002437853620000067
wherein, KpIs a proportionality coefficient, KiIs the integral coefficient, KdIs a differential coefficient;
dynamically adjusting K in a fuzzy adaptive PID control processp,Ki，KdThe value of (d) yields the optimum control quantity d (t) for the motor rotation when the length L of the binocular head baseline is changed.
Through the technical scheme, the control method of the binocular tripod head with the variable base line, provided by the invention, introduces a fuzzy self-adaptive PID control algorithm to drive the movement of the tripod head mechanism, and is favorable for improving the accuracy and the rapidity of the movement of the binocular tripod head under the condition that the movement parameters of the system are changed.
The invention realizes the tracking of the binocular cloud platform on the moving target by combining the target tracking algorithm and the self-adaptive control algorithm.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below.
Fig. 1 is a schematic flow chart of a control method of a binocular tripod head with a variable baseline according to an embodiment of the present invention;
FIG. 2 is a schematic view of target tracking of a binocular head;
FIG. 3 is a schematic view of a binocular head structure;
FIG. 4 is a schematic diagram of a binocular pan-tilt fuzzy PID control process;
FIG. 5 is a flow chart illustrating a fuzzy PID parameter generation process;
FIG. 6 is a graph of membership functions;
FIG. 7 is a graph of the behavior of conventional PID control and fuzzy PID control under certain parameters;
fig. 8 is a graph showing the behavior of the conventional PID control and the fuzzy PID control when the parameter is changed.
In the figure, A, an object; B. a first camera; C. a second camera; D. a conventional PID control curve; E. fuzzy adaptive PID control curves; F. a target point; G. a camera tracking frame; H. a translation stage; I. a pitch mechanism; J. and a horizontal rotating mechanism.
Detailed Description
The technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention.
The invention provides a control method of a binocular tripod head with a variable baseline, as shown in fig. 1, the specific implementation mode is as follows:
acquiring the posture of a machine body where a binocular pan-tilt head is located;
(1) the binocular head is arranged on a movable body, and the quaternion Q ═ Q j k of the current posture of the body is obtained through a sensor]TThe quaternion consists of a real number q and three imaginary numbers i, j, k, which respectively represent the rotation around three axes;
(2) obtaining a rotation matrix of the body according to the posture quaternion of the body
Figure BDA0002437853620000081
Will rotate the matrix RdIs shown as
Figure BDA0002437853620000082
(3) Obtaining an Euler angle omega of the current machine body according to the rotation matrix to obtain a controlled attitude;
the Euler angle of the current state of the body is
Figure BDA0002437853620000083
Figure BDA0002437853620000084
Is the body heading angle, βdAt the pitch angle of the body, αdThe transverse roll angle of the machine body is set;
Figure BDA0002437853620000085
Figure BDA0002437853620000086
Figure BDA0002437853620000087
step two, carrying out image comparison to obtain the pixel offset of a target point, thereby obtaining the absolute adjustment angle of the binocular tripod head in the pitching and horizontal rotating directions;
identifying the pixel position of the target in the camera image by a target identification algorithm, monitoring the pixel position change of the target point, as shown in fig. 2, when the target point F is in the camera tracking frame G, generating no binocular head motion instruction, and when the target point exceeds the camera tracking frame, obtaining the absolute adjustment angles delta β and delta v of the binocular head in the pitch and horizontal rotation directions respectively according to the pixel difference (delta u, delta v) between the pixel position of the target point and the pixel position of the frame central point
Figure BDA0002437853620000088
Figure BDA0002437853620000089
Figure BDA00024378536200000810
Where f is the focal length of the camera.
Step three, solving and obtaining the adjustment quantity of the binocular tripod head by forward and reverse kinematics;
(1) coordinate x to world coordinate systemw，yw，zwTransforming to obtain coordinate x of body coordinate systemd，yd，zd；
The transformation formula is as follows:
Figure BDA0002437853620000091
wherein the content of the first and second substances,
Figure BDA0002437853620000092
Figure BDA0002437853620000093
Figure BDA0002437853620000094
the coordinate system of the machine body is a rectangular coordinate system ZdThe shaft is used as a transverse rolling shaft of the machine body and represents the transverse rolling motion of the machine body; xdThe axis is used as a pitching axis of the machine body and represents pitching motion of the machine body; y isdThe axis is used as a heading axis of the machine body and represents the horizontal rotation motion of the machine body; rdIs a rotation matrix from the world coordinate system to the body coordinate system, tdRepresenting translation of the world coordinate system to the body coordinate system, txd，tyd，tzdRespectively representing the origin of the coordinate system of the body in x relative to the origin of the world coordinate systemw，yw，zwThe amount of translation in the direction of the axis,
Figure BDA0002437853620000095
βd，θdthe heading angle, the pitch angle and the roll angle of the machine body are represented and obtained by a displacement sensor arranged on the machine body;
(2) coordinate x of body coordinate systemd，yd，zdTransforming to obtain coordinate x of camera coordinate systemc，yc，zc；
The transformation formula is as follows:
Figure BDA0002437853620000096
wherein the content of the first and second substances,
Figure BDA0002437853620000101
Figure BDA0002437853620000102
Rcfor a body coordinate system to camera coordinate system rotation matrix, tcRepresenting translation of the body coordinate system to the camera coordinate system, txc，tyc，tzcRespectively representing the origin of the camera coordinate system in x relative to the origin of the body coordinate systemd，yd，zdThe translation amount in the direction is obtained from the relative installation position of the binocular head on the machine body;
Figure BDA0002437853620000103
βc，θcrespectively representing a course angle, a pitch angle and a roll angle of the camera, and obtaining the course angle, the pitch angle and the roll angle through an angle sensor; here, since the binocular head cannot be turned over, the roll angle θ with respect to the machine bodyc＝0；
(3) Coordinate x of camera coordinate systemc，yc，zcTransforming to obtain coordinates x and y of a physical coordinate system of the image;
the transformation formula is as follows:
Figure BDA0002437853620000104
wherein, acIs the vertical distance from the target to the camera, and f is the focal length of the camera;
(4) transforming the coordinates x, y of the image physical coordinate system to obtain the coordinates u, v of the image pixel coordinate system, and thus obtaining the coordinate x of the world coordinate systemw，yw，zwA positive kinematic transformation formula to the coordinates u, v of the image pixel coordinate system;
the transformation formula is as follows:
Figure BDA0002437853620000105
wherein the content of the first and second substances,
Figure BDA0002437853620000106
dx and dy are the physical sizes of image pixel points on x and y axes of an image physical coordinate system respectively, and gamma is a distortion factor of the camera;
the coordinates x from the world coordinate system are obtained by integrating the equations (8), (9), (10) and (11)w，yw，zwPositive kinematic transformation formula to coordinates u, v of the image pixel coordinate system:
Figure BDA0002437853620000107
Figure BDA0002437853620000111
(5) inverting both ends of the formula (12) to obtain coordinates u, v of the image pixel coordinate system to coordinate x of the world coordinate systemw，yw，zwThe inverse kinematics variant of (a):
Figure BDA0002437853620000112
(6) the pixel difference (delta u, delta v) between the pixel position of the target point and the pixel position between the center points of the frames and the absolute adjustment angles obtained by the formulas (6) and (7)
Figure BDA0002437853620000113
When Δ β is substituted into the equation (13), a system of equations can be obtained
Figure BDA0002437853620000114
Figure BDA0002437853620000115
By solving the system of equations (14), a matrix is obtained
Figure BDA0002437853620000116
Thus, when the translation amount of the camera pixel coordinate system is delta u, delta v, the angle change amount corresponding to the relative course angle and the pitch angle of the binocular head is obtained as
Figure BDA0002437853620000117
β′cThe target can be returned to the center of the camera frame to complete the tracking.
Step four, establishing a system motion equation related to the base line;
the structure of the binocular head is shown in fig. 3, the first camera A and the second camera B are located on a translation platform H, the translation platform H is located on a pitching mechanism I, and the pitching mechanism I is located on a horizontal rotating mechanism J. A lead screw is arranged on the translation platform H to control the length of a base line between the two cameras, and the pitching mechanism I controls pitching motion through a pitching motor and a worm gear, namely, the change amount of a pitching angle is adjusted; the horizontal rotation mechanism J controls horizontal rotation movement, namely, adjusts the change amount of the heading angle through a horizontal rotation motor and a worm gear. The specific structure can be seen in the patent number "2020101641874" previously filed by the applicant, which is named as "a camera rotating pan-tilt head with freely adjustable angle".
The system equation of motion is as follows:
Figure BDA0002437853620000121
wherein M is1+ mL is the moment of inertia of the binocular pan/tilt for horizontal rotation, M is the mass of a single camera, L is the length of the base line of the binocular pan/tilt, M1For removing the horizontal moment of inertia of the binocular head behind the camera, M2The moment of inertia of pitching motion of the binocular head and the camera; c1，C2Equivalent damping, Q, when changing course and pitch, respectively, for a binocular pan-tilt1，Q2Respectively changing course and pitching equivalent elastic coefficients for the binocular head;
Figure BDA0002437853620000122
βcrespectively a course angle and a pitch angle of the binocular pan-tilt,
Figure BDA0002437853620000123
respectively a course angular velocity and a pitch angular velocity of the binocular head,
Figure BDA0002437853620000124
respectively acquiring course angular acceleration and pitch angular acceleration of the binocular pan-tilt; t is1，T2Two input moments for driving the binocular head to change course and pitch respectively.
And step five, driving the binocular head to adjust the attitude through fuzzy self-adaptive PID control, as shown in FIG. 4.
(1) Comparing the calculated angle change quantity of the relative course angle and the pitch angle of the binocular head with the actual change quantity of the binocular head in the course angle and the pitch angle obtained by the sensor to obtain a difference value between the two, and solving the change rate of the difference value along with time;
(2) inputting the difference value and the change rate of the difference value into a fuzzy adaptive parameter PID controller, carrying out fuzzification processing on input quantity by the controller, inquiring a fuzzy matrix table to carry out parameter adjustment, then carrying out deblurring processing on output quantity, and generating optimal control quantity for rotating a pitching motor and a horizontal rotating motor when the length of a basic line of a binocular head is changed;
the optimal control amount d (t) of the motor rotation is calculated as follows:
Figure BDA0002437853620000125
wherein, KpIs a proportionality coefficient, KiIs the integral coefficient, KdIs a differential coefficient;
is a constant in the conventional PID control, and dynamically adjusts K in the fuzzy adaptive PID control process of the inventionp，Ki，KdThe value of (d) yields the optimum control quantity d (t) for the motor rotation when the length L of the binocular head baseline is changed.
(3) The optimal control quantity is amplified and converted by a driver to generate a driving signal, so that the pitching motor and the horizontal rotating motor are driven to rotate, and the binocular head reaches a preset course angle and a preset pitching angle.
Adaptive fuzzy PID control to pan-tilt course angle and pitch angle
Figure BDA0002437853620000126
βcIs the same, in the following course angle
Figure BDA0002437853620000127
The description is given for the sake of example.
When the control system obtains the given change value of the binocular head on the course angle transmitted by the instruction
Figure BDA0002437853620000131
And obtaining the actual change value of the current holder on the course angle through a sensor
Figure BDA0002437853620000132
The difference between the two is denoted as E, and the rate of change of the difference E with time is
Figure BDA0002437853620000133
The fuzzy adaptive parameter PID controller obtains the difference value E and the change rate of the difference value EcAnd generating a control quantity D (t) for the rotation of the motor, amplifying and converting the control quantity D (t) through a driver to generate a driving signal, so that the motor is driven to rotate, and the tripod head can quickly and accurately reach a preset course angle.
Using a blurring algorithm first requires a blurring process on the data, as shown in fig. 5.
1. And fuzzifying the input values E and Ec of the fuzzy controller.
(1) Firstly, fuzzy subsets of E and Ec are determined, and seven linguistic variables of NBNMNSZOPBPMPS are set to respectively represent negative big (negative big), negative middle (negative middle), negative small (negative small), zero (zero), positive big (positive big), positive middle (positive middle) and positive small (positive small). Then the fuzzy subsets defining E and Ec are each { NB, NM, NS, ZO, PB, PM, PS }.
(2) Introduction of discourse domain and K corresponding to E and Ecp，Ki，KdThe domains of (a) are all defined as { -6, -5, -4, -3, -2, -1, 0, 1, 2, 3, 4, 5, 6 }.
(3) Introducing a quantization function for a given change value
Figure BDA0002437853620000134
And actual change value
Figure BDA0002437853620000135
All have a maximum value, which we denote
Figure BDA0002437853620000136
So that the range of the deviation E is
Figure BDA0002437853620000137
The range of the rate of change Ec of the deviation with time is also set as
Figure BDA0002437853620000138
The deviation E and the deviation change rate Ec can be converted into [ -6, 6 ] through a quantization function]The corresponding value of (1). Here, the quantization is performed in a linear manner, and the quantization function conversion relationship is as follows:
Figure BDA0002437853620000139
Figure BDA00024378536200001310
(4) and determining the membership of the E and the Ec on the fuzzy subset. Membership is a value between 0 and 1 to describe the degree to which an input belongs to a fuzzy entity. Linear membership functions are used, the functional relationship of which is shown in fig. 6.
After the membership value is determined, the deviation E and the deviation change rate Ec realize the conversion from the original value to the fuzzy value represented by the fuzzy quantity.
2. And after the input data is fuzzified, establishing a fuzzy rule base. Obtaining K from fuzzy rule basep，Ki，KdIs subject toAnd (4) measuring values.
Generation of K using fuzzy rules governing industry maturationp，Ki，KdFuzzy rule table, see table 1, table 2 and table 3.
TABLE 1 fuzzy rule table of proportionality coefficients
Figure BDA0002437853620000141
TABLE 2 fuzzy control table of integral coefficient
Figure BDA0002437853620000142
TABLE 3 differential coefficient fuzzy control table
Figure BDA0002437853620000143
Obtaining two fuzzy quantities of the fuzzified error and the error change rate, and inquiring the corresponding fuzzy rule table to obtain the current fuzzified Kp，Ki，KdThree values.
3. It is also necessary to perform deblurring processing on the found target object so as to correspond to a specific physical quantity.
The input and output quantities of the previous fuzzy adaptation all adopt the same domain of discourse, namely { -6, -5, -4, -3, -2, -1, 0, 1, 2, 3, 4, 5, 6}, so that the fuzzy variable membership at the input and output at a certain moment is the same. Based on this basis, the quantized value of each output quantity is calculated by using the center-of-gravity method.
Figure BDA0002437853620000151
Wherein k is the accurate value of the output quantity of the fuzzy controller after the fuzzy is solved, kiIs a value in the fuzzy control theoretic domain, mu (k)i) Is kiA membership value of.
According to the membership assignment table of each fuzzy subset and each parameter fuzzy control model, a fuzzy matrix table of fractional order PID parameters is designed by fuzzy synthetic reasoning, and the calculated parameters are substituted into the following formula for calculation:
Figure BDA0002437853620000152
of formula (II) K'p，K′i，K′dAdaptive adjusted value for blur, Kp，Ki，KdIs an initial value of a PID parameter, is obtained by a conventional PID tuning method, and is delta Kp，ΔKi，ΔKdThree outputs of the fuzzy controller.
4. And carrying out fuzzy PID simulation on the fuzzy control part.
A comparative simulation model of a fuzzy self-adaptive PID control system and a traditional PID control system is set up in the simulink, parameters of the fuzzy self-adaptive PID control are generated through the fuzzy control table, and parameters of the traditional PID control are generated through a self-contained regulator in the simulink.
When the motion parameters of the system are not changed, as shown in fig. 7, the simulation results show that the conventional PID control produces a slight overshoot at almost the same response speed.
When the motion parameters of the system are changed, as shown in fig. 8, the simulation result shows that the conventional PID control generates a large overshoot, and the system has high rapidity and stability under the fuzzy adaptive PID control.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims (8)

1. A control method of a binocular tripod head with a variable baseline is characterized by comprising the following steps:
acquiring the posture of a machine body where a binocular pan-tilt head is located;
step two, carrying out image comparison to obtain the pixel offset of a target point, thereby obtaining the absolute adjustment angle of the binocular tripod head in the pitching and horizontal rotating directions;
step three, solving and obtaining the adjustment quantity of the binocular tripod head by forward and reverse kinematics;
step four, establishing a system motion equation related to the base line;
and step five, driving the binocular tripod head to adjust the posture through fuzzy self-adaptive PID control.
2. The control method of the binocular tripod head with the variable baseline according to claim 1, wherein the step one is as follows:
(1) the binocular head is arranged on a movable body, and the quaternion Q ═ Q ijk of the current posture of the body is obtained through a sensor]TThe quaternion consists of a real number q and three imaginary numbers i, j, k, which respectively represent the rotation around three axes;
(2) obtaining a rotation matrix of the body according to the posture quaternion of the body
Figure FDA0002437853610000011
Will rotate the matrix RdIs shown as
Figure FDA0002437853610000012
(3) Obtaining an Euler angle omega of the current machine body according to the rotation matrix to obtain a controlled attitude;
the Euler angle of the current state of the body is
Figure FDA0002437853610000013
Figure FDA0002437853610000014
Is the body heading angle, βdAt the pitch angle of the body, αdThe transverse roll angle of the machine body is set;
Figure FDA0002437853610000015
Figure FDA0002437853610000016
Figure FDA0002437853610000017
3. the control method of the binocular tripod head with the variable baseline according to claim 2, wherein the specific method in the second step is as follows:
identifying the pixel position of the target in the camera image by a target identification algorithm, monitoring the pixel position change of the target point, generating no binocular head motion instruction when the target point is in the camera tracking frame, and obtaining the absolute adjustment angles delta β and delta v of the binocular head in the pitching and horizontal rotation directions respectively according to the pixel difference (delta u, delta v) between the pixel position of the target point and the pixel position of the picture frame central point when the target point exceeds the camera tracking frame
Figure FDA0002437853610000021
Figure FDA0002437853610000022
Figure FDA0002437853610000023
Where f is the focal length of the camera.
4. The control method of the binocular tripod head with the variable base line according to claim 3, wherein the method comprises the following steps:
(1) coordinate x to world coordinate systemw，yw，zwTransforming to obtain coordinate x of body coordinate systemd，yd，zd；
(2) Coordinate x of body coordinate systemd，yd，zdTransforming to obtain coordinate x of camera coordinate systemc，yc，zc；
(3) Coordinate x of camera coordinate systemc，yc，zcTransforming to obtain coordinates x and y of a physical coordinate system of the image;
(4) transforming the coordinates x, y of the image physical coordinate system to obtain the coordinates u, v of the image pixel coordinate system, and thus obtaining the coordinate x of the world coordinate systemw，yw，zwA positive kinematic transformation formula to the coordinates u, v of the image pixel coordinate system;
(5) inverting two ends of the positive kinematics transformation formula to obtain coordinates u, v of the image pixel coordinate system to coordinates x of the world coordinate systemw，yw，zwThe inverse kinematics transformation formula of (a);
(6) the pixel difference (delta u, delta v) of the pixel position of the target point and the pixel position between the center points of the frames and the absolute adjusting angles delta β and delta v of the binocular head in the pitch and horizontal rotating directions respectively
Figure FDA0002437853610000024
Substituting the inverse kinematics transformation formula to obtain the angle change values of the relative pitch angle and the course angle of the binocular pan-tilt head of β'c，
Figure FDA0002437853610000025
Thereby the target returns to the center of the camera frame and the tracking is finished.
5. The control method of the binocular tripod head with the variable baseline according to claim 4, wherein the method comprises the following steps:
(1) coordinate x to world coordinate systemw，yw，zwThe transformation is formulated as follows:
Figure FDA0002437853610000031
wherein the content of the first and second substances,
Figure FDA0002437853610000032
Figure FDA0002437853610000033
Figure FDA0002437853610000034
the coordinate system of the machine body is a rectangular coordinate system ZdThe shaft is used as a transverse rolling shaft of the machine body and represents the transverse rolling motion of the machine body; xdThe axis is used as a pitching axis of the machine body and represents pitching motion of the machine body; y isdThe axis is used as a heading axis of the machine body and represents the horizontal rotation motion of the machine body; rdIs a rotation matrix from the world coordinate system to the body coordinate system, tdRepresenting translation of the world coordinate system to the body coordinate system, txd，tyd，tzdRespectively representing the origin of the coordinate system of the body in x relative to the origin of the world coordinate systemw，yw，zwThe amount of translation in the direction of the axis,
Figure FDA0002437853610000035
βd，θdthe heading angle, the pitch angle and the roll angle of the machine body are represented and obtained by a displacement sensor arranged on the machine body;
(2) coordinate x of body coordinate systemd，yd，zdThe transformation is formulated as follows:
Figure FDA0002437853610000036
wherein the content of the first and second substances,
Figure FDA0002437853610000037
Figure FDA0002437853610000038
Rcfor a body coordinate system to camera coordinate system rotation matrix, tcRepresenting translation of the body coordinate system to the camera coordinate system, txc，tyc，tzcRespectively representing the origin of the camera coordinate system in x relative to the origin of the body coordinate systemd，yd，zdThe translation amount in the direction is obtained from the relative installation position of the binocular head on the machine body;
Figure FDA0002437853610000041
βc，θcrespectively representing a course angle, a pitch angle and a roll angle of the camera, and obtaining the course angle, the pitch angle and the roll angle through an angle sensor; here, since the binocular head cannot be turned over, the roll angle θ with respect to the machine bodyc＝0；
(3) Coordinate x of camera coordinate systemc，yc，zcThe transformation is formulated as follows:
Figure FDA0002437853610000042
wherein z iscIs the vertical distance from the target to the camera, and f is the focal length of the camera;
(4) the formula for transforming the coordinates x, y of the physical coordinate system of the image is as follows:
Figure FDA0002437853610000043
wherein the content of the first and second substances,
Figure FDA0002437853610000044
dx and dy are the physical sizes of image pixel points on x and y axes of an image physical coordinate system respectively, and gamma is a distortion factor of the camera;
the coordinates x from the world coordinate system are obtained by integrating the equations (8), (9), (10) and (11)w，yw，zwPositive kinematic transformation formula to coordinates u, v of the image pixel coordinate system:
Figure FDA0002437853610000045
inverting both ends of the formula (12) to obtain coordinates u, v of the image pixel coordinate system to coordinate x of the world coordinate systemw，yw，zwThe inverse kinematics variant of (a):
Figure FDA0002437853610000046
Figure FDA0002437853610000051
(5) the pixel difference (delta u, delta v) between the pixel position of the target point and the pixel position between the center points of the frames and the absolute adjustment angles obtained by the formulas (6) and (7)
Figure FDA0002437853610000052
When Δ β is substituted into the equation (13), a system of equations can be obtained
Figure FDA0002437853610000053
Figure FDA0002437853610000054
By solving the system of equations (14), a matrix is obtained
Figure FDA0002437853610000055
Thus, when the translation amount of the camera pixel coordinate system is delta u, delta v, the angle change amount corresponding to the relative course angle and the pitch angle of the binocular head is obtained as
Figure FDA0002437853610000056
β′cThe target can be returned to the center of the camera frame to complete the tracking.
6. The control method of the binocular tripod head with the variable base lines according to claim 5, wherein the system motion equation is established in the fourth step as follows:
Figure FDA0002437853610000057
wherein M is1+ mL is the moment of inertia of the binocular pan/tilt for horizontal rotation, M is the mass of a single camera, L is the length of the base line of the binocular pan/tilt, M1For removing the horizontal moment of inertia of the binocular head behind the camera, M2The moment of inertia of pitching motion of the binocular head and the camera; c1，C2Equivalent damping, Q, when changing course and pitch, respectively, for a binocular pan-tilt1，Q2Respectively changing course and pitching equivalent elastic coefficients for the binocular head;
Figure FDA0002437853610000058
βcrespectively a course angle and a pitch angle of the binocular pan-tilt,
Figure FDA0002437853610000059
respectively a course angular velocity and a pitch angular velocity of the binocular head,
Figure FDA00024378536100000510
respectively acquiring course angular acceleration and pitch angular acceleration of the binocular pan-tilt; t is1，T2Respectively driving the binocular head to change course and pitchTwo input torques.
7. The control method of the binocular tripod head with the variable baseline according to claim 6, wherein the concrete method of the fifth step is as follows:
(1) comparing the calculated angle change quantity of the relative course angle and the pitch angle of the binocular head with the actual change quantity of the binocular head in the course angle and the pitch angle obtained by the sensor to obtain a difference value between the two, and solving the change rate of the difference value along with time;
(2) inputting the difference value and the change rate of the difference value into a fuzzy adaptive parameter PID controller, performing fuzzification processing on input quantity by the controller, inquiring a fuzzy matrix table to perform parameter adjustment, then performing deblurring processing on output quantity, and generating the optimal control quantity for the rotation of the motor when the length of a basic line of a binocular head is changed;
(3) the optimal control quantity is amplified and converted by the driver to generate a driving signal, so that the motor is driven to rotate, and the binocular head reaches a preset course angle and a preset pitch angle.
8. The control method of the binocular tripod head with variable baseline of claim 7, wherein the optimal control quantity d (t) of the motor rotation is calculated as follows:
Figure FDA0002437853610000061
wherein, KpIs a proportionality coefficient, KiIs the integral coefficient, KdIs a differential coefficient;
dynamically adjusting K in a fuzzy adaptive PID control processp，Ki，KdThe value of (d) yields the optimum control quantity d (t) for the motor rotation when the length L of the binocular head baseline is changed.
CN202010257293.7A 2020-04-03 2020-04-03 Control method of binocular tripod head with variable base line Active CN111556309B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010257293.7A CN111556309B (en) 2020-04-03 2020-04-03 Control method of binocular tripod head with variable base line

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010257293.7A CN111556309B (en) 2020-04-03 2020-04-03 Control method of binocular tripod head with variable base line

Publications (2)

Publication Number Publication Date
CN111556309A true CN111556309A (en) 2020-08-18
CN111556309B CN111556309B (en) 2021-10-22

Family

ID=72003811

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010257293.7A Active CN111556309B (en) 2020-04-03 2020-04-03 Control method of binocular tripod head with variable base line

Country Status (1)

Country Link
CN (1) CN111556309B (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113329181A (en) * 2021-06-08 2021-08-31 厦门四信通信科技有限公司 Angle switching method, device, equipment and storage medium of camera
WO2022126477A1 (en) * 2020-12-17 2022-06-23 深圳市大疆创新科技有限公司 Control method and device for movable platform, and movable platform

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104075688A (en) * 2013-03-29 2014-10-01 中原工学院 Distance measurement method of binocular stereoscopic gazing monitoring system
US20170351079A1 (en) * 2016-06-07 2017-12-07 Mitaka Kohki Co., Ltd. Surgical microscope system
CN109343578A (en) * 2018-11-16 2019-02-15 北京理工大学 A kind of TDI-CCD camera binocular vision system visual field alignment schemes of view-based access control model feedback

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104075688A (en) * 2013-03-29 2014-10-01 中原工学院 Distance measurement method of binocular stereoscopic gazing monitoring system
US20170351079A1 (en) * 2016-06-07 2017-12-07 Mitaka Kohki Co., Ltd. Surgical microscope system
CN109343578A (en) * 2018-11-16 2019-02-15 北京理工大学 A kind of TDI-CCD camera binocular vision system visual field alignment schemes of view-based access control model feedback

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022126477A1 (en) * 2020-12-17 2022-06-23 深圳市大疆创新科技有限公司 Control method and device for movable platform, and movable platform
CN113329181A (en) * 2021-06-08 2021-08-31 厦门四信通信科技有限公司 Angle switching method, device, equipment and storage medium of camera

Also Published As

Publication number Publication date
CN111556309B (en) 2021-10-22

Similar Documents

Publication Publication Date Title
CN111556309B (en) Control method of binocular tripod head with variable base line
CN102338969B (en) Image blurring correction device, control method thereof, and image capture apparatus
JP6151263B2 (en) Actively stabilized payload support apparatus and method
Shirzadeh et al. Vision-based control of a quadrotor utilizing artificial neural networks for tracking of moving targets
CN101612735B (en) Anti-shake device of mobile robotic vision system and anti-shake compensation control method therefor
CN113805611B (en) Video image stabilizing method based on triaxial holder
CN111552293B (en) Mobile robot formation control method based on images under visual field constraint
Nasisi et al. Adaptive servo visual robot control
DE19942900A1 (en) Arrangement for correcting camera shake on basis of detection process has shake detector, angular rate sensors on camera's co-ordinate axes, computer unit and rotation control element
CN1080062C (en) Temperature-dependant, frequency-compensated hand deviation correction device for video camera
CN104950726B (en) The time delay correction method and its device of remote controlled travelling device
CN104767930A (en) Device used for image correction and method
CN102325262A (en) Control system for stereo video camera
CN111862169B (en) Target follow-up method and device, cradle head camera and storage medium
CN111028275A (en) Tracing robot PID method based on cross-correlation image positioning matching
CN116095488A (en) Optical lens driving method and device and electronic equipment
CN111230889B (en) Control method and system for adjusting joint position of variable-load robot
US11435549B2 (en) Driving apparatus, optical apparatus, system, image pickup apparatus, and lens apparatus
CN113687627A (en) Target tracking method based on camera robot
CN114612998B (en) Remote iris acquisition system and acquisition method
JPH07244519A (en) Method for controlling motion of movable target by using picture
Stern et al. Hypersonic Pitching Control Model Development
CN108803681B (en) Brushless motor stability-increasing cradle head control method and system
CN114872048B (en) Robot steering engine angle calibration method
CN107505798A (en) Aero remote sensor based on linear fit mode real-time at the uniform velocity oblique distance focus adjustment method and device

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant