A robot following control method, system and electronic device

Abstract

The application relates to a robot following control method, system and electronic equipment, which comprises the following steps: synchronously collecting multi-source heterogeneous data, performing fusion processing by a signal processing unit, identifying a following person wearing a handheld end and locking the following person as a following target; taking a positioning signal of an ultra-wideband module as an observation value, combining self-position change data provided by an inertial measurement unit, continuously predicting and correcting a dynamic pose of the following target, and outputting a target estimated pose; according to the target estimated pose and a current pose of the robot, real-timely calculating relative distance deviation and relative angle deviation of the two; determining a current following state; a path planner generates a collision-free path from the current pose of the robot to a real-time local navigation point according to a local cost map; taking the collision-free path as input, solving specific rotating speeds of left and right drive wheels through a differential kinematics model, and driving the robot to move to the position of the handheld end; and improving following continuity in a complex environment.

Claims

1. A robot following control method, characterized in that, It includes the following steps: S1. Synchronously collect multi-source heterogeneous data through a multi-sensor module deployed on the robot and a handheld device worn by the follower. The multi-sensor module includes a 360° lidar for obtaining environmental point cloud data, a vision sensor for obtaining image data, and an inertial measurement unit for obtaining the motion pose data of the robot itself. The handheld device contains an ultra-wideband module for transmitting positioning signals. S2. The signal processing unit receives the multi-source heterogeneous data, fuses and processes the environmental point cloud data and the image data, identifies the follower wearing the handheld device from the environment and locks it as the following target, and real-time calculates the initial orientation and distance of the following target relative to the robot. S3. Taking the positioning signal of the ultra-wideband module as the observation value, combining the data of the robot's own pose change provided by the inertial measurement unit, and using the Kalman filtering algorithm to continuously predict and correct the dynamic pose of the following target, and output the filtered target estimated pose P_target(t). S4. According to the target estimated pose P_target(t) and the current pose P_robot(t) of the robot, real-time calculate the relative distance deviation ed(t) and relative angle deviation eθ(t) between the two. Determine the current following state according to the preset following distance range [d_min, d_max]: when the distance d < d_min, it is determined as a no-go zone and the robot stops moving; when the distance d > d_max, it is determined as a lost zone, trigger an alarm and enter the target search mode; when d_min ≤ d ≤ d_max, it is determined as the normal following state. S5. In the normal following state, set the target estimated pose P_target(t) as the real-time local navigation point of the robot; receive the environmental point cloud data in real-time to construct a local cost map of the surrounding environment; the path planner generates a collision-free path from the current pose P_robot(t) of the robot to the real-time local navigation point according to the local cost map in real-time. S6. Taking the collision-free path as the input, real-time extract the instantaneous linear velocity υ and angular velocity ω commands currently tracked by the robot through the path tracking algorithm, and input them to the motion controller, and calculate the specific rotation speeds of the left and right drive wheels through the differential kinematics model to drive the robot to move along the collision-free path to the position of the handheld device.

2. The robot following control method according to claim 1, characterized in that, In step S2, the fusion processing of the environmental point cloud data and the image data specifically includes: Input the image data into a pre-trained deep learning model, and output the person detection box and the corresponding confidence level. Perform clustering segmentation on the environmental point cloud data, and extract the point cloud clusters that conform to the geometric characteristics of the human leg. Associate and match the person detection box and the point cloud cluster through coordinate alignment. When the matching is successful and the confidence level exceeds the preset threshold, lock the corresponding person as the following target.

3. The robot following control method according to claim 1, characterized in that, In the Kalman filtering algorithm of step S3: The state vector X(k) = [x, y, υx, υy]^T, where (x, y) are the position coordinates of the target in the global coordinate system, and (υx, υy) are the velocity components in the corresponding directions; The observation vector Z(k) is obtained by coordinate transformation of the distance and angle measured by the ultra-wideband module; The process noise covariance matrix Q and the measurement noise covariance matrix R are adaptively adjusted according to the quality index of the ultra-wideband signal in the actual motion scenario.

4. The robot following control method according to claim 1, characterized in that, Within the preset following distance range [d_min, d_max], d_min is adjustable from 0.5 meters to 1.5 meters, and d_max is adjustable from 5 meters to 10 meters. When the lost zone alarm is triggered, the robot decelerates and stops, activates the audible and visual alarm, and simultaneously controls the 360° lidar to perform sector scanning to re-acquire the following target.

5. The robot following control method according to claim 1, characterized in that, In step S5, when constructing the local cost map, the obstacle point cloud detected by the 360° LiDAR is projected onto a two-dimensional grid, and the obstacle grid is expanded according to the robot's outer contour size to form an impassable area. The path planner uses a dynamic window method to sample candidate trajectories in the velocity space, and comprehensively evaluates the trajectory based on the obstacle spacing, the degree of approach to the target point, and the smoothness of operation, and selects the optimal trajectory that meets the safety conditions and approaches the target point as the collision-free path.

6. The robot following control method according to claim 1, characterized in that, In step S5, when the estimated pose P_target(t) of the target is set as the real-time local navigation point, if the target is occluded for a short period of time, causing the signal of the ultra-wideband module to be briefly lost, the virtual navigation point will continue to be generated based on the predicted value of the Kalman filter to maintain the robot's movement toward the predicted position until the signal is restored or the timeout occurs, at which point the robot will switch to the target search mode.

7. The robot following control method according to claim 1, characterized in that, The motion controller in step S6 adopts a fuzzy adaptive PID control algorithm, with the distance deviation and its rate of change as input to the fuzzy controller, and performs online tuning of the proportional, integral and derivative coefficients; the output values ​​of linear velocity υ and angular velocity ω are limited to the preset maximum safety range, and the acceleration and deceleration process is constrained by the S-shaped velocity curve.

8. The robot following control method according to claim 1, characterized in that, The differential kinematic model in step S6 is: υL=υ-ω·L / 2, υR=υ+ω·L / 2; Wherein, υL and υR are the linear velocities of the left and right wheels, respectively, and L is the wheel track of the left and right wheels; the motion controller converts the speed command into the motor PWM duty cycle according to the differential kinematic model, and performs closed-loop control on the motor speed.

9. A robot following control system that performs the robot following control method as described in any one of claims 1 to 8, characterized in that, include: The data acquisition module is used to simultaneously collect multi-source heterogeneous data through a 360° lidar, vision sensor, and inertial measurement unit deployed on the robot, as well as an ultra-wideband handheld terminal carried by the accompanying personnel. The target locking module is used to fuse LiDAR point cloud data and visual image data to identify and lock onto the target to be followed, and to calculate the target's initial orientation and distance relative to the robot. The pose estimation module is used to output the estimated pose P_target(t) of the target by combining the ultra-wideband module positioning signal as the observation value with the inertial measurement unit data and the Kalman filter algorithm. The state determination module is used to calculate the distance deviation between the estimated target pose P_target(t) and the robot's current pose P_robot(t), and determine the current following state based on the preset following distance range [d_min, d_max], and execute the corresponding control strategy. The path planning module is used to generate a collision-free path in real time by using the estimated pose P_target(t) of the target as the real-time local navigation point and constructing a local cost map based on the LiDAR point cloud in normal following state. The motion control module is used to extract instantaneous linear velocity and angular velocity commands through a path tracking algorithm, and drive the robot to move along a collision-free path toward the handheld end position via a motion controller and differential kinematic model.

10. An electronic device comprising a memory and a processor; the memory storing a computer program, wherein the processor, when executing the computer program, implements the robot following control method as claimed in any one of claims 1 to 8.

Images