Target tracking method based on millimeter wave radar, controller and holder control system

Abstract

The invention discloses a target tracking method based on millimeter-wave radars, a controller and a holder control system, and relates to the technical field of smart home, the method comprises the following steps: obtaining electromagnetic wave signals of a target object collected by the millimeter-wave radars in real time in a target space, the target space being in one-to-one correspondence with the millimeter-wave radars; generating a point cloud picture according to the electromagnetic wave signal; predicting a position prediction value of the target object at the next moment according to the point cloud picture; whether the position prediction value exceeds a preset threshold value or not is judged, if yes, the rotation angle of a pan-tilt steering engine is calculated, a control signal is generated according to the rotation angle to drive the pan-tilt steering engine to rotate in the motion direction of the target object, and the millimeter wave radar synchronously rotates along with the pan-tilt steering engine, and if not, the control signal is generated according to the control signal to drive the pan-tilt steering engine to rotate in the motion direction of the target object. And the holder steering engine is static. Accurate tracking of the target object in the target space can be achieved through one millimeter wave radar, and therefore the problem that dead angles existing in the space cannot be sensed is effectively solved.

Description

Technical Field

[0001] The present invention relates to the field of smart home technology, and in particular to a target tracking method, controller and pan / tilt control system based on millimeter wave radar. Background Art

[0002] In smart homes, sensor devices are usually used to receive user control commands (such as gesture commands or body posture commands, etc.), so as to achieve the purpose of conveniently controlling smart home products.

[0003] However, the detection range of each sensor device is limited. If only a single sensor device is deployed in the house, there will be blind spots that cannot be sensed; if multiple sensor devices are deployed in the house, the equipment cost will be greatly increased.

[0004] Therefore, it is necessary to develop a target tracking method to achieve blind spot-free sensing in the house through a single sensing device. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to provide a target tracking method, controller and pan-tilt control system based on millimeter-wave radar, which can achieve accurate tracking of target objects in a specific space.

[0006] In order to solve the above technical problems, the present invention provides a target tracking method based on millimeter-wave radar, including: obtaining the electromagnetic wave signal of the target object collected in real time by the millimeter-wave radar in the target space, the target space corresponding to the millimeter-wave radar one-to-one; generating a point cloud map according to the electromagnetic wave signal; predicting the position prediction value of the target object at the next moment according to the point cloud map; judging whether the position prediction value exceeds a preset threshold value, and when the judgment is yes, calculating the rotation angle of the gimbal servo, and generating a control signal according to the rotation angle to drive the gimbal servo to rotate toward the movement direction of the target object, and the millimeter-wave radar rotates synchronously with the gimbal servo, and when the judgment is no, the gimbal servo is stationary.

[0007] As an improvement to the above solution, a Kalman filter algorithm is used to predict the position prediction value of the target object at the next moment based on the point cloud image.

[0008] As an improvement to the above solution, the step of calculating the rotation angle of the pan-tilt servo includes: calculating the rotation angle of the pan-tilt servo according to the following formula:

[0009] u t =K p e t +K d (e t -e t-1 )+K v v t

[0010] Among them, u t is the rotation angle of the gimbal servo, K p is the first parameter, K d is the second parameter, K v is the gain of the target object's moving speed to the control signal, e t is the position difference between the target object and the front of the millimeter wave radar, v t is the moving speed of the target object.

[0011] As an improvement to the above solution, the step of calculating the rotation angle of the pan-tilt servo includes: calculating the rotation angle of the pan-tilt servo according to the following formula:

[0012]

[0013] Where u(t) is the rotation angle of the gimbal servo, K p is the first parameter, K d is the second parameter, K i is the third parameter, and e(t) is the position difference between the target object and the front of the millimeter-wave radar.

[0014] As an improvement to the above solution, the frequency bands of the millimeter wave radar are 24 GHz, 60 GHz and 77 GHz to 81 GHz.

[0015] As an improvement to the above solution, the target tracking method based on millimeter wave radar further includes: obtaining a command signal of the target object collected in real time by the millimeter wave radar; and controlling the working state of the smart terminal according to the command signal.

[0016] Correspondingly, the present invention also provides an adaptive gimbal controller based on millimeter-wave radar, comprising a memory and a processor, wherein the memory stores a computer program, wherein the processor implements the steps of the above-mentioned millimeter-wave radar-based target tracking method when executing the computer program.

[0017] Correspondingly, the present invention also provides an adaptive pan-tilt control system based on millimeter-wave radar, which includes a millimeter-wave radar module, a pan-tilt motor, a pan-tilt servo and the above-mentioned adaptive pan-tilt controller based on millimeter-wave radar; the millimeter-wave radar module is connected to the adaptive pan-tilt controller for real-time detection of the electromagnetic wave signal of the target object and sending the electromagnetic wave signal to the adaptive pan-tilt controller; the adaptive pan-tilt controller is connected to the pan-tilt motor for controlling the pan-tilt motor according to a control signal; the pan-tilt motor is connected to the pan-tilt servo for driving the pan-tilt servo to rotate in the direction of movement of the target object; the millimeter-wave radar module is arranged on the pan-tilt servo and rotates synchronously with the pan-tilt servo.

[0018] As an improvement of the above-mentioned solution, the adaptive gimbal control system based on millimeter-wave radar also includes a gimbal base and a carrier plate, the gimbal servo is arranged on the gimbal base, the carrier plate is arranged on the gimbal servo, and the millimeter-wave radar module is suspended on the carrier plate.

[0019] As an improvement to the above solution, the adaptive pan-tilt control system based on millimeter-wave radar further includes an intelligent terminal connected to the adaptive pan-tilt controller.

[0020] The implementation of the present invention has the following beneficial effects:

[0021] Unlike existing technologies, the present invention uses a single millimeter-wave radar to accurately track a target object in a target space, effectively resolving the issue of blind spots in space. Specifically, the present invention uses a Kalman wave target tracking method to generate a point cloud of the target object to determine its measured position. The PID / PD control algorithm then calculates the pan / tilt servo's rotation angle, achieving high accuracy.

[0022] Furthermore, the present invention can synchronously collect the command signal of the target object under the premise of achieving target tracking, thereby flexibly controlling the working state of the intelligent terminal, and has strong practicality. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 This is a flow chart of a first embodiment of a target tracking method based on millimeter wave radar according to the present invention;

[0024] Figure 2 is a schematic diagram of the PD control algorithm of the present invention;

[0025] Figure 3 is a flow chart of a second embodiment of a target tracking method based on millimeter wave radar according to the present invention;

[0026] Figure 4 It is a schematic structural diagram of an embodiment of the adaptive pan-tilt control system based on millimeter-wave radar of the present invention. DETAILED DESCRIPTION

[0027] To make the objectives, technical solutions, and advantages of the present invention more apparent, the present invention will be further described in detail below with reference to the accompanying drawings. It is hereby stated that any directional terms such as "up," "down," "left," "right," "front," "back," "inside," and "outside" that appear or will appear herein are based solely on the accompanying drawings and are not intended to limit the present invention.

[0028] See also Figure 1 , Figure 1 The flowchart of the first embodiment of the target tracking method based on millimeter wave radar of the present invention is shown, including:

[0029] S101, obtaining an electromagnetic wave signal of a target object in a target space collected in real time by a millimeter wave radar;

[0030] It should be noted that in the present invention, the target space corresponds to the millimeter-wave radar one-to-one, that is, only one millimeter-wave radar is set in one target space.

[0031] The frequency bands of millimeter-wave radar are 24GHz, 60GHz, and 77GHz to 81GHz, but are not limited to these and can be selected based on actual conditions.

[0032] S102, generating a point cloud image based on the electromagnetic wave signal;

[0033] In practical applications, the Texas Instruments IWR6843 ISK product can be used to automatically generate point cloud maps, or other algorithms can be used to generate point cloud maps.

[0034] S103, predicting the position of the target object at the next moment based on the point cloud image;

[0035] Furthermore, the Kalman filter algorithm can be used to remove the process noise and measurement noise of the target object's movement, so as to predict the position prediction value of the target object at the next moment based on the point cloud image.

[0036] When a target object moves, its position and velocity information are selected as its state, and the system transfer matrix is ​​used to predict its state at the next moment. The present invention uses millimeter-wave radar to read the target object's distance, angle, and velocity information from a distance, and calculates the target object's position and velocity information as measured values, thereby revising the prediction of the state at the next moment.

[0037] For example, consider a car moving on a smooth, straight track. The car is subject to a random external force at each moment. Assuming that there is an interval Δt between two moments, and an imprecise position estimate is obtained at each moment, the application of the Kalman filter can obtain the best tracking of the actual position and velocity of the car in the sense of the least squares method. The basic structure of the Kalman filter is as follows:

[0038] x k =Ax k-1 +w k , w k ~N(0,Q)

[0039] y k =Cx k +v k , v k ~N(0, R)

[0040] The position and velocity of the car are expressed in state space as Assume that from (k-1) to k time, random external force causes constant acceleration a k The mean is 0 and the standard deviation is σ a Gaussian distribution, so:

[0041] x k =Ax k-1 +Ga k

[0042] in,

[0043] Therefore, x k =Ax k-1 +w k , w k ~N(0,Q),

[0044]

[0045] At each moment, only the position of the car is measured:

[0046]

[0047] Since the initial position and speed of the car are known, they are initialized as:

[0048]

[0049] Therefore, millimeter-wave radar can generate several point clouds. After eliminating obviously abnormal points, the positions of the remaining reliable point clouds are averaged to obtain the measured position of the target object. Because averaging is a linear transformation, it can be expressed as a matrix.

[0050] S104, determining whether the position prediction value exceeds a preset threshold;

[0051] In actual applications, different sensitivity adjustments can be achieved according to different preset thresholds. When the position prediction value exceeds the preset threshold, it indicates that the target object has moved a long distance, about to exceed the detection range of the millimeter-wave radar, and the millimeter-wave radar angle needs to be adjusted. When the position prediction value does not exceed the preset threshold, it indicates that the target object has moved a short distance, and the millimeter-wave radar angle does not need to be adjusted.

[0052] S105, if the judgment is yes, calculating the rotation angle of the gimbal servo, and generating a control signal according to the rotation angle to drive the gimbal servo to rotate in the direction of movement of the target object, and the millimeter wave radar rotates synchronously with the gimbal servo;

[0053] It should be noted that the millimeter-wave radar is installed on the gimbal servo and rotates synchronously with the gimbal servo.

[0054] The present invention can calculate the rotation angle of the pan / tilt servo by PID algorithm and / or PD algorithm:

[0055] 1. PID algorithm

[0056] PID controller, short for proportional-integral-differential controller, is a control loop mechanism that uses feedback and can be continuously modulated. This invention calculates the rotation angle of the pan-tilt servo based on the PID control algorithm:

[0057] u t =K p e t +K d (e t -e t-1 )+K v v t

[0058] in,

[0059] u t is the rotation angle of the gimbal servo;

[0060] K p is the first parameter, which can be set according to experience and is a non-negative value;

[0061] K d is the second parameter, which can be set according to experience and is a non-negative value;

[0062] e t is the position difference between the target object and the position directly in front of the millimeter-wave radar;

[0063] v t The target object's moving speed must be considered when rotating the gimbal servo. The faster the moving speed, the greater the angle of the gimbal servo's rotation.

[0064] K v The gain of the control signal to the moving speed of the target object.

[0065] Therefore, the present invention obtains the observation of the target object's position through the point cloud at each moment, and updates the position and speed v of the target object based on this observation. t Estimate, calculate e from the position t , due to the error e in the previous step t-1 Known by u t =K p e t +K d (e t -e t-1 )+K v The angle that the gimbal servo needs to rotate at that moment can be calculated.

[0066] 2. PD Algorithm

[0067] The PD controller is a simplified version of the PID controller. The present invention adopts the PD control algorithm to adjust the sensor orientation by tracking the position difference between the target object and the sensor (ie, millimeter wave radar).

[0068] The present invention calculates the rotation angle of the pan-tilt servo according to the PD control algorithm:

[0069]

[0070] in,

[0071] u(t) is the rotation angle of the gimbal servo;

[0072] K p is the first parameter, which is a non-negative value;

[0073] K d is the second parameter, which is a non-negative value;

[0074] K i is the third parameter, which can be set according to experience and is a non-negative value;

[0075] e(t) is the position difference between the target object and the front of the millimeter-wave radar.

[0076] like Figure 2 As shown, e(t) is the difference between the target value r(t) and the process variable value y(t); P term K p e(t) is proportional to the current e(t) value, that is, when the error is larger, the selected control output (i.e., the rotation angle) is also larger. Due to the error in the system itself, using only the P term will lead to steady-state error; the D term K d de(t) / dt is the best prediction of the future trend of the error and can effectively reduce the steady-state error. The faster the error decreases, the larger the absolute value of the D term.

[0077] Furthermore, the above calculation results (i.e., the rotation angle) can be converted into control signals and transmitted to the pan / tilt motor in any manner, and the present invention does not impose any restrictions. For example:

[0078] (1) Convert the rotation angle into a control signal in the built-in module and directly transmit the control signal to the pan / tilt motor;

[0079] (2) The built-in computing module transmits the rotation angle to the external module, which converts the rotation angle into a control signal and transmits it back to the built-in computing module. The built-in computing module then transmits the control signal to the gimbal motor.

[0080] (3) The millimeter-wave radar and the gimbal motor communicate with the external module respectively. The millimeter-wave radar transmits electromagnetic wave signals to the external module to calculate the rotation angle and converts the rotation angle into a control signal. The gimbal motor then receives the control signal from the external module.

[0081] Therefore, the present invention can achieve accurate tracking of target objects in the target space through a millimeter wave radar, thereby effectively solving the problem of blind spots in the space that cannot be sensed.

[0082] S106: If the answer is no, the gimbal servo is stationary.

[0083] As can be seen from the above, the present invention is suitable for indoor scenes. The present invention obtains the measured position of the target object by generating a point cloud of the target object through the Kalman rate wave target tracking method, and then calculates the rotation angle of the gimbal servo through the PID / PD control algorithm with high accuracy.

[0084] See also Figure 3 , Figure 3 A flow chart of a second embodiment of a target tracking method based on millimeter wave radar according to the present invention is shown, including:

[0085] S201, obtaining an electromagnetic wave signal of a target object in a target space collected in real time by a millimeter wave radar;

[0086] S202, generating a point cloud image according to the electromagnetic wave signal;

[0087] S203, predicting the position of the target object at the next moment based on the point cloud image;

[0088] S204: Determine whether the position prediction value exceeds a preset threshold.

[0089] S205, if the judgment is yes, calculating the rotation angle of the gimbal servo, and generating a control signal according to the rotation angle to drive the gimbal servo to rotate in the direction of movement of the target object, and the millimeter wave radar rotates synchronously with the gimbal servo;

[0090] S206: When the judgment is negative, the gimbal servo is stationary.

[0091] S207, obtaining a command signal of the target object collected in real time by the millimeter wave radar;

[0092] The command signal may be a hand gesture signal or a body posture signal, but is not limited thereto and may be set according to actual conditions.

[0093] S208: Control the working state of the intelligent terminal according to the command signal.

[0094] and Figure 1The difference from the first embodiment shown is that this embodiment can synchronously collect the command signal of the target object while achieving target tracking, thereby flexibly controlling the working state of the smart terminal and having strong practicality.

[0095] See also Figure 4 , Figure 4 The figure shows the specific structure of the millimeter wave radar-based adaptive pan-tilt control system 100 of the present invention, which includes a millimeter wave radar module 1, a pan-tilt motor 3, a pan-tilt servo 4, and a millimeter wave radar-based adaptive pan-tilt controller 2. Specifically:

[0096] The millimeter wave radar module 1 is connected to the adaptive pan-tilt controller 2 to detect the electromagnetic wave signal of the target object in real time and send the electromagnetic wave signal to the adaptive pan-tilt controller 2;

[0097] The adaptive pan-tilt controller 2 includes a memory and a processor, wherein the memory stores a computer program, wherein the processor implements the steps of the above-mentioned target tracking method based on millimeter wave radar when executing the computer program;

[0098] The adaptive PTZ controller 2 is connected to the PTZ motor 3 and is used to control the PTZ motor 3 according to the control signal;

[0099] The pan / tilt motor 3 is connected to the pan / tilt servo 4 and is used to drive the pan / tilt servo 4 to rotate in the direction of movement of the target object;

[0100] The millimeter wave radar module 1 is mounted on the pan-tilt servo 4 and rotates synchronously with the pan-tilt servo 4 .

[0101] During operation, the adaptive pan-tilt controller 2 obtains the electromagnetic wave signal of the target object collected in real time by the millimeter wave radar 1, generates a point cloud map based on the electromagnetic wave signal, and then predicts the position prediction value of the target object at the next moment based on the point cloud map; then, the adaptive pan-tilt controller 2 determines whether the position prediction value exceeds the preset threshold value. When it is judged as no, the pan-tilt servo stops 4; when it is judged as yes, the adaptive pan-tilt controller 2 calculates the rotation angle of the pan-tilt servo and generates a control signal according to the rotation angle, and then sends the control signal to the pan-tilt motor 3. Finally, the pan-tilt motor 3 drives the pan-tilt servo 4 to rotate in the direction of movement of the target object, thereby realizing accurate tracking of the target object.

[0102] Furthermore, the adaptive gimbal control system 100 based on millimeter-wave radar also includes a gimbal base and a carrier plate, the gimbal servo 4 is arranged on the gimbal base, the carrier plate is arranged on the gimbal servo 4, and the millimeter-wave radar module 1 is suspended on the carrier plate.

[0103] In the present invention, the supporting plates of the pan-tilt base and the pan-tilt servo 4 are both punched, and the pan-tilt base and the pan-tilt servo 4 are connected by screws and nuts; the millimeter-wave radar module 1 is suspended and fixed on a supporting plate through four columns, and the supporting plate is fixed above the pan-tilt servo 4. The millimeter-wave radar module 1 should be placed vertically.

[0104] More preferably, the millimeter-wave radar-based adaptive pan-tilt control system 100 further includes an intelligent terminal connected to the adaptive pan-tilt controller 2 .

[0105] During the tracking process, the adaptive pan-tilt controller 2 obtains the command signal of the target object collected in real time by the millimeter-wave radar 1, and controls the working state of the smart terminal according to the command signal, which is highly practical.

[0106] The above is a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications are also considered to be within the scope of protection of the present invention.

Claims

1. A target tracking method based on millimeter wave radar, characterized in that: include: Acquiring electromagnetic wave signals of a target object collected in real time by a millimeter-wave radar in a target space, wherein the target space corresponds to the millimeter-wave radar in a one-to-one manner; generating a point cloud image according to the electromagnetic wave signal; Predicting a position prediction value of the target object at the next moment based on the point cloud image; Determine whether the position prediction value exceeds a preset threshold, If the answer is yes, the rotation angle of the pan-tilt servo is calculated, and a control signal is generated according to the rotation angle to drive the pan-tilt servo to rotate toward the direction of movement of the target object, and the millimeter-wave radar rotates synchronously with the pan-tilt servo. If the judgment is no, the gimbal servo is stationary.

2. The target tracking method based on millimeter wave radar according to claim 1, characterized in that: The Kalman filter algorithm is used to predict the position of the target object at the next moment based on the point cloud image.

3. The target tracking method based on millimeter wave radar according to claim 1, characterized in that: The step of calculating the rotation angle of the pan / tilt servo comprises: Calculate the rotation angle of the gimbal servo according to the following formula: you t =K p yes t +K d (e t -e t-1 )+K v v t Among them, u t is the rotation angle of the gimbal servo, K p is the first parameter, K d is the second parameter, K v is the gain of the target object's moving speed to the control signal, e t is the position difference between the target object and the front of the millimeter wave radar, v t is the moving speed of the target object.

4. The target tracking method based on millimeter wave radar according to claim 1, characterized in that: The step of calculating the rotation angle of the pan / tilt servo comprises: Calculate the rotation angle of the gimbal servo according to the following formula: Where u(t) is the rotation angle of the gimbal servo, K p is the first parameter, K d is the second parameter, K i is the third parameter, and e(t) is the position difference between the target object and the front of the millimeter-wave radar.

5. The target tracking method based on millimeter wave radar according to claim 1, characterized in that: The frequency bands of the millimeter wave radar are 24 GHz, 60 GHz and 77 GHz to 81 GHz.

6. The target tracking method based on millimeter wave radar according to claim 1, characterized in that: Also includes: Obtain the command signal of the target object collected by the millimeter-wave radar in real time; The working state of the intelligent terminal is controlled according to the command signal.

7. An adaptive pan-tilt controller based on millimeter-wave radar, comprising a memory and a processor, wherein the memory stores a computer program, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 6 are implemented.

8. An adaptive pan-tilt control system based on millimeter-wave radar, characterized in that: It includes a millimeter wave radar module, a pan-tilt motor, a pan-tilt servo, and the millimeter wave radar-based adaptive pan-tilt controller according to claim 7; The millimeter wave radar module is connected to the adaptive pan-tilt controller, and is used to detect the electromagnetic wave signal of the target object in real time and send the electromagnetic wave signal to the adaptive pan-tilt controller; The adaptive PTZ controller is connected to the PTZ motor and is used to control the PTZ motor according to a control signal; The pan / tilt motor is connected to the pan / tilt servo, and is used to drive the pan / tilt servo to rotate in the direction of movement of the target object; The millimeter wave radar module is arranged on the pan-tilt servo and rotates synchronously with the pan-tilt servo.

9. The adaptive pan-tilt control system based on millimeter-wave radar according to claim 8, characterized in that: It also includes a pan-tilt base and a carrying plate, the pan-tilt servo is arranged on the pan-tilt base, the carrying plate is arranged on the pan-tilt servo, and the millimeter wave radar module is suspended on the carrying plate.

10. The adaptive pan-tilt control system based on millimeter-wave radar according to claim 8, characterized in that: It also includes an intelligent terminal connected to the adaptive pan-tilt controller.

Images