Mobile robot navigation tracking control method and system based on visual laser fusion
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- QILU UNIVERSITY OF TECHNOLOGY (SHANDONG ACADEMY OF SCIENCES)
- Filing Date
- 2026-04-09
- Publication Date
- 2026-07-14
Smart Images
Figure CN122111081B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of autonomous perception and control technology for mobile robots, and in particular to a navigation and tracking control method and system for mobile robots based on visual-laser fusion. Background Technology
[0002] With the development of autonomous mobile robot technology, navigation robots need to have both autonomous navigation and target tracking capabilities.
[0003] In existing technologies, single sensors such as vision and lidar each have their limitations, making it difficult to achieve stable target locking and accurate ranging in complex environments at the same time. Meanwhile, navigation and tracking functions are usually handled separately, lacking a mechanism to deeply apply multi-sensor fusion data to motion control decisions. In addition, human-computer interaction methods are scarce, making it difficult for users to intuitively and quickly specify navigation paths or lock onto targets.
[0004] Therefore, the lack of high-precision perception schemes for multi-sensor fusion and intelligent control mechanisms for navigation and tracking collaboration makes it difficult for robots to obtain the precise relative state of the target object through multi-sensor fusion and to achieve smooth, jitter-free intelligent switching between the two behavior modes of trajectory navigation and target following. Summary of the Invention
[0005] To address the shortcomings of existing technologies, this invention provides a mobile robot navigation and tracking control method and system based on vision-laser fusion. It can complete task route pre-setting and target locking through a human-machine interaction terminal, and achieve high-precision positioning and state estimation of the target object by utilizing fused perception data from vision and laser radar. Combined with an adaptive navigation and tracking strategy, it enables smooth and safe human-machine collaborative movement of the robot.
[0006] To achieve the above objectives, the present invention adopts the following technical solution: Firstly, a navigation and tracking control method for mobile robots based on vision-laser fusion is provided, including: S1: Generate a reference trajectory based on the navigation key points selected by the user; receive information about the referenced object selected by the user and initialize the tracking algorithm; S2: Obtain the lateral pixel deviation between the referenced object and the center of the image through a tracking algorithm, and obtain the yaw angle by combining it with the focal length; construct a dynamic narrow-band gate based on the yaw angle, extract the corresponding angle data from the real-time point cloud data, and obtain the actual distance between the mobile robot and the referenced object; S3: Based on the actual distance between the mobile robot and the target object and the lateral pixel deviation, set threshold boundaries to divide the system state into trajectory tracking mode, stationary waiting mode and target object tracking mode; solve for the optimal control quantity in the corresponding mode; S4: Send the optimal control quantity to the robot and return to step S2.
[0007] Secondly, a mobile robot navigation and tracking control system based on vision-laser fusion is provided, including: microprocessor Rear camera unit Laser radar ranging unit GNSS / RTK positioning unit Ackerman Mobile Robot Human-computer interaction terminal ; The microprocessor Each with the rear camera unit Laser radar ranging unit GNSS / RTK positioning unit Ackerman Mobile Robot Human-computer interaction terminal connect; The rear camera unit Used to obtain image information of the referenced object and its surroundings; The laser radar ranging unit Used to acquire Ackerman mobile robots The actual distance between the referenced object and the Ackerman mobile robot Distance to the nearest obstacle in each direction; The microprocessor Used to perform the steps in the vision-laser fusion-based mobile robot navigation and tracking control method as described in the first aspect; Human-computer interaction terminal With microprocessors Communication is used to display the global map interface and real-time video monitoring screen, and to receive touch operation commands from users, for setting key points of the navigation path and selecting specific objects in the video screen.
[0008] Thirdly, an electronic device is also provided, comprising: Memory, used for non-transitory storage of computer-readable instructions; and Processor, for executing the computer-readable instructions, When the computer-readable instructions are executed by the processor, they perform the method described in the first aspect above.
[0009] Fourthly, a computer-readable storage medium is provided having a program stored thereon that, when executed by a processor, implements the method described in the first aspect above.
[0010] The above technical solution has the following advantages or beneficial effects: This invention enables map point selection and smooth trajectory generation, as well as video frame selection and target locking via a human-computer interaction terminal. This solves the problem of limited human-computer interaction methods, making it difficult for users to intuitively and quickly specify navigation paths or lock targets, greatly improving the system's usability and flexibility. Simultaneously, it employs a fusion perception strategy of visual angle-guided laser ranging, utilizing visual azimuth angles to construct dynamic gates for filtering laser point clouds. This effectively solves the problems of inaccurate ranging or susceptibility to interference with single-sensor ranging, achieving high-precision estimation of the pose of the guided object and exhibiting stronger perception robustness in complex environments.
[0011] This invention constructs a three-domain state space including a hysteresis buffer, encompassing tracking, waiting, and adjustment domains. This fundamentally eliminates the ping-pong effect during control mode switching, enabling the robot to exhibit smooth, human-like motion performance during human-robot collaboration. Furthermore, based on a dual-model predictive control architecture, it integrates control barrier functions to ensure active safety during trajectory tracking and releases constraints to enhance flexibility during target tracking, thus balancing navigation safety with target acquisition success rate. Attached Figure Description
[0012] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.
[0013] Figure 1 This is a flowchart of a mobile robot navigation and tracking control method based on visual-laser fusion in a specific embodiment of the present invention; Figure 2 This is a schematic diagram of the three-region model decision-making in a specific embodiment of the present invention; Figure 3 This is a block diagram of the trajectory tracking MPC-CBF according to a specific embodiment of the present invention.
[0014] Figure 4 This is a block diagram of the referenced object MPC in a specific embodiment of the present invention; Figure 5 This is a system structure block diagram of a mobile robot navigation and tracking control system based on visual-laser fusion in a specific embodiment of the present invention; Figure 6 This is a hardware structure diagram of a mobile robot navigation and tracking control system based on visual-laser fusion in a specific embodiment of the present invention. Detailed Implementation
[0015] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.
[0016] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the exemplary embodiments of the invention. The terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion, for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.
[0017] In this embodiment of the invention, "and / or" is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. Furthermore, in the description of this invention, "multiple" refers to two or more.
[0018] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.
[0019] All data acquisition in this embodiment is carried out in accordance with laws and regulations and with user consent, and the data is used legally.
[0020] Example 1 like Figure 1 As shown, this embodiment provides a navigation and tracking control method for a mobile robot based on visual-laser fusion, including: S1: Generate a reference trajectory based on the navigation key points selected by the user; receive information about the referenced object selected by the user and initialize the tracking algorithm; S2: Obtain the lateral pixel deviation between the referenced object and the center of the image through a tracking algorithm, and obtain the yaw angle by combining it with the focal length; construct a dynamic narrow-band gate based on the yaw angle, extract the corresponding angle data from the real-time point cloud data, and obtain the actual distance between the mobile robot and the referenced object; S3: Based on the actual distance between the mobile robot and the target object and the lateral pixel deviation, set threshold boundaries to divide the system state into trajectory tracking mode, stationary waiting mode and target object tracking mode; solve for the optimal control quantity in the corresponding mode; S4: Send the optimal control quantity to the robot and return to step S2.
[0021] The specific steps of S1 are as follows: S1.1: User selects navigation key points: The user selects navigation key points by clicking on the map interface of the human-computer interaction terminal. Navigation key points include the starting point, several waypoints, and the destination.
[0022] S1.2: Generate a reference trajectory based on the user-selected navigation key points: Receive the coordinate sequence of the selected navigation key points and use a Bezier curve interpolation algorithm to generate a smooth and continuous reference trajectory. This trajectory, after discretization, consists of a series of reference state points, specifically: (1); in, For reference state point index, The latitude and longitude are used as reference location coordinates. For reference heading angle.
[0023] S1.3: Receive information about the user-selected object and initialize the tracking algorithm: The camera unit on the mobile robot collects real-time video streams and transmits them back to the human-computer interaction terminal. The user draws a rectangle on the video screen displayed on the human-computer interaction terminal using touch dragging to accurately select the object. The coordinate information of this rectangle is received and used as a feature template to initialize the CSRT (Channel and Spatial Reliability Tracker) tracking algorithm, enabling continuous tracking of the object.
[0024] The specific steps of S2 are as follows: S2.1: Obtain the lateral pixel deviation between the referenced object and the image center using a tracking algorithm, and combine this with the focal length to obtain the yaw angle: Obtain the pixel coordinates of the referenced object in the image using the CSRT tracking algorithm, and calculate its lateral pixel deviation from the image center. (2); in, The horizontal pixel coordinates of the center of the referenced object in the image. This represents the horizontal pixel coordinates of the image center point. According to the pinhole imaging model, this horizontal pixel deviation... With the camera's focal length (Unit: pixels) together determine the yaw angle of the referenced object relative to the centerline of the mobile robot. : (3); S2.2: Construct a dynamic narrow-bandgap based on the yaw angle, and extract the corresponding angle data from the real-time acquired point cloud data to obtain the actual distance between the mobile robot and the referenced object: The yaw angle calculated based on S2.1 Constructing a dynamic narrow-bandgap gate; The lidar ranging unit in the mobile robot scans a fan-shaped area at a specific angle directly behind the robot (matching the field of view of the camera unit) to acquire point cloud data in real time. In this embodiment, the specific angle directly behind is selected as 70°. Point cloud data is extracted from the acquired point cloud data at angles within... Valid data within the range (of which) (The gate angle is set to 1.5° in this embodiment). This dynamic narrow gate filtering mechanism can effectively eliminate interference from side and background obstacles, thereby accurately locking the distance information of the target object. The extracted effective distance data is smoothed using a moving average filtering algorithm, and the final output is the actual distance between the target object and the mobile robot. .
[0025] The specific steps for S3 are as follows: S3.1: As Figure 2 As shown, based on the actual distance between the mobile robot and the target object. and absolute value of horizontal pixel deviation To divide the control states and set corresponding threshold boundaries, the system's state space is divided into three distinct control modes: Tracking mode (tracking area): When and When the target object is in an ideal following state, the system executes trajectory tracking control, and the status flag is set. ; Static waiting mode (waiting area): When and When this occurs, it indicates that the referenced object has slightly deviated, and the robot enters a static waiting state; the status flag is set. ; Referenced Object Tracking Mode (Adjustment Area): When or When this occurs, it indicates that the cited object has significantly or severely deviated from its intended path, triggering cited object tracking control, and the status flag is set. .
[0026] The definitions of each threshold are as follows: Inner layer distance threshold Defined as the minimum distance boundary that triggers the mobile robot to enter a stationary waiting mode. When the distance to the target object exceeds this value, it indicates that the target object has started to fall behind, and the mobile robot should stop moving forward and wait for it to catch up. In this embodiment, it is set as follows: .
[0027] Outer layer distance threshold This is defined as the distance boundary that triggers the mobile robot to enter the target tracking mode. When the distance to the target exceeds this value, it indicates that the target is significantly behind, and the mobile robot needs to actively adjust its pose to re-align with the target. In this embodiment, a distance of 1.5 is set. .
[0028] Maximum effective tracking distance Defined as the maximum distance at which the system can effectively track the target object. Exceeding this distance indicates the target object is lost, and the system will stop and issue an alarm. In this embodiment, the following is set: .
[0029] Horizontal pixel deviation threshold This is defined as the lateral deviation boundary that triggers the mobile robot to enter the guided object tracking mode. When the lateral offset of the guided object in the image exceeds this value, it indicates that it has significantly deviated from the guidance direction of the mobile robot and needs to be actively adjusted. In this embodiment, the camera unit has a field of view of 70° and a resolution of 640×480. Through geometric calculations, the pixel deviation corresponding to a range of approximately ±22° from the center of the field of view is approximately ±200 pixels. Therefore, a comprehensive threshold is set. (pixels).
[0030] Maximum effective lateral pixel deviation Defined as the maximum lateral deviation that the system can effectively handle; deviations exceeding this limit are considered to indicate that the tracked object has severely deviated from the tracking field of view. In this embodiment, it is set to... (pixels).
[0031] This invention sets and This design creates a hysteresis buffer region (i.e., a waiting area) in the state space. This ensures that the system does not frequently switch between the two modes of trajectory tracking and object tracking, thereby fundamentally avoiding control command jitter (i.e., the ping-pong effect) and making the behavior of the mobile robot smoother and more stable.
[0032] S3.2: Perform environmental obstacle perception and filter out key obstacle points: Acquire environmental information around the mobile robot and process data within a specific fan-shaped area directly in front of it. The point cloud data is filtered using Euclidean distance to retain only the distance to the current position of the mobile robot. Obstacles within a radius are sorted by distance from smallest to largest, and the closest one is selected from the sorted list. Key obstacles (among which) ). In polar coordinates Polar radius and polar angle of each key obstacle point Real-time conversion to two-dimensional spatial coordinates in the global coordinate system , Establish the spatial location characteristics of obstacles.
[0033] In some embodiments, the specific process of environmental obstacle perception is as follows: Environmental information around the mobile robot is acquired using a lidar ranging unit. The lidar ranging unit has 70 valid point cloud data points within every 15° range and 840 sets of valid data points within a 180° range. The mobile robot's processor processes the 840 sets of point cloud data points within a 180° fan-shaped area directly in front of it, and through Euclidean distance filtering, only retains data points that are close to the robot's current position. Obstacles within the radius are sorted by distance from smallest to largest, and the 10 closest critical obstacles are selected. The polar radius and polar angle of these 10 critical obstacles are then plotted in polar coordinates. Real-time conversion to two-dimensional spatial coordinates in the global coordinate system , Establish the spatial location characteristics of obstacles.
[0034] S3.3: Solve for the optimal control quantity under the corresponding mode, specifically: Read status flags And based on its value, the corresponding control algorithm is scheduled to solve for the optimal control quantity: S3.3.1: In trajectory tracking mode ( The following steps are performed: Read the preset reference path information, solve the trajectory tracking MPC optimization problem, and apply the selected parameters. The CBF safety constraints for key obstacle points are as follows: construct a trajectory tracking MPC-CBF model; solve the trajectory tracking MPC-CBF model to obtain the optimal control quantity in trajectory tracking mode, and ensure that the vehicle travels along the preset trajectory under the premise of obstacle avoidance.
[0035] like Figure 3 As shown, the specific problem of constructing and optimizing the trajectory tracking MPC-CBF model is as follows: Read the preset reference path information and define the state and control variables: state vector. Control quantity , The latitude and longitude coordinates of the mobile robot. For heading angle , linear velocity , Front wheel angular velocity The latitude and longitude position and heading angle can be obtained in real time by the GNSS / RTK positioning unit of the mobile robot.
[0036] Construct a kinematic model based on the classic Ackermann discrete model: (4); in, k express k time, k +1 indicates k The next control cycle of the control cycle, It means k Longitude of time Then refers to k The latitude of time Then refers to k The heading angle at any moment, Indicates the longitude at the next moment. Indicates the latitude at the next moment. This refers to the heading angle at the next moment. yes k The angular velocity of the front wheel at any given moment. To control the cycle .
[0037] Construct the objective function: (5); in, To predict the step size, To predict the state vector, specifically the robot's state vector... k Predicting the future at any moment Step coordinates and heading angle ; The reference trajectory state vector; the L2 norm term in the objective function is defined as a matrix weighted sum of squares, in the form of... , used to characterize the weighted cost of various state deviations or control energy; This is the error weight matrix, used to adjust the weights of the tracking errors of the mobile robot's lateral position, longitudinal position, and heading angle in the objective function. Its expression is: (6); in, The longitudinal position error weight measures the distance between the robot's current position and the reference point. The significance of deviations along the axis (usually referring to the vertical axis in a map coordinate system); The lateral position error weight measures the distance between the robot and the reference point. The importance of deviation in the axis direction (usually referring to the lateral distance from the preset path); Heading angle error weight: measures the difference between the robot's actual orientation and the reference heading angle. The importance of the angular deviation (in radians) between them.
[0038] Increase The weights in the algorithm will cause the controller to prioritize reducing the corresponding state errors, thereby improving the accuracy of trajectory tracking.
[0039] The input weight error is used to penalize the control variables for linear velocity and angular velocity. Its expression is: (7); in, As linear velocity weights, for the linear velocity of the mobile robot The penalty coefficient; Weighting of front wheel angular velocity: For the angular velocity of the front wheels of the mobile robot The penalty coefficient.
[0040] Increase The weights in the code will make the controller tend to generate smoother, less energy-consuming control commands, but this may come at the cost of some tracking accuracy. For the corresponding number Prediction step and the first The relaxation factor of each obstacle constraint, and This is used to prevent controller crashes by allowing minimal constraint violations in exchange for a feasible solution to the optimization problem when obstacles are extremely dense or constraints are too tight. The slack variable penalty coefficient is typically set to a value much larger than... and The positive constants of the matrix elements are used to ensure that the optimizer prioritizes using them during the optimization process. Approaching zero means pursuing path tracing performance while ensuring that hard constraints are not violated; As a control quantity, it has physical constraints: (8); in, It refers to The moment, predicting the future The control input vector for the step; Control limits, such as maximum reverse speed (0 if reversing is not allowed) and maximum angular velocity for turning left; For example, maximum linear velocity and maximum rightward angular velocity.
[0041] Define a CBF function based on Euclidean space location. : (9); in, The spatiotemporal position vector predicted by the robot. For the first The spatial coordinate vectors of the key obstacles For the radius of the mobile robot, For safety margin.
[0042] Apply instantaneous safety constraints: (10); Apply evolutionary safety constraints: (11); in, It refers to At any given moment, predicting the future... The robot's state vector for each step; The CBF attenuation coefficient has a value range of [value range missing]. This is used to adjust the convergence rate of the robot when it approaches the boundary of an obstacle. The larger the size, the earlier and smoother the obstacle avoidance intervention; The smaller the value, the closer the robot can get to the obstacle boundary.
[0043] In summary, the complete optimization problem for trajectory tracking MPC-CBF is: (12); in, This is a discrete kinematics model for a mobile robot.
[0044] The optimization problem of the trajectory tracking MPC-CBF model is solved using the QP solver to obtain the optimal control input in the trajectory tracking mode. ,in This is the optimal linear velocity command. This is the optimal angular velocity command.
[0045] S3.3.2: In the static waiting mode ( (Next): Enters standby protection state. At this time, the MPC optimization problem is not solved; zero-speed instructions are directly output. This keeps the mobile robot stationary in place until the tracked object re-enters the tracking area.
[0046] S3.3.3: In the referenced object tracing mode ( Below: based on distance deviation and lateral pixel deviation Using this as input, a cited object tracking MPC model is constructed, and the optimization problem of the cited object tracking MPC model is solved to obtain the optimal control quantity in the cited object tracking mode. In this mode, the system removes the CBF hard constraint to ensure the flexibility of the mobile robot's pose adjustment and prioritizes the realignment of the cited object.
[0047] like Figure 4As shown, the specific problem of constructing and optimizing the cited object tracking MPC model is as follows: Define state and control variables: State variables are The control quantity is .
[0048] in, The distance deviation between the mobile robot and the target object is calculated as follows: (13); Preset desired tracking distance In this embodiment, it is set to 3.5m, but other values can also be set according to actual needs.
[0049] Constructing a kinematic model: (14); in, , Let the pixel focal length of the camera unit be: (15); Define the objective function: (16); in, To predict the step size; This is the state error weight matrix, used to adjust the weights of distance deviation and lateral pixel deviation in the objective function. Its expression is: (17); in, Distance deviation weight is used to measure the distance deviation between the mobile robot and the referenced object. The importance of; The lateral pixel deviation weight measures the lateral pixel deviation of the referenced object in the camera image. The importance of.
[0050] Increase The weights in the code will cause the controller to prioritize reducing the corresponding state deviations; This is the control weight matrix, used to penalize the control variables for linear velocity and angular velocity, respectively. Its expression is: (18); in, Tracking online speed of referenced objects in MPC mode The penalty coefficient; Tracking angular velocity of the referenced object in MPC mode The penalty coefficient.
[0051] Increase The weights in the code will cause the controller to tend to generate smoother control commands.
[0052] Define physical constraints for control variables: (19); In summary, the complete optimization problem for cited object tracking MPC is: (20); Solve the optimization problem of the cited object tracking MPC model to obtain the optimal control quantity under the cited object tracking mode. .
[0053] The specific steps of S4 are as follows: The optimal control quantity obtained by solving S3 (or the zero-speed instruction in the static waiting mode) is sent to the mobile robot.
[0054] In this embodiment, the Ackerman robot is used as an example of a mobile robot for illustration: After the geometric model transformation via Ackerman steering is completed, it is sent to the Ackerman mobile robot. In the next control cycle, it returns to step S2 to achieve closed-loop rolling optimization.
[0055] The specific Ackermann steering geometry transformation is as follows: (twenty one); in, This refers to the front wheel steering angle. Angular velocity, Linear velocity, This refers to the wheelbase (distance between the front and rear wheel axles) of the Ackerman mobile robot.
[0056] It should be understood that other types of mobile robots can be used. In this case, it is only necessary to convert the obtained optimal control quantity according to the type of mobile robot and then send it to the mobile robot.
[0057] Example 2 like Figure 5 As shown, this embodiment provides a mobile robot navigation and tracking control system based on visual-laser fusion, employing the mobile robot navigation and tracking control method based on visual-laser fusion from Embodiment 1, including: microprocessor Rear camera unit Laser radar ranging unit GNSS / RTK positioning unit Ackerman Mobile Robot Human-computer interaction terminal ; microprocessor Each with the rear camera unit Laser radar ranging unit GNSS / RTK positioning unit Ackerman Mobile Robot Human-computer interaction terminal connect; Rear camera unit Used to obtain image information of the referenced object and its surroundings; LiDAR ranging unit Used to acquire Ackerman mobile robots The actual distance between the referenced object and the Ackerman mobile robot Distance to the nearest obstacle in each direction; microprocessor Used to perform the steps in the mobile robot navigation and tracking control method based on visual-laser fusion as described in Embodiment 1; Human-computer interaction terminal With microprocessors Communication is used to display the global map interface and real-time video monitoring screen, and to receive touch operation commands from users, for setting key points of the navigation path and selecting specific objects in the video screen.
[0058] In some embodiments, GNSS / RTK positioning units Used to acquire Ackerman mobile robots The latitude and longitude position and heading angle of the main body.
[0059] In some embodiments, the Ackerman mobile robot Used to receive microprocessors Control commands enable movement.
[0060] In some embodiments, such as Figure 6 As shown, the mobile robot navigation and tracking control system based on vision-laser fusion provided in Embodiment 2 can adopt the following system hardware: microprocessor Employs a Jeston Nano rear camera unit Employs a monocular camera and a lidar ranging unit Employing lidar and GNSS / RTK positioning units Using dual-antenna GNSS, Ackerman Mobile Robot Human-computer interaction terminal Using mobile terminals.
[0061] The descriptions of each embodiment in the above embodiments have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions in other embodiments.
[0062] The proposed system can be implemented in other ways. For example, the system embodiments described above are merely illustrative, and the division of modules described above is only a logical functional division. In actual implementation, there may be other division methods. For example, multiple modules may be combined or integrated into another system, or some features may be ignored or not executed.
[0063] Example 3 This embodiment also provides an electronic device, including: one or more processors, one or more memories, and one or more computer programs; wherein, the processor is connected to the memory, and the one or more computer programs are stored in the memory. When the electronic device is running, the processor executes the one or more computer programs stored in the memory to cause the electronic device to perform the method described in Embodiment 1.
[0064] It should be understood that in this embodiment, the processor can be a central processing unit (CPU), or it can be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or any conventional processor, etc.
[0065] Memory may include read-only memory and random access memory, and provides instructions and data to the processor. A portion of memory may also include non-volatile random access memory. For example, memory may also store information about the device type.
[0066] In the implementation process, each step of the above method can be completed by the integrated logic circuits in the processor hardware or by software instructions.
[0067] The method in Embodiment 1 can be directly implemented by a hardware processor, or implemented by a combination of hardware and software modules within the processor. The software modules can reside in readily available storage media in the art, such as random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, or registers. This storage medium is located in memory; the processor reads information from the memory and, in conjunction with its hardware, completes the steps of the above method. To avoid repetition, a detailed description is not provided here.
[0068] Those skilled in the art will recognize that the units and algorithm steps described in connection with the various examples of this embodiment can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this invention.
[0069] Example 4 Embodiment 4 of the present invention provides a computer-readable storage medium.
[0070] A computer-readable storage medium having a program stored thereon that, when executed by a processor, implements the steps of the method as described in Embodiment 1 of the present invention.
[0071] The detailed steps are the same as those provided in Example 1, and will not be repeated here.
[0072] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.
Claims
1. A navigation and tracking control method for a mobile robot based on vision-laser fusion, characterized in that, include: S1: Generate a reference trajectory based on the navigation key points selected by the user; Receive information about the referenced object selected by the user and initialize the tracking algorithm; S2: The lateral pixel deviation between the referenced object and the image center is obtained through a tracking algorithm, and the yaw angle is obtained by combining the focal length. Specifically: The image is image information of the object being referenced and its surroundings, acquired by the rear camera on the mobile robot; The pixel coordinates of the referenced object in the image are obtained using the CSRT tracking algorithm, and its lateral pixel deviation from the image center is calculated. ； in, The horizontal pixel coordinates of the center of the referenced object in the image. Here are the horizontal pixel coordinates of the image center point; according to the pinhole imaging model, the horizontal pixel deviation... With the camera's focal length Determine the yaw angle of the object being referenced relative to the centerline of the mobile robot. : ； A dynamic narrow-bandgap gate is constructed based on the yaw angle. Data corresponding to the angle is extracted from the real-time acquired point cloud data to obtain the actual distance between the mobile robot and the target object. Specifically: Based on yaw angle A dynamic narrow-beam gate is constructed. The lidar ranging unit in the mobile robot scans a 70° fan-shaped area directly behind the robot to acquire point cloud data. From the acquired point cloud data, the angle at which the robot is located is extracted. Valid data within the range, of which The gate half-angle is set to 1.5°; the extracted effective distance data is smoothed using a moving average filtering algorithm, and the final output is the actual distance between the referenced object and the mobile robot. ; S3: Based on the actual distance between the mobile robot and the target object and the lateral pixel deviation, set threshold boundaries respectively, and divide the system state into trajectory tracking mode, stationary waiting mode and target object tracking mode; solve for the optimal control quantity in the corresponding mode; Track tracking mode: When and When the referenced object is in an ideal following state, the system executes trajectory tracking control, and the status flag bit... ; Static waiting mode: when and If the referenced object deviates slightly, the robot enters a static waiting state, and the status flag is set. ; ; Referenced Object Tracking Pattern: When or If the referenced object has significantly or severely deviated from its intended path, referenced object tracking control will be executed, and the status flag will be set. ; in, The actual distance between the mobile robot and the referenced object. This represents the absolute value of the horizontal pixel deviation. The lateral pixel deviation threshold of the distance boundary that triggers the mobile robot to enter the object tracking mode; To set the inner distance threshold of the minimum distance boundary that triggers the mobile robot to enter the static waiting mode; To set the outer distance threshold of the distance boundary that triggers the mobile robot to enter the object tracking mode; The maximum effective tracking distance that enables the system to effectively track the referenced object; The maximum effective lateral pixel deviation that the system can effectively handle; S4: Send the optimal control quantity to the robot and return to step S2.
2. The mobile robot navigation and tracking control method based on vision-laser fusion as described in claim 1, characterized in that, In the process of solving for the optimal control quantity under the corresponding mode, the specific steps for solving the optimal control quantity under the trajectory tracking mode are as follows: Read the preset reference path information, solve the trajectory tracking MPC optimization problem, and apply the selected parameters. CBF safety constraints at key obstacle points are used to construct a trajectory tracking MPC-CBF model; Solve the trajectory tracking MPC-CBF model to obtain the optimal control input under trajectory tracking mode; Among them, the The process of selecting key obstacle points is as follows: acquire environmental information around the mobile robot, and process the 180° fan-shaped area directly in front of it. The point cloud data is filtered using Euclidean distance to retain the distance to the current position of the mobile robot. Obstacles within a radius are sorted by distance from smallest to largest, and the closest one is selected from the sorted list. A key obstacle.
3. The mobile robot navigation and tracking control method based on vision-laser fusion as described in claim 1, characterized in that, Using distance deviation and lateral pixel deviation as inputs, a cited object tracking MPC model is constructed. The optimization problem of the cited object tracking MPC model is solved to obtain the optimal control quantity under the cited object tracking mode.
4. A mobile robot navigation and tracking control system based on vision-laser fusion, characterized in that, include: microprocessor Rear camera unit Laser radar ranging unit GNSS / RTK positioning unit Ackerman Mobile Robot Human-computer interaction terminal ; The microprocessor Each with the rear camera unit Laser radar ranging unit GNSS / RTK positioning unit Ackerman Mobile Robot Human-computer interaction terminal connect; The rear camera unit Used to obtain image information of the referenced object and its surroundings; The laser radar ranging unit Used to acquire Ackerman mobile robots The actual distance between the referenced object and the Ackerman mobile robot Distance to the nearest obstacle in each direction; The microprocessor Used to perform the steps in the mobile robot navigation and tracking control method based on vision-laser fusion as described in any one of claims 1-3; Human-computer interaction terminal With microprocessors Communication is used to display the global map interface and real-time video monitoring screen, and to receive touch operation commands from users, for setting key points of the navigation path and selecting referenced objects in the video screen.
5. The mobile robot navigation and tracking control system based on vision-laser fusion as described in claim 4, characterized in that, The GNSS / RTK positioning unit Used to acquire Ackerman mobile robots The latitude and longitude position and heading angle of the main body; the Ackerman mobile robot Used to receive microprocessors Control commands enable movement.
6. An electronic device, characterized in that, It includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the mobile robot navigation and tracking control method based on visual-laser fusion as described in any one of claims 1-3.
7. A computer-readable storage medium having a computer program stored thereon, characterized in that, When executed by the processor, the program implements the steps in the mobile robot navigation and tracking control method based on vision-laser fusion as described in any one of claims 1-3.