Human body tracking and positioning system and method of indoor service robot
By collecting and processing point cloud data in an indoor service robot, and combining extended Kalman filtering and Euclidean clustering algorithms, the problem of robust detection and tracking of human targets in indoor environments is solved. Stable and accurate tracking in complex environments is achieved, adapting to the characteristics of human movement indoors and meeting the requirements of real-time performance and practicality.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- FUJIAN AGRI VOCATIONAL & TECH COLLEGE
- Filing Date
- 2025-12-26
- Publication Date
- 2026-04-21
AI Technical Summary
In indoor environments, existing technologies struggle to efficiently filter out point cloud noise, especially complex ground noise, and achieve robust detection and continuous tracking of human targets even under occlusion conditions.
Point cloud data is acquired using a depth camera or solid-state LiDAR. Combined with the robot's odometry and inertial measurement unit, the point cloud data is transformed to the global coordinate system through a coordinate transformation unit. Ground point cloud is filtered out using height adaptation and local plane fitting methods. Human body detection and tracking are performed by combining Euclidean clustering algorithm and extended Kalman filter algorithm. The observation noise matrix is optimized to adapt to the indoor environment.
It achieves stable and accurate tracking of human targets in complex environments, effectively filters out interference from uneven ground and low furniture, maintains continuous and stable tracking of human position and direction of movement, adapts to indoor human movement characteristics, improves the accuracy and stability of state estimation, and meets the requirements of real-time performance and practicality.
Smart Images
Figure CN121898366A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the fields of robotics, computer vision, and sensor fusion, and in particular to a human body tracking and positioning system and method for indoor service robots. Background Technology
[0002] With the rapid development of autonomous driving, intelligent monitoring, and human-computer interaction, sensor-based target detection and tracking technologies have gradually become a hot topic. Among these, using devices such as LiDAR and depth cameras to acquire 3D point cloud data and identify and track objects in the environment is a key technology for intelligent systems to achieve environmental perception. However, due to the high-dimensional sparsity of point cloud data and the complexity of target objects such as the human body, efficiently extracting and tracking the human body from the point cloud remains a challenging problem. Summary of the Invention
[0003] The purpose of this invention is to solve the problems of point cloud noise filtering (especially on complex ground) and robust detection and continuous tracking of human targets under occlusion in indoor service scenarios, and to provide a human tracking and positioning system and method for indoor service robots.
[0004] The technical solution adopted in this invention is:
[0005] A human tracking and positioning system for an indoor service robot, deployed on a robot platform, includes the following modules:
[0006] The point cloud data acquisition module is used to acquire environmental point cloud data through a depth camera or solid-state LiDAR, and integrates data from the robot's odometry and inertial measurement unit (IMU). The coordinate transformation unit converts the raw point cloud data to the global coordinate system.
[0007] The point cloud processing module, connected to the cloud data acquisition module, is used to preprocess the converted point cloud data. The preprocessing includes: filtering out ground point clouds using a combination of height adaptation and local plane fitting, and performing downsampling, outlier removal, and point cloud filtering outside the indoor space in sequence.
[0008] The human detection module, connected to the point cloud processing module, is used to extract potential human point cloud clusters from the preprocessed point cloud and perform feature verification on each human point cloud cluster based on the number of point clouds and the size of the 3D bounding box, so as to output the position observation information of the effective human target.
[0009] The human tracking module, connected to the human detection module, is used to establish and maintain a tracker for each valid human target. The human tracking module uses the Extended Kalman Filter (EKF) as its core algorithm. The state vector of the Extended Kalman Filter contains the target's position and velocity.
[0010] Among them, the process noise covariance (Q) of the extended Kalman filter is specifically adjusted according to the indoor human movement characteristics (start, stop, turn); the observation noise covariance matrix (R) of the extended Kalman filter is a dynamic matrix, which is obtained by correcting the spatial distribution position variance of the target point cloud itself to obtain the dynamic observation noise matrix of the Kalman filter, which is used to represent the uncertainty of the observation position.
[0011] The results output and visualization module is connected to the human body tracking module to publish and display information about the tracked human body in real time.
[0012] Furthermore, the human detection module has a built-in human detector, which is used to obtain 2D bounding boxes from the image information of the RGB-D camera in order to extract the point cloud within the corresponding 2D bounding box. The human detection module is configured to directly use the Euclidean clustering algorithm to segment all the preprocessed point clouds to obtain point cloud clusters of potential human bodies, and the clustering distance threshold is optimized for the spacing between people indoors.
[0013] Furthermore, the human detection module includes the following:
[0014] The target point cloud extraction unit is equipped with a human body detector. The human body detector obtains 2D bounding boxes from the image information of the RGB-D camera in order to extract potential human point clouds within the corresponding range.
[0015] The point cloud clustering segmentation unit is used to segment the potential human point cloud (whether from image-assisted or pure point cloud processing) using the Euclidean cluster extraction algorithm to obtain an independent human point cloud cluster corresponding to each potential human target;
[0016] The human feature verification unit is used to perform feature verification on each human point cloud cluster based on the number of point clouds and the size of the 3D bounding box, so as to output the location observation information of the valid human target.
[0017] Furthermore, the data association in the human body tracking module adopts the Global Nearest Neighbor (GNN) method combined with the Hungarian algorithm, using Mahalanobis distance or Euclidean distance as the association cost, and the association threshold is optimized for indoor cross-person and close proximity situations.
[0018] Furthermore, the extended Kalman filter algorithm of the human tracking module calculates the position variance (var) of the point cloud clusters of the currently detected target in the x, y, and z dimensions. x ,var y ,var z In order to dynamically calculate the observation noise matrix: the expression for calculating the observation noise matrix R is as follows:
[0019] R = diag{R base+k*[var x ,var y ,var z ]};
[0020] Among them, R base This represents the measurement noise of the basic sensor; k is the empirical scaling factor.
[0021] Furthermore, the state vector x of the extended Kalman filter is: [px,py,pz,vx,vy,vz] T This represents the target's position (px, py, pz) and velocity (vx, vy, vz) in a selected coordinate system (usually the global coordinate system); this model was chosen based on considerations of indoor human motion patterns (primarily positional changes, with relatively gradual velocity changes).
[0022] The process noise covariance matrix Q of the extended Kalman filter is used to quantify the effects of unmodeled dynamics or uncertainties in the system model. The process noise covariance matrix Q is set to a zero-mean Gaussian white noise series w. k ~N(0,σ 2 The Q matrix is a 6x6 diagonal matrix, and the process noise covariance matrix (Q) is specifically adjusted based on the frequent start-stop and flexible turning characteristics of indoor human motion. The extended Kalman filter state transition model adopts a constant velocity (CV) model; the state transition matrix F for time interval Δt is:
[0023]
[0024] Among them, I 3x3 Represents a 3x3 identity matrix, 0 3x3 This represents a 3x3 zero matrix.
[0025] Furthermore, the information of the tracked human body published by the results output and visualization module includes the unique ID, 3D position coordinates, velocity vector, 3D bounding box and tracking status of each tracked human body, and is provided to the robot's navigation and interaction module through ROS topics, services or API interfaces.
[0026] A method for human tracking and localization of an indoor service robot, comprising the following steps:
[0027] Step 1, Data Acquisition: Acquire environmental point cloud data using a depth camera or solid-state LiDAR and convert it to a global coordinate system;
[0028] Step 2, Point Cloud Preprocessing: Preprocess the point cloud in the global coordinate system by filtering out the ground, downsampling, removing outliers, and filtering the point cloud outside the spatial range.
[0029] Step 3, Human body detection: Extract and verify human target point cloud clusters from the preprocessed point cloud to obtain human body position observation information;
[0030] Step 4, Human Tracking: Based on the human position observation information, run the Extended Kalman Filter (EKF) to track each valid human target; wherein, during the EKF update step, the observation noise covariance matrix (R) is dynamically calculated based on the spatial distribution variance of the current human target point cloud cluster.
[0031] Step 5, Output: Output and publish the location, speed, and ID information of the tracked human body.
[0032] Furthermore, step 2 specifically includes the following steps:
[0033] Step 2-1: Set a dynamic Z-axis height threshold to initially filter point clouds that are higher than the dynamic Z-axis height threshold;
[0034] Step 2-2: Perform improved Random Sample Consistency (RANSAC) plane fitting on the local region of the remaining point cloud in the robot's direction of travel, identify and remove planar point cloud clusters with normal vectors (close to) vertically upward to handle uneven ground.
[0035] Steps 2-3 involve downsampling the point cloud after removing the ground using a voxel grid filter.
[0036] Steps 2-4: Use Statistical Outlier Removal (SOR) or Radius Filter to remove isolated noise points;
[0037] Steps 2-5 involve setting an indoor human body prior bounding box to remove point clouds outside the indoor human body prior bounding box.
[0038] Specifically, a priori knowledge bounding box that conforms to the spatial distribution of the human body in the room is set (e.g., X:[-2m,2m], Y:[0m,4m], Z:[0.2m,2.0m]), and point clouds outside this bounding box (mostly walls and distant objects) are removed.
[0039] Furthermore, step 3 specifically includes the following steps:
[0040] Step 3-1, Obtain potential human point cloud: Using the image information from the RGB-D camera, obtain a 2D bounding box through the human detector, and project it onto the depth map or point cloud to extract the potential human point cloud within the corresponding range.
[0041] Specifically, this involves combining image information (if using an RGB-D camera) or pure point cloud methods. If using an RGB-D camera, mature human detectors (such as YOLO, Mask R-CNN) can be used to detect 2D bounding boxes of the human body in the image domain, and the detected boxes can be projected onto the corresponding depth map or point cloud to extract the point cloud within the boxes. Emphasis is placed on detector selection and optimization under complex backgrounds and partial occlusion.
[0042] Step 3-2: The extracted potential human point cloud (whether from image-assisted or pure point cloud processing) is segmented using the Euclidean Cluster Extraction algorithm to obtain an independent human point cloud cluster corresponding to each potential human target.
[0043] Specifically, the key parameter (distance threshold) in the clustering segmentation in step 3-2 is optimized for the spacing between people indoors (usually 0.2-0.5m) to accommodate different levels of crowding.
[0044] Step 3-3, Human Feature Verification: Perform feature verification on each human point cloud cluster based on the number of point clouds and the size of the 3D bounding box to output the location observation information of the valid human target.
[0045] Furthermore, during human feature verification in step 3-3, the centroid of the human point cloud cluster is calculated simultaneously (a weighted average can be used, with weights selected from point depth information or confidence level). The purpose of calculating the centroid of the human point cloud cluster is to obtain the center position coordinates of the human target, which are used as position observations for tracking. The centroid is calculated by weighted averaging of the point cloud coordinates, with weights based on point depth or confidence level to improve positioning accuracy.
[0046] Furthermore, step 4 specifically includes the following steps:
[0047] Step 4-1: Use Global Nearest Neighbor (GNN) combined with the Hungarian algorithm to associate the current detected target with the existing trajectory;
[0048] Specifically, the association cost is based on the calculation of Euclidean or Mahalanobis distance, and the association threshold needs to be optimized for possible crossover or close proximity situations of indoor personnel.
[0049] Step 4-2, EKF prediction: Predict the state at the next time step using the CV model on the existing trajectory; specifically including the following steps:
[0050] Step 4-2-1: Obtain the detection location observation (z) of the successfully associated trajectory;
[0051] Step 4-2-2: Calculate the position variance (var) of the point cloud cluster of the currently detected target in the x, y, and z dimensions.x ,var y ,var z );
[0052] Step 4-2-3, dynamically calculate the observation noise matrix:
[0053] R = diag{R base +k*[var x ,var y ,var z ]};
[0054] Among them, R base This represents the measurement noise of the basic sensor; k is the empirical scaling factor;
[0055] This method introduces a specific implementation of point cloud variance-corrected Kalman filtering (for centroid tracking and action recognition). This invention innovatively incorporates the spatial distribution variance of the target point cloud itself into the observation noise estimation to reflect the uncertainty of the target's current state, thereby adaptively adjusting the filter's confidence level at the position tracking level and improving tracking robustness under occlusion or attitude changes.
[0056] Step 4-3, perform standard EKF update: calculate Kalman gain K, update state estimate x, update error covariance P, and calculate innovation covariance S using the observation noise matrix R; specifically,
[0057] Obtain the prediction error covariance P - The Kalman gain K is calculated using the observation matrix H and observation noise matrix R, which map the state vector to the observation vector. The formula is: K = P - ·H T ·S -1 The Kalman gain is a weight matrix that acts as an "arbitrator," balancing the confidence in the system model's predictions with the confidence in the current sensor observations. If the observations are very reliable (small R), the K value is large, and the system is more inclined to adopt new observations during updates; conversely, if the model predictions are more reliable (small R), the K value is large. - If the value of K is small, then the predicted value will be more trusted.
[0058] Where S0 is the predicted information covariance, calculated using the formula S0 = H·P - ·H T +R; where H represents the observation matrix that maps the state vector to the observation vector, H=[I 3x3 ,0 3x3 ], I 3x3 Represents a 3x3 identity matrix, 0 3x3 R represents a 3x3 zero matrix; R represents the observation noise matrix.
[0059] The expression for the update error covariance P is: P = (IK·H)·P - Among them, P - Let be the prediction error covariance from the prediction step, representing the uncertainty of the predicted value; the prediction error covariance at time k+1. Wherein, the error covariance at time k is P k The extended Kalman filter (EPF) is used; specifically, after incorporating new observation information, the confidence metric (i.e., uncertainty) of the state estimation accuracy is updated. Typically, a single effective observation reduces the estimation uncertainty; (IK·H) in the formula is a "reduction factor," signifying that the uncertainty of the state estimation is reduced compared to the prediction time due to the acquisition of new observation information. The updated error covariance P is then used in the next prediction step, starting a new filtering cycle.
[0060] The innovation covariance S is calculated using the observation noise matrix R. The expression for the calculation is: S = H * P * H T +R;
[0061] Specifically, the innovation covariance S is a standard component of the Extended Kalman Filter (EKF) update step, representing the covariance matrix between predicted and actual observations. S is used to calculate the Kalman gain to adjust the weights of the state estimates; H represents the observation matrix that maps the state vector to the observation vector, for position observations H = [I 3x3 ,0 3x3 ].
[0062] Using the calculated Kalman gain K, the predicted state of the system is optimally fused with the new observation data to obtain the most accurate state estimate at the current moment; the expression for the state estimate x is: x = x - +K·(zH·x - ); where x - For the predicted state from the prediction step (such as predicted position and velocity), the predicted state at time k+1 Where x k The state estimate at time k is given by: Z is the current observation value (e.g., the 3D position measured by the human detection module); (zH·x) - () represents the new information, indicating the difference between the observed value and the predicted value.
[0063] Furthermore, the information of the tracked human body in step 5 includes the unique ID, 3D position coordinates, velocity vector, 3D bounding box and tracking status of each tracked human body, and is provided to the robot's navigation and interaction module through ROS topics, services or API interfaces.
[0064] This invention, employing the above technical solutions, has the following technical advantages compared to existing technologies: 1) It can effectively process complex indoor point cloud data, especially adept at filtering out interference from uneven surfaces and low furniture (such as carpet edges, stools, and thresholds), accurately extracting human-related point clouds. 2) It achieves stable and accurate human positioning and tracking: even with partial occlusion (such as furniture obscuring legs), changes in human posture (such as sitting or squatting), and dynamic changes in the environment (such as changes in lighting or interference from moving objects), it can still maintain continuous and stable tracking of the human target's position (3D coordinates) and direction of movement (velocity vector). 3) It improves the adaptability of the tracking algorithm: it optimizes the filtering algorithm (such as EKF), considering the characteristics of indoor human movement (frequent starts and stops, flexible turning but usually limited acceleration) and the spatial distribution characteristics of the target point cloud, to improve the accuracy and stability of state estimation. 4) It ensures real-time performance and practicality: the system design fully considers the computational resource limitations and real-time interaction requirements of indoor service robots, and the algorithm strives for high efficiency while ensuring accuracy. 5) Support basic posture information: Based on positioning and tracking, it can further provide rough information on human body orientation or upper body center of mass to assist robot interaction decision-making. Attached Figure Description
[0065] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments;
[0066] Figure 1 This is a schematic diagram of the human body tracking and positioning system for an indoor service robot according to the present invention;
[0067] Figure 2 This is a flowchart illustrating the point cloud processing module of the present invention;
[0068] Figure 3 This is a flowchart illustrating the human body tracking module of the present invention. Detailed Implementation
[0069] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings.
[0070] like Figures 1 to 3 As shown in one example, this invention discloses a human body tracking and positioning system and method for indoor service robots, deployed on a robot platform, which includes the following modules:
[0071] The point cloud data acquisition module is used to acquire environmental point cloud data through a depth camera or solid-state LiDAR, and integrates robot body odometry and IMU data. The coordinate transformation unit converts the raw point cloud data to the global coordinate system.
[0072] The point cloud processing module, connected to the cloud data acquisition module, is used to preprocess the converted point cloud data. The preprocessing includes: filtering out ground point clouds using a combination of height adaptation and local plane fitting, and performing downsampling, outlier removal, and point cloud filtering outside the indoor space in sequence.
[0073] The human detection module, connected to the point cloud processing module, is used to extract potential human point cloud clusters from the preprocessed point cloud and perform feature verification on each human point cloud cluster based on the number of point clouds and the size of the 3D bounding box, so as to output the position observation information of the effective human target.
[0074] The human tracking module, connected to the human detection module, is used to establish and maintain a tracker for each valid human target. The human tracking module uses the Extended Kalman Filter (EKF) as its core algorithm. The state vector of the Extended Kalman Filter contains the target's position and velocity.
[0075] Among them, the process noise covariance (Q) of the extended Kalman filter is specifically adjusted according to the indoor human motion characteristics (start, stop, turn), for example, by increasing the noise term of the velocity component to reflect its uncertainty; the observation noise covariance matrix (R) of the extended Kalman filter is a dynamic matrix, which is obtained by correcting the spatial distribution position variance of the target point cloud itself to obtain the dynamic observation noise matrix of the Kalman filter, which is used to represent the uncertainty of the observation position.
[0076] The results output and visualization module is connected to the human body tracking module to publish and display information about the tracked human body in real time.
[0077] Furthermore, the human detection module has a built-in human detector, which is used to obtain 2D bounding boxes from the image information of the RGB-D camera in order to extract the point cloud within the corresponding 2D bounding box. The human detection module is configured to directly use the Euclidean clustering algorithm to segment all the preprocessed point clouds to obtain point cloud clusters of potential human bodies, and the clustering distance threshold is optimized for the spacing between people indoors.
[0078] Furthermore, the human detection module includes the following:
[0079] The target point cloud extraction unit is equipped with a human body detector. The human body detector obtains 2D bounding boxes from the image information of the RGB-D camera in order to extract potential human point clouds within the corresponding range.
[0080] The point cloud clustering segmentation unit is used to segment the potential human point cloud (whether from image-assisted or pure point cloud processing) using the Euclidean cluster extraction algorithm to obtain an independent human point cloud cluster corresponding to each potential human target;
[0081] The human feature verification unit is used to perform feature verification on each human point cloud cluster based on the number of point clouds and the size of the 3D bounding box, so as to output the location observation information of the valid human target.
[0082] Furthermore, the data association in the human body tracking module adopts the Global Nearest Neighbor (GNN) method combined with the Hungarian algorithm, using Mahalanobis distance or Euclidean distance as the association cost, and the association threshold is optimized for indoor cross-person and close proximity situations.
[0083] Furthermore, the extended Kalman filter algorithm of the human tracking module calculates the position variance (var) of the point cloud clusters of the currently detected target in the x, y, and z dimensions. x ,var y ,var z In order to dynamically calculate the observation noise matrix: the expression for calculating the observation noise matrix R is as follows:
[0084] R = diag{R base +k*[var x ,var y ,var z ]};
[0085] Among them, R base This represents the measurement noise of the basic sensor; k is the empirical scaling factor.
[0086] Furthermore, the state vector x of the extended Kalman filter is: [px,py,pz,vx,vy,vz] T This represents the target's position (px, py, pz) and velocity (vx, vy, vz) in a selected coordinate system (usually the global coordinate system); this model was chosen based on considerations of indoor human motion patterns (primarily positional changes, with relatively gradual velocity changes).
[0087] The process noise covariance matrix Q of the extended Kalman filter is used to quantify the effects of unmodeled dynamics or uncertainties in the system model. The process noise covariance matrix Q is set to a zero-mean Gaussian white noise series w. k ~N(0,σ 2 The Q matrix is a 6x6 diagonal matrix, and the process noise covariance matrix (Q) is specifically adjusted based on the frequent start-stop and flexible turning characteristics of indoor human motion. The extended Kalman filter state transition model adopts a constant velocity (CV) model; the state transition matrix F for time interval Δt is:
[0088]
[0089] Among them, I 3x3 Represents a 3x3 identity matrix, 0 3x3 This represents a 3x3 zero matrix.
[0090] Furthermore, the information of the tracked human body published by the results output and visualization module includes the unique ID, 3D position coordinates, velocity vector, 3D bounding box and tracking status of each tracked human body, and is provided to the robot's navigation and interaction module through ROS topics, services or API interfaces.
[0091] A method for human tracking and localization of an indoor service robot, comprising the following steps:
[0092] Step 1, Data Acquisition: Acquire environmental point cloud data using a depth camera or solid-state LiDAR and convert it to a global coordinate system;
[0093] Step 2, Point Cloud Preprocessing: Preprocess the point cloud in the global coordinate system by filtering out the ground, downsampling, removing outliers, and filtering the point cloud outside the spatial range.
[0094] Step 3, Human body detection: Extract and verify human target point cloud clusters from the preprocessed point cloud to obtain human body position observation information;
[0095] Step 4, Human Tracking: Based on the human position observation information, run the Extended Kalman Filter (EKF) to track each valid human target; wherein, during the EKF update step, the observation noise covariance matrix (R) is dynamically calculated based on the spatial distribution variance of the current human target point cloud cluster.
[0096] Step 5, Output: Output and publish the location, speed, and ID information of the tracked human body.
[0097] Furthermore, step 2 specifically includes the following steps:
[0098] Step 2-1: Set a dynamic Z-axis height threshold to initially filter point clouds that are higher than the dynamic Z-axis height threshold;
[0099] Step 2-2: Perform improved Random Sample Consistency (RANSAC) plane fitting on the local region of the remaining point cloud in the robot's direction of travel, identify and remove planar point cloud clusters with normal vectors (close to) vertically upward to handle uneven ground.
[0100] Steps 2-3 involve downsampling the point cloud after removing the ground using a voxel grid filter.
[0101] Steps 2-4: Use Statistical Outlier Removal (SOR) or Radius Filter to remove isolated noise points;
[0102] Steps 2-5 involve setting an indoor human body prior bounding box to remove point clouds outside the indoor human body prior bounding box.
[0103] Specifically, a priori knowledge bounding box that conforms to the spatial distribution of the human body in the room is set (e.g., X:[-2m,2m], Y:[0m,4m], Z:[0.2m,2.0m]), and point clouds outside this bounding box (mostly walls and distant objects) are removed.
[0104] Furthermore, step 3 specifically includes the following steps:
[0105] Step 3-1, Obtain potential human point cloud: Using the image information from the RGB-D camera, obtain a 2D bounding box through the human detector, and project it onto the depth map or point cloud to extract the potential human point cloud within the corresponding range.
[0106] Specifically, this involves combining image information (if using an RGB-D camera) or pure point cloud methods. If using an RGB-D camera, mature human detectors (such as YOLO, Mask R-CNN) can be used to detect 2D bounding boxes of the human body in the image domain, and the detected boxes can be projected onto the corresponding depth map or point cloud to extract the point cloud within the boxes. Emphasis is placed on detector selection and optimization under complex backgrounds and partial occlusion.
[0107] Step 3-2: The extracted potential human point cloud (whether from image-assisted or pure point cloud processing) is segmented using the Euclidean Cluster Extraction algorithm to obtain an independent human point cloud cluster corresponding to each potential human target.
[0108] Specifically, the key parameter (distance threshold) in the clustering segmentation in step 3-2 is optimized for the spacing between people indoors (usually 0.2-0.5m) to accommodate different levels of crowding.
[0109] Step 3-3, Human Feature Verification: Perform feature verification on each human point cloud cluster based on the number of point clouds and the size of the 3D bounding box to output the location observation information of the valid human target.
[0110] Furthermore, during the human feature verification in step 3-3, the centroid of the human point cloud cluster is calculated simultaneously (a weighted average can be used, with the weights being either point depth information or confidence level).
[0111] Furthermore, step 4 specifically includes the following steps:
[0112] Step 4-1: Use Global Nearest Neighbor (GNN) combined with the Hungarian algorithm to associate the current detected target with the existing trajectory;
[0113] Specifically, the association cost is based on the calculation of Euclidean or Mahalanobis distance, and the association threshold needs to be optimized for possible crossover or close proximity situations of indoor personnel.
[0114] Step 4-2, EKF prediction: Predict the state at the next time step using the CV model on the existing trajectory; specifically including the following steps:
[0115] Step 4-2-1: Obtain the detection location observation value (z) of the successfully associated trajectory; Observation value (z): usually the target position [px, py, pz] output by the detection module. T Alternatively, the centroid location. If the detection module can provide velocity observations (such as multi-frame correlation or optical flow), these can also be included in the observation vector.
[0116] Step 4-2-2: Calculate the position variance (var) of the point cloud cluster of the currently detected target in the x, y, and z dimensions. x ,var y ,var z Large variance indicates that the point cloud is scattered (e.g., the target is open or partially obscured) and the observation position is highly uncertain; small variance indicates that the point cloud is concentrated (e.g., standing posture) and the observation position is highly confident.
[0117] Step 4-2-3, dynamically calculate the observation noise matrix:
[0118] R = diag{R base +k*[var x ,var y ,var z ]};
[0119] Among them, R base This represents the measurement noise of the basic sensor; k is the empirical scaling factor;
[0120] Specifically, the observation noise covariance (R) is traditionally set as a fixed diagonal matrix. The key optimization of this invention is that the value of R is no longer fixed, but is closely related to the spatial distribution of the current point cloud cluster of the target.
[0121] This invention introduces a specific implementation of point cloud variance-corrected Kalman filtering (for centroid tracking and action recognition). This invention innovatively incorporates the spatial distribution variance of the target point cloud itself into the observation noise estimation to reflect the uncertainty of the target's current state. This allows for adaptive adjustment of the filter's confidence level at the position tracking level, improving tracking robustness under occlusion or attitude changes.
[0122] Step 4-3, perform standard EKF update: calculate Kalman gain K, update state estimate x, update error covariance P, and calculate innovation covariance S using the observation noise matrix R; specifically,
[0123] Obtain the prediction error covariance P - The Kalman gain K is calculated using the observation matrix H and observation noise matrix R, which map the state vector to the observation vector. The formula is: K = P - ·H T ·S -1 The Kalman gain is a weight matrix that acts as an "arbitrator," balancing the confidence in the system model's predictions with the confidence in the current sensor observations. If the observations are very reliable (small R), the K value is large, and the system is more inclined to adopt new observations during updates; conversely, if the model predictions are more reliable (small R), the K value is large. - If the value of K is small, then the predicted value will be more trusted.
[0124] Where S0 is the predicted information covariance, calculated using the formula S0 = H·P - ·H T +R; where H represents the observation matrix that maps the state vector to the observation vector, H=[I 3x3 ,0 3x3 ], I 3x3 Represents a 3x3 identity matrix, 0 3x3 R represents a 3x3 zero matrix; R represents the observation noise matrix.
[0125] The expression for the update error covariance P is: P = (IK·H)·P - Among them, P - Let be the prediction error covariance from the prediction step, representing the uncertainty of the predicted value; the prediction error covariance at time k+1. Wherein, the error covariance at time k is P k The extended Kalman filter (EPF) is used; specifically, after incorporating new observation information, the confidence metric (i.e., uncertainty) of the state estimation accuracy is updated. Typically, a single effective observation reduces the estimation uncertainty; (IK·H) in the formula is a "reduction factor," signifying that the uncertainty of the state estimation is reduced compared to the prediction time due to the acquisition of new observation information. The updated error covariance P is then used in the next prediction step, starting a new filtering cycle.
[0126] The innovation covariance S is calculated using the observation noise matrix R. The expression for the calculation is: S = H * P * H T +R;
[0127] Specifically, the innovation covariance S is a standard component of the Extended Kalman Filter (EKF) update step, representing the covariance matrix between predicted and actual observations. S is used to calculate the Kalman gain to adjust the weights of the state estimates; H represents the observation matrix that maps the state vector to the observation vector, for position observations H = [I 3x3 ,0 3x3 ].
[0128] Using the calculated Kalman gain K, the predicted state of the system is optimally fused with the new observation data to obtain the most accurate state estimate at the current moment; the expression for the state estimate x is: x = x - +K·(zH·x - ); where x - For the predicted state from the prediction step (such as predicted position and velocity), the predicted state at time k+1 Where x k The state estimate at time k is given by: Z is the current observation value (e.g., the 3D position measured by the human detection module); (zH·x) - () represents the new information, indicating the difference between the observed value and the predicted value.
[0129] Furthermore, step 4 also includes trajectory management, which includes logic such as trajectory initialization, confirmation (requires successful association for N consecutive frames), prediction (when not associated), and termination (if M consecutive frames are lost).
[0130] Furthermore, the information of the tracked human body in step 5 includes the unique ID, 3D position coordinates, velocity vector, 3D bounding box, and tracking status of each tracked human body, and is provided to the robot's navigation and interaction module through ROS topics, services, or API interfaces. The 3D bounding box is obtained based on the point cloud clusters during detection, and the tracking status includes new target, stable tracking, lost tracking, and recovery.
[0131] This invention, employing the above technical solutions, has the following technical advantages compared to existing technologies: 1) It can effectively process complex indoor point cloud data, especially adept at filtering out interference from uneven surfaces and low furniture (such as carpet edges, stools, and thresholds), accurately extracting human-related point clouds. 2) It achieves stable and accurate human positioning and tracking: even with partial occlusion (such as furniture obscuring legs), changes in human posture (such as sitting or squatting), and dynamic changes in the environment (such as changes in lighting or interference from moving objects), it can still maintain continuous and stable tracking of the human target's position (3D coordinates) and direction of movement (velocity vector). 3) It improves the adaptability of the tracking algorithm: it optimizes the filtering algorithm (such as EKF), considering the characteristics of indoor human movement (frequent starts and stops, flexible turning but usually limited acceleration) and the spatial distribution characteristics of the target point cloud, to improve the accuracy and stability of state estimation. 4) It ensures real-time performance and practicality: the system design fully considers the computational resource limitations and real-time interaction requirements of indoor service robots, and the algorithm strives for high efficiency while ensuring accuracy. 5) Supports basic posture information: Building upon positioning and tracking, it can further provide coarse information on human orientation or upper body center of mass to assist robot interaction decisions. This invention primarily relies on depth / laser point cloud data, eliminating the need for clear RGB image information, thus naturally protecting user visual privacy and making it particularly suitable for sensitive environments such as homes and hospital wards.
[0132] This invention effectively solves the robustness, accuracy, and real-time performance issues of human target detection and tracking in complex indoor environments by deeply optimizing the point cloud processing workflow (especially the innovative ground filtering method) and innovatively introducing dynamic observation noise estimation based on the target point cloud distribution variance into the extended Kalman filter. The system has been validated on a real-world service robot platform with significant results and promising application prospects.
[0133] Obviously, the described embodiments are only a part of the embodiments of this application, not all of them. Without conflict, the embodiments and features in the embodiments of this application can be combined with each other. The components of the embodiments of this application described and illustrated herein can generally be arranged and designed in various different configurations. Therefore, the detailed description of the embodiments of this application is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.
Claims
1. A human tracking and positioning system for an indoor service robot, deployed on a robot platform, characterized in that: The system includes the following modules: The point cloud data acquisition module is used to acquire environmental point cloud data through a depth camera or solid-state LiDAR, and integrates data from the robot's odometry and inertial measurement unit. The coordinate transformation unit converts the raw point cloud data to the global coordinate system. The point cloud processing module, connected to the cloud data acquisition module, is used to preprocess the converted point cloud data. The preprocessing includes: filtering out ground point clouds using a combination of height adaptation and local plane fitting, and performing downsampling, outlier removal, and point cloud filtering outside the indoor space in sequence. The human detection module, connected to the point cloud processing module, is used to extract potential human point cloud clusters from the preprocessed point cloud and perform feature verification on each human point cloud cluster based on the number of point clouds and the size of the 3D bounding box, so as to output the position observation information of the effective human target. The human tracking module, connected to the human detection module, is used to establish and maintain a tracker for each valid human target. The human tracking module uses the extended Kalman filter as its core algorithm. The state vector of the extended Kalman filter contains the target's position and velocity. The process noise covariance Q of the extended Kalman filter is specifically adjusted according to the indoor human motion characteristics. The observation noise covariance matrix R of the extended Kalman filter is a dynamic matrix. The dynamic observation noise matrix of the Kalman filter is obtained by correcting the spatial distribution position variance of the target point cloud itself, which is used to represent the uncertainty of the observation position. The results output and visualization module is connected to the human body tracking module to publish and display information about the tracked human body in real time.
2. The human body tracking and positioning system for an indoor service robot according to claim 1, characterized in that: The human body detection module includes the following units: The target point cloud extraction unit is equipped with a human body detector. The human body detector obtains 2D bounding boxes from the image information of the RGB-D camera in order to extract potential human point clouds within the corresponding range. Point cloud clustering segmentation unit is used to segment potential human point clouds using the Euclidean clustering algorithm to obtain independent human point cloud clusters corresponding to each potential human target; The human feature verification unit is used to perform feature verification on each human point cloud cluster based on the number of point clouds and the size of the 3D bounding box, so as to output the location observation information of the valid human target.
3. The human body tracking and positioning system for an indoor service robot according to claim 1, characterized in that: The extended Kalman filter algorithm of the human tracking module calculates the position variance (var) of the point cloud clusters of the currently detected target in the x, y, and z dimensions. x ,var y ,var z In order to dynamically calculate the observation noise matrix: the expression for calculating the observation noise matrix R is as follows: R=diag{R base +k*[was x ,where y ,where z ]}; Among them, R base This represents the measurement noise of the basic sensor; k is the empirical scaling factor.
4. The human body tracking and positioning system for an indoor service robot according to claim 1, characterized in that: The state vector x of the extended Kalman filter is: [px, py, pz, vx, vy, vz] T The position (px, py, pz) and velocity (vx, vy, vz) of the target in the selected coordinate system are represented. The process noise covariance matrix Q of the extended Kalman filter is used to quantify the influence of unmodeled dynamics or uncertainties in the system model. The process noise covariance matrix Q is set to a Gaussian white noise series w with zero mean. k ~N(0,σ 2 The Q matrix is a 6x6 diagonal matrix; the extended Kalman filter uses a uniform velocity model for its state transition; the state transition matrix F for time interval Δt is: Among them, I 3x3 Represents a 3x3 identity matrix, 0 3x3 This represents a 3x3 zero matrix.
5. The human body tracking and positioning system for an indoor service robot according to claim 1, characterized in that: The results output and visualization module publishes information about the tracked human body, including each tracked human body's unique ID, 3D position coordinates, velocity vector, 3D bounding box, and tracking status, which is provided to the robot's navigation and interaction module through ROS topics, services, or API interfaces.
6. A method for human body tracking and positioning of an indoor service robot, comprising the human body tracking and positioning system for an indoor service robot as described in any one of claims 1 to 5, characterized in that: The method includes the following steps: Step 1, Data Acquisition: Acquire environmental point cloud data using a depth camera or solid-state LiDAR and convert it to a global coordinate system; Step 2, Point Cloud Preprocessing: Preprocess the point cloud in the global coordinate system by filtering out the ground, downsampling, removing outliers, and filtering the point cloud outside the spatial range. Step 3, Human body detection: Extract and verify human target point cloud clusters from the preprocessed point cloud to obtain human body position observation information; Step 4, Human Tracking: Based on the human position observation information, an extended Kalman filter is run to track each valid human target; wherein, during the EKF update step, the observation noise covariance matrix R is dynamically calculated based on the spatial distribution variance of the current human target point cloud cluster; Step 5, Output: Output and publish the location, speed, and ID information of the tracked human body.
7. The method for human body tracking and positioning of an indoor service robot according to claim 6, characterized in that: Step 2 specifically includes the following steps: Step 2-1: Set a dynamic Z-axis height threshold to initially filter point clouds that are higher than the dynamic Z-axis height threshold; Step 2-2: Perform improved random sampling consistency plane fitting on the local area of the remaining point cloud in the robot's direction of travel, identify and remove planar point cloud clusters with vertically upward normal vectors, in order to handle uneven ground. Steps 2-3: Voxel grid filtering is used to downsample the point cloud after removing the ground. Steps 2-4: Use statistical outlier removal or radius filtering to remove isolated noise points; Steps 2-5 involve setting an indoor human body prior bounding box to remove point clouds outside the indoor human body prior bounding box.
8. The method for human body tracking and positioning of an indoor service robot according to claim 6, characterized in that: Step 3 specifically includes the following steps: Step 3-1, obtain potential human point cloud: On the image information of the RGB-D camera, obtain a 2D bounding box through the human detector and project it onto the depth map or point cloud to extract the potential human point cloud within the corresponding range. Step 3-2: Apply Euclidean clustering algorithm to segment the extracted potential human point cloud to obtain an independent human point cloud cluster corresponding to each potential human target. Step 3-3, Human Feature Verification: Perform feature verification on each human point cloud cluster based on the number of point clouds and the size of the 3D bounding box to output the location observation information of the valid human target.
9. A method for human body tracking and positioning of an indoor service robot according to claim 6, characterized in that: Step 4 specifically includes the following steps: Step 4-1: Use the global nearest neighbor algorithm combined with the Hungarian algorithm to associate the current detected target with the existing trajectory; Step 4-2, EKF prediction: Predict the state at the next time step using the CV model on the existing trajectory; specifically including the following steps: Step 4-2-1: Obtain the detection location observations of the successfully associated trajectories; Step 4-2-2: Calculate the position variance (var) of the point cloud cluster of the currently detected target in the x, y, and z dimensions. x ,var y ,var z ); Step 4-2-3, dynamically calculate the observation noise matrix: R=diag{R base +k*[was x ,where y ,where z ]}; Among them, R base This represents the measurement noise of the basic sensor; k is the empirical scaling factor; Step 4-3, perform standard EKF update: calculate Kalman gain K, update state estimate x, update error covariance P, and calculate innovation covariance S using the observation noise matrix R; specifically, Obtain the prediction error covariance P - The Kalman gain K is calculated using the observation matrix H and observation noise matrix R, which map the state vector to the observation vector. The formula is: K = P - ·H T ·S -1 ; Where S0 is the predicted information covariance, calculated using the formula S0 = H·P - ·H T +R; where H represents the observation matrix that maps the state vector to the observation vector, H=[I 3x3 ,0 3x3 ], I 3x3 Represents a 3x3 identity matrix, 0 3x3 R represents a 3x3 zero matrix; R represents the observation noise matrix. The expression for the update error covariance P is: P = (IK·H)·P - Among them, P - Let be the prediction error covariance from the prediction step, representing the uncertainty of the predicted value; the prediction error covariance at time k+1. Wherein, the error covariance at time k is P k Extended Kalman filter; The innovation covariance S is calculated using the observation noise matrix R. The expression for the calculation is: S = H * P * H T +R; The expression for the state estimate x is: x = x - +K·(zH·x - ); where x - The predicted state from the prediction step, the predicted state at time k+1. Where x k The state estimate at time k is given by ; Z is the observation value at the current time; (zH·x) - () represents the new information, indicating the difference between the observed value and the predicted value.
10. A method for human body tracking and positioning of an indoor service robot according to claim 6, characterized in that: The information of the tracked human body in step 5 includes the unique ID, 3D position coordinates, velocity vector, 3D bounding box and tracking status of each tracked human body, and is provided to the robot's navigation and interaction module through ROS topics, services or API interfaces.