Pan-tilt control system and method based on millimeter wave radar

Abstract

The invention discloses a cradle head control system and method based on a millimeter wave radar, and belongs to the technical field of target detection. Real-time scanning is carried out through the millimeter wave radar, a target set is obtained, a target is represented by three-dimensional coordinate information and radial speed information, and the target can be accurately detected; meanwhile, through the steps of state prediction, association operation and measured value updating, the state change of the target can be effectively tracked, and the precision and stability of target tracking are improved. After a target set is converted into a holder coordinate value, a tracking target is determined according to the distance sequence between the target and a holder original point, the deviation between the target angle and the current angle of the holder is calculated, then a PID controller is used for calculating output signals of a horizontal angle and pitch angle controller, a PWM signal is generated to drive a holder motor to rotate, and accurate control over the holder is achieved. And the holder can be quickly and accurately aligned with a target.

Description

Technical Field

[0001] The present invention relates to the technical field of target detection, and particularly relates to a pan-tilt control system and method based on a millimeter-wave radar. Background Art

[0002] A pan-tilt is a mechanical device commonly used in the fields of photography and audio, which can maintain the stability of a camera or audio device. At present, the automatic following control technology of pan-tilts is all based on vision solutions, with high costs and inapplicable in scenarios without cameras.

[0003] For example, Patent CN118962660A discloses a target tracking method, controller, and pan-tilt control system based on a millimeter-wave radar. The method includes: acquiring electromagnetic wave signals of a target object collected in real time by a millimeter-wave radar in a target space, where the target space corresponds one-to-one to the millimeter-wave radar; generating a point cloud map according to the electromagnetic wave signals; predicting a predicted value of the position of the target object at the next moment according to the point cloud map; determining whether the predicted value exceeds a preset threshold. When the determination is yes, calculating the rotation angle of the pan-tilt servo, and generating a control signal according to the rotation angle to drive the pan-tilt servo to rotate in the movement direction of the target object, and the millimeter-wave radar rotates synchronously with the pan-tilt servo. When the determination is no, the pan-tilt servo remains stationary.

[0004] However, its technology has the following problems. First, the above technology detects target object information according to the point cloud map and predicts the position information of the target object at the next moment. The reference data is single and the prediction result is inaccurate.

[0005] Second, it cannot ensure the effectiveness in dealing with dynamic target objects and cannot ensure the continuity and accuracy of target tracking.

[0006] Based on this, the present invention designs a pan-tilt control system and method based on a millimeter-wave radar to solve the above problems. Summary of the Invention

[0007] In view of the above-mentioned drawbacks of the prior art, the present invention provides a pan-tilt control system and method based on a millimeter-wave radar.

[0008] To achieve the above objectives, the present invention is implemented through the following technical solutions:

[0009] A pan-tilt control method based on a millimeter-wave radar includes the following steps:

[0010] Step 1: Initialize the parameters of the millimeter-wave radar, and set the working cycle T and the scanning range M;

[0011] Step 2: The millimeter wave radar performs real-time scanning with a time period of T. After obtaining the original data in the nth period and performing signal processing, a point cloud set Pn of one frame is obtained;

[0012] Step 3: Cluster Pn to obtain the target set A of the frame n {a n,1 ,a n,2 ,.....,a n,i}, target set A n The goal of n,i Represented by three-dimensional coordinate information and velocity information;

[0013] Step 4: By A n Calculate all targets to get the state prediction value of all targets in the current frame Perform an association operation with the current frame to calculate the current frame A n The minimum Mahalanobis distance d between each measured target j and predicted target i in ij , according to d ij Associate the measured target of the current frame with the predicted target of the previous frame, and assign each measured target of the current frame to the predicted target with the closest distance; update the measured values ​​of all targets associated with the current frame, and update the target set A n ;

[0014] Step 5: Set target set A n All targets in are converted into gimbal coordinate values ​​(α, β). The millimeter-wave radar and the gimbal are installed in the same position. Then:

[0015] α=θ1,β=φ1;

[0016] The gimbal coordinate value takes the gimbal as the origin, α is the horizontal angle of the target, and β is the pitch angle of the target;

[0017] θ1 is the azimuth angle of the target before conversion, φ1 is the pitch angle of the target before conversion;

[0018] Step 6: Obtain the distance r0 between all targets and the gimbal origin according to the gimbal coordinate value, and sort them from small to large according to r0, select the point with the smallest distance as the tracking target of the current frame, and then calculate the deviation angle between the tracking target angle and the current angle of the gimbal;

[0019] Step 7: PID controller uses the deviation angle to calculate the horizontal angle controller output signal u α (t) and the output signal u of the pitch angle controller β (t), then generate PWM signal to drive the gimbal motor to rotate;

[0020] Step 8: The millimeter-wave radar processes each frame with a working cycle T. After the current frame is processed, the processing operation of the next frame is executed.

[0021] Furthermore, in Step 3, the three-dimensional coordinate information includes distance, azimuth angle, and elevation angle, and the velocity information is the radial velocity.

[0022] Furthermore, the specific steps of Step 4 are as follows:

[0023] C1. Calculate the state quantity Xn of target a n,i ;

[0024]

[0025] where n represents the discrete time step, r n is the distance, θ n is the azimuth angle, φ n is the elevation angle, υ n is the radial velocity;

[0026] C2. Based on the state quantity n-1 of target a n-1,i in the target set A of the previous frame, predict the state prediction value

[0027] of this target in the current frame; C3. Based on the above method, calculate the state prediction values of all targets in the current frame n and perform an association operation with the current frame to calculate the minimum Mahalanobis distance d ij between each measured target j and predicted target i in the target set A ij of the current frame;

[0028] C4. According to d n associate the measured targets in the current frame with the predicted targets in the previous frame, and assign each measured target in the current frame to the predicted target with the closest distance in the current frame;

[0029] C5. Update the measured values of all targets associated in the current frame, and then update all the measured values of the targets in the target set A ij Furthermore, in C2, the state components of the state prediction value

[0030] include predicted distance, predicted azimuth angle, predicted elevation angle, and predicted radial velocity; Predicted distance:

[0031] Predicted azimuth angle:

[0032] Predicted elevation angle:

[0034] Predict the radial velocity:

[0035] is the distance estimation value of the previous frame, is the velocity estimation value of the previous frame, and Δt is the frame period. is the horizontal angle estimation value of the previous frame, is the pitch angle estimation value of the previous frame, is the velocity estimation value of the previous frame.

[0036] Furthermore, in C3, the calculation formula for the minimum Mahalanobis distance d ij is:

[0037]

[0038] where R is the measurement noise covariance matrix, and R -1 represents the inverse matrix of the measurement noise covariance matrix R, and A j,n is the matrix composed of the measurement target j in this period r i,n|n-1 is the predicted distance of target i; θ i,n|n-1 is the predicted horizontal angle of target i; φ i,n|n-1 is the predicted pitch angle of target i; is the matrix composed of the above three variables.

[0039] Furthermore, in C5, the calculation formula for updating the target measurement value is:

[0040]

[0041] is the state estimation value of this period, and α r is the gain coefficient component of the distance, and α θ is the gain coefficient component of the horizontal angle, and α φ is the gain coefficient component of the pitch angle, is the gain coefficient component; is the gain matrix, and X n is the measurement value of this period.

[0042] Furthermore, in step six, the calculation formula for the deviation between the tracking target angle and the current angle of the pan-tilt is:

[0043] Δα = α b - α n-1 , Δβ = β b - β n-1 ;

[0044] where αn-1 and β n-1 are the current horizontal angle and pitch angle of the pan-tilt head respectively, α b and β b are the horizontal angle and pitch angle of the target, Δα is the deviation angle between the horizontal angle of the tracking target and the horizontal angle of the pan-tilt head, and Δβ is the deviation angle between the pitch angle of the tracking target and the current pitch angle of the pan-tilt head.

[0045] Furthermore, in step seven, the calculation formulas for the output signals of the horizontal angle controller and the pitch angle controller are as follows:

[0046]

[0047] where, u α (t): the output signal of the horizontal angle controller, K pα : the proportional gain coefficient of horizontal angle control, K iα : the integral gain coefficient of horizontal angle control, K dα : the differential gain coefficient of horizontal angle control;

[0048] u β (t): the output signal of the pitch angle controller, K pβ : the proportional gain coefficient of pitch angle control, K iβ : the integral gain coefficient of pitch angle control, K dβ : the differential gain coefficient of pitch angle control.

[0049] Furthermore, in step seven, the steps for the drive module to drive the pan-tilt head motor to rotate are as follows:

[0050] S1. The maximum output range of the PID is from u min to u max . The mapping of it to the duty cycle of the PWM is:

[0051]

[0052] where, Du is the duty cycle value of the PWM wave output, and u(t) is the control output function;

[0053] S2. Input the PWM wave signal with the determined duty cycle into the click drive module to drive the pan-tilt head motor, and the pan-tilt head motor drives the pan-tilt head to rotate together.

[0054] To better achieve the purpose of the present invention, the present invention also provides a pan-tilt head control system based on a millimeter-wave radar, including a millimeter-wave radar module, a pan-tilt head control processing module, a pan-tilt head motor, and a pan-tilt head platform;

[0055] The millimeter-wave radar module is used to set the working cycle T and the scanning range M and obtain a point cloud set Pn of one frame, and cluster Pn to obtain the target set A of this framen {a n,1 ,a n,2 ,.....,a n,i}, by calculating all the targets of A n , the state prediction values of all targets in the current frame are obtained Perform an association operation with the current frame, and calculate each measurement target j in the current frame A n The minimum Mahalanobis distance d between the prediction target i ij . According to d ij , associate the measurement targets in the current frame with the prediction targets in the previous frame, and assign each measurement target in the current frame to the prediction target with the closest distance; update the measurement values of all associated targets in the current frame, and update the target set A n . Finally, convert the measurement values of all targets in the target set into pan-tilt coordinate values and transmit them to the pan-tilt control processing module;

[0056] The pan-tilt control processing module receives the coordinate values output by the millimeter-wave radar module, calculates the deviation angle between the target angle and the current angle of the pan-tilt, and calculates the output signal u of the horizontal angle controller α (t) and the output signal u of the pitch angle controller β (t), and then generates a PWM input driving module. The driving module sends control information to the pan-tilt motor;

[0057] The pan-tilt motor receives the control information of the driving module and performs related operations;

[0058] The pan-tilt platform is fixedly installed on the output end of the pan-tilt motor.

[0059] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. The present invention performs real-time scanning through a millimeter-wave radar, obtains a target set, and the target is represented by three-dimensional coordinate information (distance, azimuth angle, pitch angle) and radial velocity information, and can accurately detect the target;

[0060] At the same time, by processing the target, through the steps of state prediction, association operation and measurement value update, the state change of the target can be effectively tracked, and the accuracy and stability of target tracking can be improved;

[0061] 2. After converting the target set into pan-tilt coordinate values, the tracking targets are determined by sorting according to the distance between the target and the origin of the pan-tilt, and the deviation between the target angle and the current angle of the pan-tilt is calculated. Then, the output signals of the horizontal angle and pitch angle controllers are calculated by using a PID controller, and a PWM signal is generated to drive the pan-tilt motor to rotate, realizing precise control of the pan-tilt, so that the pan-tilt can quickly and accurately align with the target;

[0062] 3. The present invention processes one frame with the working cycle T, can respond to the changes of the target in real time, is applicable to dynamic scenes. The setting that the millimeter-wave radar and the pan-tilt are installed at the same position simplifies the coordinate conversion process and improves the overall performance and adaptability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0064] Figure 1 is a flowchart of a pan-tilt control method based on a millimeter-wave radar according to the present invention;

[0065] Figure 2 is a connection block diagram of a pan-tilt control system based on a millimeter-wave radar according to the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0066] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts fall within the scope of protection of the present invention.

[0067] Embodiment 1: In some embodiments, please refer to Figure 1 in the accompanying drawings of the specification, a pan-tilt control method based on a millimeter-wave radar includes the following steps:

[0068] Step 1: The millimeter-wave radar performs initialization settings, configures initialization parameters, and sets the working cycle T and the scanning range M;

[0069] Step 2: The millimeter-wave radar performs real-time scanning with the time T as the period. After obtaining the original data and performing signal processing in the nth period, a point cloud set Pn of one frame is obtained;

[0070] Step 3: Cluster Pn to obtain the target set A n {a n,1 , a n,2 ,....., a n,i};

[0071] wherein, the target set A n of the target a n,iRepresented by three-dimensional coordinate information and velocity information;

[0072] Preferably, the three-dimensional coordinate information includes distance, azimuth angle, and elevation angle;

[0073] The velocity information is the radial velocity.

[0074] Step Four: Process all targets in target set A n as follows:

[0075] C1. Calculate the state quantity Xn of target a n,i ;

[0076]

[0077] where n represents discrete time steps, r n is the distance, θ n is the azimuth angle, φ n is the elevation angle, υ n is the radial velocity;

[0078] C2. Based on the state quantity of target a in the previous frame of target set A n-1 predict the state prediction value of this target in the current frame n-1,i ; Preferably, the state components of the state prediction value

[0079] include predicted distance, predicted azimuth angle, predicted elevation angle, and predicted radial velocity; ;

[0080] Predicted distance:

[0081] Predicted azimuth angle:

[0082] Predicted elevation angle:

[0083] Predicted radial velocity:

[0084] is the distance estimated value of the previous frame, is the velocity estimated value of the previous frame, Δt is the frame period, is the horizontal angle estimated value of the previous frame, is the elevation angle estimated value of the previous frame, is the velocity estimated value of the previous frame;

[0085] C3. Based on the above method, calculate the state prediction values of all targets in the current frame and perform an association operation with the current frame to calculate target set A in the current frame nThe minimum Mahalanobis distance d between each measurement target j and the prediction target i ij ;

[0086] Preferably, the calculation method is:

[0087]

[0088] where R is the measurement noise covariance matrix, and R -1 represents the inverse matrix of the measurement noise covariance matrix R, and A j,n is the matrix composed of the measurement target j in this period r i,n|n-1 is the predicted distance of the target i; θ i,n|n-1 is the predicted horizontal angle of the target i; φ i,n|n-1 is the predicted pitch angle of the target i; is the matrix composed of the above three variables;

[0089] C4. According to d ij associate the measurement targets in the current frame with the prediction targets in the previous frame, and assign each measurement target in the current frame to the prediction target with the closest distance in the current frame;

[0090] C5. Update the measurement values of all the targets associated in the current frame, and then update the measurement values of all the targets in the target set A n ;

[0091] Preferably, the calculation formula for updating the target measurement value is:

[0092]

[0093] is the state estimation value in this period, α r is the gain coefficient component of the distance, α θ is the gain coefficient component of the horizontal angle, α φ is the gain coefficient component of the pitch angle, is the gain coefficient component; is the gain matrix, X n is the measurement value in this period;

[0094] Step Five: Convert the measurement values of all the targets in the target set A n into the pan-tilt coordinate values (α, β). Since the millimeter-wave radar and the pan-tilt are installed at the same position, then:

[0095] α = θ1, β = φ1;

[0096] The pan-tilt coordinate values take the pan-tilt as the origin, α is the horizontal angle of the target, and β is the pitch angle of the target;

[0097] θ1 is the azimuth angle of the target before conversion, and φ1 is the elevation angle of the target before conversion;

[0098] Step 6: Obtain the distance r0 between all targets and the origin of the pan-tilt according to the pan-tilt coordinate values, then sort the distances r0 from small to large. The point with the smallest distance is the tracking target of the current frame, and then calculate the deviation angle between the tracking target angle and the current angle of the pan-tilt;

[0099] Preferably, Δα = α b -α n-1 , Δβ = β b -β n-1 ;

[0100] where α n-1 , β n-1 are respectively the horizontal angle and elevation angle of the pan-tilt at the current angle, α b , β b are the horizontal angle and elevation angle of the target, Δα is the deviation angle between the horizontal angle of the tracking target and the horizontal angle of the pan-tilt, and Δβ is the deviation angle between the elevation angle of the tracking target and the current elevation angle of the pan-tilt.

[0101] Step 7: The PID controller (Proportional Integral Derivative Controller) calculates the output signal u α (t) of the horizontal angle controller and the output signal u β (t) of the elevation angle controller by using the deviation angle between the tracking target angle and the current angle of the pan-tilt, and then generates a PWM signal to input to the drive module, and the drive module drives the pan-tilt motor to rotate;

[0102] Preferably, the calculation formulas for the output signal of the horizontal angle controller and the output signal of the elevation angle controller are:

[0103]

[0104]

[0105] where, u α (t): the output signal of the horizontal angle controller, K pα : the proportional gain coefficient of horizontal angle control, K iβ : the integral gain coefficient of horizontal angle control, K dα : the differential gain coefficient of horizontal angle control;

[0106] u β (t): the output signal of the elevation angle controller, K pβ : the proportional gain coefficient of elevation angle control, K iβ : the integral gain coefficient of elevation angle control, K dβ: Differential gain coefficient for pitch angle control.

[0107] Preferably, the steps for the driving module to drive the pan-tilt motor to rotate are as follows:

[0108] S1. The maximum output range of the PID controller is from u min to u max , and its mapping to the duty cycle of PWM is:

[0109]

[0110] where Du is the duty cycle value of the PWM wave output, and u(t) is the control output function;

[0111] S2. Input the PWM wave signal with the determined duty cycle into the click driving module to drive the pan-tilt motor.

[0112] Step Eight: The radar processes one frame with a working cycle T. After the execution of the current frame is completed, the processing operation of the next frame is executed.

[0113] Embodiment Two: In some embodiments, as Figure 2 shown, a pan-tilt control system based on a millimeter-wave radar includes a pan-tilt sensing control module, a pan-tilt control motor, and a pan-tilt platform;

[0114] The pan-tilt sensing control module is used for detecting the azimuth, identifying a multi-target scenario, and can perform real-time tracking on the selected tracking object, controlling the pan-tilt platform to rotate following the target;

[0115] The pan-tilt sensing control module includes a millimeter-wave radar module and a pan-tilt control processing module;

[0116] The millimeter-wave radar module is used for setting the working cycle T and the scanning range M and obtaining a point cloud set Pn of one frame, clustering Pn to obtain the target set A n {a n,1 ,a n,2 ,.....,a n,i} of this frame. By calculating all the targets in A n , the state prediction values of all targets in the current frame are obtained and an association operation is performed with the current frame. Calculate the minimum Mahalanobis distance d n between each measured target j and the predicted target i in A of the current frame ij . According to d ij , associate the measured targets of the current frame with the predicted targets of the previous frame, and assign each measured target of the current frame to the predicted target with the closest distance; update the measured values of all the targets associated in the current frame and update the target set A n, finally, the measured values of all targets in the target set are converted into pan-tilt coordinate values and transmitted to the pan-tilt control processing module;

[0117] The pan-tilt control processing module receives the coordinate values output by the millimeter-wave radar module, calculates the deviation angle between the target angle and the current angle of the pan-tilt, and calculates the output signal u α (t) of the horizontal angle controller and the output signal u β (t) of the pitch angle controller, and then generates a PWM input driving module. The driving module sends control information to the pan-tilt motor. The pan-tilt control processing module is a PID controller.

[0118] The pan-tilt motor receives the control information of the driving module and performs related operations;

[0119] The pan-tilt platform is fixedly installed on the output end of the pan-tilt motor.

[0120] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than limiting it; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements will not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A pan-tilt control method based on millimeter-wave radar, characterized in that: It includes the following steps: One: Initialize the millimeter-wave radar and set the duty cycle T and the scanning range M; Two: The millimeter-wave radar obtains a point cloud set Pn of one frame; III: Cluster Pn to obtain the target set A of this frame n {a n,1 , a n,2 ,....., a n,i}; IV: By calculating all the targets of A n the state prediction values of all targets in the current frame are obtained Perform an association operation with the current frame, and calculate the current frame of A n the minimum Mahalanobis distance d between each measured target j and predicted target i in ij According to d ij associate the measured targets in the current frame with the predicted targets in the previous frame, and assign each measured target in the current frame to the predicted target with the closest distance; update the measured values of all targets associated in the current frame, and update the target set A n ; V: Convert the measured values of all targets in A n into pan-tilt coordinate values; Six: Obtain the distances r0 between all targets and the origin of the pan-tilt head according to the pan-tilt head coordinate values, sort them from small to large by r0, select the point with the smallest distance as the tracking target of the current frame, and then calculate the deviation angle between the tracking target angle and the current angle of the pan-tilt head; VII: The PID controller calculates the output signals u α (t) of the horizontal angle controller and the output signal u β (t) of the pitch angle controller, and then generates a PWM signal to drive the pan-tilt motor to rotate; Eight: Repeat steps one to seven.

2. The pan-tilt control method based on millimeter-wave radar according to claim 1, characterized in that In step three, the target set A n has a target a n,i represented by three-dimensional coordinate information and velocity information; The three-dimensional coordinate information includes distance, azimuth angle and elevation angle, and the velocity information is the radial velocity.

3. The pan-tilt control method based on millimeter-wave radar according to claim 1, characterized in that, The specific steps of step four are as follows: C1. Calculate the target a n,i The state quantity Xn; where n represents discrete time steps, r n is the distance, θ n is the azimuth angle, φ n is the elevation angle, υ n is the radial velocity; C2. Based on the previous frame target set A n-1 Target a in n-1,i State quantity Predict the state prediction value of this target in the current frame C3. Calculate the state prediction values of all targets in the current frame based on the above method Perform an association operation with the current frame to calculate the target set A in the current frame n Calculate the minimum Mahalanobis distance d between each measured target j and predicted target i in ij ; C4. According to d ij Associate the measurement target of the current frame with the prediction target of the previous frame, and assign each measurement target of the current frame to the prediction target that is the closest in distance in the current frame; C5. Update the measurement values for all targets associated with the current frame, and then update the measurement values of all targets in target set A n for the update.

4. The pan-tilt control method based on millimeter-wave radar according to claim 3, wherein In C2, the state prediction value has state components including predicted distance, predicted azimuth angle, predicted elevation angle, and predicted radial velocity; Predicted distance: Predicted azimuth angle: Predicted pitch angle: Predict radial velocity: is the distance estimation value of the previous frame, is the speed estimation value of the previous frame, and Δt is the frame period, is the horizontal angle estimation value of the previous frame, is the pitch angle estimation value of the previous frame, is the speed estimation value of the previous frame.

5. The pan-tilt control method based on millimeter-wave radar according to claim 3, characterized in that, In C3, the minimum Mahalanobis distance d ij is calculated by the formula: where R is the measurement noise covariance matrix, and R -1 represents the inverse matrix of the measurement noise covariance matrix R, and A j,n is the matrix composed of the measurement target j in this period r i,n|n-1 is the predicted distance of target i; θ i,n|n-1 is the predicted horizontal angle of target i; φ i,n|n-1 is the predicted pitch angle of target i; is the matrix composed of the above three variables.

6. The pan-tilt control method based on millimeter-wave radar according to claim 3, characterized in that In C5, the calculation formula for updating the target measurement value is: is the state estimate value for this period, α r is the gain coefficient component for distance, α θ is the gain coefficient component for horizontal angle, α φ is the gain coefficient component for pitch angle, is the gain coefficient component; is the gain matrix, X n is the measurement value for this period.

7. The pan-tilt control method based on millimeter-wave radar according to claim 1, characterized in that In step five, convert the measured values of all targets in the target set A n into the pan-tilt coordinate values (α, β). Since the millimeter-wave radar and the pan-tilt are installed at the same position, then: α = θ1, β = φ1; The pan-tilt head coordinate values take the pan-tilt head as the origin, α is the horizontal angle of the target, and β is the elevation angle of the target; θ1 is the azimuth angle of the target before conversion, and φ1 is the elevation angle of the target before conversion.

8. The pan-tilt control method based on millimeter-wave radar according to claim 1, characterized in that In step six, the calculation formula for the deviation angle between the tracking target angle and the current angle of the pan-tilt head is: Δα = α b -α n-1 ,Δβ = β b -β n-1 ; Among them, α n-1 , β n-1 are the current horizontal angle and pitch angle of the pan-tilt respectively, α b , β b are the horizontal angle and pitch angle of the target, Δα is the deviation angle between the horizontal angle of the tracking target and the horizontal angle of the pan-tilt, and Δβ is the deviation angle between the pitch angle of the tracking target and the current pitch angle of the pan-tilt.

9. The pan-tilt control method based on millimeter-wave radar according to claim 1, characterized in that Step 7 specifically is that the horizontal angle controller outputs a signal u α (t) and the output signal u β (t) are calculated by the following formulas: where, u α (t): the output signal of the horizontal angle controller, K pα : the proportional gain coefficient of horizontal angle control, K iα : the integral gain coefficient of horizontal angle control, K dα : the derivative gain coefficient of horizontal angle control; u β (t): Output signal of the pitch angle controller, K pβ : Proportional gain coefficient of pitch angle control, K iβ : Integral gain coefficient of pitch angle control, K dβ : Differential gain coefficient of pitch angle control; The steps for the drive module to drive the pan-tilt head motor to rotate are: S1. The maximum output range of PID is from u min to u max , and the duty cycle mapped to PWM is as follows: Among them, Du is the duty ratio of the PWM wave output, and u(t) is the control output function; S2. Input the PWM wave signal with the determined duty ratio into the click drive module to drive the pan-tilt head motor, and the pan-tilt head motor drives the pan-tilt head to rotate together.

10. A pan-tilt control system based on a millimeter-wave radar, for the pan-tilt control method based on a millimeter-wave radar according to claim 1, characterized in that, It includes a millimeter-wave radar module, a pan-tilt head control processing module, a pan-tilt head motor and a pan-tilt head platform; Millimeter wave radar module, set the working cycle T and scanning range M and obtain a point cloud set Pn of a frame, cluster Pn, and obtain the target set A of the frame n , through A n Calculate all targets to get the state prediction value of all targets in the current frame Perform an association operation with the current frame to calculate the current frame A n The minimum Mahalanobis distance d between each measured target j and predicted target i in ij , according to d ij Associate the measured target of the current frame with the predicted target of the previous frame, and assign each measured target of the current frame to the predicted target with the closest distance; update the measured values ​​of all targets associated with the current frame, and update the target set A n , and finally convert the measured values ​​of all targets into PTZ coordinate values ​​and transmit them to the PTZ control processing module; The pan-tilt control processing module receives coordinate values, calculates the deviation angle between the target angle and the current angle of the pan-tilt, and calculates the output signal u α (t) of the horizontal angle controller and the output signal u β (t) of the pitch angle controller, and then generates a PWM input driving module. The driving module sends control information to the pan-tilt motor; The pan-tilt head motor receives the control information of the drive module to perform relevant operations; The pan-tilt head platform is fixedly installed on the output end of the pan-tilt head motor.

Images