CN116068575A - Human body state identification method and device based on 2D laser radar and RGB-D camera - Google Patents

Human body state identification method and device based on 2D laser radar and RGB-D camera Download PDF

Info

Publication number
CN116068575A
CN116068575A CN202111271453.4A CN202111271453A CN116068575A CN 116068575 A CN116068575 A CN 116068575A CN 202111271453 A CN202111271453 A CN 202111271453A CN 116068575 A CN116068575 A CN 116068575A
Authority
CN
China
Prior art keywords
leg
human
data
determining
position information
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
CN202111271453.4A
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.)
Beijing Machinery Equipment Research Institute
Original Assignee
Beijing Machinery Equipment Research Institute
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 Beijing Machinery Equipment Research Institute filed Critical Beijing Machinery Equipment Research Institute
Priority to CN202111271453.4A priority Critical patent/CN116068575A/en
Publication of CN116068575A publication Critical patent/CN116068575A/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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles

Abstract

The invention relates to a human body state identification method and device based on a 2D laser radar and an RGB-D camera, belongs to the technical field of robots, and solves the problem that the 2D laser radar cannot determine the posture of a user when the user is stationary and the direction of the user in a motion state, wherein the method comprises the following steps: determining first leg data based on the grid map and the 2D lidar; determining second leg data and human joint information based on the RGB-D camera; determining human leg position information and human leg speed information based on a Kalman filter according to the first human leg data and the second human leg data; according to the human joint information, determining a human leg attitude angle; and determining the state information of the human body according to the human leg position information, the human leg speed information and the human leg attitude angle. The technical scheme provided by the invention can improve the accuracy and expand the detection range.

Description

Human body state identification method and device based on 2D laser radar and RGB-D camera
Technical Field
The invention belongs to the technical field of robots, and particularly relates to a human body state identification method and device based on a 2D laser radar and an RGB-D camera.
Background
Pedestrian detection and tracking are an important application direction in the field of target detection and tracking research, and are core technologies in the field of computer vision such as robots, for example, indoor mobile robots autonomously detect pedestrians and track target states on the premise of considering interaction with the pedestrians so as to obtain the pose and speed of the pedestrians.
Currently, robot detection uses 2D lidar to detect and track pedestrians. The 2D lidar sensors of mobile robots are often mounted below the robot, so most lidar-based human detection algorithms rely solely on detecting human leg features to identify humans in the environment.
However, detecting the human leg by laser light cannot determine the posture of the user at rest and the orientation in the moving state, but the posture has an important role in the robot to complete the navigation task. For example, assuming a stationary person facing away from the robot and the task of the robot is to deliver something to the person, the robot may deliver objects to the back of the person if the person's pose or orientation cannot be recognized.
Disclosure of Invention
In view of the above analysis, the present invention aims to provide a human body state recognition method and device based on a 2D laser radar and an RGB-D camera, which solve the problem that a robot cannot recognize the posture and orientation of a person.
The aim of the invention is mainly realized by the following technical scheme:
in one aspect, the invention provides a human body state identification method based on a 2D laser radar and an RGB-D camera, which is characterized by comprising the following steps:
determining first leg data based on the grid map and the 2D lidar;
determining second leg data and human joint information based on the RGB-D camera;
determining human leg position information and human leg speed information based on a Kalman filter according to the first human leg data and the second human leg data;
according to the human joint information, determining a human leg attitude angle;
and determining the state information of the human body according to the human leg position information, the human leg speed information and the human leg attitude angle.
Further, collecting laser data of the human legs through the 2D laser radar;
determining at least one clustering point set from the human leg laser data according to a preset laser data gradient and a preset human leg diameter, wherein the clustering point set comprises at least one laser point;
determining obstacle position information according to the grid map;
for each clustering point set, determining the distance between each laser point and an obstacle according to the obstacle position information, and calculating the proportion of the laser points with the distance smaller than a first preset value in the corresponding clustering point set;
and deleting the cluster point set with the proportion larger than a second preset value to obtain the first person leg data.
Further, determining skeleton points of an observation target and pixel coordinates of the skeleton points based on the picture shot by the RGB-D camera;
determining three-dimensional coordinates of the skeleton points according to pixel coordinates of the skeleton points, the depth image and a back projection model of the RGB-D camera;
determining the position information of the leg skeleton points and the human joint information according to the three-dimensional coordinates of the skeleton points;
and generating second leg data according to the position information of the leg framework point.
Further, the first person leg data or the second person leg data received for the first time is taken as a state quantity, and the second person leg data or the first person leg data received at the next moment is taken as an observed quantity;
according to the state quantity, carrying out Kalman filtering state prediction to obtain a state quantity at the next moment;
comparing the detection target corresponding to the state quantity at the next time with the detection target corresponding to the observed quantity, and optimizing and updating the state quantity at the next time according to the current observed quantity when the detection target and the detection target are matched;
and then, alternately taking the received first person leg data or second person leg data as the observed quantity to continuously optimize the corresponding next moment state quantity.
Further, the tracking of the detection target corresponding to the state quantity at the next moment is finished;
further, the observed quantity is set as a state quantity of the corresponding detection target.
Further, the human joint information includes: shoulder joint position information and hip joint position information;
the step of determining the posture angle of the human leg according to the human joint information comprises the following steps:
determining a shoulder joint attitude angle according to the shoulder joint position information;
determining a hip joint attitude angle according to the hip joint position information;
and determining the human leg attitude angle according to the shoulder joint attitude angle and the hip joint attitude angle.
Further, the determining the human leg posture angle according to the shoulder joint posture angle and the hip joint posture angle includes:
Figure BDA0003328131070000031
wherein θ shoulder For characterizing the joint attitude angle, θ hip For characterizing the hip joint attitude angle, θ' leg For characterizing the human leg pose angle, (x) Rshoulder ，y Rshoulder ) Is the right shoulder joint position information, (x) Lshouler ，y Lshouler ) Is left shoulder joint position information, (x) Rhip ，y Rhip ) For the hip jointSection right side position information, (x) Lhip ，y Lhip ) Is the left side position information of the hip joint.
On the other hand, the invention also provides a human body state recognition device based on the 2D laser radar and the RGB-D camera, which comprises: the device comprises a first data processing module, a second data processing module, a Kalman filter and a third data processing module;
the first data processing module is used for determining first leg data based on the grid map and the 2D laser radar;
the second data processing module is used for determining second leg data and human joint information based on the RGB-D camera; and determining the posture angle of the human legs according to the human joint information.
The Kalman filter is used for determining human leg position information and human leg speed information based on Kalman filtering according to the first human leg data and the second human leg data;
the third data processing module is used for determining the state information of the human body according to the human leg position information, the human leg speed information and the human leg attitude angle.
Further, the first data processing module is used for acquiring leg laser data through the 2D laser radar; determining at least one clustering point set from the human leg laser data according to a preset laser data gradient and a preset human leg diameter, wherein the clustering point set comprises at least one laser point; determining obstacle position information according to the grid map; for each clustering point set, determining the distance between each laser point and an obstacle according to the obstacle position information, and calculating the proportion of the laser points with the distance smaller than a first preset value in the corresponding clustering point set; and deleting the cluster point set with the proportion larger than a second preset value to obtain the first person leg data.
Further, the third data processing module is configured to take the first person leg data or the second person leg data received for the first time as a state quantity, and take the second person leg data or the first person leg data received at the next moment as an observed quantity; according to the state quantity, carrying out Kalman filtering state prediction to obtain a state quantity at the next moment; comparing the detection target corresponding to the state quantity at the next time with the detection target corresponding to the observed quantity, and optimizing and updating the state quantity at the next time according to the current observed quantity when the detection target and the detection target are matched; and then, alternately taking the received first person leg data or second person leg data as the observed quantity to continuously optimize the corresponding next moment state quantity.
Compared with the prior art, the invention can at least realize one of the following technical effects:
1. according to the pedestrian detection and tracking method integrating the 2D laser radar and the RGB-D camera, based on a Kalman filtering algorithm, the first leg data obtained by the 2D laser radar and the second leg data obtained by the RGB-D camera are mutually verified, the problem of missed detection or false detection of the 2D laser radar is solved to a certain extent by utilizing the RGB-D, meanwhile, the visual blind area of the RGB-D camera is supplemented by utilizing the 2D laser sensor, the accuracy is improved, and the detection range is enlarged.
2. By comparing the detection target corresponding to the state quantity at the next moment with the detection target corresponding to the observed quantity, whether the detection target is separated from the monitoring area or not and whether a new detection target appears in the monitoring area or not are determined in real time, so that the efficiency of tracking a plurality of targets and the accuracy of tracking detection are improved.
3. Based on the obstacle in the grid map, removing the 2D laser radar to obtain error data in the laser data so as to improve the accuracy of the first leg data, thereby further improving the accuracy of tracking detection.
Additional features and advantages of the invention will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. The objectives and other advantages of the invention may be realized and attained by the structure particularly pointed out in the written description and drawings.
Drawings
The drawings are only for purposes of illustrating particular embodiments and are not to be construed as limiting the invention, like reference numerals being used to refer to like parts throughout the several views.
Fig. 1 is a flowchart of a human body state recognition method based on a 2D lidar and an RGB-D camera according to an embodiment of the present invention;
fig. 2 is an image of skeleton points obtained based on openPose skeleton recognition according to an embodiment of the present invention;
fig. 3 is a detection result of openPose provided by an embodiment of the present invention;
fig. 4 is a flowchart of tracking a detection target based on kalman filtering according to an embodiment of the present invention.
Detailed Description
The following detailed description of preferred embodiments of the invention is made in connection with the accompanying drawings, which form a part hereof, and together with the description of the embodiments of the invention, are used to explain the principles of the invention and are not intended to limit the scope of the invention.
The 2D lidar in the prior art cannot determine the posture of the user when stationary and the orientation in the motion state, which can affect the user experience. The RGB-D camera acquires three-dimensional information by a trigonometry principle, and three-dimensional information of a target body of the ball can be obtained through conversion of the base line distance, focal length, geometric relation and coordinates of the camera. The orientation of the user may be determined based on the three-dimensional information of the human body.
However, the field angle of the RGB-D camera is small, and the blind area is also large, which results in that the robot cannot observe pedestrians outside the field angle or too close to the field angle in time, so that the technical scene of tracking and detecting pedestrians in a specific area (for example, in a house) cannot be satisfied by the RGB-D camera alone.
Based on the characteristics of a 2D laser radar and an RGB-D camera in the detection tracking field, the embodiment of the invention provides a 2D laser radar and RGB-D camera tracking method, which fuses the detection data of the 2D laser radar and the detection data of the RGB-D through Kalman filtering, and specifically comprises the following steps:
and step 1, determining first leg data based on the grid map and the 2D laser radar. The first person leg data are person leg position information based on the 2D laser radar.
In the embodiment of the invention, the process of determining the first leg data based on the grid map and the 2D laser radar is as follows:
and 11, acquiring laser data of the human legs through a 2D laser radar.
And step 12, determining at least one clustering point set from the human leg laser data according to the preset laser data gradient and the preset human leg diameter.
In an embodiment of the invention, the cluster point set comprises at least one laser point.
Step 13, determining the position information of the obstacle according to the grid map;
step 14, determining the distance between each laser point and the obstacle according to the position information of the obstacle for each cluster point set, and calculating the proportion of the laser points with the distance smaller than a first preset value in the corresponding cluster point set;
and 15, deleting the cluster point set with the proportion larger than a second preset value to obtain first person leg data.
Specifically, since invalid points exist in the 2D laser radar data and the range of the characteristics of the human legs which need to be detected is different in different scenes, the laser data is preprocessed first. Assume that one frame of laser data is
Figure BDA0003328131070000071
Wherein N is beam The number of laser beams is r' i For the scan angle theta i The distance value in the direction, the effective angle of detection is theta valid ＝[θ min ,θ max ]The effective distance is R valid ＝[r min ,r max ]Preprocessing laser data based on the constraint condition, wherein the preprocessed laser data is +.>
Figure BDA0003328131070000072
The pretreatment process comprises the following steps:
the i-th data is calculated as follows:
Figure BDA0003328131070000073
due toThe distance between adjacent laser points of the human leg feature does not vary much, so the gradient of adjacent points is less than the gradient threshold g leg . For laser data
Figure BDA0003328131070000074
Obtaining the gradient g of each laser point by adopting a median difference method i The calculation method is as follows: />
Figure BDA0003328131070000075
Then according to the laser scanning sequence, g i ＞g leg Is used as a demarcation point to form at least one cluster point set,
Figure BDA0003328131070000081
wherein C 'is the set of cluster point sets, C' 1 、c′ 2 、…、c′ beam Is a set of cluster points.
In order to improve the accuracy of the position of the human leg, the invention provides the following constraint conditions, and the collected laser data is optimized according to the constraint conditions:
first, in the human leg feature detection algorithm, the shape of the human leg is assumed to be a circle, and at least three laser data points are required to fit the shape, so that the set of the number of data is satisfied as a constraint removal deficiency. Secondly, the diameter of the leg is usually smaller than a preset threshold, and in an actual scene, the data points of the objects such as walls, tables and the like scanned by the laser are positioned on the same straight line, so that the diameter of the leg is detected to be larger than the preset threshold. Thus, based on the diameter constraints of the human leg features, the set of unsatisfied constraints is further removed. Calculating the distance between the head point and the tail point in each cluster point set, removing the cluster point set if the distance is larger than the threshold value of the diameter of the human leg, and finally obtaining the set as
Figure BDA0003328131070000082
Wherein C is the set of cluster point sets, C 1 、c 2 、…、c beam Is a set of cluster points.
For each cluster point set c i Fitting a circle by using a least square method. Specifically, first, the polar coordinate point is converted into cartesian coordinates, and the conversion formula is as follows:
Figure BDA0003328131070000083
then, establishing an overconstrained equation for the laser points in the set in a parameterized form to obtain a circle parameter (x) corresponding to each cluster point set ci ,y ci ,r ci ). The constraint equation is specifically:
Figure BDA0003328131070000084
wherein, (x) c ,y c ) As the center of a circle, r c For radius, m represents the set of points c i Laser spot number in (a). Solving the equation using pseudo-inverse, its solution being
Figure BDA0003328131070000085
Wherein V is a right singular matrix, U is a left singular matrix, and Sigma + Is a matrix of singular values.
During the tracking detection, if the human leg is relatively close to the obstacle, a part of laser points acquired by the 2D laser radar come from the obstacle. In addition, the invention describes the human leg by a circle, which can cause a very large error in the center position of the circle, thereby obtaining the wrong position of the human leg. To solve the above problem, the grid map that has been created at the time of robot navigation removes part of the false positive detection results. The specific process is as follows:
firstly converting an occupied grid map into a distance transformation map, then counting the distance between each laser point in each cluster point set and an obstacle, and if 85% of points in the set are close to the obstacle, removing the set to eliminate the influence of the obstacle on the leg position. Finally, considering that the error of the circle center position of the circle fitting is possibly overlarge, in order to improve the accuracy of the position of the human leg, the method adopts the clustering pointsThe mass center corresponding to the set is used as the coordinate of the detected human leg, and the generated first human leg data
Figure BDA0003328131070000091
Wherein l i ＝(x leg,i ,y leg,i )。
And 2, determining second leg data and human joint information based on the RGB-D camera. Wherein the second leg data is leg position information based on the RGB-D camera.
In the embodiment of the invention, the process of determining the second leg data and the human joint information is as follows:
and 21, determining skeleton points of the observation target and pixel coordinates of the skeleton points based on the picture shot by the RGB-D camera.
And step 22, determining the three-dimensional coordinates of the skeleton points according to the pixel coordinates of the skeleton points and the back projection model of the RGB-D camera.
And step 23, determining the position information of the leg skeleton points and the joint information of the human body according to the three-dimensional coordinates of the skeleton points.
And step 24, generating second leg data according to the position information of the leg skeleton point.
Specifically, the RGB-D camera adopts an open source skeleton detection algorithm OpenPose to identify key points of people. openPose is a human skeleton detector based on RGB images, and can detect 25 skeleton points, as shown in figure 2. Wherein the definition of each skeleton point is shown in table 1.
TABLE 1 skeleton Point definition
Figure BDA0003328131070000101
And determining pixel coordinates of the skeleton points based on the figure 2, and then combining the pixel coordinates of the OpenPose skeleton points, the depth image and a back projection model of the camera to obtain three-dimensional coordinates of the skeleton points in a camera coordinate system. Pixel homogeneous coordinates u passing through a skeleton point key ＝[u,v,1] T The corresponding spatial point P can be obtained key c ，
P key c ＝z c K -1 u key
Wherein z is c K is the calibration matrix of the RGB-D camera, which is the observation value of the RGB-D camera.
The posture of the human leg is calculated in combination with the shoulder joint and the hip joint detected by openPose. Defining a local coordinate system of a person as: the front of the person is the x-axis, the top is the z-axis, and the y-axis is defined by the right hand rule. The posture of the person is the included angle between the x-axis and the positive direction of the x-axis of the map coordinate system. Human leg by visual inspection is characterized by (x' leg ,y′ leg ,θ l ′ eg ) Wherein (x' leg ,y′ leg ) The posture angle theta 'of the human leg joint can be calculated by adopting the average value of the directions of the shoulder joint and the hip joint through the back projection model of the camera and the human leg bone point' leg . As shown in fig. 3, the embodiment of the invention provides a schematic diagram of human leg feature detection based on an RGB-D camera and openPose.
And 3, determining the human leg position information and the human leg speed information based on Kalman filtering according to the first human leg data and the second human leg data.
In the embodiment of the invention, the flow of the Kalman filtering is shown in FIG. 4, and comprises the following steps:
step 31, taking the first or second person leg data received at the first time as a state quantity, and taking the second or first person leg data received at the next moment as an observed quantity
In an embodiment of the invention, the first person leg data and the second person leg data are not received simultaneously, but are received alternately. Thus, the kalman filter is initialized with the first or second person leg data received for the first time. Note that, in fig. 4, the initialization is for the observation target, and not for the whole detection flow. For example, only the detection target a is detected at the previous time, and the detection target B appears at the next time, at this time, the detection of the target B is initialized with the first human leg data or the second human leg data of the detection target B received for the first time.
And step 32, carrying out Kalman filtering state prediction according to the state quantity to obtain the state quantity at the next moment.
And step 33, comparing the detection target corresponding to the state quantity at the next time with the detection target corresponding to the observed quantity, and optimizing and updating the state quantity at the next time according to the current observed quantity when the detection target and the detection target are matched.
In the embodiment of the invention, in order to realize the detection of multiple targets, the matching degree of the detection target corresponding to the state quantity at the next moment and the detection target corresponding to the observed quantity is detected through global data association, and the detection results are specifically three types: in order to facilitate the explanation of setting the detection target corresponding to the state quantity at the next moment as M and the detection target corresponding to the observed quantity as N
First, M and N are in one-to-one correspondence, at which point step 34 is performed.
Second, there is M without N corresponding to it, indicating that the detection target has left the detection area, and at this time, tracking of the detection target corresponding to the state quantity at the next time is ended, that is, the doerman filter deletion strategy is executed.
Thirdly, N corresponding to M does not exist, the new detection target appears in the detection area, and the observed quantity is set as the state quantity of the corresponding detection target, so that the Dunn filter initialization of the detection target is realized.
In the embodiment of the invention, the first leg data and the second leg data are both position information, so when the state quantity of the next moment is optimized and updated, the human body speed information is determined according to the observed quantity of the position information and the time, and the human body speed information is added into the state quantity.
And step 34, alternately taking the received first person leg data or second person leg data as an observed quantity to continuously optimize the corresponding next time state quantity.
Specifically, the state variables of one kalman filter defining the k moment are:
Figure BDA0003328131070000121
wherein x is k And y k For the position of the leg in the map coordinate system, < >>
Figure BDA0003328131070000122
And->
Figure BDA0003328131070000123
Is the speed of the person's leg in the map coordinate system.
The motion model using the uniform velocity model as the Kalman filtering comprises the following steps:
x k ＝F k x k-1 +w k
wherein F is k Is a state transition matrix, w k Gaussian white noise for state transition, Q k Is w k Is calculated as follows:
Figure BDA0003328131070000124
wherein Δt is k ＝t k -t k-1 。
Figure BDA0003328131070000125
Figure BDA0003328131070000126
Wherein sigma x 2 、σ y 2 、
Figure BDA0003328131070000127
And->
Figure BDA0003328131070000128
Respectively the k time x k 、y k 、/>
Figure BDA0003328131070000129
And->
Figure BDA00033281310700001210
Is a covariance of (c).
The observation model of the system is as follows:
z k ＝H k x k +v k
wherein H is k To observe the matrix, v k For observed white Gaussian noise, R k V is k Is a covariance matrix of (a):
Figure BDA0003328131070000131
Figure BDA0003328131070000132
Figure BDA0003328131070000133
wherein the position of the human leg in the observed quantity is represented by (x, y), delta x 2 And delta y 2 Covariance of x and y of observables, respectively.
Tracking of kalman filtering based on the above-mentioned motion model and observation model, comprising:
suppose that time k-1 has N tracker The set of individual state quantities is
Figure BDA0003328131070000134
Figure BDA0003328131070000135
State quantity of i detection targets representing moment k-1, N at moment k mes The set of observations is
Figure BDA0003328131070000136
z i,k Representing observed quantity of i detection targets at k time, then state prediction result P of i-th state quantity i,k|k-1 The method comprises the following steps:
Figure BDA0003328131070000137
P i,k|k-1 ＝F i,k P i,k-1|k-1 F i,k T +Q i,k
and adopting the global nearest neighbor data to correlate and confirm the corresponding relation between the state quantity and the observation, and finishing updating the Kalman filtering. First, constructing cost matrix
Figure BDA0003328131070000138
Matrix element c ij The determination method of (2) is as follows:
Figure BDA0003328131070000139
wherein G is a threshold value, and the proportionality coefficient s of the cost d The effect of (a) is to preferentially match the observations and state quantities of the motion,
Figure BDA00033281310700001310
the cost is characterized by the following calculation method:
Figure BDA00033281310700001311
the observed motion state is judged by the occupancy state of the grid where the observation position is located at the k and k-1 moments. Specifically, if time k is an occupied grid and time k-1 is a viable grid, then the observation is moving. The motion state of the state quantity is judged by the velocity module length at the moment k, and if the module length is larger than a specified threshold value, the state quantity is considered to be motion. Cost of (C)
Figure BDA0003328131070000141
The calculation mode of (2) is as follows:
Figure BDA0003328131070000142
wherein the residual vector
Figure BDA0003328131070000143
And residual matrix S ij,k The calculation mode of (2) is as follows:
Figure BDA0003328131070000144
S ij,k ＝H i,k P i,k|k-1 H i,k T +R i,k
and secondly, according to the determined cost matrix, adopting a Munkres algorithm to solve the optimal matching. Then, threshold value determination is carried out on each matched result. The final matching results include three classes of matched observations and traces (p, q), unmatched observations p ', unmatched traces q', as follows:
1) For matched observations p and traces q, a Kalman filter update process is performed:
Figure BDA0003328131070000145
S pq,k ＝H p,k P p,k|k-1 H p,k T +R q,k
K pq,k ＝P pq,k|k-1 H p,k T S pq,k -1
Figure BDA0003328131070000146
P p,k|k ＝(I-K pq,k H p,k )P p,k|k-1
2) For an unmatched observation p', a new filter is initialized, whose initial state is:
x＝[x leg ,y leg ,0,0] T
Figure BDA0003328131070000147
3) For the unmatched state quantity q', if the position covariance of the state quantity is too large or is not observed again for a long time, the state quantity is deleted.
And 4, determining the posture angle of the human legs according to the joint information of the human body.
In an embodiment of the present invention, human joint information includes: shoulder joint position information and hip joint position information, determining the human leg pose angle includes:
and step 41, determining the posture angle of the shoulder joint according to the position information of the shoulder joint.
Step 42, determining the hip joint attitude angle according to the hip joint position information.
And 43, determining the posture angle of the human leg according to the posture angle of the shoulder joint and the posture angle of the hip joint.
The specific process is as follows:
Figure BDA0003328131070000151
wherein θ shoulder For characterizing joint attitude angle, theta hip For characterizing hip joint attitude angle, θ l ′ eg For characterizing the leg attitude angle of a person, (x) Rshoulder ，y Rshoulder ) Is the right shoulder joint position information, (x) Lshouler ，y Lshouler ) Is left shoulder joint position information, (x) Rhip ，y Rhip ) Is hip joint right side position information, (x) Lhip ，y Lhip ) Is the left side position information of the hip joint.
In the embodiment of the invention, θ' leg Is suitable for the posture angle of the legs when the human body is in a static state. In the detection process, whether the human body is in a motion state or not is determined by Kalman filtering, if so, the human body is in the motion state according to the updated state quantity
Figure BDA0003328131070000152
And->
Figure BDA0003328131070000153
Calculating the posture of the legAngle, otherwise in theta' leg Human leg posture angle. Then the total leg attitude angle theta leg The calculation process specifically comprises the following steps: />
Figure BDA0003328131070000154
Wherein V is thres To determine whether the human body is in a speed threshold of a motion state,
Figure BDA0003328131070000161
and->
Figure BDA0003328131070000162
Is a component of the human leg in the x-direction and y-direction, therefore +.>
Figure BDA0003328131070000163
And->
Figure BDA0003328131070000164
The square root of the sum of squares of (a) is the actual speed of movement of the human leg. I.e. when the actual speed is less than V thres When the actual speed is greater than or equal to V, the human body is determined to be in a static state thres And determining that the human body is in a moving state.
And 5, determining the state information of the person according to the position information of the person's leg, the speed information of the person's leg and the attitude angle of the person's leg.
In the embodiment of the invention, the steps 1-4 obtain the state information of the person in the social space cost model by utilizing the 2D laser radar and the RGB-D camera and combining the Kalman filtering and the global data association algorithm
Figure BDA0003328131070000165
The embodiment of the invention also provides a human body state recognition device based on the 2D laser radar and the RGB-D camera, which comprises: the device comprises a first data processing module, a second data processing module, a Kalman filter and a third data processing module;
the first data processing module is used for determining first leg data based on the grid map and the 2D laser radar;
the second data processing module is used for determining second leg data and human joint information based on the RGB-D camera; and determining the posture angle of the human legs according to the joint information of the human body.
The Kalman filter is used for determining the human leg position information and the human leg speed information based on Kalman filtering according to the first human leg data and the second human leg data;
the third data processing module is used for determining the state information of the human body according to the human leg position information, the human leg speed information and the human leg attitude angle.
In the embodiment of the invention, the first data processing module is used for acquiring the leg laser data through the 2D laser radar; determining at least one clustering point set from human leg laser data according to a preset laser data gradient and a preset human leg diameter, wherein the clustering point set comprises at least one laser point; determining obstacle position information according to the grid map; for each cluster point set, determining the distance between each laser point and the obstacle according to the position information of the obstacle, and calculating the proportion of the laser points with the distance smaller than a first preset value in the corresponding cluster point set; and deleting the cluster point set with the proportion larger than a second preset value to obtain the first person leg data.
In the embodiment of the invention, the third data processing module is used for taking the first human leg data or the second human leg data received for the first time as a state quantity and taking the second human leg data or the first human leg data received at the next moment as an observed quantity; according to the state quantity, carrying out Kalman filtering state prediction to obtain the state quantity at the next moment; comparing the detection target corresponding to the state quantity at the next moment with the detection target corresponding to the observed quantity, and optimizing and updating the state quantity at the next moment according to the current observed quantity when the detection target and the detection target are matched; and then, alternately taking the received first person leg data or second person leg data as an observed quantity to continuously optimize the corresponding next moment state quantity.
The present invention is not limited to the above-mentioned embodiments, and any changes or substitutions that can be easily understood by those skilled in the art within the technical scope of the present invention are intended to be included in the scope of the present invention.

Claims (10)

1. A human body state recognition method based on a 2D lidar and an RGB-D camera, comprising:
determining first leg data based on the grid map and the 2D lidar;
determining second leg data and human joint information based on the RGB-D camera;
determining human leg position information and human leg speed information based on a Kalman filter according to the first human leg data and the second human leg data;
according to the human joint information, determining a human leg attitude angle;
and determining the state information of the human body according to the human leg position information, the human leg speed information and the human leg attitude angle.
2. The method of claim 1, wherein the determining the first leg data based on the grid map and the 2D lidar comprises:
collecting leg laser data through the 2D laser radar;
determining at least one clustering point set from the human leg laser data according to a preset laser data gradient and a preset human leg diameter, wherein the clustering point set comprises at least one laser point;
determining obstacle position information according to the grid map;
for each clustering point set, determining the distance between each laser point and an obstacle according to the obstacle position information, and calculating the proportion of the laser points with the distance smaller than a first preset value in the corresponding clustering point set;
and deleting the cluster point set with the proportion larger than a second preset value to obtain the first person leg data.
3. The method of claim 1, wherein the step of determining the position of the substrate comprises,
the determining, based on the RGB-D camera, second leg data and human joint information includes:
determining a skeleton point of an observation target and pixel coordinates of the skeleton point based on a picture shot by the RGB-D camera;
determining three-dimensional coordinates of the skeleton points according to pixel coordinates of the skeleton points, the depth image and a back projection model of the RGB-D camera;
determining the position information of the leg skeleton points and the human joint information according to the three-dimensional coordinates of the skeleton points;
and generating second leg data according to the position information of the leg framework point.
4. The method of claim 1, wherein the determining human leg position information and human leg velocity information based on kalman filtering from the first human leg data and the second human leg data comprises:
taking the first human leg data or the second human leg data received for the first time as a state quantity, and taking the second human leg data or the first human leg data received at the next moment as an observed quantity;
according to the state quantity, carrying out Kalman filtering state prediction to obtain a state quantity at the next moment;
comparing the detection target corresponding to the next time state quantity with the detection target corresponding to the observed quantity, and optimizing and updating the next time state quantity according to the observed quantity when the detection target and the detection target are matched;
and then, alternately taking the received first person leg data or second person leg data as the observed quantity to continuously optimize the corresponding next moment state quantity.
5. The method of claim 4, wherein the step of determining the position of the first electrode is performed,
when the detection target corresponding to the state quantity at the next moment of the root is not matched with the detection target corresponding to the observed quantity, the method further comprises the following steps:
ending the tracking of the detection target corresponding to the state quantity at the next moment;
and/or the number of the groups of groups,
and setting the observed quantity as a state quantity of a corresponding detection target.
6. The method of claim 1, wherein the human joint information comprises: shoulder joint position information and hip joint position information;
the step of determining the posture angle of the human leg according to the human joint information comprises the following steps:
determining a shoulder joint attitude angle according to the shoulder joint position information;
determining a hip joint attitude angle according to the hip joint position information;
and determining the human leg attitude angle according to the shoulder joint attitude angle and the hip joint attitude angle.
7. The method according to claim 1-6, wherein,
the determining the human leg posture angle according to the shoulder joint posture angle and the hip joint posture angle includes:
Figure FDA0003328131060000031
wherein θ shoulder For characterizing the joint attitude angle, θ hip For characterizing the hip joint attitude angle, θ' leg For characterizing the human leg pose angle, (x) Rshoulder ，y Rshoulder ) Is the right shoulder joint position information, (x) Lshouler ，y Lshouler ) Is left shoulder joint position information, (x) Rhip ，y Rhip ) For the hip joint right side position information, (x) Lhip ，y Lhip ) Is the left side position information of the hip joint.
8. A human body state recognition device based on a 2D lidar and an RGB-D camera, comprising: the device comprises a first data processing module, a second data processing module, a Kalman filter and a third data processing module;
the first data processing module is used for determining first leg data based on the grid map and the 2D laser radar;
the second data processing module is used for determining second leg data and human joint information based on the RGB-D camera; according to the human joint information, determining a human leg attitude angle;
the Kalman filter is used for determining human leg position information and human leg speed information based on Kalman filtering according to the first human leg data and the second human leg data;
the third data processing module is used for determining the state information of the human body according to the human leg position information, the human leg speed information and the human leg attitude angle.
9. The apparatus of claim 8, wherein the device comprises a plurality of sensors,
the first data processing module is used for acquiring leg laser data through the 2D laser radar; determining at least one clustering point set from the human leg laser data according to a preset laser data gradient and a preset human leg diameter, wherein the clustering point set comprises at least one laser point; determining obstacle position information according to the grid map; for each clustering point set, determining the distance between each laser point and an obstacle according to the obstacle position information, and calculating the proportion of the laser points with the distance smaller than a first preset value in the corresponding clustering point set; and deleting the cluster point set with the proportion larger than a second preset value to obtain the first person leg data.
10. The apparatus of claim 8, wherein the device comprises a plurality of sensors,
the third data processing module is used for taking the first person leg data or the second person leg data received for the first time as a state quantity and taking the second person leg data or the first person leg data received at the next moment as an observed quantity; according to the state quantity, carrying out Kalman filtering state prediction to obtain a state quantity at the next moment; comparing the detection target corresponding to the next time state quantity with the detection target corresponding to the observed quantity, and optimizing and updating the next time state quantity according to the observed quantity when the detection target and the detection target are matched; and then, alternately taking the received first person leg data or second person leg data as the observed quantity to continuously optimize the corresponding next moment state quantity.
CN202111271453.4A 2021-10-29 2021-10-29 Human body state identification method and device based on 2D laser radar and RGB-D camera Pending CN116068575A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111271453.4A CN116068575A (en) 2021-10-29 2021-10-29 Human body state identification method and device based on 2D laser radar and RGB-D camera

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111271453.4A CN116068575A (en) 2021-10-29 2021-10-29 Human body state identification method and device based on 2D laser radar and RGB-D camera

Publications (1)

Publication Number Publication Date
CN116068575A true CN116068575A (en) 2023-05-05

Family

ID=86177245

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111271453.4A Pending CN116068575A (en) 2021-10-29 2021-10-29 Human body state identification method and device based on 2D laser radar and RGB-D camera

Country Status (1)

Country Link
CN (1) CN116068575A (en)

Similar Documents

Publication Publication Date Title
CN111429574B (en) Mobile robot positioning method and system based on three-dimensional point cloud and vision fusion
CN108445480B (en) Mobile platform self-adaptive extended target tracking system and method based on laser radar
US11187790B2 (en) Laser scanning system, laser scanning method, movable laser scanning system, and program
Vandapel et al. Natural terrain classification using 3-d ladar data
CN111563442A (en) Slam method and system for fusing point cloud and camera image data based on laser radar
US7680300B2 (en) Visual object recognition and tracking
Yuan et al. Multisensor information fusion for people tracking with a mobile robot: A particle filtering approach
Zhang et al. Multiple vehicle-like target tracking based on the velodyne lidar
Wellhausen et al. Reliable real-time change detection and mapping for 3D LiDARs
CN114998276B (en) Robot dynamic obstacle real-time detection method based on three-dimensional point cloud
McGreavy et al. Next best view planning for object recognition in mobile robotics
CN111337011A (en) Indoor positioning method based on laser and two-dimensional code fusion
Ge et al. Visual-feature-assisted mobile robot localization in a long corridor environment
CN116068575A (en) Human body state identification method and device based on 2D laser radar and RGB-D camera
CN116109047A (en) Intelligent scheduling method based on three-dimensional intelligent detection
Li et al. Mobile robot map building based on laser ranging and kinect
Song et al. Target localization using rgb-d camera and lidar sensor fusion for relative navigation
Yu et al. Indoor Localization Based on Fusion of AprilTag and Adaptive Monte Carlo
CN113792645A (en) AI eyeball fusing image and laser radar
Shaik et al. 2D localization in large areas using inexpensive rgbd camera augmented with visual tags
Ding et al. Fusing Stereopsis & Corner Sparse Optical Flow for Real-time Obstacle Avoidance of Unmanned Aerial Vehicle
Lyu et al. Multi-sensor fusion based obstacle localization technology
CN111239761A (en) Method for indoor real-time establishment of two-dimensional map
Ikoma Multiple pedestrians tracking with composite sensor of laser range finder and omni-directional camera by SMC implementation of PHD filter
Zou et al. An automatic calibration between an omni-directional camera and a laser rangefinder for dynamic scenes reconstruction

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