Robot tracking control method, system, device and storage medium adapting to irregular discrete trajectory

Abstract

The present invention discloses a robot tracking control method, system, device, and storage medium that adapt to irregular discrete trajectories. The method belongs to the field of robotics. The method includes dynamically selecting discrete points based on changes in a reference trajectory to quantify changes in the curvature of the reference trajectory; dynamically selecting an appropriate prediction step size based on the changes in the curvature of the reference trajectory; discretizing the robot kinematic model to obtain a discrete kinematic model of the robot; defining a trajectory tracking error function and constructing a trajectory tracking objective function in combination with the discrete kinematic model of the robot; and solving the objective function to obtain a robot trajectory tracking controller that causes the robot to move along the reference trajectory, thereby achieving the trajectory tracking objective. The present invention provides a robot trajectory tracking control method with dynamic adaptability for irregularly changing discrete reference trajectories, which is beneficial for improving the robot's trajectory tracking accuracy and driving efficiency, and has important practical value.

Description

Technical Field

[0001] The present invention belongs to the field of robotics technology, and in particular relates to a robot tracking control method, system, device and storage medium that adapt to irregular discrete trajectories. Background Art

[0002] In recent years, with the development of robotics, wheeled mobile robots have been widely used in substation inspections. Substations are relatively closed environments, and their inspection trajectories are often irregular. It is difficult to describe the robot's reference trajectory using a continuous function, so a series of discrete points is often used instead.

[0003] Patent application number 202410991401.1, titled "Trajectory Tracking Control Method, System, Device, and Storage Medium for Improving Robot Cornering Accuracy and Inspection Efficiency," discloses discretizing the robot's kinematic model using the Euler forward difference method to obtain a discrete kinematic model. The method also defines a trajectory tracking error function and constructs a trajectory tracking objective function based on the robot's discrete kinematic model. The objective function is solved to obtain a predictive control-based robot trajectory tracking controller that tracks the robot's motion trajectory to a specified reference trajectory. When the robot is traveling on a straight reference trajectory, it is expected to inspect at maximum speed. When traveling on a curve with a large curvature, it is expected to reduce the inspection speed to improve cornering accuracy. However, a problem with the aforementioned patent is that when describing the discrete curvature of a reference point, the number of adjacent reference points selected is fixed at "L," and the predictive control step size is fixed at "1." This means that the robot's trajectory tracking control algorithm lacks the ability to adjust to irregular discrete reference trajectories, especially when the curvature of the curve varies irregularly, resulting in insufficient robot trajectory tracking accuracy. Summary of the Invention

[0004] The purpose of the present invention is to provide a robot tracking control method, system, device and storage medium that are adaptable to irregular discrete trajectories, so as to solve the problems that the existing technology cannot dynamically quantify the curvature changes of irregular discrete reference trajectories, the robot has a unique step length, lacks dynamic adaptability, and thus has low robot trajectory tracking accuracy.

[0005] The embodiment of the present application is implemented as follows: a robot tracking control method adapted to irregular discrete trajectories, comprising:

[0006] According to the changes of the reference trajectory, the number of discrete points is dynamically selected to quantify the curvature changes of the reference trajectory;

[0007] Dynamically select the appropriate prediction step size based on the curvature change of the reference trajectory;

[0008] The robot kinematic model is discretized using Euler forward difference method to obtain the robot discrete kinematic model;

[0009] Defining a trajectory tracking error function and constructing a trajectory tracking objective function in combination with the robot discrete kinematic model;

[0010] Solve the objective function to obtain a robot trajectory tracking controller, so that the robot moves according to the reference trajectory, thereby achieving the trajectory tracking goal.

[0011] Optionally, in some embodiments of the present application, the curvature of the reference trajectory for:

[0012] ;

[0013] in, is the index value, , is the number of discrete points selected dynamically, , Round to the nearest integer. is an integer, For discrete points To the string The distance, chord By point and Two points confirmed; and / or

[0014] Prediction step length The formula for dynamic selection is:

[0015] ;

[0016] in, is an integer; is an integer; is the number of discrete points selected dynamically, The value range is , Round to the nearest integer; and / or

[0017] The robot kinematic model is:

[0018] ;

[0019] Where, is the robot’s posture angle, is the linear velocity of the robot during motion, , is the steering angular velocity, For robots Direction and The velocity component in the direction, is the coordinate of the robot's center of mass in the moving plane, is the horizontal axis, is the vertical axis, The control quantity of the model is used, and the robot kinematic model is discretized using the Euler forward difference method. The obtained robot discrete kinematic model is:

[0020] ;

[0021] Where, is the sampling period, for The status information of the robot at all times, for The linear velocity of the robot at the moment, for The robot's steering angular velocity at the moment, for Status information of the robot at all times; and / or

[0022] Define the trajectory tracking error function for

[0023] ;

[0024] Where, , For the The horizontal coordinate of the actual motion trajectory of the discrete point robot With the reference trajectory horizontal coordinate Deviation; For the The vertical coordinate of the actual motion trajectory of the discrete point robot The vertical coordinate of the reference trajectory Deviation; For the The attitude angle of the actual motion trajectory of the robot at each discrete point Attitude angle with reference trajectory deviation.

[0025] Optionally, in some embodiments of the present application, the curvature of the reference trajectory The methods of obtaining include:

[0026] Dynamic selection discrete points to quantify the curvature change of the discrete point trajectory, where is an integer;

[0027] At discrete points Consider both the left and right sides discrete points, that is , ,in is an integer;

[0028] calculate Each discrete point within the range The average value of the coordinate information to get the mean point for:

[0029] ;

[0030] in, is the horizontal coordinate of each discrete point, is the ordinate of each discrete point;

[0031] connect and Two points determine a straight line , calculate in sequence , discrete points To the straight line distance And sum it up to get the offset of the discrete point trajectory for:

[0032] ;

[0033] Consider the size of the robot , then the relative offset of the discrete point trajectory is for:

[0034] ;

[0035] in, is an integer; is the size of the robot, which is a constant; The value range is ; is the offset of the discrete point trajectory;

[0036] At discrete points At, dynamically select discrete points The formula is as follows:

[0037] ;

[0038] in, is an integer, is the relative offset of the discrete point trajectory, is the scaling factor, The value range is ;

[0039] based on , using the cumulative distance from the point to the chord, the curvature of the reference trajectory for:

[0040] ;

[0041] in, is the index value, , For discrete points To the string The distance, chord By point and Two points confirmed.

[0042] Optionally, in some embodiments of the present application, based on the formula ;

[0043] Define the desired speed of the robot :

[0044] ;

[0045] in, For the robot at discrete points Expected driving speed; is the maximum speed of the robot; is the scaling factor, For discrete points curvature;

[0046] Defines the actual driving speed of the robot and the expected driving speed deviation :

[0047] ;

[0048] Assume that the current discrete points , , future prediction points, then the trajectory tracking objective function to be optimized is for:

[0049] ;

[0050] in, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot Expected driving speed The deviation, is the weight coefficient; is the index value; is the appropriate prediction step size selected dynamically; is the number of discrete points that make up the reference trajectory; is the Euclidean norm.

[0051] Optionally, in some embodiments of the present application, it is assumed that the current discrete points , , consider later points, then the trajectory tracking objective function to be optimized is for:

[0052] ;

[0053] in, is the weight matrix, defined as a diagonal matrix; is the linear speed of the robot and expected line speed The deviation, is the weight coefficient; is the index value; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; is the discrete point index value;

[0054] In the discrete points , , design robot trajectory tracking controller , so that the trajectory tracking objective function to be optimized Take the minimum value.

[0055] Optionally, in some embodiments of the present application, the method for obtaining a robot trajectory tracking controller includes:

[0056] Given by The reference trajectory of the robot consists of discrete points ;

[0057] Will get the controller Initialization of the parameters involved;

[0058] Make sure the robot is The trajectory tracking objective function to be optimized at all times ;

[0059] Minimize the objective function , get the controller ;

[0060] According to the controller ,Sure The robot's control input at each moment is used to update the robot's discrete kinematics model. The posture state at the moment;

[0061] Determine whether the robot has reached the last discrete point of the reference trajectory. If "yes", the loop ends; if "no", it goes to the next moment. , and jump to S53 to continue the loop.

[0062] Optionally, in some embodiments of the present application, the initialization parameters include a sampling period of the robot control , the reference trajectory of the robot , the robot's initial position , the robot's initial posture angle , maximum line speed , maximum steering angular velocity , Start a loop; and / or

[0063] The objective function for:

[0064] ;

[0065] in, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot Expected driving speed The deviation, is the weight coefficient; is the index value; is the appropriate prediction step size selected dynamically; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; and / or

[0066] ;

[0067] in, is the weight matrix, defined as a diagonal matrix; is the linear speed of the robot and expected line speed The deviation, is the weight coefficient; is the index value; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; and / or

[0068] To ensure the safety and controllability of the mobile robot trajectory tracking task, a controller is designed The value range of satisfies the constraints:

[0069] ;

[0070] Where, is the maximum speed of the robot, is the maximum steering angular velocity of the robot; Under the constraint of ,get Optimal control sequence at time , select The first two elements of the optimal control sequence at time As Moment-by-moment robot trajectory tracking controller.

[0071] Accordingly, the embodiment of the present application further provides a robot tracking and control system that is adaptable to irregular discrete trajectories, including:

[0072] A module for quantifying the curvature change of the reference trajectory is used to dynamically select discrete points according to the change of the reference trajectory to quantify the curvature change of the reference trajectory;

[0073] The prediction step size module is used to dynamically select the appropriate prediction step size according to the curvature change of the reference trajectory;

[0074] The robot discrete kinematics model module is used to discretize the robot kinematics model using the Euler forward difference method to obtain the robot discrete kinematics model;

[0075] A trajectory tracking objective function module is used to define a trajectory tracking error function and construct a trajectory tracking objective function in combination with the robot discrete kinematic model;

[0076] The trajectory tracking target module is used to solve the objective function and obtain the robot trajectory tracking controller to make the robot move according to the reference trajectory, thereby achieving the trajectory tracking target.

[0077] Accordingly, an embodiment of the present application also provides a computer device, including a storage and a processor, wherein the storage stores a computer program, and when the computer program is executed by the processor, the processor performs the steps of the above method.

[0078] Accordingly, an embodiment of the present application further provides a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, the processor executes the steps of the above method.

[0079] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are:

[0080] In this application, first, a method for dynamically selecting discrete points to quantify the change in reference trajectory curvature is proposed based on the change in the reference trajectory. This method has dynamic adaptability and can more accurately capture the curvature change of discrete points, which is beneficial to improving the robot's trajectory tracking accuracy. Secondly, a dynamic adjustment method for the prediction step size is proposed, which can dynamically select an appropriate prediction step size based on the change in the reference trajectory curvature, ensuring that the appropriate prediction step size is selected for trajectory tracking in different scenarios. This method has dynamic adaptability and is beneficial to improving the robot's trajectory tracking accuracy. Thirdly, the robot's kinematic model is discretized using the Euler forward difference method to obtain a discrete kinematic model of the robot. Then, a trajectory tracking error function is defined, and based on the dynamic discrete point curvature description method and the dynamic adjustment method of the prediction step size, the objective function of the trajectory tracking control problem is constructed in combination with model predictive control. Then, the trajectory tracking objective function is minimized to obtain a robot trajectory tracking controller. Finally, the effectiveness of the method is verified by simulation of two sets of irregular reference trajectories. The present invention provides a robot trajectory tracking control method with dynamic adaptability for irregularly changing discrete reference trajectories, which is beneficial to improving the robot's trajectory tracking accuracy and driving efficiency, and has important practical value. BRIEF DESCRIPTION OF THE DRAWINGS

[0081] Figure 1 The present invention provides a flow chart of a robot tracking control method that adapts to irregular discrete trajectories;

[0082] Figure 2 Schematic diagram of dynamically selecting discrete points for the present invention;

[0083] Figure 3 A flowchart of a method for obtaining a robot trajectory tracking controller according to the present invention;

[0084] Figure 4 This is the curve trajectory tracking diagram of the present invention;

[0085] Figure 5 This is the broken line trajectory tracking diagram of the present invention. DETAILED DESCRIPTION

[0086] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0087] The technical solution of this application is as follows:

[0088] First, see Figure 1 , an embodiment of the present application provides a robot tracking control method adapted to irregular discrete trajectories, comprising:

[0089] S1. According to the change of the reference trajectory, the number of discrete points is dynamically selected to quantify the curvature change of the reference trajectory;

[0090] S2. Dynamically select the appropriate prediction step size based on the curvature change of the reference trajectory;

[0091] S3. Discretize the robot kinematic model using the Euler forward difference method to obtain a discrete kinematic model of the robot;

[0092] S4. defining a trajectory tracking error function and constructing a trajectory tracking objective function in combination with the robot discrete kinematics model;

[0093] S5. Solve the objective function to obtain a robot trajectory tracking controller, so that the robot moves according to the reference trajectory, thereby achieving the trajectory tracking goal.

[0094] In the technical solution of this application, first, a method for dynamically selecting discrete points to quantify the change in reference trajectory curvature is proposed based on the change in the reference trajectory. This method has dynamic adaptability and can more accurately capture the curvature change of discrete points, which is beneficial to improving the robot's trajectory tracking accuracy. Secondly, a method for dynamically adjusting the prediction step size is proposed. It can dynamically select an appropriate prediction step size based on the change in the reference trajectory curvature, ensuring that the appropriate prediction step size is selected for trajectory tracking in different scenarios. This method has dynamic adaptability and is beneficial to improving the robot's trajectory tracking accuracy. Thirdly, the robot's kinematic model is discretized using the Euler forward difference method to obtain a discrete kinematic model of the robot. Then, a trajectory tracking error function is defined, and based on the dynamic discrete point curvature description method and the dynamic adjustment method of the prediction step size, the objective function of the trajectory tracking control problem is constructed in combination with model predictive control. Then, the trajectory tracking objective function is minimized to obtain a robot trajectory tracking controller. Finally, the effectiveness of this method is verified by simulation of two sets of irregular reference trajectories. The present invention provides a robot trajectory tracking control method with dynamic adaptability for irregularly changing discrete reference trajectories, which is beneficial to improving the robot's trajectory tracking accuracy and driving efficiency, and has important practical value.

[0095] When quantifying the curvature of the reference trajectory, the number of discrete points selected in this application is dynamically selected based on the changes in the reference trajectory and is not unique; this can better reflect the characteristics of the reference trajectory; specifically, in areas with small curvature or straight lines, the trajectory changes relatively smoothly, and selecting more discrete points can increase the smoothness of the data, making the quantization result closer to the true curvature; while in curved areas with larger curvatures, the trajectory changes greatly. If too many discrete points are selected, the trajectory changes of the curve may be over-smoothed, resulting in an inability to accurately reflect the true changes in the curvature of the curve.

[0096] When the robot of the present application performs predictive control, the prediction step size is dynamically adjusted and is not unique. This allows for better adaptation to different complex and changeable reference trajectories. Specifically, in areas with small curvature or flat straight lines, the trajectory changes relatively slowly, and there is no need to overly consider long-term trajectory changes. Choosing a smaller prediction step size can reduce prediction errors while reducing computational complexity. In curved areas with larger curvatures, the trajectory changes significantly, and it is necessary to predict trajectories over a longer timeframe to better cope with future changes. Choosing a larger prediction step size helps capture the overall changing trend of the curve, so that control measures can be taken in advance and the robot's cornering accuracy can be improved. Therefore, the method of the present application can better adapt to different changing reference trajectories and has more practical application value.

[0097] In S1:

[0098] In some embodiments, the curvature of the reference trajectory for:

[0099] ;

[0100] in, is the index value, , For discrete points To the string The distance, chord By point and Two points confirmed.

[0101] It can be understood that the curvature of the reference trajectory is the dynamic discrete point curvature , is the discrete point index value, , is the number of discrete points that make up the reference trajectory.

[0102] See also Figure 2 , further, the curvature of the reference trajectory The methods of obtaining include:

[0103] S11. Dynamic selection discrete points to quantify the curvature change of the discrete point trajectory, where is an integer;

[0104] S12, at discrete points Consider both the left and right sides discrete points, that is , ,in is an integer;

[0105] S13. Calculation Each discrete point within the range The average value of the coordinate information to get the mean point for:

[0106] ;

[0107] in, is the horizontal coordinate of each discrete point, is the ordinate of each discrete point;

[0108] S14. Connection and Two points determine a straight line , calculate in sequence , discrete points To the straight line distance And sum it up to get the offset of the discrete point trajectory for:

[0109] ;

[0110] Consider the size of the robot , then the relative offset of the discrete point trajectory is for:

[0111] ;

[0112] in, is an integer; is the size of the robot, which is a constant; The value range is ; is the offset of the discrete point trajectory;

[0113] S15, at discrete points At, dynamically select discrete points The formula is as follows:

[0114] ;

[0115] in, is an integer, is the relative offset of the discrete point trajectory, is the scaling factor, The value range is , Round off to the nearest integer;

[0116] S16, based on , using the cumulative distance from the point to the chord, the curvature of the reference trajectory for:

[0117] ;

[0118] in, is the index value, , For discrete points To the string The distance, chord By point and Two points confirmed.

[0119] It can be understood that according to the changes of the discrete points of the reference trajectory, the dynamic selection discrete points to quantify the curvature change of the discrete point trajectory.

[0120] Understandably, when , For discrete points Left side discrete points; when , For discrete points Right side discrete points.

[0121] I understand. Round off to the nearest integer.

[0122] I understand. is the scaling factor, used to change the relative offset Dynamically select discrete points degree of impact.

[0123] It can be understood from the formula , , , It can be seen that when the robot is traveling on a straight reference trajectory, the offset , ,at this time , choose the maximum number of discrete points To quantify the curvature change of the point; when the robot travels on a curve with large curvature, , ,at this time , choose to quantify the curvature change of the point with a smaller number of discrete points.

[0124] In S2:

[0125] In some embodiments, the prediction step size The formula for dynamic selection is:

[0126] ;

[0127] in, is an integer; is an integer; is the number of discrete points selected dynamically, The value range is , Round off to the nearest integer.

[0128] I understand. The value is related to the curvature change of the reference trajectory.

[0129] In the S3:

[0130] In some embodiments, the robot kinematic model is:

[0131] ;

[0132] Where, is the robot’s posture angle, is the linear velocity of the robot during motion, , is the steering angular velocity, For robots Direction and The velocity component in the direction, is the coordinate of the robot's center of mass in the moving plane, is the horizontal axis, is the vertical axis, is the control quantity of the model.

[0133] Furthermore, the robot kinematic model is discretized using the Euler forward difference method, and the obtained robot discrete kinematic model is:

[0134] ;

[0135] Where, is the sampling period, for The status information of the robot at all times, for The linear velocity of the robot at the moment, for The robot's steering angular velocity at the moment, for Status information of the robot at all times.

[0136] In said S4:

[0137] In some embodiments, a trajectory tracking error function is defined for

[0138] ;

[0139] Where, , For the The horizontal coordinate of the actual motion trajectory of the discrete point robot With the reference trajectory horizontal coordinate Deviation; For the The vertical coordinate of the actual motion trajectory of the discrete point robot The vertical coordinate of the reference trajectory Deviation; For the The attitude angle of the actual motion trajectory of the robot at each discrete point Attitude angle with reference trajectory deviation.

[0140] It can be understood that the reference trajectory is The reference attitude angle of discrete points The horizontal and vertical coordinates of the reference trajectory and Calculated.

[0141] Further, Here's how to get it:

[0142] make:

[0143] ;

[0144] Where, For the The horizontal coordinate of the robot reference trajectory With the The horizontal coordinate of the robot reference trajectory The difference between For the The vertical coordinate of the robot reference trajectory With the The vertical coordinate of the robot reference trajectory The difference between

[0145] but:

[0146] ;

[0147] Where, For the The horizontal coordinate of the robot reference trajectory With the The horizontal coordinate of the robot reference trajectory The difference between For the The vertical coordinate of the robot reference trajectory With the The vertical coordinate of the robot reference trajectory The difference.

[0148] Furthermore, based on the formula ;

[0149] Define the desired speed of the robot :

[0150] ;

[0151] in, For the robot at discrete points Expected driving speed; is the maximum speed of the robot; is the scaling factor, For discrete points curvature.

[0152] I understand. is the scaling factor, which is used to change the impact of curvature changes on the expected driving speed.

[0153] It can be understood that when the robot is traveling on a straight reference trajectory, , the robot is expected to travel at the maximum speed; when the robot travels on a curve with large curvature, , it is expected that the robot will reduce its driving speed, improve its cornering accuracy, and shorten the overall driving time.

[0154] Furthermore, we define the actual robot's driving speed. and the expected driving speed deviation :

[0155] ;

[0156] Assume that the current discrete points , , future prediction points, then the trajectory tracking objective function to be optimized is for:

[0157] ;

[0158] in, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot Expected driving speed The deviation, is the weight coefficient; is the index value; is the appropriate prediction step size selected dynamically; is the number of discrete points that make up the reference trajectory; is the Euclidean norm.

[0159] Furthermore, suppose that the current discrete points , , consider later points, then the trajectory tracking objective function to be optimized is for:

[0160] ;

[0161] in, is the weight matrix, defined as a diagonal matrix; is the linear speed of the robot and expected line speed The deviation, is the weight coefficient; is the index value; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; is the discrete point index value;

[0162] In the discrete points , , design robot trajectory tracking controller , so that the trajectory tracking objective function to be optimized Take the minimum value.

[0163] I understand. is the weight matrix, defined as a diagonal matrix whose diagonal elements reflect the objective function right The trajectory tracking error function is defined as exist Sensitivity to tracking error in direction.

[0164] I understand. is the weight coefficient, which reflects the sensitivity of speed control under different curvatures. The larger the value of , the more attention is paid to whether the robot speed changes according to the expected speed during robot trajectory tracking.

[0165] In said S05:

[0166] In some embodiments, a method for obtaining a robot trajectory tracking controller includes:

[0167] S51, given by The reference trajectory of the robot consists of discrete points ;

[0168] S52, will get the controller Initialization of the parameters involved;

[0169] S53, confirm that the robot is The trajectory tracking objective function to be optimized at all times ;

[0170] S54, minimizing and solving the objective function , get the controller ;

[0171] S55, according to the controller ,Sure The robot's control input at each moment is used to update the robot's discrete kinematics model. The posture state at the moment;

[0172] S56, determine whether the robot has reached the last discrete point of the reference trajectory, "yes", then end the loop; "no", then go to the next moment , and jump to S53 to continue the loop.

[0173] In the S52:

[0174] In some embodiments, the initialization parameters include the sampling period of the robot control , the reference trajectory of the robot , the robot's initial position , the robot's initial posture angle , maximum line speed , maximum steering angular velocity , Start the loop.

[0175] In said S53:

[0176] In some embodiments, the objective function for:

[0177] ;

[0178] in, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot Expected driving speed The deviation, is the weight coefficient; is the index value; is the appropriate prediction step size selected dynamically; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; or

[0179] ;

[0180] in, is the weight matrix, defined as a diagonal matrix; is the linear speed of the robot and expected line speed The deviation, is the weight coefficient; is the index value; is the number of discrete points that make up the reference trajectory; is the Euclidean norm.

[0181] In said S54:

[0182] In some embodiments, to ensure the safety and controllability of the mobile robot trajectory tracking task, a controller is designed The value range of satisfies the constraints:

[0183] ;

[0184] Where, is the maximum speed of the robot, is the maximum steering angular velocity of the robot.

[0185] Further, in Under the constraint of ,get Optimal control sequence at time , select The first two elements of the optimal control sequence at time As Moment-by-moment robot trajectory tracking controller.

[0186] In a second aspect, embodiments of the present application provide a robot tracking and control system that adapts to irregular discrete trajectories, including:

[0187] A module for quantifying the curvature change of the reference trajectory is used to dynamically select discrete points according to the change of the reference trajectory to quantify the curvature change of the reference trajectory;

[0188] The prediction step size module is used to dynamically select the appropriate prediction step size according to the curvature change of the reference trajectory;

[0189] The robot discrete kinematics model module is used to discretize the robot kinematics model using the Euler forward difference method to obtain the robot discrete kinematics model;

[0190] A trajectory tracking objective function module is used to define a trajectory tracking error function and construct a trajectory tracking objective function in combination with the robot discrete kinematic model;

[0191] The trajectory tracking target module is used to solve the objective function and obtain the robot trajectory tracking controller to make the robot move according to the reference trajectory, thereby achieving the trajectory tracking target.

[0192] In the curvature change module of the quantified reference trajectory:

[0193] In some embodiments, the curvature of the reference trajectory for:

[0194] ;

[0195] in, is the index value, , For discrete points To the string The distance, chord By point and Two points confirmed.

[0196] It can be understood that the curvature of the reference trajectory is the dynamic discrete point curvature , is the discrete point index value, , is the number of discrete points that make up the reference trajectory.

[0197] See also Figure 2 , further, the curvature of the reference trajectory The methods of obtaining include:

[0198] S11. Dynamic selection discrete points to quantify the curvature change of the discrete point trajectory, where is an integer;

[0199] S12, at discrete points Consider both the left and right sides discrete points, that is , ,in is an integer;

[0200] S13. Calculation Each discrete point within the range The average value of the coordinate information to get the mean point for:

[0201] ;

[0202] in, is the horizontal coordinate of each discrete point, is the ordinate of each discrete point;

[0203] S14. Connection and Two points determine a straight line , calculate in sequence , discrete points To the straight line distance And sum it up to get the offset of the discrete point trajectory for:

[0204] ;

[0205] Consider the size of the robot , then the relative offset of the discrete point trajectory is for:

[0206] ;

[0207] in, is an integer; is the size of the robot, which is a constant; The value range is ; is the offset of the discrete point trajectory;

[0208] S15, at discrete points At, dynamically select discrete points The formula is as follows:

[0209] ;

[0210] in, is an integer, is the relative offset of the discrete point trajectory, is the scaling factor, The value range is , Round off to the nearest integer;

[0211] S16, based on , using the cumulative distance from the point to the chord, the curvature of the reference trajectory for:

[0212] ;

[0213] in, is the index value, , For discrete points To the string The distance, chord By point and Two points confirmed.

[0214] It can be understood that according to the changes of the discrete points of the reference trajectory, the dynamic selection discrete points to quantify the curvature change of the discrete point trajectory.

[0215] Understandably, when , For discrete points Left side discrete points; when , For discrete points Right side discrete points.

[0216] I understand. Round off to the nearest integer.

[0217] I understand. is the scaling factor, used to change the relative offset Dynamically select discrete points degree of impact.

[0218] It can be understood from the formula , , , It can be seen that when the robot is traveling on a straight reference trajectory, the offset , ,at this time , choose the maximum number of discrete points To quantify the curvature change of the point; when the robot travels on a curve with large curvature, , ,at this time , choose to quantify the curvature change of the point with a smaller number of discrete points.

[0219] In the prediction step module:

[0220] In some embodiments, the prediction step size The formula for dynamic selection is:

[0221] ;

[0222] in, is an integer; is an integer; is the number of discrete points selected dynamically, The value range is , Round off to the nearest integer.

[0223] I understand. The value is related to the curvature change of the reference trajectory.

[0224] In the robot discrete kinematics model module:

[0225] In some embodiments, the robot kinematic model is:

[0226]

[0227] Where, is the robot’s posture angle, is the linear velocity of the robot during motion, , is the steering angular velocity, For robots Direction and The velocity component in the direction, is the coordinate of the robot's center of mass in the moving plane, is the horizontal axis, is the vertical axis, is the control quantity of the model.

[0228] Furthermore, the robot kinematic model is discretized using the Euler forward difference method, and the obtained robot discrete kinematic model is:

[0229] ;

[0230] Where, is the sampling period, for The status information of the robot at all times, for The linear velocity of the robot at the moment, for The robot's steering angular velocity at the moment, for Status information of the robot at all times.

[0231] In the trajectory tracking objective function module:

[0232] In some embodiments, a trajectory tracking error function is defined for

[0233] ;

[0234] Where, , For the The horizontal coordinate of the actual motion trajectory of the discrete point robot With the reference trajectory horizontal coordinate Deviation; For the The vertical coordinate of the actual motion trajectory of the discrete point robot The vertical coordinate of the reference trajectory Deviation; For the The attitude angle of the actual motion trajectory of the robot at each discrete point Attitude angle with reference trajectory deviation.

[0235] It can be understood that the reference trajectory is The reference attitude angle of discrete points The horizontal and vertical coordinates of the reference trajectory and Calculated.

[0236] Further, Here's how to get it:

[0237] make:

[0238] ;

[0239] Where, For the The horizontal coordinate of the robot reference trajectory With the The horizontal coordinate of the robot reference trajectory The difference between For the The vertical coordinate of the robot reference trajectory With the The vertical coordinate of the robot reference trajectory The difference between

[0240] but:

[0241] ;

[0242] Where, For the The horizontal coordinate of the robot reference trajectory With the The horizontal coordinate of the robot reference trajectory The difference between For the The vertical coordinate of the robot reference trajectory With the The vertical coordinate of the robot reference trajectory The difference.

[0243] Furthermore, based on the formula ;

[0244] Define the desired speed of the robot :

[0245] ;

[0246] in, For the robot at discrete points Expected driving speed; is the maximum speed of the robot; is the scaling factor, For discrete points curvature.

[0247] I understand. is the scaling factor, which is used to change the impact of curvature changes on the expected driving speed.

[0248] It can be understood that when the robot is traveling on a straight reference trajectory, , the robot is expected to travel at the maximum speed; when the robot travels on a curve with large curvature, , it is expected that the robot will reduce its driving speed, improve its cornering accuracy, and shorten the overall driving time.

[0249] Furthermore, we define the actual robot's driving speed. and the expected driving speed deviation :

[0250] ;

[0251] Assume that the current discrete points , , future prediction points, then the trajectory tracking objective function to be optimized is for:

[0252] ;

[0253] in, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot Expected driving speed The deviation, is the weight coefficient; is the index value; is the appropriate prediction step size selected dynamically; is the number of discrete points that make up the reference trajectory; is the Euclidean norm.

[0254] Furthermore, suppose that the current discrete points , , consider later points, then the trajectory tracking objective function to be optimized is for:

[0255] ;

[0256] in, is the weight matrix, defined as a diagonal matrix; is the linear speed of the robot and expected line speed The deviation, is the weight coefficient; is the index value; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; is the discrete point index value;

[0257] In the discrete points , , design robot trajectory tracking controller , so that the trajectory tracking objective function to be optimized Take the minimum value.

[0258] I understand. is the weight matrix, defined as a diagonal matrix whose diagonal elements reflect the objective function right The trajectory tracking error function is defined as exist Sensitivity to tracking error in direction.

[0259] I understand. is the weight coefficient, which reflects the sensitivity of speed control under different curvatures. The larger the value of , the more attention is paid to whether the robot speed changes according to the expected speed during robot trajectory tracking.

[0260] In the trajectory tracking target module:

[0261] In some embodiments, a method for obtaining a robot trajectory tracking controller includes:

[0262] S51, given by The reference trajectory of the robot consists of discrete points ;

[0263] S52, will get the controller Initialization of the parameters involved;

[0264] S53, confirm that the robot is The trajectory tracking objective function to be optimized at all times ;

[0265] S54, minimizing and solving the objective function , get the controller ;

[0266] S55, according to the controller ,Sure The robot's control input at each moment is used to update the robot's discrete kinematics model. The posture state at the moment;

[0267] S56, determine whether the robot has reached the last discrete point of the reference trajectory, "yes", then end the loop; "no", then go to the next moment , and jump to S53 to continue the loop.

[0268] In the S52:

[0269] In some embodiments, the initialization parameters include the sampling period of the robot control , the reference trajectory of the robot , the robot's initial position , the robot's initial posture angle , maximum line speed , maximum steering angular velocity , Start the loop.

[0270] In said S53:

[0271] In some embodiments, the objective function for:

[0272] ;

[0273] in, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot Expected driving speed The deviation, is the weight coefficient; is the index value; is the appropriate prediction step size selected dynamically; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; or

[0274] ;

[0275] in, is the weight matrix, defined as a diagonal matrix; is the linear speed of the robot and expected line speed The deviation, is the weight coefficient; is the index value; is the number of discrete points that make up the reference trajectory; is the Euclidean norm.

[0276] In said S54:

[0277] In some embodiments, to ensure the safety and controllability of the mobile robot trajectory tracking task, a controller is designed The value range of satisfies the constraints:

[0278] ;

[0279] Where, is the maximum speed of the robot, is the maximum steering angular velocity of the robot.

[0280] Further, in Under the constraint of ,get Optimal control sequence at time , select The first two elements of the optimal control sequence at time As Moment-by-moment robot trajectory tracking controller.

[0281] In a third aspect, the present application provides a computer device comprising a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of the robot tracking control method adapted to irregular discrete trajectories as described above.

[0282] The computer device may be a desktop computer, a notebook computer, a PDA, a cloud server, etc. The computer device may interact with the user via a keyboard, a mouse, a remote control, a touchpad, or a voice control device.

[0283] The memory includes at least one type of readable storage medium, including flash memory, hard disk, multimedia card, card-type memory (e.g., SD or D-interface display memory), random access memory (RAM), static random access memory (SRAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), programmable read-only memory (PROM), magnetic memory, magnetic disk, optical disk, etc. In some embodiments, the memory may be an internal storage unit of the computer device, such as the hard disk or internal memory of the computer device. In other embodiments, the memory may also be an external storage device of the computer device, such as a plug-in hard disk equipped with the computer device, a Smart Media Card (SMC), a Secure Digital (SD) card, a Flash Card, etc. Of course, the memory may also include both the internal storage unit of the computer device and its external storage device. In this embodiment, the memory is often used to store the operating system and various application software installed on the computer device, such as the program code of the robot tracking control method adapted to irregular discrete trajectories. In addition, the memory may also be used to temporarily store various types of data that have been output or are about to be output.

[0284] In some embodiments, the processor may be a central processing unit (CPU), a controller, a microcontroller, a microprocessor, or other data processing chip. The processor is generally used to control the overall operation of the computer device. In this embodiment, the processor is used to execute program code stored in the memory or process data, such as executing program code of a robot tracking control method adapted to irregular discrete trajectories.

[0285] In a fourth aspect, the present application provides a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, the processor executes the steps of the robot tracking control method adapted to irregular discrete trajectories as described above.

[0286] The computer-readable storage medium stores an interface display program, and the interface display program can be executed by at least one processor to enable the at least one processor to perform the steps of the robot tracking control method adapted to irregular discrete trajectories as described above.

[0287] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment method can be implemented by means of software plus the necessary general hardware platform, and of course it can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk), including a number of instructions for enabling a terminal device (which can be a mobile phone, computer, server or network device, etc.) to execute the robot tracking control method adapted to irregular discrete trajectories described in the embodiment of the present application.

[0288] The present invention is further described below in conjunction with simulation verification.

[0289] Simulation Verification

[0290] The present invention is directed to two different reference trajectories. The trajectory tracking simulation results are obtained in this environment.

[0291] Curve tracking simulation

[0292] Reference trajectory: , initialization parameters: , , , , , , , , , , , , ;

[0293] “^—” is the reference trajectory, “*--” is the tracking trajectory, and the simulation results are as follows Figure 4 shown

[0294] from Figure 4 It can be seen that the experimental simulation of curve trajectory tracking has achieved good results, proving the effectiveness of the algorithm for curve trajectory tracking.

[0295] Line tracking simulation

[0296] The reference trajectory is an irregular broken line trajectory, and the initialization parameters are: , , , , , , , , , , , , ; “^—” is the reference trajectory, “*--” is the tracking trajectory, the simulation results are as follows Figure 5 shown.

[0297] from Figure 5 It can be seen that the experiment achieved good results in simulating the tracking of irregularly changing broken line trajectories, proving the effectiveness of the algorithm in tracking broken line trajectories.

[0298] Figure 4 and Figure 5 The middle arrow indicates a local enlarged view of the indicated location.

[0299] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A robot tracking control method adapted to irregular discrete trajectories, characterized in that: The steps include: According to the changes of the reference trajectory, the number of discrete points is dynamically selected to quantify the curvature changes of the reference trajectory; Dynamically select the prediction step size based on the curvature change of the reference trajectory; The robot kinematic model is discretized using Euler forward difference method to obtain the robot discrete kinematic model; Defining a trajectory tracking error function and constructing a trajectory tracking objective function in combination with the robot discrete kinematic model; Solving the objective function to obtain a robot trajectory tracking controller, so that the robot moves along the reference trajectory, thereby achieving the trajectory tracking goal; Curvature of the reference trajectory for: ； in, is the index value, , is the number of discrete points selected dynamically, , Round to the nearest integer. is an integer, For discrete points To the string The distance, chord By point and Two points are confirmed; Prediction step length The formula for dynamic selection is: ； in, is an integer; is an integer; is the number of discrete points selected dynamically, The value range is , Round off to the nearest integer; The robot kinematic model is: ； Where, is the robot’s posture angle, is the linear velocity of the robot during motion, , is the steering angular velocity, For robots Direction and The velocity component in the direction, is the coordinate of the robot's center of mass in the moving plane, is the horizontal axis, is the vertical axis, As the control quantity of the model, the robot kinematic model is discretized using the Euler forward difference method. The obtained robot discrete kinematic model is: ； Where, is the sampling period, for The status information of the robot at all times, for The linear velocity of the robot at the moment, for The robot's steering angular velocity at the moment, for Status information of the robot at all times; Define the trajectory tracking error function for ； Where, , For the The horizontal coordinate of the actual motion trajectory of the discrete point robot With the reference trajectory horizontal coordinate Deviation; For the The vertical coordinate of the actual motion trajectory of the discrete point robot The vertical coordinate of the reference trajectory Deviation; For the The attitude angle of the actual motion trajectory of the robot at each discrete point Attitude angle with reference trajectory Deviation; Based on the formula ; Define the desired speed of the robot : ； in, For the robot at discrete points Expected driving speed; is the maximum speed of the robot; is the scaling factor, For discrete points curvature; Defines the actual driving speed of the robot and the expected driving speed deviation : ； Assume that the current discrete points , , future prediction points, then the trajectory tracking objective function to be optimized is for: ； in, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot Expected driving speed The deviation, is the weight coefficient; is the index value; is the dynamically selected prediction step size; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; Assume that the current discrete points , , consider later points, then the trajectory tracking objective function to be optimized is for: ； in, is the weight matrix, defined as a diagonal matrix; is the linear speed of the robot and expected line speed The deviation, is the weight coefficient; is the index value; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; is the discrete point index value; In the discrete points , , design robot trajectory tracking controller , so that the trajectory tracking objective function to be optimized Take the minimum value.

2. The robot tracking control method adapted to irregular discrete trajectories according to claim 1, characterized in that: Curvature of the reference trajectory The methods of obtaining include: Dynamic selection discrete points to quantify the curvature change of the discrete point trajectory, where is an integer; At discrete points Consider both the left and right sides discrete points, that is , ,in is an integer; calculate Each discrete point within the range The average value of the coordinate information to get the mean point for: ； in, is the horizontal coordinate of each discrete point, is the ordinate of each discrete point; connect and Two points determine a straight line , calculate in sequence , discrete points To the straight line distance And sum it up to get the offset of the discrete point trajectory for: ； Consider the size of the robot , then the relative offset of the discrete point trajectory is for: ； in, is an integer; is the size of the robot, which is a constant; The value range is ; is the offset of the discrete point trajectory; At discrete points At, dynamically select discrete points The formula is as follows: ； in, is an integer, is the relative offset of the discrete point trajectory, is the scaling factor, The value range is ; based on , using the cumulative distance from the point to the chord, the curvature of the reference trajectory for: ； in, is the index value, , For discrete points To the string The distance, chord By point and Two points confirmed.

3. The robot tracking control method adapted to irregular discrete trajectories according to claim 1, characterized in that: The method of obtaining the robot trajectory tracking controller includes: Given by The reference trajectory of the robot consists of discrete points ; Will get the controller Initialization of the parameters involved; Make sure the robot is The trajectory tracking objective function to be optimized at all times ; Minimize the objective function , get the controller ; According to the controller ,Sure The robot's control input at each moment is used to update the robot's discrete kinematics model. The posture state at the moment; Determine whether the robot has reached the last discrete point of the reference trajectory. If "yes", the loop ends; if "no", it goes to the next moment. , and jump to step 1 to confirm that the robot is The trajectory tracking objective function to be optimized at all times Continue the cycle.

4. The robot tracking control method adapted to irregular discrete trajectories according to claim 3, characterized in that: The initialization parameters include the sampling period of the robot control , the reference trajectory of the robot , the robot's initial position , the robot's initial posture angle , maximum line speed , maximum steering angular velocity , Start a loop; and / or The objective function for: ； in, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot Expected driving speed The deviation, is the weight coefficient; is the index value; is the dynamically selected prediction step size; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; and / or ； in, is the weight matrix, defined as a diagonal matrix; is the linear speed of the robot and expected line speed The deviation, is the weight coefficient; is the index value; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; and / or To ensure the safety and controllability of the mobile robot trajectory tracking task, a controller is designed The value range of satisfies the constraints: ； Where, is the maximum speed of the robot, is the maximum steering angular velocity of the robot; Under the constraint of ,get Optimal control sequence at time , select The first two elements of the optimal control sequence at time As Moment-by-moment robot trajectory tracking controller.

5. A robot tracking control system adapted to irregular discrete trajectories, characterized in that: The method according to any one of claims 1 to 4 is performed, wherein the system comprises: A module for quantifying the curvature change of the reference trajectory is used to dynamically select discrete points according to the change of the reference trajectory to quantify the curvature change of the reference trajectory; The prediction step size module is used to dynamically select the prediction step size according to the curvature change of the reference trajectory; The robot discrete kinematics model module is used to discretize the robot kinematics model using the Euler forward difference method to obtain the robot discrete kinematics model; A trajectory tracking objective function module is used to define a trajectory tracking error function and construct a trajectory tracking objective function in combination with the robot discrete kinematic model; The trajectory tracking target module is used to solve the objective function and obtain the robot trajectory tracking controller to make the robot move according to the reference trajectory, thereby achieving the trajectory tracking target; Curvature of the reference trajectory for: ； in, is the index value, , is the number of discrete points selected dynamically, , Round to the nearest integer. is an integer, For discrete points To the string The distance, chord By point and Two points are confirmed; Prediction step length The formula for dynamic selection is: ； in, is an integer; is an integer; is the number of discrete points selected dynamically, The value range is , Round off to the nearest integer; The robot kinematic model is: ； Where, is the robot’s posture angle, is the linear velocity of the robot during motion, , is the steering angular velocity, For robots Direction and The velocity component in the direction, is the coordinate of the robot's center of mass in the moving plane, is the horizontal axis, is the vertical axis, As the control quantity of the model, the robot kinematic model is discretized using the Euler forward difference method. The obtained robot discrete kinematic model is: ； Where, is the sampling period, for The status information of the robot at all times, for The linear velocity of the robot at the moment, for The robot's steering angular velocity at the moment, for Status information of the robot at all times; Define the trajectory tracking error function for ； Where, , For the The horizontal coordinate of the actual motion trajectory of the discrete point robot With the reference trajectory horizontal coordinate Deviation; For the The vertical coordinate of the actual motion trajectory of the discrete point robot The vertical coordinate of the reference trajectory Deviation; For the The attitude angle of the actual motion trajectory of the robot at each discrete point Attitude angle with reference trajectory Deviation; Based on the formula ; Define the desired speed of the robot : ； in, For the robot at discrete points Expected driving speed; is the maximum speed of the robot; is the scaling factor, For discrete points curvature; Defines the actual driving speed of the robot and the expected driving speed deviation : ； Assume that the current discrete points , , future prediction points, then the trajectory tracking objective function to be optimized is for: ； in, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot Expected driving speed The deviation, is the weight coefficient; is the index value; is the dynamically selected prediction step size; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; Assume that the current discrete points , , consider later points, then the trajectory tracking objective function to be optimized is for: ； in, is the weight matrix, defined as a diagonal matrix; is the linear speed of the robot and expected line speed The deviation, is the weight coefficient; is the index value; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; is the discrete point index value; In the discrete points , , design robot trajectory tracking controller , so that the trajectory tracking objective function to be optimized Take the minimum value.

6. Computer equipment, characterized in that The method comprises a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of the method according to any one of claims 1 to 4.

7. A computer-readable storage medium, characterized in that A computer program is stored, and when the computer program is executed by a processor, the processor is caused to perform the steps of the method according to any one of claims 1 to 4.

Images