A full-airborne pure-vision-based four-legged robot body obstacle avoidance following method and system

Abstract

The application discloses a kind of four-legged robot body obstacle avoidance following method and system based on full airborne pure vision, comprising: obtaining the pixel coordinates and depth information of target pedestrian and obstacle;Using state estimator, low-frequency visual measurement is converted into high-frequency continuous target prediction state;According to the relative position of obstacle, calculate the lateral obstacle avoidance speed instruction;Using image space motion model to solve target tracking control speed instruction;According to the distance of the nearest obstacle, dynamically adjust the fusion weight, generate three-dimensional integrated motion instruction containing longitudinal, lateral and yaw, and send to four-legged robot high-level motion controller.The application realizes stable following and safe obstacle avoidance in complex scene without relying on laser radar, UWB beacon, external motion capture system or offline calculation, improves the autonomy, robustness and deployment flexibility of four-legged robot target following task.

Description

Technical Field

[0001] This invention belongs to the field of robot autonomous control and computer vision technology, specifically relating to a method and system for obstacle avoidance and following of a quadruped robot based on fully airborne pure vision. Background Technology

[0002] Vision-driven robot control has long been widely used in security patrols, human-robot interaction, and service robots. Compared to wheeled platforms, quadruped robots possess superior adaptability to unstructured terrain. However, achieving robust, purely vision-based quadruped robot following in real-world dynamic environments remains a significant challenge. Existing robot following systems largely rely on auxiliary sensors or off-board computing resources, such as LiDAR, ultra-wideband (UWB) beacons, or external motion capture systems, to acquire reliable distance or attitude information. These solutions not only increase system complexity but are also difficult to deploy in environments lacking external infrastructure.

[0003] While pure vision-based airborne solutions offer a more natural deployment, they suffer from issues such as low visual perception update frequency, motion blur, and target occlusion. Directly using low-frequency, noisy visual measurements for closed-loop control can easily lead to system response delays, tracking oscillations, and even target loss. Furthermore, image-based visual servoing (IBVS) is often used as a local controller to keep the target centered, but when combined with traditional local obstacle avoidance planners, motion conflicts frequently arise. For example, to avoid an obstacle on the left, the obstacle avoidance planner might instruct the robot to move to the right, while image-based visual servoing might instruct it to yaw to the left to keep the target in the field of view. This undecoupled conflict can lead to target loss or collisions.

[0004] Therefore, there is an urgent need for a pure vision control framework that can run entirely on the airborne end, compensate for perception delays, and achieve seamless integration of tracking and obstacle avoidance. Summary of the Invention

[0005] Purpose of the invention: The purpose of this invention is to provide a method and system for obstacle avoidance and following of a quadruped robot based on full-airborne pure vision, so as to significantly improve the following stability and safety of the robot in complex environments.

[0006] Technical solution: The method described in this invention includes the following steps:

[0007] The system acquires real-time RGB and depth images from an airborne stereo camera, uses a target detection unit to detect pedestrians and obstacles, and obtains the pixel coordinate bounding boxes, category labels, and confidence scores of each detected object. It also combines a multi-target tracking unit to perform data association on pedestrians and maintain consistent IDs, extracting the horizontal pixel coordinates, vertical pixel coordinates, and depth of pedestrians in the image coordinate system. At the same time, it extracts the pixel coordinates and depth information of obstacles in the image coordinate system.

[0008] A state estimator is constructed, and the horizontal pixel coordinates and depth of the target pedestrian are used as measurements to update and correct the state estimator. During the gaps when there is no visual measurement, the state prediction is performed based on a constant velocity motion model with a high frequency control cycle, and the high frequency continuous target prediction state is output to compensate for the delay of visual perception and the sampling rate mismatch.

[0009] A decoupled artificial potential field visual obstacle avoidance method is adopted. The position of the obstacle in the robot body coordinate system is calculated based on the obstacle pixel coordinates and depth information, and the repulsive force of each obstacle is calculated. The total repulsive force of all obstacles detected in the field of view is strictly projected onto the lateral coordinate axis of the quadruped robot to generate a lateral obstacle avoidance speed command.

[0010] A spatial motion model is constructed with the horizontal pixel coordinates and depth of the target pedestrian in the image coordinate system as state variables and the longitudinal velocity and yaw angular velocity of the robot as control inputs. The target tracking control speed command is solved based on a nonlinear model predictive control framework.

[0011] The dynamic fusion weight function is calculated based on the distance between the robot and the nearest obstacle. The target tracking control speed command and the lateral obstacle avoidance speed command are adaptively fused. Finally, a three-dimensional comprehensive motion command containing longitudinal velocity, lateral velocity and yaw angular velocity is generated and sent to the quadruped robot's high-level motion controller to drive the quadruped robot's movement.

[0012] Furthermore, the state estimator is a Kalman filter, and its state vector is... ,in Indicates the first The horizontal pixel coordinates of the target pedestrian in the image coordinate system for each control cycle. Indicates the depth of the target pedestrian relative to the camera. Indicates the rate of change of the horizontal pixel coordinate. Indicates the rate of change of depth. This indicates the matrix transpose.

[0013] Furthermore, an orthogonal decomposition strategy is adopted to strictly project the total repulsive force onto the lateral coordinate axis of the quadruped robot, so that the obstacle avoidance command only acts on the lateral movement dimension of the quadruped robot, while keeping the control of the longitudinal speed and yaw rate required for target tracking independent, thereby avoiding interference of obstacle avoidance behavior with the visual tracking process and outputting lateral obstacle avoidance speed command.

[0014] Furthermore, the state variables of the image spatial motion model Defined as , No. Control input for each control cycle Defined as ;in, Indicates the first The horizontal pixel coordinates of the target pedestrian in the image coordinate system for each control cycle. Indicates the depth of the target pedestrian relative to the camera. For the robot's longitudinal velocity, Let be the yaw rate of the robot about its vertical axis. This is the transpose of the matrix;

[0015] The image-space motion model first determines the current initial state based on the high-frequency target prediction state, and then establishes a continuous-time prediction model using the robot's longitudinal velocity and yaw rate as control inputs. The forward Euler method is used to discretize the continuous-time prediction model, resulting in a discrete-time prediction model, which is then used in the prediction time domain. The objective function is optimized by the ACADOS optimizer. The objective function is used to minimize the weighted sum of the tracking error cost of the deviation of the target state from the desired state, the control input amplitude cost, the smoothing cost of the change of control input in adjacent control cycles, and the error cost of the predicted time-domain terminal state.

[0016] Furthermore, the nonlinear model predictive control framework incorporates state constraints to satisfy social distancing and safe following distance requirements during the solution process. State constraints used to meet image field of view requirements and control constraints used to satisfy the physical limits of the actuator. , Output target tracking control speed command; where, To predict the first in the time domain The depth corresponding to each prediction step To predict the first in the time domain The horizontal pixel coordinates corresponding to each prediction step To predict the first in the time domain The longitudinal velocity corresponding to each prediction step To predict the first in the time domain The yaw rate corresponding to each prediction step and These are the lower and upper limits of the allowed following depth, respectively. and These are the lower and upper limits of the allowed horizontal pixel coordinates, respectively. and These are the lower and upper limits of the longitudinal velocity, respectively. and These are the lower and upper limits of the yaw rate, respectively.

[0017] Furthermore, dynamic adaptive fusion of lateral obstacle avoidance speed commands and target tracking control speed commands is achieved, including:

[0018] The fusion weights are dynamically calculated based on the distance between the robot and the nearest obstacle detected by the camera. ,in The distance between the robot and the nearest obstacle. A positive parameter to control the smoothness of weight transition. A safe distance is reserved between the robot and obstacles; the fusion rule is: when the obstacle distance is greater than or equal to a preset threshold, When the value approaches 0, target tracking control is the primary function; when the obstacle is smaller than a preset threshold, [the control is activated]. Gradually increase the weight of lateral obstacle avoidance control to achieve a smooth integration of tracking control and obstacle avoidance control.

[0019] Based on the same inventive concept, the system described in this invention includes:

[0020] The visual detection and tracking module is used to acquire real-time RGB and depth images from the airborne stereo camera, detect pedestrians and obstacles using the target detection unit, and obtain the pixel coordinate bounding box, category label, and confidence score of each detected object. It also combines the multi-target tracking unit to perform data association on pedestrians and maintain a consistent ID, extracting the horizontal pixel coordinates, vertical pixel coordinates, and depth of pedestrians in the image coordinate system. At the same time, it extracts the pixel coordinates and depth information of obstacles in the image coordinate system.

[0021] Delayed perception prediction module: used to build a state estimator, using the horizontal pixel coordinates and depth of the target pedestrian as measurements to update and correct the state estimator; during the gaps without visual measurement, it performs state prediction based on a constant velocity motion model with a high-frequency control cycle, outputting a high-frequency continuous target prediction state, thereby compensating for the delay and sampling rate mismatch problem of visual perception.

[0022] Decoupled artificial potential field visual obstacle avoidance module: It is used to calculate the position of the obstacle in the robot body coordinate system based on the obstacle pixel coordinates and depth information, and calculate the repulsive force of each obstacle; it strictly projects the total repulsive force of all obstacles detected in the field of view onto the lateral coordinate axis of the quadruped robot to generate lateral obstacle avoidance speed command.

[0023] Image-based visual servoing-nonlinear model prediction tracking control module: used to construct an image space motion model with the horizontal pixel coordinates and depth of the target pedestrian in the image coordinate system as state variables and the robot's longitudinal velocity and yaw angular velocity as control inputs, and solve the target tracking control speed command based on the nonlinear model prediction control framework;

[0024] Command fusion and output module: The fusion module calculates a dynamic fusion weight function based on the distance between the robot and the nearest obstacle, and adaptively fuses the target tracking control speed command and the lateral obstacle avoidance speed command; finally, it generates a three-dimensional comprehensive motion command containing longitudinal velocity, lateral velocity and yaw angular velocity and sends it to the quadruped robot's high-level motion controller to drive the quadruped robot's movement.

[0025] Based on the same inventive concept, the present invention provides an electronic device, comprising:

[0026] Memory, used to store computer programs;

[0027] A processor for executing the computer program to implement the method.

[0028] Based on the same inventive concept, the present invention provides a non-volatile storage medium for storing a computer program, wherein the computer program implements the method described thereon when executed by a processor.

[0029] Based on the same inventive concept, the present invention provides a computer program product, including a computer program / instructions that, when executed by a processor, implement the method described herein.

[0030] Beneficial effects: Compared with the prior art, the significant technical effects of the present invention are as follows:

[0031] (1) The system oscillation problem caused by the mismatch between visual perception delay and high-frequency control is solved. This invention does not directly use low-frequency visual detection data for control, but introduces a delay perception prediction mechanism. This mechanism uses a state estimator (such as Kalman filter) to fuse 5-10Hz low-frequency visual measurement and constant velocity motion model, and outputs a continuous and smooth target prediction state with a 50Hz high-frequency control cycle during the gap without visual measurement. This improvement effectively compensates for the delay and low sampling rate of pure visual perception and significantly suppresses tracking jitter caused by sudden target movement or temporary loss of visual features.

[0032] (2) Eliminates the control conflict between traditional obstacle avoidance and visual target servoing; This invention does not directly apply the traditional local obstacle avoidance planner, but proposes an innovative decoupled obstacle avoidance strategy based on orthogonal decomposition; By strictly constraining the total repulsive force calculated by the artificial potential field method and projecting it onto the lateral movement dimension of the quadruped robot, this invention successfully integrates the longitudinal and yaw tracking control speed commands required for target tracking with the lateral obstacle avoidance speed commands in an adaptive manner, ensuring that the robot can keep the target stably in the center of the field of vision while avoiding obstacles laterally, thus avoiding the loss of the target due to obstacle avoidance behavior;

[0033] (3) It achieves safe and forward-looking target following under multiple constraints; the present invention constructs a specific image space motion model and combines it with the nonlinear model predictive control (NMPC) framework to incorporate high-frequency prediction states into the rolling optimization; in the solution process, social safety distance constraints, image field boundary constraints and physical limit constraints of the actuator are explicitly considered, so that the robot's motion commands not only have the foresight to predict the future, but also strictly guarantee physical safety and tracking without going out of bounds in complex dynamic scenarios. Attached Figure Description

[0034] Figure 1 This is a flowchart of the obstacle avoidance and following method described in this invention;

[0035] Figure 2 This is a flowchart of the visual detection and tracking method described in an embodiment of the present invention;

[0036] Figure 3 This is a schematic diagram illustrating the working principle of delay-sensing prediction as described in an embodiment of the present invention;

[0037] Figure 4 This is a flowchart of the decoupled artificial potential field visual obstacle avoidance algorithm described in an embodiment of the present invention;

[0038] Figure 5 This is a flowchart of the image-based visual servoing-nonlinear model prediction tracking control algorithm described in an embodiment of the present invention;

[0039] Figure 6 This is a flowchart of the motion control command fusion and output algorithm for the quadruped robot described in an embodiment of the present invention. Detailed Implementation

[0040] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0041] The overall system structure of this invention includes visual detection and tracking, delay-aware prediction, decoupled artificial potential field visual obstacle avoidance, image-based visual servoing-nonlinear model prediction and tracking control, and command fusion and output. The system of this invention is as follows: Figure 1The process described is as follows: Visual detection and tracking acquires real-time RGB and depth images from an onboard stereo camera, outputting low-frequency visual detection data containing the pixel coordinates and depth information of the target pedestrian and the obstacle set; delayed perception prediction outputs a high-frequency target prediction state based on the low-frequency visual detection data of the target pedestrian; decoupled artificial potential field visual obstacle avoidance outputs lateral obstacle avoidance speed commands based on the obstacle set; image-based visual servoing-nonlinear model prediction and tracking control outputs target tracking control speed commands based on the high-frequency target prediction state and desired state; command fusion and output calculates weights based on the distance to the nearest obstacle and fuses them to output a three-dimensional comprehensive motion command, which is sent to the quadruped robot's high-level motion controller to drive the robot's movement. After the robot moves, it changes the camera's observation, forming a closed loop of perception, prediction, obstacle avoidance, and control.

[0042] In one embodiment, the present invention provides a system deployment implementation method in which a fully airborne, vision-based quadruped robot obstacle avoidance and following system is deployed on a quadruped robot platform. The robot's bottom layer uses an existing quadruped robot high-level motion controller as the execution layer, and the upper layer deploys a fully airborne, vision-based target following and obstacle avoidance control system. A stereo camera is mounted forward on the platform, which is equipped with an onboard computing platform. The quadruped robot platform can be a Unitree Go2, the stereo camera can be an Intel RealSense D435i stereo camera, and the onboard computing platform can be a Jetson Orin NX 16GB. The above specific models are merely one optional implementation method and do not constitute a limitation on the scope of protection of the present invention.

[0043] The present invention describes a fully airborne, vision-based quadruped robot obstacle avoidance and following method, the overall process of which is as follows: Figure 1 As shown, it includes the following steps:

[0044] S1. Acquire real-time RGB and depth images captured by the airborne stereo camera. Identify target pedestrians and obstacles through the target detection unit, and obtain the bounding box, category information and confidence of each detected object. Combine the multi-target tracking unit to perform data association on the target pedestrians and maintain consistent IDs. Extract the set of pixel coordinates and depth information of the target pedestrians and the pixel coordinates and depth information of the obstacles to form low-frequency visual detection data.

[0045] The airborne stereo camera acquires RGB images and depth data to perform target detection, multi-target tracking, and obstacle information extraction.

[0046] S2. Construct a Kalman filter state estimator, and use the low-frequency visual detection data of the target pedestrian output from S1 as the measurement value for updating and correction; during the interval without visual measurement, use a high-frequency control cycle to perform state prediction based on a constant velocity motion model, and output the high-frequency target prediction state.

[0047] Horizontal pixel coordinates of the target pedestrian and depth Perform state estimation and high-frequency prediction;

[0048] S3. The decoupled artificial potential field visual obstacle avoidance method is adopted. The position of the obstacle in the robot body coordinate system is obtained according to the obstacle pixel coordinates and depth information. The repulsive force of each obstacle is calculated and the total repulsive force of the obstacles in the field of view is projected onto the lateral coordinate axis of the quadruped robot to generate the lateral obstacle avoidance speed command.

[0049] S4. Construct an image space motion model with the horizontal pixel coordinates and depth of the target pedestrian in the image coordinate system as state variables and the longitudinal velocity and yaw angular velocity of the robot as control inputs, and solve the target tracking control speed command based on nonlinear model predictive control.

[0050] Solving longitudinal velocity based on image space motion model and nonlinear model predictive control and yaw rate ;

[0051] S5. Calculate the dynamic fusion weight based on the distance between the robot and the nearest obstacle, adaptively fuse the target tracking control speed command output by S4 and the lateral obstacle avoidance speed command output by S3, and finally generate a three-dimensional comprehensive motion command including longitudinal speed, lateral speed and yaw angle velocity, and send it to the quadruped robot's high-level motion controller to drive the quadruped robot to move.

[0052] The target tracking control speed command and the lateral obstacle avoidance speed command are adaptively fused based on the nearest obstacle distance, and the final three-dimensional integrated motion command is sent to the quadruped robot's high-level motion controller.

[0053] The implementation steps will now be described in detail with reference to the accompanying drawings in the embodiments of the present invention.

[0054] S1. Visual Detection and Tracking: Obtain the pixel coordinates and depth information of the target pedestrian and obstacles. See also Figure 2 As shown, a real-time RGB image sequence is acquired using an airborne stereo camera. and depth image sequences The RGB image sequence and the depth image sequence first enter the object detection unit, which performs the detection of pedestrians and obstacles and obtains the pixel coordinate bounding boxes of the detected objects (including pedestrians and obstacles). Category tags and confidence level ;in Number the objects to be tested. , These are the horizontal and vertical pixel coordinates of the bottom left corner of the pixel coordinate bounding box, respectively. , These represent the width and height of the bounding box, respectively, in pixel coordinates.

[0055] The pixel coordinate bounding box of the target pedestrian is fed into the multi-target tracking unit. This unit performs data association based on bounding box overlap, appearance feature similarity, and motion consistency to maintain a consistent ID for the target pedestrian. At the same time, it extracts the horizontal pixel coordinates of the target pedestrian in the image coordinate system. Vertical pixel coordinates and depth ;in The horizontal and vertical pixel coordinates of the center of the bounding box of the target pedestrian's pixel coordinates can be obtained respectively, and the calculation method is as follows: ,in, These are the horizontal and vertical pixel coordinates of the bottom left corner of the target pedestrian's pixel coordinate bounding box, respectively. These represent the width and height of the bounding box for the target pedestrian's pixel coordinates, respectively.

[0056] Meanwhile, non-target pedestrians, tables and chairs, walls, boxes, columns, and other detected objects that may obstruct the robot's passage are all considered obstacles. The pixel coordinates and depth information of each obstacle in the image coordinate system are extracted to construct an obstacle set containing the pixel coordinates and depth information of the obstacles. ,in For the first The pixel coordinates and depth information of each obstacle , This represents the total number of obstacles within the field of view.

[0057] Finally, this step outputs low-frequency visual detection data, including the horizontal and vertical pixel coordinates and depth of the target pedestrian in the image coordinate system, as well as a set of obstacles containing pixel coordinates and depth information of each obstacle. This data serves as input for the subsequent S2 delayed perception prediction step and the S3 decoupled artificial potential field visual obstacle avoidance step. Due to limitations in onboard computing power, the frequency of the low-frequency visual detection data output in this step is 5-10Hz.

[0058] Both the target detection method and the multi-target tracking algorithm can use existing modules. The target detection method includes, but is not limited to, the YOLO-World target detection model based on deep learning, and the multi-target tracking algorithm includes, but is not limited to, the tracking-by-detection algorithm or the BoxMOT tracking method based on appearance feature association.

[0059] S2, Delayed Perception Prediction: Utilizes a state estimator to convert low-frequency visual measurements into high-frequency, continuous target prediction states. See also Figure 3As shown, a state estimator is constructed, preferably a standard Kalman filter, but other filtering algorithms with the same state estimation function can also be used. (Unified notation follows) The state vector for each high-frequency control cycle is: ,in Indicates the first The horizontal pixel coordinates of the target pedestrian in the image coordinate system for each high-frequency control cycle. Indicates the depth of the target relative to the camera. Indicates the rate of change of the horizontal pixel coordinate. Indicates the rate of change of depth. This represents the matrix transpose. The short-term motion of the target pedestrian is approximated as a constant-velocity motion model. During the intervals without visual measurement, state prediction is performed using a 50Hz high-frequency control cycle. The state prediction equation is: ,in The prior state of the current period is predicted based on the posterior state of the previous period. This is the posterior state of the previous cycle. Based on sampling period The state transition matrix constructed based on the constant velocity assumption, This is process noise. Can be written as When the low-frequency visual detection data output by S1 is obtained, the horizontal pixel coordinates and depth information of the target pedestrian in the image coordinate system are extracted as measurement values. ,in The horizontal pixel coordinates of the detected pedestrian are measured. The depth measurement value of the detected target pedestrian; based on the observation equation The state estimator is updated and corrected, where For the measurement matrix, To measure noise, the prediction-update loop described above first performs an update correction when low-frequency visual measurements arrive, then performs high-frequency state prediction within adjacent visual measurement intervals, outputting the high-frequency target prediction state. This compensates for the mismatch between visual perception delay and sampling rate, whereby... To predict the horizontal pixel coordinates, To predict depth, To predict the rate of change of horizontal pixel coordinates, To predict the rate of depth change.

[0060] S3. Decoupling Artificial Potential Field Visual Obstacle Avoidance: Calculate lateral obstacle avoidance speed commands based on the relative position of obstacles. In previous studies, visual servo control often prioritizes keeping the target in the center of the field of view, while traditional obstacle avoidance methods often prioritize maintaining a safe distance, which may lead to conflicts in the control direction. This invention proposes an obstacle avoidance method based on motion dimension decoupling, see [link to relevant documentation]. Figure 4 As shown, obstacle avoidance is not treated as an independent local planner directly overriding the tracking instructions. Instead, it is decoupled from the visual servoing target as a constrained lateral correction mechanism. By employing the existing basic artificial potential field method architecture and combining it with the decoupling design of this invention, a decoupled artificial potential field method is obtained. Based on the relative positions between the robot and obstacles detected by the camera, the first... The obstacle exerts a repulsive force on the robot. This step uses... Indicates obstacle number, using This indicates the total number of obstacles within the field of view.

[0061] Based on the obstacle pixel coordinates and depth information output in step S1, the obstacle pixel coordinates are converted into position vectors in the robot's body coordinate system, and the distance between the obstacle and the robot is calculated: , For robots and the first The distance between the obstacles Let be the position vector of the robot in the robot's body coordinate system. For the first The position vector of an obstacle in the robot's body coordinate system;

[0062] When the distance between the obstacle and the robot Less than or equal to the radius of influence of the obstacle At that time, the first The repulsive force vector exerted by an obstacle on the robot can be expressed as... ;when hour, .in, For repulsion gain. Within the field of view The total repulsive force vector is obtained by summing the repulsive force vectors of each obstacle. Subsequently, the total repulsive force vector... By precisely projecting the data onto the lateral coordinate axis of the quadruped robot, the lateral obstacle avoidance speed command is obtained. ,in Let be the unit vector of the robot's horizontal coordinate axis. The proportionality coefficient from repulsive force to lateral velocity. This represents the amplitude limiting function. and These are the lower and upper limits of lateral velocity, respectively. Output ,when hour Output Output in other cases .

[0063] It should be noted that: repulsion gain Used to adjust obstacle avoidance strength and obstacle influence radius. Used to define the range within which an obstacle produces a repulsive effect.

[0064] Using the orthogonal decomposition strategy described above, this step outputs the lateral obstacle avoidance speed command. The lateral obstacle avoidance speed command is used as input to S5 and only applies to the lateral movement dimension of the quadruped robot.

[0065] S4. Image-based visual servoing - nonlinear model predictive tracking control: Utilizing an image-space motion model to solve for the target tracking control velocity command. See also... Figure 5 As shown, the horizontal pixel coordinates of the target pedestrian in the image coordinate system are constructed. With depth For state variables, take the robot's longitudinal velocity With yaw rate To control the spatial motion model of the input image. Let the state variable be... The control input is denoted as ,in For the first The longitudinal speed of the robot in each control cycle For the first The yaw rate of the robot around the vertical axis in each control cycle. This image-space motion model first determines the current initial state based on the high-frequency target prediction state output by S2, and then establishes a prediction model using the robot's longitudinal velocity and yaw rate as control inputs. Its continuous-time dynamics can be expressed as:

[0066] ;

[0067] in, The target horizontal pixel coordinates, The depth of the target relative to the camera. The horizontal pixel coordinates of the camera's optical center. For camera focal length, The longitudinal velocity of the robot over continuous time. Let be the robot's yaw rate in continuous time. The continuous model is discretized using the forward Euler method to obtain the discrete-time prediction model. ,in The state for the next control cycle. This represents the current control cycle state. For current control input, To control the cycle, The function is discretized. Based on this nonlinear model, a predictive control method is designed in the prediction time domain. The following objective function is optimized using the ACADOS optimizer. :

[0068]

[0069]

[0070] in, To predict the length of the time domain, This is the index of the time-domain prediction step in the objective function. , This is the desired state, and can be set according to specific circumstances. The desired horizontal pixel coordinates (generally set to the horizontal pixel coordinates relative to the camera's optical center). equal), The desired tracking depth is set based on the size of the quadruped robot and tracking requirements. Here is the state error weight matrix. To control the input weight matrix, To control the input increment weight matrix, This is the terminal state weight matrix. This indicates the change in control input between adjacent control cycles. To predict the first in the time domain The control inputs corresponding to each prediction step To predict the first in the time domain The control inputs corresponding to each prediction step To predict the first in the time domain The predicted state corresponding to each prediction step. To predict the time-domain terminal state, the objective function is used to minimize the weighted sum of four types of costs.

[0071] It should be noted that the above objective function In the formula, the first term represents the tracking error cost of the target state deviating from the desired state, the second term represents the control input amplitude cost, the third term represents the smoothing cost of the change in control input between adjacent control cycles, and the fourth term represents the error cost of the predicted terminal state in the time domain. The control input in the prediction time domain, the control input of the previous control step, the predicted state, and the terminal state correspond to the relevant variables in the formula.

[0072] Constraints such as safe distance, image field of view, and physical limits of the actuator are imposed.

[0073] When solving the rolling optimization problem, several constraints are introduced: Used to meet social distancing and safe following distance requirements, Used to meet image field of view boundary requirements, and Used to satisfy the physical limits of the actuator; among which... To predict the first in the time domain The depth corresponding to each prediction step To predict the first in the time domain The horizontal pixel coordinates corresponding to each prediction step To predict the first in the time domain The longitudinal velocity corresponding to each prediction step To predict the first in the time domain The yaw rate corresponding to each prediction step and These are the lower and upper limits of the allowed following depth, respectively. and These are the lower and upper limits of the allowed horizontal pixel coordinates, respectively. and These are the lower and upper limits of the longitudinal velocity, respectively. and These are the lower and upper limits of the yaw rate, respectively.

[0074] The preferred model predictive control method is a nonlinear model predictive control method using the aforementioned motion model and objective function; other model-based optimization control methods may also be used. The optimizer is preferably the ACADOS optimizer, but other numerical optimizers capable of solving quadratic programming problems may also be used. The high-frequency target prediction state output by S2 is then used. In and As an optimization problem, at time... Given the initial state input, solve for the optimal control sequence and extract the first control input. As a target tracking control speed command, For the optimal longitudinal velocity, The optimal yaw rate is [the value of the yaw rate].

[0075] Using the above strategy, this step outputs the target tracking control speed command. The target tracking control speed command will be used as input to S5.

[0076] S5. Command Fusion and Output: Based on the distance to the nearest obstacle, the fusion weights are dynamically adjusted to generate a comprehensive 3D motion command including longitudinal, lateral, and yaw motion, which is then sent to the quadruped robot's higher-level motion controller. (See also...) Figure 6 As shown, the distance between the robot and the nearest obstacle is detected by the camera. Dynamic calculation of fusion weights Its expression is ,in This indicates the distance between the robot and the nearest obstacle in its field of vision. A positive parameter to control the smoothness of weight transition. A safe distance is maintained between the robot and obstacles. The output of S4... That is, including the optimal longitudinal velocity With optimal yaw rate The target tracking control speed command, and the lateral obstacle avoidance speed command output by S3. Perform adaptive fusion.

[0077] The obtained three-dimensional integrated motion control command can be expressed as: .

[0078] When the obstacle is far away, i.e., when the obstacle distance is greater than or equal to a preset threshold, the fusion weight approaches 0, and target tracking control takes precedence. When the obstacle is close, i.e., when the obstacle is less than the preset threshold, the fusion weight gradually increases, enhancing the weight of lateral obstacle avoidance control, thereby achieving a smooth fusion of target tracking control and lateral obstacle avoidance control. The final generated three-dimensional integrated motion command (including longitudinal, lateral, and yaw dimensions) is sent to the quadruped robot's high-level motion controller to drive the quadruped robot's movement. Through the aforementioned dynamic weight function, the target tracking control speed command and the lateral obstacle avoidance speed command are adaptively fused to achieve unified control for stable following and safe obstacle avoidance.

[0079] In the experiments of this embodiment, the system was tested in both the Gazebo simulation environment and real indoor and outdoor scenarios, including smooth turns, sharp turns, multi-person interference, chase-and-escape interactions, long-distance following, and obstacle presence. The results show that the system can achieve stable following under fully airborne, purely visual conditions; compared with a visual following baseline without delay compensation, the system shows significant improvements in both pixel error and distance error.

[0080] Based on the same inventive concept, the system described in this invention includes:

[0081] The visual detection and tracking module is used to acquire real-time RGB and depth images from the airborne stereo camera, detect pedestrians and obstacles using the target detection unit, and obtain the pixel coordinate bounding box, category label, and confidence score of each detected object. It also combines the multi-target tracking unit to perform data association on pedestrians and maintain a consistent ID, extracting the horizontal pixel coordinates, vertical pixel coordinates, and depth of pedestrians in the image coordinate system. At the same time, it extracts the pixel coordinates and depth information of obstacles in the image coordinate system.

[0082] Delayed perception prediction module: used to build a state estimator, using the horizontal pixel coordinates and depth of the target pedestrian as measurements to update and correct the state estimator; during the gaps without visual measurement, it performs state prediction based on a constant velocity motion model with a high-frequency control cycle, outputting a high-frequency continuous target prediction state, thereby compensating for the delay and sampling rate mismatch problem of visual perception.

[0083] Decoupled artificial potential field visual obstacle avoidance module: It is used to calculate the position of the obstacle in the robot body coordinate system based on the obstacle pixel coordinates and depth information, and calculate the repulsive force of each obstacle; it strictly projects the total repulsive force of all obstacles detected in the field of view onto the lateral coordinate axis of the quadruped robot to generate lateral obstacle avoidance speed command.

[0084] Image-based visual servoing-nonlinear model prediction tracking control module: used to construct an image space motion model with the horizontal pixel coordinates and depth of the target pedestrian in the image coordinate system as state variables and the robot's longitudinal velocity and yaw angular velocity as control inputs, and solve the target tracking control speed command based on the nonlinear model prediction control framework;

[0085] Command fusion and output module: The fusion module calculates a dynamic fusion weight function based on the distance between the robot and the nearest obstacle, and adaptively fuses the target tracking control speed command and the lateral obstacle avoidance speed command; finally, it generates a three-dimensional comprehensive motion command containing longitudinal velocity, lateral velocity and yaw angular velocity and sends it to the quadruped robot's high-level motion controller to drive the quadruped robot's movement.

[0086] The system is deployed on a quadruped robot platform, which has a stereo camera mounted in front of it. The system performs target state estimation, obstacle avoidance decision-making, and predictive tracking control optimization in the onboard computing unit, and finally outputs three-dimensional integrated motion commands to the quadruped robot's high-level motion controller.

[0087] Based on the same inventive concept, the present invention provides an electronic device, comprising:

[0088] Memory, used to store computer programs;

[0089] A processor for executing the computer program to implement the method.

[0090] Based on the same inventive concept, the present invention provides a non-volatile storage medium for storing a computer program, wherein the computer program implements the method described thereon when executed by a processor.

[0091] Based on the same inventive concept, the present invention provides a computer program product, including a computer program / instructions that, when executed by a processor, implement the method described herein.

[0092] In summary, the method of this invention, based on an onboard stereo camera and onboard computing unit of a quadruped robot, acquires the pixel coordinates and depth information of the target pedestrian and obstacles. It obtains the horizontal pixel coordinates and depth of the target pedestrian through target detection and multi-target tracking, and treats non-target pedestrians and obstructing objects as obstacles, inputting them into a decoupled artificial potential field visual obstacle avoidance module. A Kalman filter is used to update and correct the low-frequency visual detection data, and a high-frequency target prediction state is output based on a constant-velocity motion model within the high-frequency control cycle. A tracking control architecture tightly coupled with image-based visual servoing and nonlinear model predictive control is constructed, and a decoupled artificial potential field visual obstacle avoidance strategy is adopted. The longitudinal velocity and yaw rate required for target tracking are decoupled and adaptively fused with the lateral velocity required for obstacle avoidance, ultimately outputting a three-dimensional comprehensive motion command. This invention achieves stable following and safe obstacle avoidance in complex scenes without relying on lidar, UWB beacons, external motion capture systems, or offline computing, improving the autonomy, robustness, and deployment flexibility of quadruped robot target following tasks.

Claims

1. A method for obstacle avoidance and following of a quadruped robot based on fully airborne pure vision, characterized in that, Includes the following steps: The system acquires real-time RGB and depth images from an airborne stereo camera, uses a target detection unit to detect pedestrians and obstacles, and obtains the pixel coordinate bounding boxes, category labels, and confidence scores of each detected object. It also combines a multi-target tracking unit to perform data association on pedestrians and maintain consistent IDs, extracting the horizontal pixel coordinates, vertical pixel coordinates, and depth of pedestrians in the image coordinate system. At the same time, it extracts the pixel coordinates and depth information of obstacles in the image coordinate system. A state estimator is constructed, and the horizontal pixel coordinates and depth of the target pedestrian are used as measurements to update and correct the state estimator. During the gaps when there is no visual measurement, the state prediction is performed based on a constant velocity motion model with a high frequency control cycle, and the high frequency continuous target prediction state is output to compensate for the delay of visual perception and the sampling rate mismatch. A decoupled artificial potential field visual obstacle avoidance method is adopted. The position of the obstacle in the robot body coordinate system is calculated based on the obstacle pixel coordinates and depth information, and the repulsive force of each obstacle is calculated. The total repulsive force of all obstacles detected in the field of view is strictly projected onto the lateral coordinate axis of the quadruped robot to generate a lateral obstacle avoidance speed command. A spatial motion model is constructed with the horizontal pixel coordinates and depth of the target pedestrian in the image coordinate system as state variables and the longitudinal velocity and yaw angular velocity of the robot as control inputs. The target tracking control speed command is solved based on a nonlinear model predictive control framework. The dynamic fusion weight function is calculated based on the distance between the robot and the nearest obstacle. The target tracking control speed command and the lateral obstacle avoidance speed command are adaptively fused. Finally, a three-dimensional comprehensive motion command containing longitudinal velocity, lateral velocity and yaw angular velocity is generated and sent to the quadruped robot's high-level motion controller to drive the quadruped robot's movement.

2. The method according to claim 1, characterized in that, The state estimator is a Kalman filter, and its state vector is: ,in Indicates the first The horizontal pixel coordinates of the target pedestrian in the image coordinate system for each control cycle. Indicates the depth of the target pedestrian relative to the camera. Indicates the rate of change of the horizontal pixel coordinate. Indicates the rate of change of depth. This indicates the matrix transpose.

3. The method according to claim 1, characterized in that, An orthogonal decomposition strategy that strictly projects the total repulsive force onto the lateral coordinate axis of the quadruped robot ensures that the obstacle avoidance command only acts on the lateral movement dimension of the quadruped robot, while keeping the control of the longitudinal velocity and yaw rate required for target tracking independent, thereby avoiding interference of obstacle avoidance behavior with the visual tracking process and outputting a lateral obstacle avoidance speed command.

4. The method according to claim 1, characterized in that, The state variables of the image spatial motion model Defined as , No. Control input for each control cycle Defined as ;in, Indicates the first The horizontal pixel coordinates of the target pedestrian in the image coordinate system for each control cycle. Indicates the depth of the target pedestrian relative to the camera. For the robot's longitudinal velocity, Let be the yaw rate of the robot about its vertical axis. This is the transpose of the matrix; The image-space motion model first determines the current initial state based on the high-frequency target prediction state, and then establishes a continuous-time prediction model using the robot's longitudinal velocity and yaw rate as control inputs. The forward Euler method is used to discretize the continuous-time prediction model, resulting in a discrete-time prediction model, which is then used in the prediction time domain. The objective function is optimized by the ACADOS optimizer. The objective function is used to minimize the weighted sum of the tracking error cost of the deviation of the target state from the desired state, the control input amplitude cost, the smoothing cost of the change of control input in adjacent control cycles, and the error cost of the predicted time-domain terminal state.

5. The method according to claim 1, characterized in that, The nonlinear model predictive control framework incorporates state constraints to satisfy social distancing and safe following distance requirements during the solution process. State constraints used to meet image field of view requirements and control constraints used to satisfy the physical limits of the actuator. , Output target tracking control speed command; where, To predict the first in the time domain The depth corresponding to each prediction step To predict the first in the time domain The horizontal pixel coordinates corresponding to each prediction step To predict the first in the time domain The longitudinal velocity corresponding to each prediction step To predict the first in the time domain The yaw rate corresponding to each prediction step and These are the lower and upper limits of the allowed following depth, respectively. and These are the lower and upper limits of the allowed horizontal pixel coordinates, respectively. and These are the lower and upper limits of the longitudinal velocity, respectively. and These are the lower and upper limits of the yaw rate, respectively.

6. The method according to claim 1, characterized in that, Dynamic adaptive fusion of lateral obstacle avoidance speed commands and target tracking control speed commands is achieved, including: The fusion weights are dynamically calculated based on the distance between the robot and the nearest obstacle detected by the camera. ,in The distance between the robot and the nearest obstacle. A positive parameter to control the smoothness of weight transition. A safe distance is reserved between the robot and obstacles; the fusion rule is: when the distance to the obstacle is greater than or equal to a preset threshold, When the value approaches 0, target tracking control is the primary function; when the obstacle is smaller than a preset threshold, [the control is activated]. Gradually increase the weight of lateral obstacle avoidance control to achieve a smooth integration of tracking control and obstacle avoidance control.

7. A quadruped robot obstacle avoidance and following system based on fully airborne pure vision, characterized in that, include: The visual detection and tracking module is used to acquire real-time RGB and depth images from the airborne stereo camera, detect pedestrians and obstacles using the target detection unit, and obtain the pixel coordinate bounding box, category label, and confidence score of each detected object. It also combines the multi-target tracking unit to perform data association on pedestrians and maintain a consistent ID, extracting the horizontal pixel coordinates, vertical pixel coordinates, and depth of pedestrians in the image coordinate system. At the same time, it extracts the pixel coordinates and depth information of obstacles in the image coordinate system. Delayed perception prediction module: used to build a state estimator, using the horizontal pixel coordinates and depth of the target pedestrian as measurements to update and correct the state estimator; during the gaps without visual measurement, it performs state prediction based on a constant velocity motion model with a high-frequency control cycle, outputting a high-frequency continuous target prediction state, thereby compensating for the delay and sampling rate mismatch problem of visual perception. Decoupled artificial potential field visual obstacle avoidance module: It is used to calculate the position of the obstacle in the robot body coordinate system based on the obstacle pixel coordinates and depth information, and calculate the repulsive force of each obstacle; it strictly projects the total repulsive force of all obstacles detected in the field of view onto the lateral coordinate axis of the quadruped robot to generate lateral obstacle avoidance speed command. Image-based visual servoing-nonlinear model prediction tracking control module: used to construct an image space motion model with the horizontal pixel coordinates and depth of the target pedestrian in the image coordinate system as state variables and the robot's longitudinal velocity and yaw angular velocity as control inputs, and solve the target tracking control speed command based on the nonlinear model prediction control framework; Command fusion and output module: The fusion module calculates a dynamic fusion weight function based on the distance between the robot and the nearest obstacle, and adaptively fuses the target tracking control speed command and the lateral obstacle avoidance speed command; finally, it generates a three-dimensional comprehensive motion command containing longitudinal velocity, lateral velocity and yaw angular velocity and sends it to the quadruped robot's high-level motion controller to drive the quadruped robot's movement.

8. An electronic device, characterized in that, include: Memory, used to store computer programs; A processor for executing the computer program to implement the method as described in any one of claims 1-6.

9. A non-volatile storage medium, characterized in that, Used to store a computer program, wherein the computer program, when executed by a processor, implements the method as described in any one of claims 1-6.

10. A computer program product comprising a computer program / instructions, characterized in that, When the computer program / instructions are executed by the processor, they implement the method described in any one of claims 1-6.

Images