CN115220028A - Millimeter wave-based non-portable equipment positioning and household activity sensing method - Google Patents

Millimeter wave-based non-portable equipment positioning and household activity sensing method Download PDF

Info

Publication number
CN115220028A
CN115220028A CN202210826642.1A CN202210826642A CN115220028A CN 115220028 A CN115220028 A CN 115220028A CN 202210826642 A CN202210826642 A CN 202210826642A CN 115220028 A CN115220028 A CN 115220028A
Authority
CN
China
Prior art keywords
point cloud
millimeter wave
positioning
point
wave radar
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210826642.1A
Other languages
Chinese (zh)
Inventor
陈锐志
李维
吴源
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Deqing Zhilu Navigation Technology Co ltd
Original Assignee
Zhejiang Deqing Zhilu Navigation Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhejiang Deqing Zhilu Navigation Technology Co ltd filed Critical Zhejiang Deqing Zhilu Navigation Technology Co ltd
Priority to CN202210826642.1A priority Critical patent/CN115220028A/en
Publication of CN115220028A publication Critical patent/CN115220028A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/06—Systems determining position data of a target
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
    • G01S7/40—Means for monitoring or calibrating

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

The invention discloses a millimeter wave-based non-portable equipment positioning and home activity sensing method, relates to the technical field of indoor positioning and activity sensing, and aims to solve the problem that a non-portable equipment positioning and home activity sensing technology for realizing whole-area sensing, safety and reliability and privacy protection is lacked; starting from radar point cloud data, a complete radar sensor space layout and parameter calibration scheme is designed, multi-radar cooperation continuous tracking and positioning can be achieved, the problems of shielding in the environment and long-distance signal attenuation are solved, and the large-range complex indoor environment can be met; according to the parameter characteristics of the distance measurement and angle measurement range of the sensor and the physical characteristics of the radar point cloud, a point cloud preprocessing method is designed, the multi-path problem of the point cloud under the indoor environment is effectively eliminated, and the robustness of the subsequent positioning technology is improved.

Description

Millimeter wave-based non-portable equipment positioning and household activity sensing method
Technical Field
The invention relates to the technical field of indoor positioning and activity sensing, in particular to a millimeter wave-based non-portable equipment positioning and household activity sensing method.
Background
The aging of population is one of the serious problems to be solved urgently in the rapid development of cities, and the indoor old people assisted living health monitoring technology becomes an important research field. Location estimation is a key requirement for implementing an indoor LBS (location based service), such as healthcare, security management, etc. For the health of the old and the perception of the household activities, not only the position of the user needs to be estimated in real time, but also the user cannot be required to carry professional positioning equipment all the time.
Currently, there are many methods for estimating indoor position, such as bluetooth, UWB, audio, etc. These methods can provide highly accurate tracking results, but must meet the premise that the user and his signal receiving device are not readily separable, which is a challenge for the elderly, who cannot always remember to wear or carry the device, and who may always be misplaced. Additionally, vision-based technologies can enable device-no-carry positioning, however, given privacy concerns and camera invasiveness, vision-based technologies have had little user acceptance in both home and business environments.
Equipment-free positioning based on the millimeter wave radar is also researched at present, but the existing scheme focuses on sensing slow moving person information including breathing, turning head and other actions from the original electronic signal level. The precondition for obtaining the above-mentioned micro-motion is that a sufficiently close sensing distance is required.
In summary, a technology for realizing location without a portable device and sensing activities at home under the conditions of whole-area sensing, safety, reliability and privacy protection is lacking at present.
Disclosure of Invention
In view of the problems in the prior art, the invention discloses a millimeter wave-based portable device-free positioning and home activity sensing method.
As a preferred technical scheme of the invention, the millimeter wave radar sensor space layout and parameter calibration comprises a millimeter wave radar space layout scheme for positioning scene size and layout, so that the requirement of full coverage on the scene is met, and meanwhile, the sensor layout cost is maximally reduced; and the millimeter wave radar sensor parameter calibration system calibrates the internal parameters and the external parameters of the sensor.
As a preferred technical scheme of the invention, the interference point cloud eliminating method based on the self parameters of the millimeter wave radar sensor and the spatial geometric constraint of the millimeter wave radar sensor comprises a multipath false point eliminating method based on the parameter constraints of the distance measuring range, the field angle and the like of the millimeter wave radar sensor and the spatial geometric constraint of a positioning scene and an environment background point cloud eliminating method based on the millimeter wave radar point cloud characteristics in the unmanned static environment.
As a preferred technical scheme of the invention, the pedestrian target positioning and tracking based on millimeter radar point cloud data comprises the steps of designing a user positioning algorithm based on optics clustering algorithm according to the continuity and the aggregativeness of dynamic target reflection point cloud on the spatial distribution; on the basis of the point cloud primary clustering, dynamic multipath false point elimination is realized according to the characteristics of the point cloud such as radial speed and the like; and further designing a target tracking and trajectory estimation algorithm based on the Hungarian algorithm and the Kalman filtering algorithm.
As a preferred technical scheme of the invention, the sensing of the user household activity based on the millimeter wave radar comprises designing a sensing method of the user household activity based on the combination of a positioning and tracking result of the millimeter wave radar and semantic information of a space map.
As a preferred technical scheme of the invention, the installation coordinates of the millimeter wave radar sensor are designed according to the effective coverage range of signals of the millimeter wave radar sensor and the area size of a scene, the installation coordinates comprise X, Y and Z, postures and installation quantity M, the specific layout criteria are that the postures are pitch angles pitch, roll angles roll and course angles yaw, the installation height is between 2 meters and 3.5 meters, the installation position is close to a wall corner or a wall surface, the pitch angle of the installation posture is between-10 degrees and-20 degrees, the roll angles are 0 +/-0.5 degrees, the course angles are manually adjusted according to the shape of a positioning space and are calibrated by a subsequent calibration method; the internal parameters of the sensor refer to a millimeter wave radar ranging error scaling factor, and the external parameters of the sensor comprise the posture and the position of the millimeter wave radar sensor relative to a local navigation coordinate system; the millimeter wave radar sensors are arranged in a scene, and internal parameters and external parameters of each millimeter wave radar sensor are calibrated, wherein the calibration method comprises the following steps:
step 101, external parameter calibration: the conversion formula of the point cloud data between the radar sensor coordinate system and the positioning coordinate system can be expressed as follows:
Figure BDA0003746838160000031
wherein P is n ＝[x n ,y n ,z n ] T Representing the coordinates of the point cloud in a localization coordinate system, P r ＝[x r ,y r ,z r ] T Representing the coordinates of the point cloud in the sensor coordinate system,
Figure BDA0003746838160000032
transformation matrix representing sensor coordinate system to positioning coordinate system, comprising three-dimensional rotation R = [ pitch, roll, yaw =] T And three-dimensional translation t = [ t ] x ,t y ,t z ] T (ii) a Wherein, the pitch angle pitch and roll angle roll in the three-dimensional rotation are calibrated by directly measuring the true value of the pitch angle pitch and roll angle roll by a goniometer; the course angle yaw and the three-dimensional translation t are calibrated by using a least square method:
wherein P is n ＝[x n ,y n ,z n ] T Representing the coordinates of the point cloud in a localization coordinate system, P r ＝[x r ,y r ,z r ] T Representing the coordinates of the point cloud in the sensor coordinate system,
Figure BDA0003746838160000033
transformation matrix representing sensor coordinate system to positioning coordinate system, comprising three-dimensional rotation R = [ pitch, roll, yaw =] T And three-dimensional translation t = [ t ] x ,t y ,t z ] T (ii) a Wherein pitch and yaw in three-dimensional rotationThe roll angle roll is calibrated by directly measuring the true value of the roll angle roll by an angle meter; the course angle yaw and the three-dimensional translation t are calibrated by using a least square method:
placing corner reflectors for point cloud calibration at calibration points of N known positions in positioning space, and recording true values of each calibration point in a positioning coordinate system
Figure BDA0003746838160000034
And measured values in the sensor coordinate system
Figure BDA0003746838160000035
Wherein i =1,2, \8230, N; converting the measured values in the sensor coordinate system to the intermediate transition coordinate system according to the known measured pitch angle and roll angle:
Figure BDA0003746838160000036
the transformation relationship from the intermediate transition coordinate system to the positioning coordinate system can be expressed as:
Figure BDA0003746838160000037
wherein
Figure BDA0003746838160000038
The point cloud coordinate conversion relationship from the intermediate transition coordinate system to the positioning coordinate system can be rearranged as:
Figure BDA0003746838160000041
order to
Figure BDA0003746838160000042
The above formula is described again
Figure BDA0003746838160000043
N (≧ 2) sets of measurements-true values can constitute N (≧ 2) of the above-described observation equations:
Figure BDA0003746838160000044
further order
Figure BDA0003746838160000045
H＝[H 1 H 2 … H N ] T (ii) a Thus, by using the small two-times algorithm, X can be solved by the following formula:
X＝(H T H) -1 H T Y
wherein the heading parameter can be selected from
Figure BDA0003746838160000046
Calculating to obtain;
step 102, calibrating internal parameters:
transformation matrix from known sensor coordinate system to positioning coordinate system
Figure BDA0003746838160000047
On the premise of (3), the three-axis error scaling factor of the radar point cloud measured by the millimeter wave radar sensing is as follows: s = [ S ] x s y s z ] T Then, there is the following conversion relationship:
Figure BDA0003746838160000048
and (3) solving a triaxial error scaling factor S of the millimeter wave radar by using N (more than or equal to 2) groups of measurement-true values and a least square algorithm.
As a preferred technical scheme of the invention, the multipath false point eliminating method based on parameter constraints such as the range of the millimeter wave radar sensor, the field angle and the like and the space geometric constraint of the positioning scene comprises the following steps:
step 201, directly eliminating point clouds with distance measurement values larger than a distance measurement range by using the set distance measurement range r of the millimeter wave radar sensor;
202, utilizing the self horizontal and vertical field angle parameter theta of the millimeter wave radar sensor h ,θ v Directly removing the point clouds of which the horizontal incident angles and the vertical incident angles are larger than the corresponding field angles in the measured point clouds;
and step 203, directly removing the point clouds exceeding any parameters in the measured point clouds by using the geometric size parameters [ length, width and height ] of the positioning space.
As a preferred technical scheme of the invention, the method for eliminating the environmental background point cloud based on the millimeter wave radar point cloud characteristics in the unmanned static environment comprises the following steps:
step 204, collecting millimeter wave radar point cloud data in an unmanned static environment within a period of time, and establishing a background point cloud data set E;
and step 205, eliminating the point clouds appearing in the background point cloud data set from each frame of point cloud data measured in real time to realize background point cloud elimination.
As a preferred technical scheme of the invention, the step of designing the user positioning algorithm based on the optics clustering algorithm according to the continuity and the aggregation of the dynamic target reflection point cloud on the spatial distribution comprises the following steps:
consider a point cloud data set X = { X = 1 ,x 2 ,…,x N And calculating the core distance cd of each point cloud by the following steps under the condition of giving a clustering radius parameter epsilon and the minimum point cloud number M in the radius 1 ,…,cd i ,…,cd N And achievable distance rd = { rd = } 1 ,…,rd i ,…,rd N And point cloud output list P:
step 301, for any point cloud data set X = { X = 1 ,x 2 ,…,x N Giving a clustering parameter which belongs to E and M;
step 302, calculating a distance matrix D between all point clouds, initializing whether an element of an access list V for marking whether the point clouds are processed is zero, and indicating that all the point clouds are not accessed; initializing seed list S as null; initializing a point cloud output list P to be empty;
step 303, taking out any point cloud which is not visited from the point cloud data set X, and adding the point cloud into the seed list S;
step 304, take out the first point cloud x from the seed list S i Wherein, i is more than or equal to 1 and less than or equal to N represents the serial number of the point cloud in the set X; set this point to visited in V and x i Adding the data to the tail of the output list P; obtaining x i A point cloud set H in the neighborhood radius is formed; if the number of sets H is less than M, the point cloud x i Is set to infinity, x i Judging as a non-core point; otherwise, x is changed i Determining as a core point, and calculating the core distance cd of the point cloud according to the following formula i And the achievable distance rd of each point in H j ：
Figure BDA0003746838160000051
Figure BDA0003746838160000052
Wherein d (-) represents the Euclidean distance, H j (x i ) Representing a distance x in the set H i J is more than or equal to 1 and less than or equal to num (H) at the j-th point; finally, adding the point clouds in the set H into a seed list S, and sorting according to the reachable distance;
step 305, for the same point cloud in the list S, keeping the point cloud with the minimum reachable distance, and updating the reachable distance of the point cloud to the reachable distance of the point cloud;
step 306, if the point cloud data set X is not empty, repeating the steps 3,4 and 5; otherwise, outputting an output list P of the point cloud, and the core distance and the reachable distance of each point;
step 307, set a new neighborhood radius
Figure BDA0003746838160000061
According to the output point cloud core distance cd, the achievable distance rd and the point cloud output list P, determining the cluster label of each point cloud through the following algorithm:
Figure BDA0003746838160000062
step 308, extracting point clouds with the same cluster label to form a set according to the cluster label L to which each point cloud output in the step 7 belongs, and taking the average position of the point clouds in the set as the positioning result of the cluster;
if the seed list S is not empty in the above steps, repeating the steps 4 and 5; otherwise, executing step 6; based on the point cloud primary clustering, according to the characteristics of point cloud radial velocity and the like, dynamic multipath false point elimination is realized, and for each clustering cluster, if the mean value of the radial velocity of the point cloud in the cluster is smaller than a set threshold mu and the variance of the radial velocity of the point cloud in the cluster is smaller than a set threshold sigma, all the point clouds in the cluster are judged as multipath false points and eliminated;
according to the further design of a target tracking and trajectory estimation algorithm based on the Hungarian algorithm and the Kalman filtering algorithm, the target tracking algorithm based on the Hungarian algorithm is designed, firstly, a new tracking task is created for a point cloud cluster entering a new scene, and the tracking task which cannot be associated within a certain time is deleted; and then, associating the output result of the point cloud cluster at the current moment with the current tracking task by using a Hungarian algorithm, and mainly comprising the following steps of:
suppose that the tracking task set at the current time is U = { U = 1 ,u 2 ,…,u n In which u i ＝{p i ,v i }，p i And v i Respectively representing trace tasks u i The position and the speed of the current moment under a positioning coordinate system; the set of positioning results of each point cloud cluster at the current moment is W = { W = { (W) 1 ,w 2 ,…,w m Therein of
Figure BDA0003746838160000071
And
Figure BDA0003746838160000072
respectively representing point cloud cluster positioning results w i Position and phase of current time in positioning coordinate systemFor millimeter wave radar sensor position P L The radial velocity of (a); constructing a distance matrix between U and W elements
Figure BDA0003746838160000073
Wherein the ith row and the jth column
Figure BDA0003746838160000074
Indicating the position u i To position w j The Euclidean distance of (c); if m ≠ n, then the matrix is aligned
Figure BDA0003746838160000075
Adding all-zero virtual rows or columns to form a square matrix; then, a global best association is found by:
step 401, let the matrix
Figure BDA0003746838160000076
Subtracting the minimum number in each row from each row, and subtracting the minimum number in each column from each column to obtain the matrix
Figure BDA0003746838160000077
Step 402, decovering the matrix with a minimum of row and column combinations
Figure BDA0003746838160000078
If the required minimum number of rows and columns is equal to the maximum of both m and n, an optimal association can be made between
Figure BDA0003746838160000079
If found, executing step 4; otherwise, executing step 3 and continuing to update
Figure BDA00037468381600000710
Step 403, decover the matrix with minimal combinations of rows and columns in step 402
Figure BDA00037468381600000711
Finding the minimum value of the other uncovered elements after all the zero elements are found, wherein the minimum value is defined as s, and all the zero elements are taken as
Figure BDA00037468381600000712
S is subtracted from the uncovered elements and let
Figure BDA00037468381600000713
The element covered by both the row and column in step 402 is added to s to obtain a new one
Figure BDA00037468381600000714
Then, step 402 is executed;
step 404, at
Figure BDA00037468381600000715
The row i and column j in which the zero element is located represent the corresponding u i And w j There may be a correlation if the row and column in which the element is located is removed, leaving a sub-matrix in the remainder
Figure BDA00037468381600000716
If an optimal association can still be found, u is determined i And w j Performing correlation output, and continuing to output the rest sub-matrixes
Figure BDA00037468381600000717
Finding the best correlation; otherwise u i And w j The association can not be carried out, and the rest elements which are zero are inspected;
designing a trajectory estimation algorithm based on a Kalman filtering algorithm:
tracking task and point cloud cluster positioning result on one pair of associations at the moment of k +1
Figure BDA0003746838160000081
Firstly, a system equation is established by using the assumption of uniform linear motion, and
Figure BDA0003746838160000082
shape at the next momentAnd (3) state estimation:
Figure BDA0003746838160000083
wherein epsilon k Representing zero mean Gaussian white noise in the state estimation process, and delta t representing the time interval of adjacent moments; positioning result of point cloud cluster at k +1 moment
Figure BDA0003746838160000084
And predicting the estimation result
Figure BDA0003746838160000085
The relationship between the two can be expressed by the following measurement equation:
Figure BDA0003746838160000086
wherein
Figure BDA0003746838160000087
Representing a velocity vector
Figure BDA0003746838160000088
And the slave position
Figure BDA0003746838160000089
To millimeter wave radar sensor position P L Direction vector of (2)
Figure BDA00037468381600000810
The included angle between them; rho k Represents the measured zero-mean gaussian white noise; the method for estimating the current tracking track state by using the Kalman filtering method mainly comprises the following steps:
step 405, predicting the current state:
Figure BDA00037468381600000811
wherein
Figure BDA00037468381600000812
A state transition matrix is represented that represents the state transition,
Figure BDA00037468381600000813
representing the state estimation error covariance matrix, Q k Representing state noise error covariance;
step 406 updates the predicted state with the measurement: firstly, calculating Kalman gain:
Figure BDA00037468381600000814
then the state is updated:
Figure BDA00037468381600000815
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA00037468381600000816
representing a measurement matrix; r is k Representing the measurement noise error covariance matrix.
As a preferred technical scheme of the invention, the user home activity perception method based on the combination of the millimeter wave radar positioning and tracking result and the space map semantic information comprises the following steps;
step 501, utilizing the target positioning and tracking result and combining semantic information of a space map to realize the household activity perception of a user;
step 502, according to the household activity sensing result, counting the daily work and rest rule of the user, according to the high correlation between the spatial position and the spatial activity in the household environment, when the positioning target is detected to be in the same position area within a certain time, judging that the positioning target is engaged in the household activity related to the area, and setting the activity type set needing to be sensed as A = { a = 1 ,a 2 ,…,a n In which a is i ＝{name i ,region i ,duration i Denotes the ith activity, name i Representing the name, region, of the activity i Indicates the area extent, duration, of the activity i Indicates the most determined type of activityA low duration threshold;
step 503, when the positioning target is present in a region within a period of time Γ i Accumulated time t ofDuration or more i Then it is determined that the primary activity during the period of time includes an activity name i 。
The invention has the beneficial effects that: 1. starting from radar point cloud data, a complete radar sensor space layout and parameter calibration scheme is designed, multi-radar cooperation continuous tracking and positioning can be achieved, the problems of shielding in the environment and long-distance signal attenuation are solved, and the large-range complex indoor environment can be met.
2. According to the parameter characteristics of the distance measurement and angle measurement range of the sensor and the physical characteristics of the radar point cloud, a point cloud preprocessing method is designed, the multi-path problem of the point cloud under the indoor environment is effectively eliminated, and the robustness of the subsequent positioning technology is improved.
3. According to the method, the trajectory estimation is carried out by using the Kalman filtering algorithm based on millimeter radar point cloud data, the target can be predicted according to historical data by using Kalman filtering under the condition that the target is lost, and the smoothness of target tracking is ensured.
4. The method relies on continuous position perception of the target user in an indoor complex scene, designs the user home activity perception method, provides useful data for analyzing the home living state of the target user, and can provide basic data for application services such as home health maintenance of the old on the premise of no privacy disclosure.
Drawings
In order to more clearly illustrate the detailed description of the invention or the technical solutions in the prior art, the drawings that are needed in the detailed description of the invention or the prior art will be briefly described below. The elements or parts are not necessarily drawn to scale in all figures.
FIG. 1 is a schematic flow chart of the millimeter wave based portable device location and home activity awareness technology of the present invention;
FIG. 2 is a schematic overall algorithm flow diagram of the present invention;
FIG. 3 is a schematic diagram of the sensor layout and sensor coordinates and positioning coordinates framework of the present invention;
FIG. 4 is a schematic diagram of a final layout scheme of the millimeter wave radar of the present invention;
FIG. 5 is a first diagram illustrating a point cloud data preprocessing result of the millimeter wave radar according to the present invention;
FIG. 6 is a diagram II illustrating a point cloud data preprocessing result of the millimeter wave radar according to the present invention;
FIG. 7 is a schematic diagram of a cluster localization result based on point cloud according to the present invention;
FIG. 8 is a diagram illustrating the result of the target tracking trajectory continuously according to the present invention.
Detailed Description
The technical scheme of the invention is clearly and completely described in the following with reference to the accompanying drawings. In the description of the present invention, it should be noted that the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc., indicate orientations or positional relationships based on the orientations or positional relationships shown in the drawings, and are only for convenience of description and simplicity of description, but do not indicate or imply that the device or element being referred to must have a particular orientation, be constructed and operated in a particular orientation, and thus, should not be construed as limiting the present invention. Furthermore, the terms "first," "second," and "third" are used for descriptive purposes only and are not to be construed as indicating or implying a relative importance.
In the description of the present invention, it should be noted that, unless otherwise explicitly specified or limited, the terms "mounted," "connected," and "connected" are to be construed broadly and may be, for example, fixedly connected, detachably connected, or integrally connected; can be mechanically or electrically connected; they may be connected directly or indirectly through intervening media, or they may be interconnected between two elements. The specific meanings of the above terms in the present invention can be understood in a specific case to those of ordinary skill in the art.
Example 1
As shown in fig. 1 to 8, the invention discloses a millimeter wave-based non-portable device positioning and home activity sensing method, which adopts the technical scheme that the method comprises sensor arrangement, parameter calibration, data preprocessing, target positioning and tracking, and home activity sensing.
The technology of the scheme comprises the following specific steps:
step 1, spatial layout and parameter calibration of millimeter wave radar sensor
According to effective coverage of millimeter wave radar sensor signal and scene area size, design millimeter wave radar sensor's installation coordinate, the installation coordinate has X, Y, Z axle, gesture and installation quantity M, the gesture still includes pitch angle pitch, roll angle roll, course angle yaw, and specific rule of laying is: the installation height is 2 meters, the installation position is close to a wall corner or a wall surface, the pitch angle of the installation posture is-10 degrees, the roll angle is 0 degree, the course angle is manually adjusted according to the shape of the positioning space and is calibrated by a subsequent calibration method, as shown in figure 3;
the parameter calibration of the sensor is divided into internal parameters and external parameters, the internal parameters of the sensor refer to radar ranging error scaling factors, and the external parameters comprise the attitude and the position of the millimeter-wave radar sensor relative to a local navigation coordinate system; the millimeter wave radar sensors are arranged in a scene, and internal parameters and external parameters of each millimeter wave radar sensor are calibrated, wherein the calibration method comprises the following steps:
step 101, external parameter calibration: the conversion formula of the point cloud data between the radar sensor coordinate system and the positioning coordinate system can be expressed as follows:
Figure BDA0003746838160000111
wherein P is n ＝[x n ,y n ,z n ] T Representing the coordinates of the point cloud in a localization coordinate system, P r ＝[x r ,y r ,z r ] T Representing the coordinates of the point cloud in the sensor coordinate system,
Figure BDA0003746838160000112
representing the sensor coordinate systemTransformation matrix to positioning coordinate system comprising three-dimensional rotation R = [ pitch, roll, yaw =] T And three-dimensional translation t = [ t ] x ,t y ,t z ] T (ii) a Wherein, the pitch angle pitch and roll angle roll in the three-dimensional rotation are calibrated by directly measuring the true value of the pitch angle pitch and roll angle roll by a goniometer; the course angle yaw and the three-dimensional translation t are calibrated by using a least square method:
placing corner reflectors for point cloud calibration at calibration points of N known positions in positioning space, and recording true values of each calibration point in a positioning coordinate system
Figure BDA0003746838160000113
And measured values in the sensor coordinate system
Figure BDA0003746838160000121
Wherein i =1,2, \8230;, N; converting the measured values in the sensor coordinate system to the intermediate transition coordinate system according to the known measured pitch angle and roll angle:
Figure BDA0003746838160000122
the transformation relationship from the intermediate transition coordinate system to the positioning coordinate system can be expressed as:
Figure BDA0003746838160000123
wherein
Figure BDA0003746838160000124
The point cloud coordinate transformation relationship from the intermediate transition coordinate system to the positioning coordinate system can be rearranged as:
Figure BDA0003746838160000125
order to
Figure BDA0003746838160000126
The above formula is described again
Figure BDA0003746838160000127
N (≧ 2) sets of measurements-true values can constitute N (≧ 2) of the above-described observation equations:
Figure BDA0003746838160000128
further make a command
Figure BDA0003746838160000129
H＝[H 1 H 2 … H N ] T (ii) a Thus, by using the small two-times algorithm, X can be obtained by solving the following formula:
X＝(H T H) -1 H T Y
wherein the heading parameter can be selected from
Figure BDA00037468381600001210
Calculating to obtain;
step 102, internal parameter calibration:
transformation matrix from known sensor coordinate system to positioning coordinate system
Figure BDA00037468381600001211
On the premise of (1), the triaxial error scaling factor of the radar point cloud measured by the millimeter wave radar sensing is as follows: s = [ S ] x s y s z ] T Then, there is the following conversion relationship:
Figure BDA00037468381600001212
by utilizing N (more than or equal to 2) groups of measurement-true values and a least square algorithm, a triaxial error scaling factor S of the millimeter wave radar can be obtained;
step 2, a multipath false point removing method based on parameter constraints such as a range finding range and a field angle of a millimeter wave radar sensor and a positioning scene space geometric constraint and an environment background point cloud removing method based on millimeter wave radar point cloud characteristics in an unmanned static environment
Step 201, parameter constraint
Aiming at the indoor multipath influence, firstly, parameter constraint methods such as a ranging range and a field angle of a millimeter wave radar sensor are provided, and point clouds with ranging values larger than the ranging range are directly removed by utilizing the set ranging range r of the millimeter wave radar sensor; secondly, utilizing the parameter theta of the horizontal and vertical field angles of the millimeter wave radar sensor h ,θ v Directly removing the point clouds of which the horizontal incident angles and the vertical incident angles are larger than the corresponding field angles in the measured point clouds;
step 202, space geometry constraint
Then, the geometric size parameters [ length, width and height ] of the positioning space are utilized to directly remove the point clouds exceeding any parameter in the measured point clouds, and the results before and after the step 2 are shown in fig. 5 and fig. 6;
step 203, removing the environmental background point cloud
Firstly, millimeter wave radar point cloud data in an unmanned static environment within a period of time are collected, and a background point cloud data set E is established; secondly, in each frame of point cloud data F measured in real time, point clouds appearing in the background point cloud data set are removed, background point cloud removal is achieved, and the formula for removing the background point clouds is as follows:
D＝d(F i (x,y),E j (x,y))+δd(F i (v),E j (v))
d (-) represents the Euclidean distance, delta represents a coefficient, if D is smaller than a given threshold value, the point i in the current frame F is considered to be the same as the point cloud j in the background point cloud data set E, and then the point cloud is removed;
step 3, designing a user positioning algorithm based on optics clustering algorithm according to the continuity and aggregation of the dynamic target reflection point cloud on the spatial distribution
Step 301, user positioning based on optics clustering algorithm
Consider a point cloud data set X = { X = 1 ,x 2 ,…,x N And (4) under the condition of giving a clustering radius parameter epsilon and the minimum point cloud number M in the radius, carrying out the following stepsStep of calculating the core distance cd = { cd) of each point cloud 1 ,…,cd i ,…,cd N And achievable distance rd = { rd = } 1 ,…,rd i ,…,rd N And outputting a point cloud list P, wherein the specific process is as follows:
step 301, for any point cloud data set X = { X = 1 ,x 2 ,…,x N Giving a clustering parameter which belongs to E and M;
step 302, calculating a distance matrix D between all point clouds, initializing whether an element of an access list V for marking whether the point clouds are processed is zero, and indicating that all the point clouds are not accessed; initializing seed list S as null; initializing a point cloud output list P to be null;
step 303, taking out any one point cloud which is not visited from the point cloud data set X, and adding the point cloud into the seed list S;
step 304, take out the first point cloud x from the seed list S i Wherein, i is more than or equal to 1 and less than or equal to N represents the serial number of the point cloud in the set X; set this point to visited in V and x i Adding the data to the tail of the output list P; obtaining x i A point cloud set H in a neighborhood radius is formed; if the number of sets H is less than M, point cloud x is processed i Is set to infinity, x i Judging as a non-core point; otherwise, x is then i Judging as a core point, and calculating the core distance cd of the point cloud by the following formula i And the achievable distance rd of each point in H j ：
Figure BDA0003746838160000141
Figure BDA0003746838160000142
Wherein d (-) represents the Euclidean distance, H j (x i ) Representing the distance x in the set H i J is more than or equal to 1 and less than or equal to num (H) at the point close to the jth; finally, adding the point clouds in the set H into a seed list S, and sorting according to the reachable distance;
step 305, for the same point cloud in the list S, keeping the point cloud with the minimum reachable distance, and updating the reachable distance of the point cloud to the reachable distance of the point cloud; if the seed list S is not empty, steps 304 and 305 are repeated; otherwise, go to step 306;
step 306, if the point cloud data set X is not empty, repeating the steps (303), (304) and (305); otherwise, outputting an output list P of the point cloud, and the core distance and the reachable distance of each point;
step 307, set a new neighborhood radius
Figure BDA0003746838160000151
According to the output point cloud core distance cd, the achievable distance rd and the point cloud output list P, determining the cluster label of each point cloud by the following algorithm:
Figure BDA0003746838160000152
308, extracting point clouds with the same cluster label to form a set according to the cluster label L to which each point cloud output in the 307 belongs, and taking the average position of the point clouds in the set as the positioning result of the cluster;
step 4, based on the point cloud initial clustering, according to the characteristics of the point cloud such as radial speed and the like, eliminating dynamic multipath false points; further designing a target tracking and trajectory estimation algorithm based on the Hungarian algorithm and the Kalman filtering algorithm;
dynamic multipath false point elimination: according to the characteristics of the point cloud such as radial velocity and the like, dynamic multi-path false point elimination is realized, and for each cluster, if the mean value of the radial velocity of the point cloud in the cluster is smaller than a set threshold value mu and the variance of the radial velocity is smaller than a set threshold value sigma, all the point clouds in the cluster are judged as multi-path false points and eliminated;
as shown in fig. 8, the target tracking and trajectory estimation algorithm based on the hungarian algorithm and the kalman filter algorithm specifically includes the following steps:
designing a target tracking algorithm based on the Hungarian algorithm: firstly, establishing a new tracking task for a point cloud cluster newly entering a scene, and deleting the tracking task which cannot be associated within a certain continuous time;
then, the Hungarian algorithm is utilized to correlate the output result of the point cloud cluster at the current moment with the current tracking task, and the method mainly comprises the following steps:
suppose that the tracking task set at the current time is U = { U = 1 ,u 2 ,…,u n H, where u i ＝{p i ,v i }，p i And v i Respectively representing trace tasks u i The position and the speed of the current moment under a positioning coordinate system; the set of positioning results of each point cloud cluster at the current moment is W = { W = { (W) 1 ,w 2 ,…,w m Therein of
Figure BDA0003746838160000161
And
Figure BDA0003746838160000162
respectively represent point cloud cluster positioning results w i The position of the current moment in the positioning coordinate system and the position P relative to the millimeter wave radar sensor L The radial velocity of (a); constructing a distance matrix between U and W elements
Figure BDA0003746838160000163
Wherein the ith row and jth column elements
Figure BDA0003746838160000164
Indicates the position u i To position w j The Euclidean distance of (c); if m is not equal to n, then the matrix is aligned
Figure BDA0003746838160000165
Adding all-zero virtual rows or columns to form a square matrix; then, a global best association is found by the following steps:
step 401, let the matrix
Figure BDA0003746838160000166
Subtracting the minimum number in each row from each row, and subtracting the minimum number in each column from each column to obtain the matrix
Figure BDA0003746838160000167
Step 402, decovering the matrix with a minimum of row and column combinations
Figure BDA0003746838160000168
If the required minimum number of rows and columns equals the maximum of m and n, an optimal association may be made between all the zero elements in m
Figure BDA0003746838160000169
Is found, step 404 is performed; otherwise, go to step 403 to continue updating
Figure BDA00037468381600001610
Step 403, decover the matrix with minimal combinations of rows and columns in step 402
Figure BDA00037468381600001611
After all the zero elements in the list, find the minimum value (defined as s) in the rest uncovered elements, and then all the zero elements in the list are covered
Figure BDA00037468381600001612
Subtract s from the uncovered elements and let
Figure BDA00037468381600001613
The element covered by the row and column in step 402 at the same time is added by s to obtain a new one
Figure BDA00037468381600001614
Then step 402 is executed;
step 404, in
Figure BDA00037468381600001615
The row i and column j in which the zero element is located represent the corresponding u i And w j There may be a correlation if the row and column in which the element is located is removed, leaving a sub-matrix
Figure BDA00037468381600001616
If an optimal association can still be found, u is determined i And w j Performing correlation output, and continuing to output the residual sub-matrixes
Figure BDA00037468381600001617
Finding the best correlation; otherwise u i And w j The association can not be carried out, and the rest elements which are zero are inspected;
designing a trajectory estimation algorithm based on a Kalman filtering algorithm:
tracking task and point cloud cluster positioning result on one pair of association at the moment of k +1
Figure BDA0003746838160000171
Firstly, a system equation is established by utilizing the assumption of uniform linear motion, and
Figure BDA0003746838160000172
the state at the next instant is estimated:
Figure BDA0003746838160000173
wherein epsilon k Expressing zero mean Gaussian white noise in the state estimation process, wherein delta t expresses the time interval of adjacent moments;
positioning result of point cloud cluster at k +1 moment
Figure BDA0003746838160000174
And predicting the estimation result
Figure BDA0003746838160000175
The relationship between the two can be expressed by the following measurement equation:
Figure BDA0003746838160000176
wherein
Figure BDA0003746838160000177
Representing a velocity vector
Figure BDA0003746838160000178
And the slave position
Figure BDA0003746838160000179
To millimeter wave radar sensor position P L Direction vector of
Figure BDA00037468381600001710
The included angle between them; rho k Represents the measured zero-mean gaussian white noise; the method for estimating the current tracking track state by using the Kalman filtering method mainly comprises the following steps:
step 405, predicting the current state:
Figure BDA00037468381600001711
wherein
Figure BDA00037468381600001712
A state transition matrix is represented that represents the state transition,
Figure BDA00037468381600001713
representing the state estimation error covariance matrix, Q k Representing state noise error covariance;
step 406, updating the predicted state with the measurement value: firstly, calculating Kalman gain:
Figure BDA00037468381600001714
then the state is updated:
Figure BDA00037468381600001715
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA00037468381600001716
representing a measurement matrix; r k Representing a measurement noise error covariance matrix；
Step 5, the household activity perception realizes the household activity perception of the user by utilizing the target positioning and tracking result and combining the semantic information of a space map, such as information of a kitchen, a living room, a bedroom, a bathroom, a door and the like, and comprises sleeping, cooking, going to the toilet, going out, going home, watching television and the like; then, according to the household activity sensing result, counting the daily work and rest rules of the user; according to the high correlation between the space position and the space activity in the home environment, when the positioning target is detected to be in the same position area within a certain time, judging that the positioning target is engaged in the home activity related to the area; for example, when the position of the positioning target is continuously monitored in the kitchen, the home activity of the target user is judged to be 'cooking'; let a = { a ] be the set of activity types that need to be perceived 1 ,a 2 ,…,a n In which a is i ＝{name i ,region i ,duration i Denotes the ith activity, name i Representing the name, region, of the activity i Region extent, duration, representing activity i A minimum duration threshold indicating a determination as to the type of activity; when the positioning target is in a certain time gamma, the positioning target appears in a certain region i The cumulative time Γ' of (a) is equal to or more than duration i Then it is determined that the primary activity during the period of time includes an activity name i 。
Although the present invention has been described in detail with reference to the specific embodiments thereof, the present invention is not limited to the above embodiments, and various changes can be made without departing from the gist of the present invention within the knowledge of those skilled in the art without departing from the scope of the present invention.

Claims (10)

1. A millimeter wave-based method for positioning non-carried equipment and sensing household activities is characterized in that: comprising the following steps:
step 1, spatial arrangement and parameter calibration of millimeter wave radar sensors;
step 2, processing point cloud data of the millimeter wave radar;
step 3, positioning and tracking the pedestrian target based on the millimeter radar point cloud data;
and 4, sensing the household activities of the user based on the millimeter wave radar.
2. The millimeter wave based carrierless location and home activity awareness method of claim 1, wherein: the millimeter wave radar sensor space layout and parameter calibration comprises a millimeter wave radar space layout scheme of positioning scene size and layout; and the millimeter wave radar sensor parameter calibration system calibrates the internal parameters and the external parameters of the sensor.
3. The millimeter wave based portable device-less location and home activity awareness method of claim 1, wherein: the interference point cloud eliminating method based on the self parameters of the millimeter wave radar sensor and the spatial geometrical constraint of the millimeter wave radar sensor comprises a multipath false point eliminating method based on the parameter constraints of the millimeter wave radar sensor such as the range and the field angle and the spatial geometrical constraint of a positioning scene and an environment background point cloud eliminating method based on the millimeter wave radar point cloud characteristics in the unmanned static environment.
4. The millimeter wave based carrierless location and home activity awareness method of claim 1, wherein: the pedestrian target positioning and tracking based on millimeter radar point cloud data comprises the steps of designing a user positioning algorithm based on an optics clustering algorithm according to the continuity and the aggregation of dynamic target reflection point clouds in spatial distribution; on the basis of the point cloud primary clustering, dynamic multipath false point elimination is realized according to the characteristics of the point cloud such as radial speed and the like; and further designing a target tracking and trajectory estimation algorithm based on the Hungarian algorithm and the Kalman filtering algorithm.
5. The millimeter wave based carrierless location and home activity awareness method of claim 1, wherein: the sensing of the user household activities based on the millimeter wave radar comprises designing a sensing method of the user household activities based on the combination of positioning and tracking results of the millimeter wave radar and semantic information of a space map.
6. The millimeter wave based portable device-less location and home activity awareness method of claim 2, wherein: the method comprises the following steps that according to the effective coverage range of signals of the millimeter wave radar sensor and the size of a scene area, the installation coordinates of the millimeter wave radar sensor are designed, wherein the installation coordinates comprise X, Y and Z, postures and installation numbers M, the specific layout criteria are that the postures are pitch angles pitch, roll angles roll and course angles yaw, the installation height is 2-3.5M, the installation position is close to a wall angle or a wall surface, the pitch angle of the installation posture is-10-20 degrees, the roll angle is 0 +/-0.5 degrees, the course angle is manually adjusted according to the shape of a positioning space, and the course angle is calibrated by a subsequent calibration method;
the internal parameters of the sensor refer to a millimeter wave radar ranging error scaling factor, and the external parameters of the sensor comprise the attitude and the position of the millimeter wave radar sensor relative to a local navigation coordinate system; the millimeter wave radar sensors are arranged in a scene, and internal parameters and external parameters of each millimeter wave radar sensor are calibrated, wherein the calibration method comprises the following steps:
step 101, external parameter calibration: the conversion formula of the point cloud data between the radar sensor coordinate system and the positioning coordinate system can be expressed as follows:
Figure FDA0003746838150000021
wherein P is n ＝[x n ，y n ，z n ] T Representing the coordinates of the point cloud in a localization coordinate system, P r ＝[x r ，y r ，z r ] T Representing the coordinates of the point cloud in the sensor coordinate system,
Figure FDA0003746838150000024
transformation matrix representing sensor coordinate system to positioning coordinate system, comprising three-dimensional rotation R = [ pitch, roll, yaw =] T And three-dimensional translation t = [ t ] x ，t y ，t z ] T (ii) a Wherein, the pitch angle pitch and roll angle roll in the three-dimensional rotation are calibrated by directly measuring the true value of the pitch angle pitch and roll angle roll by a goniometer; the course angle yaw and the three-dimensional translation t are calibrated by using a least square method:
wherein P is n ＝[x n ，y n ，z n ] T Representing the coordinates of the point cloud in a localization coordinate system, P r ＝[x r ，y r ，z r ] T Representing the coordinates of the point cloud in the sensor coordinate system,
Figure FDA0003746838150000023
transformation matrix representing sensor coordinate system to positioning coordinate system, comprising three-dimensional rotation R = [ pitch, roll, yaw =] T And three-dimensional translation t = [ t ] x ，t y ，t z ] T (ii) a Wherein, the pitch angle pitch and roll angle roll in the three-dimensional rotation are calibrated by directly measuring the true value of the pitch angle pitch and roll angle roll by a goniometer; the course angle yaw and the three-dimensional translation t are calibrated by using a least square method:
placing corner reflectors for point cloud calibration at calibration points of N known positions in positioning space, and recording true values of each calibration point in a positioning coordinate system
Figure FDA0003746838150000022
And measured values in the sensor coordinate system
Figure FDA0003746838150000031
Wherein i =1,2,. N; and converting the measured values in the sensor coordinate system to the intermediate transition coordinate system according to the known measured pitch angle and roll angle:
Figure FDA0003746838150000032
the transformation relationship from the intermediate transition coordinate system to the positioning coordinate system can be expressed as:
Figure FDA0003746838150000033
wherein
Figure FDA0003746838150000034
The point cloud coordinate conversion relationship from the intermediate transition coordinate system to the positioning coordinate system can be rearranged as:
Figure FDA0003746838150000035
order to
Figure FDA0003746838150000036
The above formula is described again
Figure FDA0003746838150000037
N (≧ 2) sets of measurement-true values may constitute N (≧ 2) of the above-described observation equations:
Figure FDA0003746838150000038
further order
Figure FDA0003746838150000039
H＝[H 1 H 2 …H N ] T (ii) a Thus, by using the small two-times algorithm, X can be solved by the following formula:
X＝(H T H) -1 H T Y
wherein the heading parameter can be selected from
Figure FDA00037468381500000310
Calculating to obtain;
step 102, calibrating internal parameters:
transformation matrix from known sensor coordinate system to positioning coordinate system
Figure FDA00037468381500000311
On the premise of (1), the triaxial error scaling factor of the radar point cloud measured by the millimeter wave radar sensing is as follows: s = [ S ] x s y s z ] T Then, there is the following conversion relationship:
Figure FDA00037468381500000312
and (3) measuring a true value by using N (more than or equal to 2) groups and a least square algorithm to obtain a triaxial error scaling factor S of the millimeter wave radar.
7. The millimeter wave based portable device-less positioning and home activity awareness method according to claim 3, wherein: the multipath false point removing method based on parameter constraints such as the range of the millimeter wave radar sensor, the field angle and the like and the space geometric constraint of the positioning scene comprises the following steps:
step 201, directly eliminating point clouds with distance measurement values larger than a distance measurement range by using the set distance measurement range r of the millimeter wave radar sensor;
202, utilizing the self horizontal and vertical field angle parameter theta of the millimeter wave radar sensor h ，θ v Directly removing the point clouds of which the horizontal incident angles and the vertical incident angles are larger than the corresponding field angles in the measured point clouds;
and step 203, directly removing the point clouds exceeding any parameters in the measured point clouds by using the geometric size parameters [ length, width and height ] of the positioning space.
8. The millimeter wave based portable device-less location and home activity awareness method of claim 3, wherein: the method for eliminating the environmental background point cloud based on the millimeter wave radar point cloud characteristics in the unmanned static environment comprises the following steps:
step 204, collecting millimeter wave radar point cloud data in an unmanned static environment within a period of time, and establishing a background point cloud data set E;
step 205, removing the point clouds appearing in the background point cloud data set from each frame of point cloud data measured in real time, so as to remove the background point clouds.
9. The millimeter wave based portable device-less location and home activity awareness method of claim 4, wherein: the method for designing the user positioning algorithm based on the optics clustering algorithm according to the continuity and the aggregation of the dynamic target reflection point cloud on the spatial distribution comprises the following steps:
consider a point cloud data set X = { X = 1 ，x 2 ，...，x N And calculating the core distance cd = { cd) of each point cloud under the conditions that the parameter epsilon of the clustering radius is given and the minimum point cloud number M in the radius is given by the following steps 1 ，...，cd i ，...，cd N And achievable distance rd = { rd = } 1 ，...，rd i ，...，rd N And point cloud output list P:
step 301, for any point cloud data set X = { X = 1 ，x 2 ，...，x N Giving a clustering parameter belonging to E and M;
step 302, calculating a distance matrix D between all point clouds, initializing whether an element of an access list V for marking whether the point clouds are processed is zero, and indicating that all the point clouds are not accessed; initializing seed list S as null; initializing a point cloud output list P to be empty;
step 303, taking out any point cloud which is not visited from the point cloud data set X, and adding the point cloud into the seed list S;
step 304, take out the first point cloud x from the seed list S i Wherein, i is more than or equal to 1 and less than or equal to N represents the serial number of the point cloud in the set X; set this point to visited in V and x i Adding the data to the tail of the output list P; obtaining x i A point cloud set H in the neighborhood radius is formed; if the number of sets H is less than M, the point cloud x i Reach and core distance ofSet to infinity, x i Judging as a non-core point; otherwise, x is then i Judging as a core point, and calculating the core distance cd of the point cloud by the following formula i And the achievable distance rd of each point in H j ：
Figure FDA0003746838150000051
Figure FDA0003746838150000052
Wherein d (-) represents the Euclidean distance, H j (x i ) Representing the distance x in the set H i J is more than or equal to 1 and less than or equal to num (H) at the j-th point; finally, adding the point clouds in the set H into a seed list S, and sorting according to the reachable distance;
step 305, for the same point cloud in the list S, keeping the point cloud with the minimum reachable distance, and updating the reachable distance of the point cloud to the reachable distance of the point cloud;
step 306, if the point cloud data set X is not empty, repeating the steps 3,4 and 5; otherwise, outputting an output list P of the point cloud, and the core distance and the reachable distance of each point;
step 307, set a new neighborhood radius
Figure FDA0003746838150000053
According to the output point cloud core distance cd, the achievable distance rd and the point cloud output list P, determining the cluster label of each point cloud through the following algorithm:
Figure FDA0003746838150000054
Figure FDA0003746838150000061
step 308, extracting point clouds with the same cluster labels to form a set according to the cluster labels L to which the point clouds output in the step 7 belong, and taking the average position of the point clouds in the set as a positioning result of the cluster;
if the seed list S is not empty in the above steps, repeating the steps 4 and 5; otherwise, executing step 6; based on the point cloud primary clustering, according to the characteristics of point cloud radial velocity and the like, dynamic multipath false point elimination is realized, and for each clustering cluster, if the mean value of the radial velocity of the point cloud in the cluster is smaller than a set threshold mu and the variance of the radial velocity of the point cloud in the cluster is smaller than a set threshold sigma, all the point clouds in the cluster are judged as multipath false points and eliminated; according to the further design of a target tracking and trajectory estimation algorithm based on the Hungarian algorithm and the Kalman filtering algorithm, the target tracking algorithm based on the Hungarian algorithm is designed, firstly, a new tracking task is created for a point cloud cluster entering a new scene, and the tracking task which cannot be associated within a certain time is deleted; and then, associating the output result of the point cloud cluster at the current moment with the current tracking task by using a Hungarian algorithm, and mainly comprising the following steps of: suppose that the tracking task set at the current time is U = { U = 1 ，u 2 ，...，u n In which u i ＝{p i ，v i }，p i And v i Respectively representing trace tasks u i The position and the speed of the current moment under a positioning coordinate system; the set of positioning results of each point cloud cluster at the current moment is W = { W = 1 ，w 2 ，...，w m Therein of
Figure FDA0003746838150000067
Figure FDA0003746838150000068
And
Figure FDA0003746838150000069
respectively represent point cloud cluster positioning results w i The position of the current moment in the positioning coordinate system and the position P relative to the millimeter wave radar sensor L The radial velocity of (a); constructing a distance matrix between U and W elements
Figure FDA0003746838150000062
Wherein the ith row and jth column elements
Figure FDA0003746838150000063
Indicates the position u i To position w j The Euclidean distance of (c); if m is not equal to n, then the matrix is aligned
Figure FDA0003746838150000064
Adding all zero virtual rows or columns to form a square matrix; then, a global best association is found by the following steps:
step 401, let the matrix
Figure FDA0003746838150000065
Subtracting the minimum number in each row from each row, and subtracting the minimum number in each column from each column to obtain the matrix
Figure FDA0003746838150000066
Step 402, decovering the matrix with a minimum of row and column combinations
Figure FDA0003746838150000071
If the required minimum number of rows and columns is equal to the maximum of both m and n, an optimal association can be made between
Figure FDA0003746838150000072
Is found, step 4 is executed; otherwise, executing step 3 and continuing to update
Figure FDA0003746838150000073
In step 403, the matrix is uncovered with the minimum combination of rows and columns in step 402
Figure FDA0003746838150000074
Finding the minimum value of the other uncovered elements after all the zero elements are found, wherein the minimum value is defined as s, and all the uncovered elements are defined as
Figure FDA0003746838150000075
S is subtracted from the uncovered elements and let
Figure FDA0003746838150000076
The element covered by the row and column in step 402 at the same time is added by s to obtain a new one
Figure FDA0003746838150000077
Then step 402 is executed;
step 404, in
Figure FDA0003746838150000078
The row i and column j in which the zero element is located represent the corresponding u i And w j There may be a correlation if the row and column in which the element is located is removed, leaving a sub-matrix in the remainder
Figure FDA0003746838150000079
If an optimal association can still be found, u is determined i And w j Performing correlation output, and continuing to output the rest sub-matrixes
Figure FDA00037468381500000710
Finding the best correlation; otherwise u i And w j The association can not be carried out, and the rest elements which are zero are inspected;
designing a trajectory estimation algorithm based on a Kalman filtering algorithm:
tracking task and point cloud cluster positioning result on one pair of association at the moment of k +1
Figure FDA00037468381500000711
Firstly, a system is created by utilizing the assumption of uniform linear motionProgram, pair
Figure FDA00037468381500000712
The state at the next instant is estimated:
Figure FDA00037468381500000713
wherein epsilon k Representing zero mean Gaussian white noise in the state estimation process, and delta t representing the time interval of adjacent moments;
positioning result of point cloud cluster at k +1 moment
Figure FDA00037468381500000714
And predicting the estimation result
Figure FDA00037468381500000715
The relationship between the two can be expressed by the following measurement equation:
Figure FDA00037468381500000716
wherein
Figure FDA00037468381500000717
Representing a velocity vector
Figure FDA00037468381500000718
And the slave position
Figure FDA00037468381500000719
To millimeter wave radar sensor position P L Direction vector of
Figure FDA00037468381500000720
The included angle therebetween; ρ is a unit of a gradient k Represents the measured zero-mean gaussian white noise; using Kalman filtering method to estimate current tracking track stateThe method comprises the following steps:
step 405, predicting the current state:
Figure FDA0003746838150000081
wherein
Figure FDA0003746838150000082
A state transition matrix is represented that represents the state transition,
Figure FDA0003746838150000088
representing the state estimation error covariance matrix, Q k Representing state noise error covariance;
step 406 updates the predicted state with the measurement: firstly, calculating Kalman gain:
Figure FDA0003746838150000083
then the state is updated:
Figure FDA0003746838150000084
and
Figure FDA0003746838150000085
Figure FDA0003746838150000086
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003746838150000087
representing a measurement matrix; r k Representing the measurement noise error covariance matrix.
10. The millimeter wave based portable device-less location and home activity awareness method of claim 5, wherein: the user home activity perception method based on the combination of the millimeter wave radar positioning and tracking result and the space map semantic information comprises the following steps;
step 501, utilizing the target positioning and tracking result and combining semantic information of a space map to realize the household activity perception of a user;
step 502, according to the house activity sensing result, counting the daily work and rest rule of the user, according to the high correlation between the space position and the space activity in the house environment, when the positioning target is detected to be in the same position area within a certain time, judging that the positioning target is engaged in the house activity related to the area, and setting the activity type set needing sensing as A = { a = 1 ，a 2 ，...，a n In which a is i ＝{name i ，region i ，duration i Denotes the ith activity, name i Representing the name, region, of the activity i Indicates the area extent, duration, of the activity i A minimum duration threshold indicating a determination as to the type of activity;
step 503, when the positioning target appears in a region within a period of time Γ i The cumulative time Γ' of (a) is equal to or more than duration i Then it is determined that the primary activity during the period of time includes an activity name i 。
CN202210826642.1A 2022-07-14 2022-07-14 Millimeter wave-based non-portable equipment positioning and household activity sensing method Pending CN115220028A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210826642.1A CN115220028A (en) 2022-07-14 2022-07-14 Millimeter wave-based non-portable equipment positioning and household activity sensing method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210826642.1A CN115220028A (en) 2022-07-14 2022-07-14 Millimeter wave-based non-portable equipment positioning and household activity sensing method

Publications (1)

Publication Number Publication Date
CN115220028A true CN115220028A (en) 2022-10-21

Family

ID=83611707

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210826642.1A Pending CN115220028A (en) 2022-07-14 2022-07-14 Millimeter wave-based non-portable equipment positioning and household activity sensing method

Country Status (1)

Country Link
CN (1) CN115220028A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115542308A (en) * 2022-12-05 2022-12-30 德心智能科技(常州)有限公司 Indoor personnel detection method, device, equipment and medium based on millimeter wave radar

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115542308A (en) * 2022-12-05 2022-12-30 德心智能科技(常州)有限公司 Indoor personnel detection method, device, equipment and medium based on millimeter wave radar

Similar Documents

Publication Publication Date Title
CN110244715B (en) Multi-mobile-robot high-precision cooperative tracking method based on ultra wide band technology
CN103472823B (en) A kind of grating map creating method of intelligent robot
CN105760811B (en) Global map closed loop matching process and device
CN104766320B (en) Many Bernoulli Jacob under thresholding is measured filter Faint target detection and tracking
CN110388926B (en) Indoor positioning method based on mobile phone geomagnetism and scene image
WO2017121168A1 (en) Cluster-based magnetic positioning method, device and system
CN109557532B (en) Tracking method before detection based on three-dimensional Hough transform and radar target detection system
CN112782664B (en) Toilet falling detection method based on millimeter wave radar
CN106448431A (en) Mobile phone sensor-based indoor planar graph construction method adopting crowdsourcing mode
CN107607091A (en) A kind of method for measuring unmanned plane during flying flight path
Chiang et al. Magnetic field-based localization in factories using neural network with robotic sampling
CN111964667B (en) geomagnetic-INS (inertial navigation System) integrated navigation method based on particle filter algorithm
CN113777600A (en) Multi-millimeter-wave radar cooperative positioning tracking method
CN115220028A (en) Millimeter wave-based non-portable equipment positioning and household activity sensing method
Kumar et al. A unified grid-based wandering pattern detection algorithm
CN110599800A (en) Parking lot parking space state monitoring system and monitoring method
CN106954190A (en) A kind of WIFI indoor orientation methods based on index mapping domain
CN115166750A (en) Quantitative precipitation estimation method based on dual-polarization Doppler radar data
CN102297692A (en) Self-localization method of intelligent wheelchair in corner areas
Özcan et al. Accurate and precise distance estimation for noisy IR sensor readings contaminated by outliers
CN114924225A (en) High-precision indoor positioning method, device, equipment and medium
CN114598990A (en) UWB and IMU based non-line-of-sight error compensation indoor positioning method and system
CN114994656A (en) Indoor personnel tracking method based on millimeter wave radar
CN117516517A (en) Passive fusion positioning method and system in indoor environment and electronic equipment
CN113341392A (en) Human behavior classification method based on multi-station radar micro-Doppler motion direction finding

Legal Events

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