Path tracking method, system, controller and readable storage medium based on look-ahead optimization and curvature adaptation

Abstract

The application relates to the technical field of path planning, and discloses a path tracking method and system based on prospective optimization and curvature self-adaption, a controller and a readable storage medium, the method comprising the following steps: acquiring the current pose and global path information of a target robot and transferring to a vehicle body coordinate system; calculating the real-time prospective distance of the target robot according to the global path information to determine a prospective point; calculating the position error and the angle error according to the prospective point, and calculating the real-time PID parameters according to the position error and the angle error; calculating the real-time path curvature according to the prospective point, determining the maximum linear velocity according to the real-time path curvature, and determining the current speed and the motion direction of the target robot according to the maximum linear velocity and the real-time PID parameters. Through the combination of the double PID controller and the pure tracking algorithm, the high-precision path tracking of the omnidirectional mobile robot is realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of path planning, in particular to a path tracking method and system based on look-ahead optimization and curvature self-adaptation, a controller and a computer readable storage medium. BACKGROUND

[0002] With the rapid development of mobile robot technology, omnidirectional mobile robots have been widely used in warehouse logistics, industrial inspection, service robots and other fields due to their flexible three-degree-of-freedom motion capability (lateral, longitudinal and rotation).

[0003] However, the existing method does not dynamically adjust the speed according to the local curvature of the path, resulting in trajectory deviation or mechanical wear due to excessive speed on high-curvature sections. At the same time, there is a lack of interpolation optimization of look-ahead points and real-time calculation of path curvature radius, limiting the path tracking accuracy.

[0004] Therefore, the prior art still needs to be improved and developed. SUMMARY

[0005] The main purpose of the present application is to provide a path tracking method and system based on look-ahead optimization and curvature self-adaptation, a controller and a computer readable storage medium, aiming to solve the problem of trajectory tracking on high-curvature sections in the prior art, lack of look-ahead optimization and curvature radius optimization, resulting in trajectory deviation and mechanical wear.

[0006] To achieve the above purpose, the present application provides a path tracking method based on look-ahead optimization and curvature self-adaptation, which comprises the following steps:

[0007] Obtain the current pose of the target robot and the global path information, and transfer the global path information to the vehicle coordinate system according to the current pose;

[0008] According to the global path information, calculate the real-time look-ahead distance of the target robot in the vehicle coordinate system, and determine the look-ahead point according to the real-time look-ahead distance;

[0009] Calculate the position error and angle error according to the look-ahead point, and calculate the real-time PID parameter according to the position error and the angle error;

[0010] Calculate the real-time path curvature according to the look-ahead point, determine the maximum linear speed according to the real-time path curvature, and determine the current speed and motion direction of the target robot according to the maximum linear speed and the real-time PID parameter.

[0011] Optionally, the path tracking method based on look-ahead optimization and curvature adaptation, wherein the current pose of the target robot and global path information are obtained, and the global path information is transferred to a vehicle body coordinate system according to the current pose, specifically comprising:

[0012] An initial coordinate of the target robot in a world coordinate system and global path information in the world coordinate system are obtained.

[0013] A current pose of the target robot is obtained, a vehicle body coordinate system is constructed, and the initial coordinate and the global path information are converted to the vehicle body coordinate system according to the current pose.

[0014] The current pose represents a conversion relationship of converting the world coordinate system to the vehicle body coordinate system.

[0015] Optionally, the path tracking method based on look-ahead optimization and curvature adaptation, wherein the real-time look-ahead distance of the target robot in the vehicle body coordinate system is calculated according to the global path information, and a look-ahead point is determined according to the real-time look-ahead distance, specifically comprising:

[0016] A preset parameter input by a user is obtained, and a real-time look-ahead distance is calculated according to a current linear velocity in the global path information and the preset parameter:

[0017]

[0018] wherein, the real-time look-ahead distance, the basic look-ahead distance, the velocity gain coefficient, the current linear velocity.

[0019] The look-ahead point is determined on the global path according to the real-time look-ahead distance.

[0020] Optionally, the path tracking method based on look-ahead optimization and curvature adaptation, wherein the real-time PID parameter includes an angular velocity control quantity and a linear velocity control quantity.

[0021] The position error and the angle error are calculated according to the look-ahead point, and the real-time PID parameter is calculated according to the position error and the angle error, specifically comprising:

[0022] The look-ahead point coordinate of the look-ahead point in the vehicle body coordinate system is determined according to the position of the look-ahead point on the global path.

[0023] The angle error is calculated according to the current course in the global path information and the target path direction, and the angular velocity control quantity is calculated according to the angle error.​

[0024] ;

[0025] wherein, represents an angular velocity control quantity, represents a proportion of a rotation PID, represents an integral of a rotation PID, represents a differential gain of a rotation PID, represents an angle error, represents a differential of ; represents a differential of a current time ;

[0026] According to the look-ahead point coordinates and initial coordinates, a position error is calculated, and the linear velocity control quantity is calculated in combination with the position error:

[0027] ;

[0028] wherein, represents a linear velocity control quantity, represents a proportion of a translation PID, represents an integral of a translation PID, represents a differential gain of a translation PID, represents a position error, represents a differential of .

[0029] Optionally, the path tracking method based on look-ahead optimization and curvature adaptation, wherein the look-ahead point coordinates of the look-ahead point in the vehicle body coordinate system are determined according to the position of the look-ahead point on the global path, specifically comprising:

[0030] According to the look-ahead point, a first coordinate of a first adjacent point and a second coordinate of a second adjacent point on the global path are determined;

[0031] wherein, the first adjacent point represents a point closest to the initial coordinate and less than the real-time look-ahead distance on the global path, and the second adjacent point represents a point closest to the initial coordinate and not less than the real-time look-ahead distance on the global path;

[0032] According to the look-ahead point, the first coordinate and the second coordinate, a look-ahead circle is constructed:

[0033] ;

[0034] wherein, represents an abscissa of a look-ahead point, represents an ordinate of a look-ahead point, represents the input horizontal coordinate for constructing the look-ahead circle, represents the input vertical coordinate for constructing the look-ahead circle;

[0035] According to the first coordinate and the second coordinate, a line segment between the first adjacent point and the second adjacent point is determined, and according to the look-ahead circle and the line segment, a look-ahead point coordinate is calculated:

[0036] ;

[0037] ;

[0038] ;

[0039] ;

[0040] ;

[0041] ;

[0042] wherein, represents the calculation process of the intersection of the look-ahead circle and the line segment, represents the horizontal coordinate of the first adjacent point, represents the vertical coordinate of the first adjacent point, represents the horizontal coordinate of the second adjacent point, represents the vertical coordinate of the second adjacent point, represents the span of the first adjacent point and the second adjacent point on the horizontal axis, represents the span of the first adjacent point and the second adjacent point on the vertical axis, represents the square of the distance between the first adjacent point and the second adjacent point, represents the length of the line segment.

[0043] Optionally, the path tracking method based on look-ahead optimization and curvature self-adaption, wherein the real-time path curvature is calculated according to the look-ahead point, the maximum linear velocity is determined according to the real-time path curvature, and the current speed and motion direction of the target robot are determined according to the maximum linear velocity and the real-time PID parameter, specifically comprising:

[0044] According to the look-ahead point coordinate, the first coordinate and the second coordinate, the curvature radius is calculated by combining the three-point circle fitting method:

[0045] ;

[0046] wherein, represents the curvature radius;

[0047] Determine the real-time path curvature according to the radius of curvature, and determine the maximum linear velocity in combination with the linear velocity control amount:

[0048] ;

[0049] wherein, represents the maximum linear velocity, represents the minimum speed ratio coefficient, represents the real-time path curvature, represents the minimum real-time path curvature, represents the maximum real-time path curvature, represents solving the maximum linear velocity;

[0050] Determine the value of the current speed of the target robot according to the maximum linear velocity, and determine the direction of the current speed of the target robot according to the angular velocity control amount.

[0051] Optionally, the path tracking method based on look-ahead optimization and curvature adaptation, wherein the real-time path curvature is calculated according to the look-ahead point, the maximum linear velocity is determined according to the real-time path curvature, and the current speed and motion direction of the target robot are determined according to the maximum linear velocity and the real-time PID parameter, and then further comprising:

[0052] Obtain the current position of the target robot, and perform environment perception according to the current position to obtain perception information;

[0053] Determine whether there is a collision risk in the current path of the target robot according to the perception information and the current speed;

[0054] If there is a collision risk, stop the task of the target robot.

[0055] In addition, in order to achieve the above-mentioned purpose, the application further provides a path tracking system based on look-ahead optimization and curvature adaptation, wherein the path tracking system based on look-ahead optimization and curvature adaptation comprises:

[0056] A coordinate conversion module is configured to obtain the current pose of the target robot and global path information, and convert the global path information into the vehicle body coordinate system according to the current pose;

[0057] A distance calculation module is configured to calculate the real-time look-ahead distance of the target robot in the vehicle body coordinate system according to the global path information, and determine the look-ahead point according to the real-time look-ahead distance;

[0058] A parameter calculation module is configured to calculate the position error and the angle error according to the look-ahead point, and calculate the real-time PID parameter according to the position error and the angle error;

[0059] a speed control module for calculating a real-time path curvature according to the look-ahead point, determining a maximum linear speed according to the real-time path curvature, and determining a current speed and a moving direction of the target robot according to the maximum linear speed and the real-time PID parameter.

[0060] In addition, to achieve the above object, the application further provides a controller, wherein the controller comprises a memory, a processor, and a look-ahead optimization and curvature adaptation based path tracking program stored in the memory and executable on the processor, and the look-ahead optimization and curvature adaptation based path tracking program implements the steps of the look-ahead optimization and curvature adaptation based path tracking method when executed by the processor.

[0061] In addition, to achieve the above object, the application further provides a computer readable storage medium, wherein the computer readable storage medium stores a look-ahead optimization and curvature adaptation based path tracking program, and the look-ahead optimization and curvature adaptation based path tracking program implements the steps of the look-ahead optimization and curvature adaptation based path tracking method when executed by a processor.

[0062] In the application, the current pose of a target robot and global path information are acquired, and the global path information is transferred into a vehicle body coordinate system according to the current pose; a real-time look-ahead distance of the target robot is calculated in the vehicle body coordinate system according to the global path information, and a look-ahead point is determined according to the real-time look-ahead distance; a position error and an angle error are calculated according to the look-ahead point, and a real-time PID parameter is calculated according to the position error and the angle error; a real-time path curvature is calculated according to the look-ahead point, a maximum linear speed is determined according to the real-time path curvature, and a current speed and a moving direction of the target robot are determined according to the maximum linear speed and the real-time PID parameter. The application realizes high-precision path tracking of an omnidirectional mobile robot by combining a double-PID controller with a pure pursuit algorithm. BRIEF DESCRIPTION OF DRAWINGS

[0063] Figure 1 is a flowchart of a preferred embodiment of the look-ahead optimization and curvature adaptation based path tracking method of the application;

[0064] Figure 2 is a control flowchart of a preferred embodiment of the look-ahead optimization and curvature adaptation based path tracking method of the application;

[0065] Figure 3 is a structure diagram of a preferred embodiment of the look-ahead optimization and curvature adaptation based path tracking system of the application;

[0066] Figure 4 is a structure diagram of a preferred embodiment of the controller of the application. DETAILED DESCRIPTION

[0067] In order to make the objects, technical solutions and advantages of the present application clearer and more apparent, the present application will be further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific examples described herein are only used to explain the present application and do not limit the present application.

[0068] The path tracking method based on look-ahead optimization and curvature adaptation described in the preferred embodiment of the present application, as shown in Figure 1 The path tracking method based on look-ahead optimization and curvature adaptation includes the following steps:

[0069] Step S10, the current pose of the target robot and the global path information are obtained, and the global path information is transferred to the vehicle coordinate system according to the current pose.

[0070] The traditional path tracking control method has problems such as complex parameter adjustment, insufficient dynamic response, and weak anti-interference ability when dealing with omni-directional motion, and the traditional PID (Proportional-Integral-Derivative) controller is prone to integral saturation and overshoot oscillation in a multi-degree-of-freedom coupled scene. Therefore, the present application relates to a PID control method for an omni-directional mobile robot. First, the planned path is transferred from the world coordinate system to the internal vehicle coordinate system of the robot through the robot pose, so as to balance the low-speed tracking accuracy and high-speed motion smoothness.

[0071] Specifically, the initial coordinates of the target robot in the world coordinate system and the global path information in the world coordinate system are obtained; the current pose of the target robot is obtained, a vehicle coordinate system is constructed, and the initial coordinates and the global path information are converted to the vehicle coordinate system according to the current pose; wherein the current pose represents the conversion relationship of converting the world coordinate system to the vehicle coordinate system.

[0072] As shown in Figure 2 The relationship between the world coordinate system and the vehicle coordinate system is maintained through the navigation framework, and both coordinate systems are three-dimensional rectangular coordinate systems with the horizontal axis forward, the vertical axis left, and the vertical axis upward. The current pose of the target robot represents the conversion relationship between the world coordinate system and the vehicle coordinate system, which is obtained by the PID controller to realize the conversion of the initial coordinates of the target robot, and further transfer the global path information from the world coordinate system to the vehicle coordinate system for subsequent calculation of the look-ahead distance and determination of the look-ahead point.

[0073] Step S20, according to the global path information, the real-time look-ahead distance of the target robot is calculated in the vehicle coordinate system, and the look-ahead point is determined according to the real-time look-ahead distance.

[0074] Specifically, the preset parameters input by the user are acquired, and the real-time look-ahead distance is calculated according to the current linear velocity in the global path information and the preset parameters:

[0075] ;

[0076] Wherein, The real-time look-ahead distance represents the distance between the current position of the target robot and the target point position at the current time. The basic look-ahead distance represents the distance between the current position of the target robot and the target point position at the current time. The speed gain coefficient represents the distance between the current position of the target robot and the target point position at the current time. The current linear velocity represents the distance between the current position of the target robot and the target point position at the current time.

[0077] The real-time look-ahead distance represents the distance between the current position of the target robot and the target point position at the current time. The existing pure tracking algorithm depends on a fixed look-ahead distance and cannot be dynamically adjusted according to the real-time speed of the robot, resulting in tracking lag at low speed and path cutting at high speed.

[0078] Step S30, according to the look-ahead point, the position error and the angle error are calculated, and the real-time PID parameters are calculated according to the position error and the angle error.

[0079] The real-time PID parameters include angular velocity control and linear velocity control. The PID channel for independent control of translation and rotation is used to suppress error accumulation through integral limiting mechanism, realize multi-degree-of-freedom motion decoupling and stable tracking, and reduce motion coupling interference.

[0080] Specifically, according to the position of the look-ahead point on the global path, the look-ahead point coordinates of the look-ahead point in the vehicle coordinate system are determined; the angle error is calculated according to the current course in the global path information and the target path direction, and the angular velocity control is calculated according to the angle error:

[0081] ;

[0082] Wherein, The angular velocity control represents the distance between the current position of the target robot and the target point position at the current time. The proportional of the rotation PID represents the distance between the current position of the target robot and the target point position at the current time. denotes the integral of the rotation PID, denotes the differential gain of the rotation PID, denotes the angle error, denotes the differential of the position error, denotes the differential of the position error, denotes the differential of the current time ; according to the lookahead point coordinates and the initial coordinates, a position error is calculated, and the linear velocity control quantity is calculated in combination with the position error:

[0083] ;

[0084] wherein, denotes the linear velocity control quantity, denotes the proportion of the translation PID, denotes the integral of the translation PID, denotes the differential gain of the translation PID, denotes the position error, denotes the differential of the position error.

[0085] For the PID control of the double channel, first, the motion state of the rotation dimension is adjusted. If the angle error is within the preset range (i.e., the target robot is on the target track, and the moving direction is consistent with the direction of the target track), the current track is kept unchanged. If the angle error is too large, the angular velocity control quantity is calculated accordingly, and the heading of the target robot is adjusted according to the angular velocity control quantity. Further, after adjusting the heading of the target robot, the speed of the target robot is calculated through the translation PID controller channel, so as to limit the speed.

[0086] Through the angle error and the position error, the integral anti-saturation mechanism is used to effectively suppress the error accumulation caused by external disturbance, so as to ensure that the target robot can quickly recover to stable tracking after speed fluctuation or temporary slipping. The lookahead distance is monitored in real time, based on the interpolation method and local path clipping, the unnecessary calculation overhead is reduced, and a speed output frequency of 500Hz can be realized.

[0087] Further, according to the lookahead point, a first coordinate of a first adjacent point and a second coordinate of a second adjacent point on the global path are determined; wherein the first adjacent point represents a point on the global path closest to the initial coordinate and less than the real-time lookahead distance, and the second adjacent point represents a point on the global path closest to the initial coordinate and not less than the real-time lookahead distance; according to the lookahead point, the first coordinate and the second coordinate, a lookahead circle is constructed:

[0088] ;

[0089] wherein,​ represents the horizontal coordinate of the look-ahead point, represents the ordinate of the look-ahead point, Indicates the horizontal coordinate input for constructing the look-ahead circle, represents the vertical coordinate input for constructing the look-ahead circle; based on the first coordinate and the second coordinate, determining the line segment between the first adjacent point and the second adjacent point, and calculating the look-ahead point coordinates based on the look-ahead circle and the line segment:

[0090] ;

[0091] ;

[0092] ;

[0093] ;

[0094] ;

[0095] ;

[0096] in, Represents the calculation process of the intersection of the look-ahead circle and the line segment, represents the horizontal coordinate of the first adjacent point, represents the ordinate of the first adjacent point, represents the horizontal coordinate of the second adjacent point, represents the ordinate of the second adjacent point, Indicates the span between the first adjacent point and the second adjacent point on the horizontal axis, Indicates the span between the first adjacent point and the second adjacent point on the vertical axis, represents the square of the distance between the first adjacent point and the second adjacent point, Indicates the length of the line segment.

[0097] Traditional PID controllers are designed for single-degree-of-freedom linear motion and fail to consider the multi-degree-of-freedom coupling characteristics of omnidirectional mobile robots. Their fixed gain parameters make it difficult to adapt to complex path curvature changes, leading to cumulative errors in the integral term (integral saturation), causing overshoot or oscillation, and significantly degrading performance in sharp turns or around dynamic obstacles. Therefore, the present invention independently calculates the target robot's motion state in the translational and rotational dimensions, and uses dynamic look-ahead distance and dual PID decoupling control to perform real-time path tracking on the robot. This significantly improves the robot's path tracking accuracy in both straight and curved paths, effectively reducing tracking deviations caused by the coupling effects of traditional single PID control. By decoupling lateral and longitudinal velocities and implementing a rotational alignment strategy, it is possible to calculate only the output translational velocity, ignoring the angular velocity, to meet the needs of different application scenarios.

[0098] Step S40, calculating real-time path curvature according to the foresight point, determining maximum linear velocity according to the real-time path curvature, and determining the current speed and motion direction of the target robot according to the maximum linear velocity and the real-time PID parameters.

[0099] The existing method does not dynamically adjust the speed according to the local curvature of the path, resulting in trajectory deviation or mechanical wear due to excessive speed on high curvature sections. The present application calculates the path curvature based on three-point circle fitting, automatically reduces the speed when turning sharply, and proportionally decays the speed when the curvature exceeds the set threshold to avoid path deviation caused by centrifugal force. By calculating the curvature of the target robot at the current time through three consecutive points on the global path, the maximum linear velocity allowed at the current time is limited, and the speed adjustment of the target robot is realized, which automatically reduces the speed on high curvature path sections to reduce the risk of trajectory deviation.

[0100] Specifically, the curvature radius is calculated according to the foresight point coordinates, the first coordinates and the second coordinates, combined with the three-point circle fitting method:

[0101] ;

[0102] wherein, the real-time path curvature is determined according to the curvature radius, and the maximum linear velocity is determined in combination with the linear velocity control amount:

[0103] ;

[0104] wherein, the maximum linear velocity is represented by Vmax, the minimum speed proportion coefficient is represented by Kmin, the real-time path curvature is represented by K, the minimum real-time path curvature is represented by Kmin, the maximum real-time path curvature is represented by Kmax, the maximum linear velocity is solved, and the current speed of the target robot is determined according to the value of the maximum linear velocity and the direction of the current speed of the target robot is determined according to the angular velocity control amount.

[0105] Wherein, the linear velocity control amount calculated by the PID controller may cause the linear velocity of the target robot to be less than the maximum linear velocity, or greater than the maximum linear velocity. This calculation result depends on the position error and the PID parameters set by the user, so through real-time curvature calculation, the linear velocity of the target robot is monitored and adjusted in real time, so that the robot can stop smoothly at the end position, and the oscillation near the end point caused by excessive speed and inertia is suppressed.

[0106] Furthermore, the current position of the target robot is obtained, and environmental perception is performed based on the current position to obtain perception information; based on the perception information and the current speed, it is determined whether there is a collision risk in the current path of the target robot; if there is a collision risk, the mission of the target robot is stopped.

[0107] After completing the target robot's path tracking, the safety decision module determines whether it can move. Based on the current speed and environmental perception information, it predicts the trajectory for three control cycles. If a collision risk is detected, the vehicle is immediately stopped to ensure safety. Simultaneously, a corresponding speed command is generated based on the PID controller and the current curvature. If the collision risk is resolved, the target robot continues to move according to the speed command until it completes the entire path control task.

[0108] The present invention realizes high-precision path tracking of an omnidirectional mobile robot by combining a dual PID controller with a pure tracking algorithm.

[0109] Furthermore, if Figure 3 As shown, based on the above-mentioned path tracking method based on forward optimization and curvature adaptation, the present invention also provides a path tracking system based on forward optimization and curvature adaptation, wherein the path tracking system based on forward optimization and curvature adaptation includes:

[0110] A coordinate conversion module 51 is used to obtain the current posture and global path information of the target robot, and transfer the global path information to the vehicle body coordinate system according to the current posture;

[0111] a distance calculation module 52 for calculating a real-time forward distance of the target robot in the vehicle coordinate system according to the global path information, and determining a forward point according to the real-time forward distance;

[0112] a parameter calculation module 53, configured to calculate a position error and an angle error according to the look-ahead point, and calculate a real-time PID parameter according to the position error and the angle error;

[0113] The speed control module 54 is used to calculate the real-time path curvature according to the look-ahead point, determine the maximum linear velocity according to the real-time path curvature, and determine the current speed and movement direction of the target robot according to the maximum linear velocity and the real-time PID parameters.

[0114] Furthermore, if Figure 4 As shown, based on the above-mentioned path tracking method and system based on forward-looking optimization and curvature adaptation, the present invention also provides a controller, which includes a processor 10, a memory 20 and a display 30. Figure 4Only some components of the controller are shown, but it should be understood that all of the components shown need not be implemented and that a greater or lesser number of components can be implemented.

[0115] The memory 20 can be an internal storage unit of the controller in some embodiments, such as a hard disk or a memory of the controller. The memory 20 can also be an external storage device of the controller in other embodiments, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) card, a Flash Card, etc. Further, the memory 20 can include both an internal storage unit and an external storage device of the controller. The memory 20 is used to store application software and various data installed on the controller, such as program codes of the controller, etc. The memory 20 can also be used to temporarily store data that has been output or will be output. In an embodiment, the memory 20 stores a path tracking program based on look-ahead optimization and curvature adaptation 40, which can be executed by the processor 10 to implement the path tracking method based on look-ahead optimization and curvature adaptation in the present application.

[0116] The processor 10 can be a Central Processing Unit (CPU), a microprocessor or other data processing chip in some embodiments, which is used to run program codes or process data stored in the memory 20, such as to execute the path tracking method based on look-ahead optimization and curvature adaptation, etc.

[0117] The display 30 can be an LED display, a liquid crystal display, a touch liquid crystal display, an OLED (Organic Light-Emitting Diode) touch, etc. in some embodiments. The display 30 is used to display information of the controller and to display a visualized user interface. The components of the controller communicate with each other through a system bus.

[0118] In an embodiment, the following steps are implemented when the processor 10 executes the path tracking program based on look-ahead optimization and curvature adaptation 40 in the memory 20:

[0119] Obtaining a current pose of a target robot and global path information, and transferring the global path information into a vehicle body coordinate system according to the current pose;

[0120] According to the global path information, calculating a real-time look-ahead distance of the target robot in the vehicle body coordinate system, and determining a look-ahead point according to the real-time look-ahead distance.

[0121] calculating a position error and an angle error according to the look-ahead point, calculating a real-time PID parameter according to the position error and the angle error;

[0122] calculating a real-time path curvature according to the look-ahead point, determining a maximum linear velocity according to the real-time path curvature, and determining a current velocity and a moving direction of the target robot according to the maximum linear velocity and the real-time PID parameter.

[0123] wherein the current pose of the target robot and global path information are obtained, and the global path information is transferred into a vehicle body coordinate system according to the current pose, and specifically comprising:

[0124] obtaining an initial coordinate of the target robot in a world coordinate system and global path information in the world coordinate system;

[0125] obtaining a current pose of the target robot, constructing a vehicle body coordinate system, and converting the initial coordinate and the global path information into the vehicle body coordinate system according to the current pose;

[0126] wherein the current pose represents a conversion relationship of converting the world coordinate system into the vehicle body coordinate system.

[0127] wherein the real-time look-ahead distance of the target robot in the vehicle body coordinate system is calculated according to the global path information, and a look-ahead point is determined according to the real-time look-ahead distance, and specifically comprising:

[0128] obtaining a preset parameter input by a user, and calculating a real-time look-ahead distance according to a current linear velocity in the global path information and the preset parameter:

[0129] ;

[0130] wherein, the real-time look-ahead distance is represented by L, the basic look-ahead distance is represented by L0, the velocity gain coefficient is represented by K, the current linear velocity is represented by v;

[0131] determining a look-ahead point on the global path according to the real-time look-ahead distance.

[0132] wherein the real-time PID parameter comprises an angular velocity control quantity and a linear velocity control quantity;

[0133] calculating a position error and an angle error according to the look-ahead point, calculating a real-time PID parameter according to the position error and the angle error, and specifically comprising:

[0134] determining a look-ahead point coordinate of the look-ahead point in the vehicle body coordinate system according to a position of the look-ahead point on the global path;

[0135] calculating an angle error according to a current course in the global path information and a target path direction, and calculating the angular velocity control quantity according to the angle error:

[0136] ;

[0137] wherein, the angular velocity control quantity is denoted as ω, a proportion of the rotation PID is denoted as Kp, an integral of the rotation PID is denoted as Ki, a differential gain of the rotation PID is denoted as Kd, the angle error is denoted as e, a differential of e is denoted as de / dt, a differential of the current time t is denoted as dt / dt; calculating a position error according to the look-ahead point coordinate and an initial coordinate, and calculating the linear velocity control quantity in combination with the position error:

[0138]

[0139] ;

[0140] wherein, the linear velocity control quantity is denoted as v, a proportion of the translation PID is denoted as Kp, an integral of the translation PID is denoted as Ki, a differential gain of the translation PID is denoted as Kd, the position error is denoted as e, a differential of e is denoted as de / dt.

[0141] wherein, the determining of the look-ahead point coordinate of the look-ahead point in the vehicle body coordinate system according to the position of the look-ahead point on the global path specifically comprises:

[0142] determining a first coordinate of a first adjacent point and a second coordinate of a second adjacent point on the global path according to the look-ahead point;

[0143] wherein, the first adjacent point represents a point closest to the initial coordinate and smaller than the real-time look-ahead distance on the global path, and the second adjacent point represents a point closest to the initial coordinate and not smaller than the real-time look-ahead distance on the global path;

[0144] constructing a look-ahead circle according to the look-ahead point, the first coordinate and the second coordinate:

[0145] ​​​ ;

[0146] wherein, represents the abscissa of the look-ahead point, represents the ordinate of the look-ahead point, represents the abscissa input for constructing the look-ahead circle, represents the ordinate input for constructing the look-ahead circle;

[0147] According to the first coordinate and the second coordinate, a line segment between the first adjacent point and the second adjacent point is determined, and according to the look-ahead circle and the line segment, a look-ahead point coordinate is calculated:

[0148] ;

[0149] ;

[0150] ;

[0151] ;

[0152] ;

[0153] ;

[0154] wherein, represents the calculation process of the intersection of the look-ahead circle and the line segment, represents the abscissa of the first adjacent point, represents the ordinate of the first adjacent point, represents the abscissa of the second adjacent point, represents the ordinate of the second adjacent point, represents the span of the first adjacent point and the second adjacent point on the abscissa axis, represents the span of the first adjacent point and the second adjacent point on the ordinate axis, represents the square of the distance of the first adjacent point and the second adjacent point, represents the length of the line segment.

[0155] According to the look-ahead point, the real-time path curvature is calculated, the maximum linear velocity is determined according to the real-time path curvature, and the current speed and motion direction of the target robot are determined according to the maximum linear velocity and the real-time PID parameter, specifically including:

[0156] According to the look-ahead point coordinate, the first coordinate and the second coordinate, the curvature radius is calculated by combining the three-point circle fitting method:

[0157] ;

[0158] wherein, represents a radius of curvature;

[0159] determining a real-time path curvature according to the radius of curvature, and determining a maximum linear velocity in combination with the linear velocity control quantity:

[0160]

[0161] wherein, represents a maximum linear velocity, represents a minimum velocity proportional coefficient, represents a real-time path curvature, represents a minimum real-time path curvature, represents a maximum real-time path curvature, represents solving the maximum linear velocity;

[0162] determining a value of a current velocity of the target robot according to the maximum linear velocity, and determining a direction of the current velocity of the target robot according to the angular velocity control quantity.

[0163] wherein, the real-time path curvature is calculated according to the look-ahead point, the maximum linear velocity is determined according to the real-time path curvature, and the current velocity and the motion direction of the target robot are determined according to the maximum linear velocity and the real-time PID parameter, and the method further comprises:

[0164] obtaining a current position of the target robot, and performing environment perception according to the current position to obtain perception information;

[0165] judging whether a current path of the target robot has a collision risk according to the perception information and the current velocity;

[0166] if the collision risk exists, stopping a task of the target robot.

[0167] The application further provides a computer readable storage medium, wherein the computer readable storage medium stores a path tracking program based on look-ahead optimization and curvature adaptation, and the path tracking program based on look-ahead optimization and curvature adaptation is executed by a processor to realize steps of the path tracking method based on look-ahead optimization and curvature adaptation.

[0168] ​In summary, the application provides a path tracking method based on prospective optimization and curvature self-adaption and related equipment, the method comprising: obtaining current pose and global path information of a target robot, and transferring the global path information to a vehicle body coordinate system according to the current pose; calculating a real-time prospective distance of the target robot in the vehicle body coordinate system according to the global path information, and determining a prospective point according to the real-time prospective distance; calculating a position error and an angle error according to the prospective point, and calculating real-time PID parameters according to the position error and the angle error; calculating a real-time path curvature according to the prospective point, determining a maximum linear velocity according to the real-time path curvature, and determining a current velocity and a movement direction of the target robot according to the maximum linear velocity and the real-time PID parameters. The application realizes high-precision path tracking of an omnidirectional mobile robot by combining a double-PID controller with a pure pursuit algorithm.

[0169] It should be noted that, in this document, the terms "comprising", "including", or any other variant thereof are intended to cover a non-exclusive inclusion, such that processes, methods, articles, or controllers that comprise a list of elements do not include only those elements recited, but also other elements not expressly listed or inherent to such processes, methods, articles, or controllers. Without more limitations, an element defined by the phrase "comprising a" does not exclude the existence of additional identical elements in the process, method, article, or controller that includes the element.

[0170] Of course, those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by a computer program instructing relevant hardware (such as a processor, a controller, etc.) to complete, and the program can be stored in a computer-readable computer-readable storage medium, and the program can include the processes of the above-mentioned method embodiments when executed. The computer-readable storage medium can be a memory, a magnetic disc, an optical disc, etc.

[0171] It should be understood that the application is not limited to the above examples, and those skilled in the art can make improvements or changes according to the above description, and all these improvements and changes should be within the protection scope of the claims of the application.

Claims

1. A path tracking method based on forward optimization and curvature adaptation, characterized in that: The path tracking method based on forward optimization and curvature adaptation includes: Acquire the current posture and global path information of the target robot, and transfer the global path information to the vehicle body coordinate system according to the current posture; Calculating a real-time forward distance of the target robot in the vehicle coordinate system according to the global path information, and determining a forward point according to the real-time forward distance; Calculating the real-time forward distance of the target robot in the vehicle coordinate system according to the global path information, and determining the forward point according to the real-time forward distance specifically includes: Obtain the preset parameters input by the user, and calculate the real-time forward distance based on the current linear speed in the global path information and the preset parameters: ； in, represents the real-time look-ahead distance, represents the basic look-ahead distance, represents the speed gain coefficient, Indicates the current linear speed; Determining a look-ahead point on a global path according to the real-time look-ahead distance; Real-time PID parameters include angular velocity control quantity and linear velocity control quantity; Calculating the position error and the angle error according to the look-ahead point, and calculating the real-time PID parameters according to the position error and the angle error, specifically includes: Determining the forward-looking point coordinates of the forward-looking point in the vehicle body coordinate system according to the position of the forward-looking point on the global path; According to the current route and target path direction in the global path information, the angle error is calculated, and the angular velocity control amount is calculated according to the angle error: ； in, represents the angular velocity control amount, Indicates the ratio of the rotation PID, represents the integral of the rotary PID, represents the differential gain of the rotary PID, represents the angle error, Express The differential of Indicates the current time The differential of According to the forward point coordinates and the initial coordinates, the position error is calculated, and the linear speed control amount is calculated in combination with the position error: ； in, Indicates the linear speed control value, Indicates the ratio of the translation PID, represents the integral of the translation PID, represents the differential gain of the translation PID, represents the position error, Express The differential of Calculate the position error and the angle error according to the look-ahead point, and calculate the real-time PID parameters according to the position error and the angle error; A real-time path curvature is calculated according to the look-ahead point, a maximum linear velocity is determined according to the real-time path curvature, and a current velocity and a movement direction of the target robot are determined according to the maximum linear velocity and the real-time PID parameters.

2. The path tracking method based on forward optimization and curvature adaptation according to claim 1, characterized in that: The step of obtaining the current posture and global path information of the target robot and transferring the global path information to the vehicle coordinate system according to the current posture specifically includes: Obtaining initial coordinates of the target robot in a world coordinate system and global path information in the world coordinate system; Acquire the current posture of the target robot, construct a vehicle body coordinate system, and convert the initial coordinates and the global path information into the vehicle body coordinate system according to the current posture; The current posture represents a conversion relationship from the world coordinate system to the vehicle coordinate system.

3. The path tracking method based on forward optimization and curvature adaptation according to claim 1, characterized in that: Determining the forward-looking point coordinates of the forward-looking point in the vehicle body coordinate system according to the position of the forward-looking point on the global path specifically includes: determining, on the global path, first coordinates of a first adjacent point and second coordinates of a second adjacent point according to the forward-looking point; The first adjacent point represents a point on the global path that is closest to the initial coordinates and is smaller than the real-time look-ahead distance, and the second adjacent point represents a point on the global path that is closest to the initial coordinates and is not smaller than the real-time look-ahead distance; Construct a forward-looking circle according to the forward-looking point, the first coordinate, and the second coordinate: ； in, represents the horizontal coordinate of the look-ahead point, represents the ordinate of the look-ahead point, Indicates the horizontal coordinate input for constructing the look-ahead circle, Indicates the vertical coordinate input for constructing the look-ahead circle; Determine a line segment between the first adjacent point and the second adjacent point based on the first coordinate and the second coordinate, and calculate the forward point coordinates based on the forward circle and the line segment: ； ； ； ； ； ； in, Represents the calculation process of the intersection of the look-ahead circle and the line segment, represents the horizontal coordinate of the first adjacent point, represents the ordinate of the first adjacent point, represents the horizontal coordinate of the second adjacent point, represents the ordinate of the second adjacent point, Indicates the span between the first adjacent point and the second adjacent point on the horizontal axis, Indicates the span between the first adjacent point and the second adjacent point on the vertical axis, represents the square of the distance between the first adjacent point and the second adjacent point, Indicates the length of the line segment.

4. The path tracking method based on forward optimization and curvature adaptation according to claim 3, characterized in that: The calculating of the real-time path curvature according to the look-ahead point, determining the maximum linear velocity according to the real-time path curvature, and determining the current velocity and movement direction of the target robot according to the maximum linear velocity and the real-time PID parameter specifically includes: The curvature radius is calculated based on the forward point coordinates, the first coordinates, and the second coordinates using a three-point circle fitting method: ； in, represents the radius of curvature; The real-time path curvature is determined according to the curvature radius, and the maximum linear speed is determined in combination with the linear speed control amount: ； in, Indicates the maximum line speed, represents the minimum speed proportional coefficient, represents the real-time path curvature, represents the minimum real-time path curvature, represents the maximum real-time path curvature, Indicates the maximum linear velocity to be solved; The value of the current velocity of the target robot is determined according to the maximum linear velocity, and the direction of the current velocity of the target robot is determined according to the angular velocity control amount.

5. The path tracking method based on forward optimization and curvature adaptation according to claim 1, characterized in that: The method further comprises calculating a real-time path curvature according to the look-ahead point, determining a maximum linear velocity according to the real-time path curvature, and determining a current velocity and a movement direction of the target robot according to the maximum linear velocity and the real-time PID parameter. Acquiring the current position of the target robot, performing environmental perception based on the current position, and obtaining perception information; Determining whether there is a collision risk on the current path of the target robot based on the perception information and the current speed; If the collision risk exists, the mission of the target robot is stopped.

6. A path tracking system based on forward optimization and curvature adaptation, characterized in that: The path tracking system based on forward optimization and curvature adaptation is applied to the path tracking method based on forward optimization and curvature adaptation according to any one of claims 1 to 5, and the path tracking system based on forward optimization and curvature adaptation includes: A coordinate conversion module is used to obtain the current posture and global path information of the target robot, and transfer the global path information to the vehicle body coordinate system according to the current posture; a distance calculation module, configured to calculate a real-time forward distance of the target robot in the vehicle coordinate system according to the global path information, and determine a forward point according to the real-time forward distance; a parameter calculation module, configured to calculate a position error and an angle error according to the look-ahead point, and calculate a real-time PID parameter according to the position error and the angle error; A speed control module is used to calculate the real-time path curvature according to the look-ahead point, determine the maximum linear velocity according to the real-time path curvature, and determine the current speed and movement direction of the target robot according to the maximum linear velocity and the real-time PID parameters.

7. A controller, characterized in that: The controller includes: a memory, a processor, and a path tracking program based on forward optimization and curvature adaptation stored in the memory and executable on the processor. When the path tracking program based on forward optimization and curvature adaptation is executed by the processor, the steps of the path tracking method based on forward optimization and curvature adaptation as described in any one of claims 1 to 5 are implemented.

8. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a path tracking program based on forward optimization and curvature adaptation. When the path tracking program based on forward optimization and curvature adaptation is executed by a processor, the steps of the path tracking method based on forward optimization and curvature adaptation as described in any one of claims 1 to 5 are implemented.

Images