Square differential robot adaptive path tracking method and system based on target point driving

Abstract

The application discloses a square differential robot adaptive path tracking method and system based on target point driving, which comprises the following steps: S1, acquiring a global path, a current pose of the robot and an environment cost map; S2, calculating a linear velocity of the robot; S3, constructing a target point distance set and selecting a target point therefrom; S4, calculating an angular velocity of the robot; S5, predicting a motion trajectory of the robot based on the linear velocity and the angular velocity; S6, detecting whether the predicted trajectory is an infeasible trajectory; if yes, updating the target point distance set; S7, detecting rotation feasibility, and when rotation cannot be performed, controlling the robot to move along a current heading, continuously detecting the rotation feasibility, so as to obtain a target pose satisfying rotation constraints; S8, when all the target points correspond to infeasible trajectories, adjusting the linear velocity and returning to S3; and S9, smoothing the output linear velocity and angular velocity. The application improves the continuity and stability of robot motion.

Description

Technical Field

[0001] This invention relates to trajectory prediction control technology, specifically to an adaptive path tracking method and system for a square differential robot based on target point driving. Background Technology

[0002] With the widespread application of mobile robots in warehousing and distribution, indoor cleaning, and logistics transportation, path tracking control has become a key technology for achieving autonomous robot navigation. Differential drive robots achieve linear motion and steering by adjusting the speed of the left and right drive wheels, offering advantages such as simple structure, flexible control, and the ability to rotate in place. However, for differential robots with rectangular or square bodies, the actual area occupied by the robot varies significantly depending on its direction of travel when moving in narrow passages, corners, or near obstacles. This can easily lead to situations where the path is passable, but the robot's actual outline collides with the path.

[0003] Existing path tracking methods mainly include pure tracking, dynamic window and model predictive control. The pure tracking method selects a target point ahead of the planned path and calculates the steering control quantity based on the geometric relationship between the target point and the robot's current pose. It has a simple structure and high computational efficiency (Reference [1] Coulter RC Implementation of the PurePursuit Path Tracking Algorithm[R]. Carnegie Mellon University, TechnicalReport CMU-RI-TR-92-01, 1992.). The adjusted pure tracking method adjusts the robot's linear velocity based on the path curvature and obstacle distance, which improves the tracking safety in confined spaces (Reference [2] Macenski S., Singh S., Martín F., Ginés J. Regulated Pure Pursuit for Robot Path Tracking[J].Autonomous Robots, 2023, 47(6): 685-694. DOI: 10.1007 / s10514-023-10097-6.). The dynamic window method samples linear velocity and angular velocity in velocity space, predicts the robot's trajectory over a certain period of time, and selects control variables based on target direction, obstacle distance and motion speed (Reference [3] Fox D., Burgard W., Thrun S. The Dynamic Window Approach to Collision Avoidance[J]. IEEE Robotics & Automation Magazine, 1997, 4(1): 23-33. DOI: 10.1109 / 100.580977).

[0004] The methods described above can achieve robot path tracking in general environments, but target point selection, speed adjustment, and collision detection are usually relatively independent. When the predicted trajectory corresponding to the target point is infeasible, existing methods struggle to quickly recover a feasible trajectory by gradually adjusting the target point distance and linear velocity. Furthermore, some methods use the robot's center point or a circular outline for collision detection, which fails to accurately reflect the space occupancy of a square differential robot during rotation. When the robot approaches an obstacle and cannot rotate in place, there is also a lack of corresponding motion recovery mechanisms.

[0005] The existing technology has the following main drawbacks:

[0006] (1) Existing target point-driven methods typically use a fixed or single-time calculated forward-looking distance. When the target point distance is large, the robot may deviate from the path near curves and obstacles; when the target point distance is small, frequent turning and motion oscillations are likely to occur. Existing methods lack a mechanism for iteratively screening multiple candidate target points based on the feasibility of the predicted trajectory.

[0007] (2) Existing methods typically perform target point selection and speed adjustment separately. When the trajectory corresponding to the current target point collides, it is difficult to re-search for a feasible trajectory by sequentially shortening the target point distance and reducing the linear velocity, which can easily lead to control stagnation or frequent replanning.

[0008] (3) Some collision detection methods simplify the robot to a center point or a circular outline, which cannot accurately represent the actual area occupied by the square robot at different heading angles. In particular, they are prone to ignoring the risk of collision between the four corners and obstacles when the robot turns.

[0009] (4) When a square differential robot is near a wall or obstacle, there may be a situation where the current position can accommodate the robot, but it cannot safely rotate in place. Existing path tracking methods lack a recovery mechanism for rotation-infeasible states, which can easily cause the robot to stall.

[0010] (5) Adjusting the target point or speed may cause sudden changes in linear velocity and angular velocity. If only the maximum speed and acceleration are limited, a large jerk may still be generated, resulting in unstable robot motion, impact on drive components, and increased path tracking error.

[0011] Therefore, an adaptive path tracking method that comprehensively considers target point selection, speed adjustment, rectangular contour collision detection, and rotation feasibility is needed. Summary of the Invention

[0012] This invention addresses the problems of insufficient adaptability in target point selection, easy collision in predicted trajectory, and limited rotation in place when a square differential robot performs path tracking in a narrow environment. It provides an adaptive path tracking method and system for a square differential robot based on target point driving.

[0013] To achieve the above objectives, the present invention adopts the following technical solution: An adaptive path tracking method for a square differential robot based on target point driving includes the following steps: S1. Obtain the global path, the robot's current pose, and the environmental cost map. The robot is a differential drive mobile robot with a rectangular geometric profile. S2. Calculate the robot's linear velocity based on the distance between the robot and the obstacle, the curvature of the global path, and the deviation between the robot's current heading angle and the path direction; S3. Construct a set of target point distances on the global path, and select target points from the target point distance set according to preset rules; S4. Calculate the robot's angular velocity based on the directional relationship between the target point and the robot's current pose; S5. Predict the robot's trajectory based on linear velocity and angular velocity; S6. Combine the environmental cost map to detect whether the predicted motion trajectory is an infeasible trajectory; when the predicted motion trajectory is determined to be an infeasible trajectory, update the target point distance set and recalculate the control quantity; S7. Perform rigid body collision detection based on the robot's rectangular geometric contour to determine the robot's rotation feasibility. When it is detected that the robot cannot perform a rotation operation in the current pose, control the robot to move forward along the current heading direction and continuously detect the rotation feasibility during the movement to obtain the target pose that satisfies the rotation constraints. S8. When all target points in the target point distance set correspond to infeasible trajectories, adjust the linear velocity and return to step S3. S9. Smooth the final output linear velocity and angular velocity.

[0014] To optimize the above technical solution, the specific measures also include: Furthermore, in S1, the global path is represented as a sequence of path points. :

[0015] in, Indicates the first Path points, This indicates the position coordinates of the path point in the global coordinate system. Indicates the total number of path points; The robot's current pose is represented as follows:

[0016] in, These represent the robot's position coordinates in the global coordinate system. Indicates the robot's heading angle; The environmental cost map is represented as a two-dimensional raster function:

[0017] in, Indicates position The value of a location is used to characterize the distance relationship between that location and an obstacle or the risk of collision. The robot's geometric profile is represented by a rectangular model:

[0018] in, This represents the geometric profile of the robot in its own coordinate system. Indicates the length of the robot. Indicates the robot's width. Represents the coordinates of a point in the robot's local coordinate system; The robot's motion satisfies a differential drive kinematic model.

[0019] Furthermore, S3 specifically refers to: Construct a set of distances to target points based on the robot's current position along the global path. :

[0020] in, This represents the path distance from the robot's current position to the m-th candidate target point along the path direction. This represents the number of candidate target points. Based on the target point distance set, and under the premise of satisfying feasibility constraints, target points are selected according to a preset priority rule, wherein the priority rule is to prioritize target points that are farther away from the robot's current position; The target point corresponds to the path function:

[0021] in, Represents a path parameterization function. This indicates the robot's projected position on the path. Indicates candidate target points; The step of selecting a target point from the target point distance set according to a preset rule specifically involves selecting the maximum distance from the target point distance set.

[0022] The target point corresponding to the maximum distance is used as the current control target.

[0023] Furthermore, S5 specifically refers to: Based on the linear velocity With angular velocity Discrete-time forward prediction is performed on the robot's motion trajectory, and the predicted trajectory is represented as a state sequence.

[0024] in, Indicates the first Predicted pose of the step Indicates the number of prediction steps; The predicted pose is calculated recursively using a differential-driven kinematic model, and its update relationship is as follows:

[0025]

[0026]

[0027] in, To control the cycle.

[0028] Furthermore, in S6, the step of combining the environmental cost map to detect and predict whether the motion trajectory is an infeasible trajectory specifically involves: Based on the environmental cost map, collision detection is performed on the predicted trajectory. When any point in the predicted trajectory satisfies... When the time comes, the trajectory is determined to be an infeasible trajectory; in, Indicates position The value of the time, The collision detection threshold; The specific steps for updating the target point distance set are as follows: Remove the target point corresponding to the infeasible trajectory from the target point distance set; The target point distance set is represented as

[0029] in, Represents the set of distances to the target point. This represents the path distance from the robot's current position to the m-th candidate target point along the path direction. Indicates the number of candidate target points; When corresponding to a certain target point distance When the trajectory is not feasible, perform a set update operation:

[0030] in, Indicates the distance to the target point corresponding to the infeasible trajectory, " represents the set difference operation, that is, from set Remove elements .

[0031] Furthermore, in S7, the step of performing rigid body collision detection based on the robot's rectangular geometric contour to determine the feasibility of the robot's rotation specifically involves: The robot's geometric profile is represented by a rectangular model.

[0032] in, This represents the geometric profile of the robot in its own coordinate system. Indicates the length of the robot. Indicates the robot's width. Represents the coordinates of a point in the robot's local coordinate system; The robot's pose in the global coordinate system is represented as follows:

[0033] in, Indicates the robot's position coordinates. Indicates the heading angle; Based on the pose, the rectangular geometric contour is mapped to the global coordinate system through rigid body transformation, which is expressed as follows:

[0034] in, This represents the robot's geometric profile in the global coordinate system. Represents a two-dimensional rotation matrix; The rotation matrix is ​​represented as follows:

[0035] During trajectory prediction, collision detection is performed on the geometric contour corresponding to each pose in the predicted trajectory. When the conditions are met... At that time, it is determined that the robot has collided at that pose; in, Indicates the first in the predicted trajectory Each position / pose This represents any point in the geometric contour. Indicates position The value of the time, This indicates the collision detection threshold.

[0036] Furthermore, in S7, when it is detected that the robot cannot perform a rotation operation in the current pose, the robot is controlled to move forward along the current heading direction, and the feasibility of rotation is continuously detected during the movement to obtain a target pose that satisfies the rotation constraints. Specifically, this means: When it is detected that the robot's rotation operation is not feasible in the current pose, the feasibility recovery mechanism is triggered; The feasibility of rotation is expressed by a feasibility determination function as follows:

[0037] in, Indicates the robot's pose. Indicates the feasibility of rotation; When satisfied At this time, the robot is controlled to make forward trial movements along the current heading direction, and its trial pose sequence is represented as follows:

[0038] in, For the first The tentative stance of the tentative step. For the first The position coordinates of the trial step, Indicates the heading angle;

[0039] in, , Indicates the forward exploratory step size. Indicates the maximum number of trial steps; During the trial process, the feasibility of rotation is determined for each trial pose. When the rotation operation is deemed feasible in the given pose, the robot is controlled to move to the corresponding pose and then the rotation is executed. When satisfied If it is determined that the current direction cannot be restored to rotational capability, the robot stops moving.

[0040] Furthermore, S8 specifically refers to: When the target point distance set is empty, it is determined that there is no target point that satisfies the feasibility constraint under the current linear velocity conditions; When the target point distance set is empty, the linear velocity is adjusted, including proportionally reducing the linear velocity, with the update rule being as follows:

[0041] in, Indicates the current linear velocity. Denotes the speed reduction factor, and satisfies... .

[0042] Furthermore, S9 specifically refers to: The linear velocity and angular velocity are smoothed based on jerk constraints. During online speed smoothing, the linear velocity, linear acceleration, and linear accelerometer at the current moment are defined as follows: , and Its update relationship is

[0043] in, Indicates the control cycle. Represents the discrete time step; The linear acceleration satisfies the constraint condition.

[0044] in, Indicates the maximum permissible linear jerk. This represents a saturation function, used to restrict the input to a given interval; For a smoothing process of angular velocity, angular velocity, angular acceleration, and angular jerk are defined as follows: , and Its update relationship is

[0045]

[0046] in, Indicates the current angular velocity. Represents angular acceleration. Indicates angular jerk; The angular acceleration satisfies the following constraints:

[0047] in, This indicates the maximum permissible angular jerk.

[0048] This invention also proposes an adaptive path tracking system for a square differential robot based on target point driving, comprising: The data acquisition module is used to acquire the global path, the robot's current pose, and the environmental cost map, wherein the robot is a differential drive mobile robot with a rectangular geometric profile. The linear velocity calculation module is used to calculate the robot's linear velocity based on the distance between the robot and obstacles, the curvature of the global path, and the deviation between the robot's current heading angle and the path direction. The target point selection module is used to construct a target point distance set on the global path and select target points from the target point distance set according to preset rules; The angular velocity calculation module is used to calculate the robot's angular velocity based on the directional relationship between the target point and the robot's current pose. The trajectory prediction module is used to predict the robot's motion trajectory forward based on the linear velocity and angular velocity. The first collision detection module is used to detect whether the predicted motion trajectory is an infeasible trajectory by combining the environmental cost map; when the predicted motion trajectory is determined to be an infeasible trajectory, the target point distance set is updated and the control quantity is recalculated. The second collision detection module is used to perform rigid body collision detection based on the rectangular geometric contour of the robot to determine the rotation feasibility of the robot. When it is detected that the robot cannot perform a rotation operation in the current pose, the robot is controlled to move forward along the current heading direction, and the rotation feasibility is continuously detected during the movement to obtain a target pose that meets the rotation constraints. The linear velocity adjustment module is used to adjust the linear velocity when all target points in the target point distance set correspond to infeasible trajectories. The smoothing module is used to smooth the final output linear velocity and angular velocity.

[0049] The beneficial effects of this invention are: (1) The present invention takes into account both the tracking efficiency of straight road sections and the tracking accuracy of curves, and can adaptively select target points based on the feasibility of the predicted trajectory.

[0050] (2) Rigid body collision detection based on the rectangular geometric contour of the robot during trajectory prediction can accurately reflect the actual space occupied by the square robot.

[0051] (3) By moving forward along the current heading direction and continuously detecting the feasibility of rotation, a target pose that can safely perform rotation operation is searched, so that the square differential robot can safely rotate in place when it approaches an obstacle without stopping.

[0052] (4) By adding jerk constraints, the final output linear velocity and angular velocity are smoothed to improve the continuity and stability of the robot motion. Attached Figure Description

[0053] Figure 1 This is a flowchart of the adaptive path tracking method for a square differential robot based on target point driving proposed in this invention. Detailed Implementation

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

[0055] Example 1 This invention proposes an adaptive path tracking method for a square differential robot based on target point driving, such as... Figure 1 As shown, it includes the following steps: S1. Obtain the global path, the robot's current pose, and the environmental cost map, wherein the robot is a differential-driven mobile robot with a rectangular geometric profile; the global path is represented as a sequence of path points:

[0056] in, Indicates the first Path points, This indicates the position coordinates of the path point in the global coordinate system. Indicates the total number of path points; The robot's current pose is represented as follows:

[0057] in, These represent the robot's position coordinates in the global coordinate system. Indicates the robot's heading angle; The environmental cost map is represented as a two-dimensional raster function:

[0058] in, Indicates position The value of a location is used to characterize the distance relationship between that location and an obstacle or the risk of collision. The robot's geometric profile is represented by a rectangular model:

[0059] in, This represents the geometric profile of the robot in its own coordinate system. Indicates the length of the robot. Indicates the robot's width. Represents the coordinates of a point in the robot's local coordinate system; The robot's motion satisfies a differential drive kinematic model.

[0060] S2. Calculate the robot's linear velocity based on the distance between the robot and the obstacle, the curvature of the global path, and the deviation between the robot's current heading angle and the path direction; S3. Construct a target point distance set on the global path, and select target points from the target point distance set according to preset rules; S3 specifically involves: Construct a set of distances to target points based on the robot's current position along the global path:

[0061] in, This represents the path distance from the robot's current position to the m-th candidate target point along the path direction. This represents the number of candidate target points. Based on the target point distance set, and under the premise of satisfying feasibility constraints, target points are selected according to a preset priority rule, wherein the priority rule is to prioritize target points that are farther away from the robot's current position; The target point corresponds to the path function:

[0062] in, Represents a path parameterization function. This indicates the robot's projected position on the path. Indicates candidate target points; The step of selecting a target point from the target point distance set according to a preset rule specifically involves selecting the maximum distance from the target point distance set.

[0063] The target point corresponding to the maximum distance is used as the current control target.

[0064] S4. Calculate the robot's angular velocity based on the directional relationship between the target point and the robot's current pose; S5. Predict the robot's trajectory forward based on the linear velocity and angular velocity; S5 specifically involves: Based on the linear velocity With angular velocity Discrete-time forward prediction is performed on the robot's motion trajectory, and the predicted trajectory is represented as a state sequence.

[0065] in, Indicates the first Predicted pose of the step Indicates the number of prediction steps; The predicted pose is calculated recursively using a differential-driven kinematic model, and its update relationship is as follows:

[0066]

[0067]

[0068] in, To control the cycle.

[0069] S6. Detect whether the predicted trajectory is an infeasible trajectory by combining the environmental cost map; when the predicted trajectory is determined to be an infeasible trajectory, update the target point distance set and recalculate the control quantity; in S6, the step of detecting whether the predicted trajectory is an infeasible trajectory by combining the environmental cost map specifically involves: Based on the environmental cost map, collision detection is performed on the predicted trajectory. When any point in the predicted trajectory satisfies... When the time comes, the trajectory is determined to be an infeasible trajectory; in, Indicates position The value of the time, The collision detection threshold; The specific steps for updating the target point distance set are as follows: Remove the target point corresponding to the infeasible trajectory from the target point distance set; The target point distance set is represented as

[0070] in, Represents the set of distances to the target point. This represents the path distance from the robot's current position to the m-th candidate target point along the path direction. Indicates the number of candidate target points; When corresponding to a certain target point distance When the trajectory is not feasible, perform a set update operation:

[0071] in, Indicates the distance to the target point corresponding to the infeasible trajectory, " represents the set difference operation, that is, from set Remove elements .

[0072] S7. Based on the robot's rectangular geometric contour, perform rigid body collision detection to determine the robot's rotation feasibility. When it is detected that the robot cannot perform a rotation operation in the current pose, control the robot to move forward along the current heading direction, and continuously detect rotation feasibility during the movement to obtain a target pose that satisfies the rotation constraints; in S7, the step of performing rigid body collision detection based on the robot's rectangular geometric contour to determine the robot's rotation feasibility specifically includes: The robot's geometric profile is represented by a rectangular model as follows:

[0073] in, This represents the geometric profile of the robot in its own coordinate system. Indicates the length of the robot. Indicates the robot's width. Represents the coordinates of a point in the robot's local coordinate system; The robot's pose in the global coordinate system is represented as follows:

[0074] in, Indicates the robot's position coordinates. Indicates the heading angle; Based on the pose, the rectangular geometric contour is mapped to the global coordinate system through rigid body transformation, which can be expressed as:

[0075] in, This represents the robot's geometric profile in the global coordinate system. Represents a two-dimensional rotation matrix; The rotation matrix is ​​represented as follows:

[0076] During trajectory prediction, collision detection is performed on the geometric contour corresponding to each pose in the predicted trajectory. When the conditions are met... At that time, it is determined that the robot has collided at that pose; in, Indicates the first in the predicted trajectory Each position / pose This represents any point in the geometric contour. Indicates position The value of the time, This indicates the collision detection threshold.

[0077] In S7, when it is detected that the robot cannot perform a rotation operation in the current pose, the robot is controlled to move forward along the current heading direction, and the feasibility of rotation is continuously detected during the movement to obtain a target pose that satisfies the rotation constraints. Specifically, this means: When it is detected that the robot's rotation operation is not feasible in the current pose, the feasibility recovery mechanism is triggered; The feasibility of rotation is expressed by a feasibility determination function as follows:

[0078] in, Indicates the robot's pose. Indicates the feasibility of rotation; When satisfied At this time, the robot is controlled to make forward trial movements along the current heading direction, and its trial pose sequence is represented as follows:

[0079] in, For the first The tentative stance of the tentative step. For the first The position coordinates of the trial step, Indicates the heading angle;

[0080] in, , Indicates the forward exploratory step size. Indicates the maximum number of trial steps; During the trial process, the feasibility of rotation is determined for each trial pose. When the rotation operation is deemed feasible in the given pose, the robot is controlled to move to the corresponding pose and then the rotation is executed. When satisfied If it is determined that the current direction cannot be restored to rotational capability, the robot stops moving.

[0081] S8. When the target point's distance corresponds to an infeasible trajectory for all target points in the set, adjust the linear velocity and return to step S3; S8 specifically involves: When the target point distance set is empty, it is determined that there is no target point satisfying the feasibility constraint under the current linear velocity conditions. Here, "target point distance set is empty" is represented as follows:

[0082] in, Represents the set of distances to the target point. Represents the empty set; When the target point distance set is empty, the linear velocity is adjusted, including proportionally reducing the linear velocity, with the update rule being as follows:

[0083] in, Indicates the current linear velocity. Denotes the speed reduction factor, and satisfies... .

[0084] S9. Smooth the final output linear velocity and angular velocity. Specifically, S9 is as follows: The linear velocity and angular velocity are smoothed based on jerk constraints. During online speed smoothing, the linear velocity, linear acceleration, and linear accelerometer at the current moment are defined as follows: , and Its update relationship is

[0085] in, Indicates the control period. Represents the discrete time step; The linear acceleration satisfies the constraint condition.

[0086] in, Indicates the maximum permissible linear jerk. This represents a saturation function, used to restrict the input to a given interval; For a smoothing process of angular velocity, angular velocity, angular acceleration, and angular jerk are defined as follows: , and Its update relationship is

[0087]

[0088] in, Indicates the current angular velocity. Represents angular acceleration. Indicates angular jerk; The angular acceleration satisfies the following constraint conditions:

[0089] in, This indicates the maximum permissible angular jerk.

[0090] Example 2 This invention proposes an adaptive path tracking system for a square differential robot based on target point driving, corresponding to the method in Embodiment 1, comprising: The data acquisition module is used to acquire the global path, the robot's current pose, and the environmental cost map, where the robot is a differential drive mobile robot with a rectangular geometric profile. The linear velocity calculation module is used to calculate the robot's linear velocity based on the distance between the robot and obstacles, the curvature of the global path, and the deviation between the robot's current heading angle and the path direction. The target point selection module is used to construct a target point distance set on the global path and select target points from the target point distance set according to preset rules; The angular velocity calculation module is used to calculate the robot's angular velocity based on the directional relationship between the target point and the robot's current pose. The trajectory prediction module is used to predict the robot's motion trajectory forward based on the linear velocity and angular velocity. The first collision detection module is used to detect whether the predicted motion trajectory is an infeasible trajectory by combining the environmental cost map; when the predicted motion trajectory is determined to be an infeasible trajectory, the target point distance set is updated and the control quantity is recalculated. The second collision detection module is used to perform rigid body collision detection based on the rectangular geometric contour of the robot to determine the rotation feasibility of the robot. When it is detected that the robot cannot perform a rotation operation in the current pose, the robot is controlled to move forward along the current heading direction, and the rotation feasibility is continuously detected during the movement to obtain a target pose that meets the rotation constraints. The linear velocity adjustment module is used to adjust the linear velocity when all target points in the target point distance set correspond to infeasible trajectories. The smoothing module is used to smooth the final output linear velocity and angular velocity.

[0091] The implementation methods of each module and its function in the system are completely consistent with the steps of the method in Implementation Example 1, so they will not be repeated here.

[0092] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this application 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 application.

[0093] The above are merely preferred embodiments of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of the present invention should be considered within the scope of protection of the present invention.

Claims

1. An adaptive path tracking method for a square differential robot based on target point driving, characterized in that, Includes the following steps: S1. Obtain the global path, the robot's current pose, and the environmental cost map. The robot is a differential drive mobile robot with a rectangular geometric profile. S2. Calculate the robot's linear velocity based on the distance between the robot and the obstacle, the curvature of the global path, and the deviation between the robot's current heading angle and the path direction; S3. Construct a set of target point distances on the global path, and select target points from the target point distance set according to preset rules; S4. Calculate the robot's angular velocity based on the directional relationship between the target point and the robot's current pose; S5. Predict the robot's trajectory based on linear velocity and angular velocity; S6. Combine the environmental cost map to detect whether the predicted motion trajectory is an infeasible trajectory; when the predicted motion trajectory is determined to be an infeasible trajectory, update the target point distance set and recalculate the control quantity; S7. Perform rigid body collision detection based on the robot's rectangular geometric contour to determine the robot's rotation feasibility. When it is detected that the robot cannot perform a rotation operation in the current pose, control the robot to move forward along the current heading direction and continuously detect the rotation feasibility during the movement to obtain the target pose that satisfies the rotation constraints. S8. When all target points in the target point distance set correspond to infeasible trajectories, adjust the linear velocity and return to step S3. S9. Smooth the final output linear velocity and angular velocity.

2. The adaptive path tracking method for a square differential robot based on target point driving as described in claim 1, characterized in that, In S1, the global path is represented as a sequence of path points. : in, Indicates the first Path points, This indicates the position coordinates of the path point in the global coordinate system. Indicates the total number of path points; The robot's current pose is represented as follows: in, Indicates the robot's pose. These represent the robot's position coordinates in the global coordinate system. Indicates the robot's heading angle; The environmental cost map is represented as a two-dimensional raster function: in, Indicates position The value of a location is used to characterize the distance relationship between that location and an obstacle or the risk of collision. The robot's geometric profile is represented by a rectangular model: in, This represents the geometric profile of the robot in its own coordinate system. Indicates the length of the robot. Indicates the robot's width. Represents the coordinates of a point in the robot's local coordinate system; The robot's motion satisfies a differential drive kinematic model.

3. The adaptive path tracking method for a square differential robot based on target point driving as described in claim 1, characterized in that, S3 specifically refers to: Construct a set of distances to target points based on the robot's current position along the global path. : in, This represents the path distance from the robot's current position to the m-th candidate target point along the path direction. This represents the number of candidate target points. Based on the target point distance set, and under the premise of satisfying feasibility constraints, target points are selected according to a preset priority rule, wherein the priority rule is to prioritize target points that are farther away from the robot's current position; The target point corresponds to the path function: in, Represents a path parameterization function. This indicates the robot's projected position on the path. Indicates candidate target points; The step of selecting a target point from the target point distance set according to a preset rule specifically involves selecting the maximum distance from the target point distance set. The maximum distance The corresponding target point is used as the current control target.

4. The adaptive path tracking method for a square differential robot based on target point driving as described in claim 1, characterized in that, S5 specifically refers to: Based on the linear velocity With angular velocity Discrete-time forward prediction is performed on the robot's motion trajectory, and the predicted trajectory is represented as a state sequence: in, Indicates the first Predicted pose of the step Indicates the number of prediction steps; The predicted pose is calculated recursively using a differential-driven kinematic model, and its update relationship is as follows: in, To control the cycle.

5. The adaptive path tracking method for a square differential robot based on target point driving as described in claim 1, characterized in that, In S6, the step of combining the environmental cost map to detect and predict whether the motion trajectory is an infeasible trajectory specifically involves: Based on the environmental cost map, collision detection is performed on the predicted trajectory. When any point in the predicted trajectory satisfies... When the time comes, the trajectory is determined to be an infeasible trajectory; in, Indicates position The value of the time, The collision detection threshold; The specific steps for updating the target point distance set are as follows: Remove the target point corresponding to the infeasible trajectory from the target point distance set; The target point distance set is represented as follows: in, Represents the set of distances to the target point. This represents the path distance from the robot's current position to the m-th candidate target point along the path direction. Indicates the number of candidate target points; When corresponding to a certain target point distance When the trajectory is not feasible, perform a set update operation: in, This represents the distance to the target point corresponding to the infeasible trajectory. " represents the set difference operation, that is, from set Remove elements .

6. The adaptive path tracking method for a square differential robot based on target point driving as described in claim 1, characterized in that, In S7, the step of performing rigid body collision detection based on the robot's rectangular geometric contour to determine the robot's rotation feasibility specifically involves: The robot's geometric profile is represented by a rectangular model as follows: in, This represents the geometric profile of the robot in its own coordinate system. Indicates the length of the robot. Indicates the robot's width. Represents the coordinates of a point in the robot's local coordinate system; The robot's pose in the global coordinate system Represented as: in, Indicates the robot's position coordinates. Indicates the heading angle; Based on the pose, the rectangular geometric contour is mapped to the global coordinate system through rigid body transformation, which can be expressed as: in, This represents the robot's geometric profile in the global coordinate system. Represents a two-dimensional rotation matrix; The rotation matrix is ​​represented as: During trajectory prediction, collision detection is performed on the geometric contour corresponding to each pose in the predicted trajectory. When the conditions are met... At that time, it is determined that the robot has collided at that pose; in, Indicates the first in the predicted trajectory Each position / pose This represents any point in the geometric contour. Indicates position The value of the time, This indicates the collision detection threshold.

7. The adaptive path tracking method for a square differential robot based on target point driving as described in claim 1, characterized in that, In S7, when it is detected that the robot cannot perform a rotation operation in the current pose, the robot is controlled to move forward along the current heading direction, and the feasibility of rotation is continuously detected during the movement to obtain a target pose that satisfies the rotation constraints. Specifically, this means: When it is detected that the robot's rotation operation is not feasible in the current pose, the feasibility recovery mechanism is triggered; The feasibility of rotation is expressed by a feasibility determination function as follows: in, Indicates the robot's pose. Indicates the feasibility of rotation; When satisfied At this time, the robot is controlled to make forward trial movements along the current heading direction, and its trial pose sequence is represented as follows: in, For the first The tentative stance of the tentative step. For the first The position coordinates of the trial step, Indicates the heading angle; in, , Indicates the forward exploratory step size. Indicates the maximum number of trial steps; During the trial process, the feasibility of rotation is determined for each trial pose. When the rotation operation is deemed feasible in the given pose, the robot is controlled to move to the corresponding pose and then the rotation is executed. When satisfied If it is determined that the current direction cannot be restored to rotational capability, the robot stops moving.

8. The adaptive path tracking method for a square differential robot based on target point driving as described in claim 1, characterized in that, S8 specifically refers to: When the target point distance set is empty, it is determined that there is no target point that satisfies the feasibility constraint under the current linear velocity conditions; When the target point distance set is empty, the linear velocity is adjusted, including proportionally reducing the linear velocity, with the update rule being: in, Indicates the current linear velocity. Denotes the speed reduction factor, and satisfies... .

9. The adaptive path tracking method for a square differential robot based on target point driving as described in claim 1, characterized in that, S9 specifically refers to: The linear velocity and angular velocity are smoothed based on jerk constraints. During online speed smoothing, the linear velocity, linear acceleration, and linear accelerometer at the current moment are defined as follows: , and Its update relationship is: in, Indicates the control period. Represents the discrete time step; The linear acceleration satisfies the following constraints: in, Indicates the maximum permissible linear jerk. This represents a saturation function, used to restrict the input to a given interval; For a smoothing process of angular velocity, angular velocity, angular acceleration, and angular jerk are defined as follows: , and Its update relationship is: in, Indicates the current angular velocity. Represents angular acceleration. Indicates angular jerk; The angular acceleration satisfies the following constraints: in, This indicates the maximum permissible angular jerk.

10. An adaptive path tracking system for a square differential robot based on target point-driven operation, characterized in that, include: The data acquisition module is used to acquire the global path, the robot's current pose, and the environmental cost map, wherein the robot is a differential drive mobile robot with a rectangular geometric profile. The linear velocity calculation module is used to calculate the robot's linear velocity based on the distance between the robot and obstacles, the curvature of the global path, and the deviation between the robot's current heading angle and the path direction. The target point selection module is used to construct a target point distance set on the global path and select target points from the target point distance set according to preset rules; The angular velocity calculation module is used to calculate the robot's angular velocity based on the directional relationship between the target point and the robot's current pose. The trajectory prediction module is used to predict the robot's motion trajectory forward based on the linear velocity and angular velocity. The first collision detection module is used to detect whether the predicted motion trajectory is an infeasible trajectory by combining the environmental cost map; when the predicted motion trajectory is determined to be an infeasible trajectory, the target point distance set is updated and the control quantity is recalculated. The second collision detection module is used to perform rigid body collision detection based on the rectangular geometric contour of the robot to determine the rotation feasibility of the robot. When it is detected that the robot cannot perform a rotation operation in the current pose, the robot is controlled to move forward along the current heading direction, and the rotation feasibility is continuously detected during the movement to obtain a target pose that meets the rotation constraints. The linear velocity adjustment module is used to adjust the linear velocity when all target points in the target point distance set correspond to infeasible trajectories. The smoothing module is used to smooth the final output linear velocity and angular velocity.

Images