Positioning method and system based on fusion of millimeter wave phased array radar and imu

Abstract

The application belongs to the technical field of power construction safety, and discloses a positioning method and system based on fusion of millimeter wave phased array radar and IMU, which comprises the following steps: when a moving target enters the monitoring area of a positioning module, the data output by the millimeter wave phased array radar is processed by a digital signal processor to obtain point cloud data, and at the same time, an inertial measurement unit sends unique device identification, acceleration and angular velocity data to an MCU controller; the MCU controller matches the point cloud data, acceleration and angular velocity data of the same moving target; and the data is fused by using a multi-target tracking algorithm to obtain the position of the moving target and predict the position at the next moment; whether the moving target enters the early warning area electronic fence is judged according to the position or the predicted position of the moving target, and if yes, an alarm is given and the monitoring platform is uploaded and other positioning modules are notified. The application fuses the millimeter wave radar and the inertial measurement, improves the target tracking accuracy and stability, and is suitable for real-time safety early warning of moving targets during power construction.

Description

Technical Field

[0001] This invention belongs to the field of power construction safety technology, and relates to a positioning method and system based on the fusion of millimeter-wave phased array radar and IMU. Background Technology

[0002] High-voltage power construction sites present numerous high-risk factors, especially safety accidents caused by personnel accidentally approaching live conductors or machinery entering dangerous areas. These accidents often result in serious injuries and equipment damage. Traditional safety protection mainly relies on physical fencing, manual monitoring, or low-precision alarm devices. However, these methods have shortcomings such as inaccurate positioning and delayed response, making it difficult to provide timely and effective safety guarantees in complex and ever-changing construction environments.

[0003] In recent years, millimeter-wave phased array radar has gained attention in the field of industrial safety monitoring due to its advantages such as high-precision ranging and strong environmental adaptability. Millimeter-wave phased array radar can operate stably in environments such as wind, rain, and thunderstorms, and can detect the position and movement of personnel at long distances without the need for sensors to be mounted on them. Particularly in public safety and the Industrial Internet of Things (IIoT), it is possible to use millimeter-wave phased array radar to construct "electronic fences" to automatically monitor personnel positions. However, millimeter-wave phased array radar also has its drawbacks; when a target suddenly accelerates, the target warning may not be timely. Therefore, there is an urgent need for a positioning system based on the fusion of millimeter-wave phased array radar and IMU (Integrated Measurement Unit) to achieve high-precision, real-time, all-weather monitoring and positioning to ensure the safety of personnel and equipment on site. Summary of the Invention

[0004] The purpose of this invention is to provide a positioning method based on the fusion of millimeter-wave phased array radar and IMU. By combining the positioning module and the inertial measurement unit, the position of each moving target is obtained and the position of each moving target at the next moment is predicted, thereby realizing the positioning and trajectory prediction of moving targets and ensuring the safety of on-site personnel and equipment.

[0005] Another objective of this invention is to provide a positioning system based on the fusion of millimeter-wave phased array radar and IMU.

[0006] To achieve the above objectives, the technical solution adopted by this invention is as follows:

[0007] A positioning method based on the fusion of millimeter-wave phased array radar and IMU includes the following steps:

[0008] S1. When a moving target equipped with an inertial measurement unit enters a work area equipped with at least two sets of positioning modules, the positioning module with a corresponding monitoring area detects the moving target and sends the radar data of the detected moving target to a digital signal processor for processing through the millimeter-wave phased array radar in the positioning module; wherein, the monitoring areas of each positioning module are combined to cover the entire work area.

[0009] S2. The digital signal processor outputs point cloud data corresponding to the moving target and sends it to the MCU controller of the corresponding positioning module; at the same time, each inertial measurement unit wirelessly sends its unique device identification information and the detected acceleration and angular velocity data to the MCU controller of the corresponding positioning module.

[0010] S3. The MCU controller generates corresponding radar candidate target trajectories based on the point cloud data of the same moving target, and generates IMU candidate trajectories of the corresponding moving target based on the unique device identification information, acceleration and angular velocity data sent by each inertial measurement unit; based on the target association algorithm, the radar candidate target trajectories and IMU candidate trajectories at the same sampling time are associated and matched through an association threshold; after determining that they belong to the same moving target, a matching relationship is established between the point cloud data of the moving target and the corresponding unique device identification information.

[0011] S4. The MCU controller inputs the point cloud data, acceleration and angular velocity data of the same moving target into the multi-target tracking algorithm to obtain the position of the moving target and predict the position of the moving target at the next moment.

[0012] S5. Based on the position of the moving target or the predicted position at the next moment, the MCU controller determines whether it has entered the preset warning area electronic fence; if so, it will issue an alarm through the alarm unit, and wirelessly send the position of the moving target and the alarm signal to the monitoring platform, and wirelessly send them to the other positioning modules.

[0013] As a limitation, in step S3, the process of generating the corresponding radar candidate target trajectory is as follows: the MCU controller performs target detection, clustering and trajectory initialization on the received point cloud data to generate the radar candidate target trajectory;

[0014] Based on the target association algorithm, the process of associating and matching radar candidate target trajectories and IMU candidate trajectories at the same sampling time through an association threshold is as follows:

[0015] Based on the target association algorithm, an association cost is constructed according to the position distance, velocity difference, acceleration direction consistency, and timestamp difference between radar candidate target trajectories and IMU candidate trajectories at the same sampling time. The radar candidate target trajectories and IMU candidate trajectories are matched one by one according to the association threshold. When the association cost between a radar candidate target trajectory and an IMU candidate trajectory is less than the association threshold, it is determined that the point cloud data of the radar candidate target trajectory and the inertial measurement unit corresponding to the IMU candidate trajectory belong to the same moving target. A matching relationship is established between the point cloud data of the moving target and the unique device identification information of the corresponding inertial measurement unit.

[0016] As a second limitation, in step S3, the target association algorithm adopts the nearest neighbor association algorithm, the joint probability data association algorithm, or the Hungarian matching algorithm;

[0017] The multi-target tracking algorithm includes an extended Kalman filter algorithm, a trajectory prediction algorithm, a support vector machine algorithm for auxiliary identification of moving target categories, and a reinforcement learning algorithm for adaptive adjustment of filter parameters.

[0018] The extended Kalman filter algorithm is used to fuse and estimate the state of the corresponding moving target based on the point cloud data, acceleration and angular velocity data of the same moving target after matching, so as to obtain the state estimation result of the moving target;

[0019] Support vector machines are used to perform category-assisted identification of moving targets based on point cloud data, acceleration and angular velocity data of the same moving target after matching, and the identification results are used as auxiliary information for target association and early warning judgment.

[0020] The trajectory prediction algorithm is used to obtain the position of the moving target based on the state estimation result of the moving target output by the extended Kalman filter algorithm, and to predict the position of the moving target at the next moment.

[0021] Reinforcement learning algorithms are used to dynamically adjust the parameters of the process noise covariance matrix and the observation noise covariance matrix in the extended Kalman filter algorithm.

[0022] As a further limitation, the formula for the prediction model of the extended Kalman filter algorithm is:

[0023]

[0024]

[0025] in, It is the predicted value of the moving target's state. It is the prediction error covariance matrix. It is the state transition matrix. It is the process noise covariance matrix. It is the predicted value of the moving target's state at time k-1. It is the control input at time k-1. It is the error covariance matrix of the predicted state at time k-1; T is the transpose;

[0026] The formula for the update model of the extended Kalman filter algorithm is:

[0027]

[0028]

[0029]

[0030] in, It is Kalman gain. These are observed values. It is the observation noise covariance matrix. It is the observation matrix. It is a posterior state estimate. It is the error covariance matrix. It is an identity matrix.

[0031] As a further limitation, the formula for the trajectory prediction algorithm is:

[0032]

[0033] in, It is the predicted state of the moving target. It is a reference trajectory. yes Time-based control input, yes Time-based control input, and It is a weight matrix. It refers to the number of prediction steps.

[0034] As a further limitation, the target classification formula of the support vector machine is:

[0035]

[0036] in, It's training data. It's a tag. It is a Lagrange multiplier. Bias term, It is the inner product. Classification results It is the total number of training data;

[0037] The reinforcement learning algorithm strategy is optimized using the Q-learning algorithm.

[0038] The formula for the Q-learning algorithm is:

[0039]

[0040] in, It is a state Take action below Expected return It's the learning rate. It's an instant reward. It is a discount factor. It is a state Take action below Expected return.

[0041] As a third limitation, in step S1, the process by which the millimeter-wave phased array radar in the positioning module sends the radar data of the detected moving target to the digital signal processor for processing is as follows:

[0042] S11. The millimeter-wave phased array radar in the positioning module transmits frequency-modulated continuous wave signals to the monitoring area through its multi-transmitter, multi-receiver antenna and receives the echo signals scattered by each moving target in the monitoring area. The echo signals are mixed with the transmitted frequency-modulated continuous wave signals to obtain the beat intermediate frequency signals. The analog-to-digital converter in the millimeter-wave phased array radar samples the beat intermediate frequency signals to obtain the echo discrete sequence, and sends it to the digital signal processor.

[0043] S12. After receiving the echo discrete sequence, the digital signal processor first performs a one-dimensional Fourier transform along the fast time dimension to obtain the distance of the moving target; then it performs a two-dimensional Fourier transform along the slow time dimension to obtain the radial velocity of the moving target; then it uses a digital beamforming algorithm or a MIMO virtual array angle estimation algorithm to estimate the angle of the received echo discrete sequence to obtain the azimuth of the moving target; and constructs a three-dimensional data cube of distance-velocity-angle based on the distance, radial velocity, and azimuth of the moving target. It uses a constant false alarm rate (CFAR) detection algorithm to detect the target in the three-dimensional data cube, and outputs point cloud data containing the distance, radial velocity, and azimuth of the moving target for each target.

[0044] As a fourth limitation, in step S5, when the monitoring areas of the positioning modules overlap, if a moving target enters the overlapping area, the MCU controllers of each positioning module communicate wirelessly, exchange the positions of the moving target obtained by each module or the predicted positions of the moving target at the next moment, and perform weighted fusion based on the position estimation accuracy of each positioning module to obtain the fused position of the moving target or the position of the moving target at the next moment; when the fused position of the moving target or the position of the moving target at the next moment is within the electronic fence of the warning area, the MCU controllers in the positioning modules all trigger an alarm through the alarm unit and wirelessly send the position of the moving target and the alarm signal to the monitoring platform.

[0045] The present invention also provides a positioning system based on the fusion of millimeter-wave phased array radar and IMU, for realizing the above-mentioned positioning method based on the fusion of millimeter-wave phased array radar and IMU, including at least two sets of positioning modules, at least one inertial measurement unit and a monitoring platform.

[0046] Each positioning module includes a millimeter-wave phased array radar, a digital signal processor, an MCU controller, and an alarm unit. The output of the millimeter-wave phased array radar is connected to the input of the MCU controller via the digital signal processor. The output of the MCU controller is connected to the input of the alarm unit, and the MCU controller is wirelessly connected to the monitoring platform. The MCU controllers of each positioning module are wirelessly connected to each other. The MCU controller has embedded multi-target tracking and target association algorithms, and is equipped with an electronic fence for the warning area and association threshold.

[0047] Each inertial measurement unit has a unique device identification information. The inertial measurement unit is wirelessly connected to the MCU controller in each positioning module and sends the unique device identification information, as well as the acceleration and angular velocity data of the detected moving target, to the MCU controller.

[0048] In each positioning module, a millimeter-wave phased array radar is used to detect radar data of moving targets. After processing by a digital signal processor, the radar outputs point cloud data corresponding to each moving target and sends it to the MCU controller. The MCU controller uses a multi-target tracking algorithm, a target association algorithm, and an association threshold to match and fuse the point cloud data, acceleration, and angular velocity data of the same moving target to obtain the position of the moving target and predict its position at the next moment. Based on the position of the moving target or the predicted position at the next moment, it determines whether the target has entered the warning area electronic fence. If so, it triggers an alarm through the alarm unit and wirelessly sends the position of the moving target and the alarm signal to the monitoring platform.

[0049] As a limitation, the millimeter-wave phased array radar uses a 57~64GHz broadband phased array radar as its core sensor, and the millimeter-wave phased array radar includes no less than three transmitting antennas and four receiving antennas.

[0050] The present invention, by adopting the above-described technical solution, achieves the following technical advancements compared to existing technologies:

[0051] (1) This invention achieves accurate identification and continuous tracking of multiple moving targets by spatiotemporally matching the point cloud data obtained by the positioning module with the acceleration, angular velocity and unique device identification information provided by the inertial measurement unit. On this basis, the multi-target tracking algorithm not only outputs the position of the moving target in real time, but also predicts the position at the next moment, effectively overcoming the defects of single millimeter-wave phased array radar in tracking lag or loss when the target suddenly changes speed or is briefly blocked. At the same time, the monitoring areas of multiple positioning modules are connected to each other to cover the entire work area, ensuring comprehensive, timely and reliable safety monitoring of moving targets in the power construction scenario.

[0052] (2) In the positioning module of the present invention, the radar data detected by the millimeter-wave phased array radar is processed by the digital signal processor to generate point cloud data, which has high resolution and anti-environmental interference characteristics. Combined with the acceleration and angular velocity data provided by the inertial measurement unit, higher accuracy multi-target trajectory estimation can be achieved in complex motion scenarios.

[0053] (3) The acceleration and angular velocity information provided by the inertial measurement unit in this invention are used to compensate for the gap in the scanning cycle of the millimeter-wave phased array radar; this fusion greatly improves the timeliness and trajectory continuity of the positioning of fast-moving targets, such as running people and moving vehicles.

[0054] (4) In this invention, the millimeter-wave phased array radar uses a 57~64GHz broadband phased array radar as the core sensor. The millimeter-wave phased array radar transmits frequency-modulated continuous wave signals to the monitoring area through its multi-transmit and multi-receive antennas and receives the echo signals scattered by each moving target in the monitoring area. After mixing and sampling, the echo discrete sequence is obtained and sent to the digital signal processor. After receiving the echo discrete sequence, the digital signal processor performs one-dimensional Fourier transform, two-dimensional Fourier transform, angle estimation and constant false alarm detection algorithm processing to obtain point cloud data corresponding to each moving target, including the distance, radial velocity and azimuth angle of the moving target, so as to achieve centimeter-level positioning accuracy for personnel and equipment. Compared with the traditional method, this invention can accurately detect the position of multiple targets at a long distance without attaching tags.

[0055] (5) The present invention can be applied in complex environments. Multiple positioning modules work together through wireless communication to achieve seamless positioning of moving targets over a wider range, ensuring reliable coverage in complex field environments.

[0056] In summary, by combining millimeter-wave phased array radar and inertial measurement unit, this invention greatly improves the accuracy and stability of moving target tracking. This invention can be used to provide timely and effective safety assurance for moving targets during power construction. Attached Figure Description

[0057] Figure 1 The diagram shown is a flowchart of the positioning method based on the fusion of millimeter-wave phased array radar and IMU according to Embodiment 1 of the present invention.

[0058] Figure 2 The diagram shown is a structural block diagram of a positioning system based on the fusion of millimeter-wave phased array radar and IMU according to Embodiment 2 of the present invention.

[0059] Figure 3 The figure shown is a schematic diagram of the position of the positioning module in Embodiment 2 of the present invention.

[0060] In the diagram: 1. First positioning module; 2. Second positioning module; 3. Third positioning module; 4. Main transformer. Detailed Implementation

[0061] To better explain and facilitate understanding of the present invention, the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0062] Example 1

[0063] like Figure 1 As shown, this embodiment is a positioning method based on the fusion of millimeter-wave phased array radar and IMU, including the following steps:

[0064] S1. When a moving target equipped with an inertial measurement unit enters a work area equipped with at least two sets of positioning modules, the positioning module with a corresponding monitoring area detects the moving target and sends the radar data of the detected moving target to a digital signal processor for processing through the millimeter-wave phased array radar in the positioning module; wherein, the monitoring areas of each positioning module are combined to cover the entire work area.

[0065] In this step, the millimeter-wave phased array radar in the positioning module sends the radar data of the detected moving target to the digital signal processor for processing as follows:

[0066] S11. The millimeter-wave phased array radar in the positioning module transmits frequency-modulated continuous wave signals to the monitoring area through its multi-transmitter, multi-receiver antenna and receives the echo signals scattered by each moving target in the monitoring area. The echo signals are mixed with the transmitted frequency-modulated continuous wave signals to obtain the beat intermediate frequency signals. The analog-to-digital converter in the millimeter-wave phased array radar samples the beat intermediate frequency signals to obtain the echo discrete sequence, and sends it to the digital signal processor.

[0067] S12. After receiving the echo discrete sequence, the digital signal processor first performs a one-dimensional Fourier transform along the fast time dimension to obtain the distance of the moving target; then it performs a two-dimensional Fourier transform along the slow time dimension to obtain the radial velocity of the moving target; then it uses a digital beamforming algorithm or a MIMO virtual array angle estimation algorithm to estimate the angle of the received echo discrete sequence to obtain the azimuth of the moving target; and constructs a three-dimensional data cube of distance-velocity-angle based on the distance, radial velocity, and azimuth of the moving target. It uses a constant false alarm rate (CFAR) detection algorithm to detect the target in the three-dimensional data cube, and outputs point cloud data containing the distance, radial velocity, and azimuth of the moving target for each target.

[0068] S2. The digital signal processor outputs point cloud data corresponding to the moving target and sends it to the MCU controller of the corresponding positioning module; at the same time, each inertial measurement unit wirelessly sends its unique device identification information and the detected acceleration and angular velocity data to the MCU controller of the corresponding positioning module.

[0069] S3. The MCU controller generates corresponding radar candidate target trajectories based on the point cloud data of the same moving target, and generates IMU candidate trajectories of the corresponding moving target based on the unique device identification information, acceleration and angular velocity data sent by each inertial measurement unit; based on the target association algorithm, the radar candidate target trajectories and IMU candidate trajectories at the same sampling time are associated and matched through the association threshold; after determining that they belong to the same moving target, the matching relationship between the point cloud data of the moving target and the corresponding unique device identification information is established.

[0070] In this step, the target association algorithm adopts the nearest neighbor association algorithm, the joint probabilistic data association algorithm, or the Hungarian matching algorithm.

[0071] When one or more moving targets enter the monitoring area of ​​the positioning module, the MCU controller generates corresponding radar candidate target trajectories based on the point cloud data of each moving target, i.e., generates one or more radar candidate target trajectories. In addition, the MCU controller generates corresponding IMU candidate trajectories based on the unique device identification information, acceleration, and angular velocity data sent by the inertial measurement unit of each moving target, i.e., generates one or more IMU candidate trajectories.

[0072] The process of generating the corresponding radar candidate target trajectory is as follows: the MCU controller performs target detection, clustering and trajectory initialization on the received point cloud data to generate the radar candidate target trajectory.

[0073] Based on the target association algorithm, the process of associating and matching radar candidate target trajectories and IMU candidate trajectories at the same sampling time through an association threshold is as follows: Based on the target association algorithm, an association cost is constructed according to the position distance, velocity difference, acceleration direction consistency, and timestamp difference between the radar candidate target trajectory and the IMU candidate trajectory at the same sampling time, and the radar candidate target trajectory and the IMU candidate trajectory are matched one by one according to the association threshold. When the association cost between a radar candidate target trajectory and an IMU candidate trajectory is less than the association threshold, it is determined that the point cloud data of the radar candidate target trajectory and the inertial measurement unit corresponding to the IMU candidate trajectory belong to the same moving target, and a matching relationship is established between the point cloud data of the moving target and the unique device identification information of the corresponding inertial measurement unit.

[0074] Specifically, when the association cost between a radar candidate target trajectory and multiple IMU candidate trajectories is less than the association threshold, the MCU controller, based on the unique device identification information carried by each IMU candidate trajectory, simultaneously associates the radar candidate target trajectory with the corresponding multiple IMU candidate trajectories, determines that the point cloud data of the radar candidate target trajectory and the inertial measurement units of the corresponding IMU candidate trajectories belong to the same moving target, and maintains the state estimation of the corresponding moving target based on the unique device identification information; when the spatial position and motion state of different moving targets differ, the radar candidate target trajectory and IMU candidate trajectory automatically restore the one-to-one correspondence matching relationship.

[0075] S4. The MCU controller inputs the point cloud data, acceleration and angular velocity data of the same moving target into the multi-target tracking algorithm to obtain the position of the moving target and predict the position of the moving target at the next moment.

[0076] In this step, the multi-target tracking algorithm includes an extended Kalman filter (EPF) algorithm, a trajectory prediction algorithm, a support vector machine (SVM) algorithm for auxiliary identification of moving target categories, and a reinforcement learning algorithm for adaptive adjustment of filter parameters. Specifically, the EPF algorithm is used to fuse and estimate the state of the corresponding moving target based on the point cloud data, acceleration, and angular velocity data of the same moving target after matching, obtaining the moving target state estimation result; the SVM algorithm is used to perform auxiliary identification of the moving target category based on the point cloud data, acceleration, and angular velocity data of the same moving target after matching, and uses the identification result as auxiliary information for target association and early warning judgment; the trajectory prediction algorithm is used to obtain the position of the moving target based on the moving target state estimation result output by the EPF algorithm, and predict the position of the moving target at the next moment; the reinforcement learning algorithm is used to dynamically adjust the process noise covariance matrix and observation noise covariance matrix parameters in the EPF algorithm to improve positioning accuracy and trajectory prediction accuracy.

[0077] The formula for the prediction model of the extended Kalman filter algorithm is:

[0078]

[0079]

[0080] in, It is the predicted value of the moving target's state. It is the prediction error covariance matrix. It is the state transition matrix. It is the process noise covariance matrix. It is the predicted value of the moving target's state at time k-1. It is the control input at time k-1. It is the error covariance matrix of the predicted state at time k-1; T is the transpose.

[0081] The formula for the update model of the extended Kalman filter algorithm is:

[0082]

[0083]

[0084]

[0085] in, It is Kalman gain. These are observed values. It is the observation noise covariance matrix. It is the observation matrix. It is a posterior state estimate. It is the error covariance matrix. It is an identity matrix.

[0086] The formula for the trajectory prediction algorithm is:

[0087]

[0088] in, It is the predicted state of the moving target. It is a reference trajectory. yes Time-based control input, yes Time-based control input, and It is a weight matrix. It refers to the number of prediction steps.

[0089] The target classification formula for support vector machines is:

[0090]

[0091] in, It is training data, including the acceleration and angular velocity data of the moving target. It's a tag. It is a Lagrange multiplier. Bias term, It is the inner product. Classification results It represents the total number of training data.

[0092] The reinforcement learning algorithm uses the Q-learning algorithm for policy optimization. The formula for the Q-learning algorithm is:

[0093]

[0094] in, It is a state Take action below Expected return It's the learning rate. It's an instant reward. It is a discount factor. It is a state Take action below Expected return.

[0095] S5. Based on the position of the moving target or the predicted position at the next moment, the MCU controller determines whether it has entered the preset warning area electronic fence; if so, it will issue an alarm through the alarm unit, and wirelessly send the position of the moving target and the alarm signal to the monitoring platform, and wirelessly send them to the other positioning modules.

[0096] In this step, the warning area electronic fence is a virtual warning area electronic fence formed based on a distance threshold from the warning area boundary to the center of the warning area. Based on the position of the moving target or its predicted position at the next moment, the distance from that position to the center of the warning area is calculated to determine whether the target is within the preset warning area electronic fence.

[0097] The distance threshold from the boundary of the warning area to the center of the warning area can be dynamically adjusted according to different voltage levels and operating conditions to ensure that the safe distance meets the requirements of the "Safety Work Regulations for Power Construction". Compared with fixed physical fences, this embodiment provides more flexible and comprehensive protection by setting up electronic fences for the warning area, and has the advantages of being adjustable and real-time.

[0098] In this step, when the monitoring areas of the positioning modules overlap, if a moving target enters the overlapping area, the MCU controllers of each positioning module communicate wirelessly, exchange the positions of the moving target obtained by each module or the predicted positions of the moving target at the next moment, and perform weighted fusion based on the position estimation accuracy of each positioning module to obtain the fused position of the moving target or the position of the moving target at the next moment. When the fused position of the moving target or the position of the moving target at the next moment is within the electronic fence of the warning area, the MCU controllers in the positioning modules will all sound an alarm through the alarm unit and wirelessly send the position of the moving target and the alarm signal to the monitoring platform.

[0099] Example 2

[0100] like Figure 2 As shown, this embodiment is a positioning system based on the fusion of millimeter-wave phased array radar and IMU, including at least two positioning modules, at least one inertial measurement unit and a monitoring platform.

[0101] Each positioning module includes a millimeter-wave phased array radar, a digital signal processor, an MCU controller, and an alarm unit. The output of the millimeter-wave phased array radar is connected to the input of the MCU controller through the digital signal processor. The output of the MCU controller is connected to the input of the alarm unit, and the MCU controller is wirelessly connected to the monitoring platform. The MCU controllers of each positioning module are wirelessly connected to each other. The MCU controller has embedded multi-target tracking algorithms and target association algorithms, and is equipped with an electronic fence for the warning area and association threshold thresholds.

[0102] Specifically, the MCU controller can wirelessly connect to the monitoring platform through the second wireless communication unit; the MCU controllers of each positioning module can wirelessly connect to each other through the third wireless communication unit.

[0103] Each inertial measurement unit (IMU) has a unique device identification information. The IMU is wirelessly connected to the MCU controller in each positioning module, and sends the unique device identification information, as well as the acceleration and angular velocity data of the detected moving target, to the MCU controller.

[0104] Specifically, the inertial measurement unit may include an inertial sensor and a first wireless communication unit. The inertial sensor has unique device identification information. The inertial sensor is wirelessly connected to the MCU controller in each positioning module through the first wireless communication unit, and is used to send the corresponding unique device identification information, as well as the acceleration and angular velocity data of the detected corresponding moving target, to the MCU controller.

[0105] In each positioning module of this embodiment, the millimeter-wave phased array radar is used to detect radar data of moving targets. After processing by the digital signal processor, it outputs point cloud data corresponding to each moving target and sends it to the MCU controller. The MCU controller uses a multi-target tracking algorithm, a target association algorithm, and an association threshold to match and fuse the point cloud data, acceleration, and angular velocity data of the same moving target to obtain the position of the moving target and predict its position at the next moment. Based on the position of the moving target or the predicted position at the next moment, it determines whether the target has entered the warning area electronic fence. If so, it alarms through the alarm unit and wirelessly sends the position of the moving target and the alarm signal to the monitoring platform.

[0106] In this embodiment, the millimeter-wave phased array radar uses a 57~64GHz broadband phased array radar as the core sensor, and the millimeter-wave phased array radar includes no fewer than three transmitting antennas and four receiving antennas. The positioning module's housing has an IP54 protection rating, adaptable to temperatures of -40~105℃ and strong electromagnetic interference. The first wireless communication unit, the second wireless communication unit, and the third wireless communication unit all use any one of the following modules: Bluetooth module, WIFI module, ZigBee module, and LoRa module.

[0107] To further illustrate the method of this embodiment, this embodiment uses an application example from a high-voltage substation maintenance operation site to describe the working process.

[0108] When performing maintenance on a high-voltage substation, three sets of positioning modules are installed within the maintenance work area: positioning module 1, positioning module 2, and positioning module 3. Figure 3 As shown, the first positioning module 1 is installed on the wall on one side of the main transformer 4, the second positioning module 2 is installed on the opposite wall, and the third positioning module 3 is installed on the tower at the entrance of the high-voltage substation. All three positioning modules have a monitoring area, and the combined monitoring area covers the entire working area. The monitoring areas of the three positioning modules overlap.

[0109] A small inertial sensor is fixed to the safety helmet of each maintenance worker and wirelessly connected to the MCU controller in each positioning module via Bluetooth module to provide unique equipment identification information as well as acceleration and angular velocity data of the maintenance worker's movement.

[0110] In the three positioning modules, the center of the main transformer 4 is used as the center of the warning area. Each MCU controller is set with two sets of distance thresholds from the boundary of the warning area to the center of the main transformer 4, forming two virtual warning area electronic fences. The first warning area electronic fence has a distance threshold of 3 meters from the boundary of the warning area to the center of the main transformer 4, that is, the area within 3 meters around the main transformer 4 is designated as the red warning area. The second warning area electronic fence has a distance of 3 meters from the inner boundary of the warning area to the center of the main transformer 4, and a distance of 5 meters from the outer boundary of the warning area to the center of the main transformer 4, that is, the area within 3 to 5 meters around the main transformer 4 is designated as the blue warning area.

[0111] The location of the maintenance personnel is obtained from the MCU controller of the positioning module. To the main transformer 4 center The distance is: ,like , If the distance threshold of the red alert zone is reached, it is determined that maintenance personnel have entered the red alert zone, triggering a red alert. An alarm is then triggered via the alarm unit, and the location of the maintenance personnel and the alarm signal are transmitted to the monitoring platform via the second wireless communication unit. , If the maximum value of the distance threshold for the blue alert zone is reached, it is determined that maintenance personnel have entered the blue alert zone, triggering a blue alert. An alarm is then triggered via the alarm unit, and the location of the maintenance personnel and the alarm signal are transmitted to the monitoring platform via the second wireless communication unit. If so, it is determined that the maintenance personnel are in the safe zone.

[0112] When the MCU controller of the positioning module calculates the distance to the center of main transformer 4 based on the predicted location of the maintenance personnel at the next moment, ,like However, if the maintenance personnel are actually still in the safe zone, it is assumed that they have entered the blue warning zone in the next moment, triggering a blue warning. An alarm will be triggered via the alarm unit, and the location of the maintenance personnel and the alarm signal will be transmitted to the monitoring platform via the second wireless communication unit. Similarly, if... However, if the maintenance personnel are actually in the blue warning zone, it is assumed that the maintenance personnel will enter the red warning zone in the next moment, triggering the red warning. The alarm unit will then issue an alarm and send the location of the maintenance personnel and the alarm signal to the monitoring platform through the second wireless communication unit.

[0113] A specific example is:

[0114] When maintenance work begins, the three positioning modules are activated and connected to the monitoring platform of the high-voltage substation via the second wireless communication unit. After the maintenance work begins, the first positioning module 1 detects a maintenance worker P1 approaching the main transformer 4 at a distance of approximately 6 meters, outside the safe zone. The first positioning module 1 begins tracking the maintenance worker P1. As the worker approaches the main transformer 4 to inspect the equipment, the first positioning module 1 receives and processes the discrete echo sequence of the worker's signals. Combining this with acceleration and angular velocity data detected by inertial sensors, the module uses a multi-target tracking algorithm to determine the worker's position and predict their next location. When the worker enters the blue warning zone boundary (5 meters from the center of the main transformer 4), the first positioning module 1 immediately triggers a blue warning, sounding an alarm. It then transmits the worker's position and alarm signal to the monitoring platform via the second wireless communication unit and to the remaining positioning modules via the third wireless communication unit. Upon receiving the alarm signal, the monitoring platform displays a blue alert.

[0115] Maintenance worker P1 then proceeded to a closer inspection, approaching the center of main transformer 4 and closing to within 3 meters, placing him at the boundary of the red warning zone. Due to the overlapping monitoring areas of the first positioning module 1 and the second positioning module 2, the second positioning module 2 also began receiving the discrete echo sequence of maintenance worker P1's signals. This data, combined with acceleration and angular velocity data detected by inertial sensors, aided in tracking. The first and second positioning modules 1 and 2 exchanged their predicted positions for maintenance worker P1 at the next moment via the third communication unit. After weighted fusion, if the fused result indicated that maintenance worker P1 was expected to enter the red warning zone, a red warning was immediately triggered, triggering a rapid buzzer alarm. The position of maintenance worker P1 and the alarm signal were then transmitted to the monitoring platform via the second wireless communication unit. Upon receiving the alarm signal, the monitoring platform marked it as a red alert and displayed an alarm window. Upon receiving the alarm, the on-site monitoring personnel quickly reminded maintenance worker P1 to retreat to avoid accidentally touching main transformer 4. The entire process, from maintenance worker P1 crossing the line to the alarm, took only about 100 milliseconds. After maintenance personnel P1 retreated to a safe distance, the first positioning module 1 detected that they had returned to the blue warning zone or even the safe zone, and the alarm was immediately downgraded or automatically deactivated.

[0116] During this process, maintenance personnel P2 entered the monitoring area of ​​the third positioning module 3, such as near the entrance of the high-voltage substation. Because they were not near dangerous equipment and only moved within the safe zone, no alarms were triggered. The inertial sensors of the third positioning module 3 and maintenance personnel P2 tracked P2 and sent their location to the monitoring platform for record-keeping.

[0117] The three positioning modules each process the targets within their respective monitoring areas. Simultaneously, they are integrated through a monitoring platform. On-site monitoring personnel can see the location and trajectory of maintenance personnel P1 and P2, as well as alarm records triggered by maintenance personnel P1, on the monitoring platform. If a maintenance personnel leaves the monitoring area of ​​one positioning module and enters the monitoring area of ​​another positioning module, the positioning module can ensure that the maintenance personnel are tracked continuously.

[0118] This embodiment demonstrates the working process and effects of the present invention: the positioning module accurately acquires the personnel's location using millimeter-wave phased array radar, the inertial sensor ensures accurate motion prediction, the electronic fence in the warning area promptly identifies boundary crossing behavior, and multi-device collaboration ensures comprehensive monitoring without blind spots. When personnel approach the dangerous main transformer 4, an alarm is quickly issued, successfully preventing a potential electric shock accident. Practice has proven that this embodiment can greatly improve operational safety in power locations such as substations.

[0119] It should be noted that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art can still modify the technical solutions described in the above embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A positioning method based on the fusion of millimeter-wave phased array radar and IMU, characterized in that, Includes the following steps: S1. When a moving target equipped with an inertial measurement unit enters a work area equipped with at least two sets of positioning modules, the positioning module with a corresponding monitoring area detects the moving target and sends the radar data of the detected moving target to a digital signal processor for processing through the millimeter-wave phased array radar in the positioning module; wherein, the monitoring areas of each positioning module are combined to cover the entire work area. S2. The digital signal processor outputs point cloud data corresponding to the moving target and sends it to the MCU controller of the corresponding positioning module; at the same time, each inertial measurement unit wirelessly sends its unique device identification information and the detected acceleration and angular velocity data to the MCU controller of the corresponding positioning module. S3. The MCU controller generates corresponding radar candidate target trajectories based on the point cloud data of the same moving target, and generates IMU candidate trajectories of the corresponding moving target based on the unique device identification information, acceleration and angular velocity data sent by each inertial measurement unit; based on the target association algorithm, the radar candidate target trajectories and IMU candidate trajectories at the same sampling time are associated and matched through an association threshold; after determining that they belong to the same moving target, a matching relationship is established between the point cloud data of the moving target and the corresponding unique device identification information. S4. The MCU controller inputs the point cloud data, acceleration and angular velocity data of the same moving target into the multi-target tracking algorithm to obtain the position of the moving target and predict the position of the moving target at the next moment. S5. Based on the position of the moving target or the predicted position at the next moment, the MCU controller determines whether it has entered the preset warning area electronic fence; if so, it will issue an alarm through the alarm unit, and wirelessly send the position of the moving target and the alarm signal to the monitoring platform, and wirelessly send them to the other positioning modules.

2. The positioning method based on the fusion of millimeter-wave phased array radar and IMU according to claim 1, characterized in that, In step S3, the process of generating the corresponding radar candidate target trajectory is as follows: the MCU controller performs target detection, clustering and trajectory initialization on the received point cloud data to generate the radar candidate target trajectory; Based on the target association algorithm, the process of associating and matching radar candidate target trajectories and IMU candidate trajectories at the same sampling time through an association threshold is as follows: Based on the target association algorithm, an association cost is constructed according to the position distance, velocity difference, acceleration direction consistency and timestamp difference between radar candidate target trajectories and IMU candidate trajectories at the same sampling time. The radar candidate target trajectories and IMU candidate trajectories are matched one by one according to the association threshold. When the association cost between a radar candidate target trajectory and an IMU candidate trajectory is less than the association threshold, it is determined that the point cloud data of the radar candidate target trajectory and the inertial measurement unit corresponding to the IMU candidate trajectory belong to the same moving target, and a matching relationship is established between the point cloud data of the moving target and the unique device identification information of the corresponding inertial measurement unit.

3. The positioning method based on the fusion of millimeter-wave phased array radar and IMU according to claim 1 or 2, characterized in that, In step S3, the target association algorithm adopts the nearest neighbor association algorithm, the joint probability data association algorithm, or the Hungarian matching algorithm; The multi-target tracking algorithm includes an extended Kalman filter algorithm, a trajectory prediction algorithm, a support vector machine algorithm for auxiliary identification of moving target categories, and a reinforcement learning algorithm for adaptive adjustment of filter parameters. The extended Kalman filter algorithm is used to fuse and estimate the state of the corresponding moving target based on the point cloud data, acceleration and angular velocity data of the same moving target after matching, so as to obtain the state estimation result of the moving target; Support vector machines are used to perform category-assisted identification of moving targets based on point cloud data, acceleration and angular velocity data of the same moving target after matching, and the identification results are used as auxiliary information for target association and early warning judgment. The trajectory prediction algorithm is used to obtain the position of the moving target based on the state estimation result of the moving target output by the extended Kalman filter algorithm, and to predict the position of the moving target at the next moment. Reinforcement learning algorithms are used to dynamically adjust the parameters of the process noise covariance matrix and the observation noise covariance matrix in the extended Kalman filter algorithm.

4. The positioning method based on the fusion of millimeter-wave phased array radar and IMU according to claim 3, characterized in that, The formula for the prediction model of the extended Kalman filter algorithm is as follows: in, It is the predicted value of the moving target's state. It is the prediction error covariance matrix. It is the state transition matrix. It is the process noise covariance matrix. It is the predicted value of the moving target's state at time k-1. It is the control input at time k-1. It is the error covariance matrix of the predicted state at time k-1; T is the transpose; The formula for the update model of the extended Kalman filter algorithm is: in, It is Kalman gain. These are observed values. It is the observation noise covariance matrix. It is the observation matrix. It is a posterior state estimate. It is the error covariance matrix. It is an identity matrix.

5. The positioning method based on the fusion of millimeter-wave phased array radar and IMU according to claim 3, characterized in that, The formula for the trajectory prediction algorithm is: in, It is the predicted state of the moving target. It is a reference trajectory. yes Time-based control input, yes Time-based control input, and It is a weight matrix. It refers to the number of prediction steps.

6. The positioning method based on the fusion of millimeter-wave phased array radar and IMU according to claim 3, characterized in that, The target classification formula of the support vector machine is: in, It is training data. It's a tag. It is a Lagrange multiplier. Bias term, It is the inner product. Classification results It is the total number of training data; The reinforcement learning algorithm strategy is optimized using the Q-learning algorithm. The formula for the Q-learning algorithm is: in, It is a state Take action below Expected return It's the learning rate. It's an instant reward. It is a discount factor. It is a state Take action below Expected return.

7. The positioning method based on the fusion of millimeter-wave phased array radar and IMU according to claim 1 or 2, characterized in that, In step S1, the process by which the millimeter-wave phased array radar in the positioning module sends the radar data of the detected moving target to the digital signal processor for processing is as follows: S11. The millimeter-wave phased array radar in the positioning module transmits frequency-modulated continuous wave signals to the monitoring area through its multi-transmitter, multi-receiver antenna and receives the echo signals scattered by each moving target in the monitoring area. The echo signals are mixed with the transmitted frequency-modulated continuous wave signals to obtain the beat intermediate frequency signals. The analog-to-digital converter in the millimeter-wave phased array radar samples the beat intermediate frequency signals to obtain the echo discrete sequence, and sends it to the digital signal processor. S12. After receiving the echo discrete sequence, the digital signal processor first performs a one-dimensional Fourier transform along the fast time dimension to obtain the distance of the moving target; then it performs a two-dimensional Fourier transform along the slow time dimension to obtain the radial velocity of the moving target; then it uses a digital beamforming algorithm or a MIMO virtual array angle estimation algorithm to estimate the angle of the received echo discrete sequence to obtain the azimuth of the moving target. A three-dimensional data cube of distance-velocity-angle is constructed based on the distance, radial velocity, and azimuth of the moving target. A constant false alarm rate (CFAR) detection algorithm is used to detect the target in the three-dimensional data cube. After detection, point cloud data containing the distance, radial velocity, and azimuth of the moving target are output.

8. The positioning method based on the fusion of millimeter-wave phased array radar and IMU according to claim 1 or 2, characterized in that, In step S5, when the monitoring areas of the positioning modules overlap, if a moving target enters the overlapping area, the MCU controllers of each positioning module communicate wirelessly, exchange the positions of the moving target obtained by each module or the predicted positions of the moving target at the next moment, and perform weighted fusion based on the position estimation accuracy of each positioning module to obtain the fused position of the moving target or the position of the moving target at the next moment; when the fused position of the moving target or the position of the moving target at the next moment is within the electronic fence of the warning area, the MCU controllers in the positioning modules all trigger an alarm through the alarm unit and wirelessly send the position of the moving target and the alarm signal to the monitoring platform.

9. A positioning system based on the fusion of millimeter-wave phased array radar and IMU, used to implement the positioning method based on the fusion of millimeter-wave phased array radar and IMU as described in any one of claims 1-8, characterized in that, It includes at least two positioning modules, at least one inertial measurement unit, and a monitoring platform; Each positioning module includes a millimeter-wave phased array radar, a digital signal processor, an MCU controller, and an alarm unit. The output of the millimeter-wave phased array radar is connected to the input of the MCU controller via the digital signal processor. The output of the MCU controller is connected to the input of the alarm unit, and the MCU controller is wirelessly connected to the monitoring platform. The MCU controllers of each positioning module are wirelessly connected to each other. The MCU controller has embedded multi-target tracking and target association algorithms, and is equipped with an electronic fence for the warning area and association threshold. Each inertial measurement unit has a unique device identification information. The inertial measurement unit is wirelessly connected to the MCU controller in each positioning module and sends the unique device identification information, as well as the acceleration and angular velocity data of the detected moving target, to the MCU controller. In each positioning module, a millimeter-wave phased array radar is used to detect radar data of moving targets. After processing by a digital signal processor, the radar outputs point cloud data corresponding to each moving target and sends it to the MCU controller. The MCU controller uses a multi-target tracking algorithm, a target association algorithm, and an association threshold to match and fuse the point cloud data, acceleration, and angular velocity data of the same moving target to obtain the position of the moving target and predict its position at the next moment. Based on the position of the moving target or the predicted position at the next moment, it determines whether the target has entered the warning area electronic fence. If so, it triggers an alarm through the alarm unit and wirelessly sends the position of the moving target and the alarm signal to the monitoring platform.

10. The positioning system based on the fusion of millimeter-wave phased array radar and IMU according to claim 9, characterized in that, The millimeter-wave phased array radar uses a 57-64GHz broadband phased array radar as its core sensor, and includes no fewer than three transmitting antennas and four receiving antennas.

Images