Robot tracking control method, system and equipment suitable for irregular discrete trajectory and storage medium

Abstract

The invention discloses a robot tracking control method, system and device adapted to an irregular discrete trajectory, and a storage medium, and belongs to the technical field of robots, the method comprises the following steps: dynamically selecting discrete points according to the change of a reference trajectory to quantify the curvature change of the reference trajectory; according to the curvature change of the reference trajectory, dynamically selecting an appropriate prediction step length; discretizing the robot kinematic model to obtain a robot discrete kinematic model; defining a trajectory tracking error function, and constructing a trajectory tracking target function in combination with the robot discrete kinematics model; and solving the target function, obtaining a robot trajectory tracking controller, and enabling the robot to move according to a reference trajectory, thereby completing a trajectory tracking target. The robot trajectory tracking control method with the dynamic adaptive capacity is provided for discrete reference trajectories which change irregularly, the robot trajectory tracking precision and the driving efficiency can be improved, and the robot trajectory tracking control method has important practical value.

Description

Technical Field

[0001] The present invention belongs to the technical field of robotics, and particularly relates to a robot tracking control method, system, device, and storage medium adapted 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 enclosed application scenarios, and their inspection trajectories are often irregular paths, making it difficult to describe the reference trajectory of the robot with a continuous function. Instead, a series of discrete points are often used to describe it.

[0003] The patent with the application number 202410991401.1 and the patent name "Trajectory Tracking Control Method, System, Device, and Storage Medium for Improving Robot Cornering Accuracy and Inspection Efficiency" discloses discretizing the robot kinematic model using the Euler forward difference method to obtain the 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; solving the objective function to obtain a robot trajectory tracking controller based on predictive control, enabling the motion trajectory of the robot to track a specified reference trajectory. When the robot is traveling on a straight reference trajectory, it is expected that the robot will perform inspections at the maximum speed; when the robot is traveling on a large-curvature corner trajectory, it is expected that the robot will reduce the inspection speed to improve cornering accuracy. However, the problem with the above patent is that when describing the discrete curvature of a certain reference point, the number of other adjacent reference points selected is fixed at "L" points, and the robot predictive control step size is fixed at "1" step. For irregular discrete reference trajectories, especially in the case of irregularly changing corner curvatures, the trajectory tracking control algorithm of the robot lacks corresponding adjustment capabilities, resulting in insufficient trajectory tracking accuracy of the robot. Summary of the Invention

[0004] The purpose of the present invention is to provide a robot tracking control method, system, device, and storage medium adapted to irregular discrete trajectories to solve the problems that the prior art cannot dynamically quantify the curvature change of irregular discrete reference trajectories, the robot step size is unique, lacking dynamic adaptability, resulting in low trajectory tracking accuracy of the robot.

[0005] The embodiment of the present application is implemented as follows. The robot tracking control method adapted to irregular discrete trajectories includes: Dynamically select the number of discrete points according to the change of the reference trajectory to quantify the curvature change of the reference trajectory; Dynamically select an appropriate prediction step size according to the curvature change of the reference trajectory; Discretize the robot kinematic model using the Euler forward difference method to obtain the discrete kinematic model of the robot; Define the trajectory tracking error function, and construct a trajectory tracking objective function in combination with the discrete kinematic model of the robot; Solve the objective function to obtain a robot trajectory tracking controller, so that the robot moves along the reference trajectory, thereby achieving the trajectory tracking objective.

[0006] Optionally, in some embodiments of the present application, the curvature of the reference trajectory is: ; wherein, is an index value, , is the dynamically selected number of discrete points, , rounds to an integer, is an integer, is the discrete point to the chord distance, the chord is determined by the points and two points; and / or The formula for dynamically selecting the prediction step is: ; wherein, is an integer; is an integer; is the dynamically selected number of discrete points, has a value range of , rounds to an integer; and / or The kinematic model of the robot is: ; In the formula, is the attitude angle of the robot, is the linear velocity during the movement of the robot, , is the steering angular velocity, is the velocity component of the robot in the direction and direction, is the coordinate of the robot's center of mass in the moving plane, is the abscissa, is the ordinate, is the control quantity of the model. The robot kinematic model is discretized using the Euler forward difference method, and the obtained discrete kinematic model of the robot is: ; In the formula, 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 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.

[0007] Optionally, in some embodiments of the present application, the 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 this straight line distance and sum them up to obtain the offset of the discrete point trajectory which is: ; Considering the size of the robot is , then the relative offset of the discrete point trajectory is: ; wherein, is an integer; is the size of the robot and is a fixed constant; the value range of ; is the offset of the discrete point trajectory; At the discrete point , the formula for dynamically selecting the discrete point is as follows: ; wherein, is an integer, is the relative offset of the discrete point trajectory, is the scaling coefficient, the value range of ; Based on , using the cumulative distance from a point to a chord, the curvature of the reference trajectory is: ; wherein, is the index value, , is the distance from the discrete point to the chord , and the chord is determined by the two points and .

[0008] Optionally, in some embodiments of the present application, based on the formula ; define the expected speed of the robot: ; wherein, is the expected driving speed of the robot at the discrete point ; is the maximum driving speed of the robot; is the scaling coefficient, is the discrete point The curvature; Define the actual driving speed of the robot and the desired driving speed deviation : ; Assume that currently at the th discrete point , , predict forward points, then the trajectory tracking objective function to be optimized is: ; Among them, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot and the deviation of the desired driving speed , is the weight coefficient; is the index value; is the appropriately selected dynamic prediction step size; is the number of discrete points that make up the reference trajectory; is the Euclidean norm.

[0009] Optionally, in some embodiments of the present application, assume that currently at the th discrete point , , consider forward points, then the trajectory tracking objective function to be optimized is: ; Among them, is the weight matrix, defined as a diagonal matrix; is the linear velocity of the robot and the deviation of the desired linear velocity , 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; At the th discrete point , , design the robot trajectory tracking controller , so that the trajectory tracking objective function to be optimized takes the minimum value.

[0010] Optionally, in some embodiments of the present application, the method for obtaining a 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 S53 to continue the loop.

[0011] 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 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 appropriate prediction step size selected dynamically; 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 The deviation from the expected linear velocity 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 safe and controllable trajectory tracking task of the mobile robot, a controller is designed The value range of satisfies the constraint: ; In the formula, is the maximum driving speed of the robot, is the maximum steering angular velocity of the robot; Under the constraint of minimize and solve the objective function to obtain the optimal control sequence at time Select the first two elements of the optimal control sequence at time as the robot trajectory tracking controller at time

[0012] Correspondingly, the embodiment of the present application also provides a robot tracking control system adapted to an irregular discrete trajectory, including: A curvature change module for quantifying the reference trajectory, which is used to dynamically select the number of discrete points according to the change of the reference trajectory to quantify the curvature change of the reference trajectory; A prediction step module, which is used to dynamically select an appropriate prediction step according to the curvature change of the reference trajectory; A robot discrete kinematic model module, which is used to discretize the robot kinematic model by the Euler forward difference method to obtain the robot discrete kinematic model; A trajectory tracking objective function module, which is used to define a trajectory tracking error function and construct a trajectory tracking objective function in combination with the robot discrete kinematic model; A trajectory tracking objective module, which is used to solve the objective function to obtain a robot trajectory tracking controller, so that the robot moves according to the reference trajectory, thereby completing the trajectory tracking objective.

[0013] Correspondingly, the embodiment of the present application also provides a computer device, including a storage and a processor. When the computer program stored in the storage is executed by the processor, the processor executes the steps of the above method.

[0014] Correspondingly, the embodiment of the present application also provides a computer-readable storage medium, storing a computer program, which when executed by a processor, causes the processor to execute the steps of the above method.

[0015] In summary, due to the adoption of the above technical solutions, the beneficial effects of the present invention are as follows: In this application, first, according to the change of the reference trajectory, a method for dynamically selecting the number of discrete points to quantify the curvature change of the reference trajectory is proposed. It has dynamic adaptability, can capture the curvature change of discrete points more accurately, and is beneficial to improving the robot trajectory tracking accuracy. Second, a method for dynamically adjusting the prediction step length is proposed, which can dynamically select an appropriate prediction step length according to the change of the reference trajectory curvature, ensure that an appropriate prediction step length is selected for trajectory tracking in different scenarios, has dynamic adaptability, and is beneficial to improving the robot trajectory tracking accuracy. Third, the Euler forward difference method is used to discretize the kinematic model of the robot to obtain the 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 length, the objective function of the trajectory tracking control problem is constructed by combining model predictive control. Next, by minimizing and solving the trajectory tracking objective function, a robot trajectory tracking controller is obtained. Finally, through simulations with two groups of irregular reference trajectories, the effectiveness of this method is verified. 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 trajectory tracking accuracy and driving efficiency and has important practical value. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 is a flowchart of the robot tracking control method provided by the present invention for adapting to irregular discrete trajectories; Figure 2 is a schematic diagram of dynamically selecting discrete points according to the present invention; Figure 3 is a flowchart of the method for obtaining the robot trajectory tracking controller according to the present invention; Figure 4 is a curve trajectory tracking diagram according to the present invention; Figure 5 is a broken line trajectory tracking diagram according to the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0017] In order to make the objectives, technical solutions and advantages of the present invention clearer, 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 used to limit the present invention.

[0018] The technical solution of this application is as follows: In the first aspect, please refer to Figure 1 , the embodiments of this application provide a robot tracking control method for adapting to irregular discrete trajectories, including: S1. Dynamically select the number of discrete points according to the change of the reference trajectory to quantify the curvature change of the reference trajectory; S2. Dynamically select an appropriate prediction step size according to the curvature change of the reference trajectory; S3. Discretize the kinematic model of the robot using the Euler forward difference method to obtain the discrete kinematic model of the robot; S4. Define a trajectory tracking error function and construct a trajectory tracking objective function in combination with the discrete kinematic model of the robot; S5. Solve the objective function to obtain a robot trajectory tracking controller, enabling the robot to move along the reference trajectory, thereby achieving the trajectory tracking objective.

[0019] In the technical solution of this application, first, according to the change of the reference trajectory, a method for dynamically selecting the number of discrete points to quantify the curvature change of the reference trajectory is proposed. It has dynamic adaptability, can capture the curvature change of discrete points more accurately, and is beneficial to improving the robot trajectory tracking accuracy. Second, a method for dynamically adjusting the prediction step size is proposed, which can dynamically select an appropriate prediction step size according to the change of the curvature of the reference trajectory, ensuring that an appropriate prediction step size is selected for trajectory tracking in different scenarios, having dynamic adaptability, and being beneficial to improving the robot trajectory tracking accuracy. Third, the Euler forward difference method is used to discretize the kinematic model of the robot to obtain the 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, a trajectory tracking control problem objective function is constructed in combination with model predictive control. Next, by minimizing and solving the trajectory tracking objective function, a robot trajectory tracking controller is obtained. Finally, through simulations with two groups of irregular reference trajectories, the effectiveness of this method is verified. 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 trajectory tracking accuracy and driving efficiency and has important practical value.

[0020] When quantifying the curvature of the reference trajectory in this application, the number of discrete points selected is dynamically selected according to the change of the reference trajectory and is not unique; this can better reflect the characteristics of the reference trajectory. Specifically, in the small curvature or straight line area, the trajectory change is relatively gentle. Selecting more discrete points can increase the smoothness of the data and make the quantization result closer to the true curvature. In the curved area with a large curvature, the trajectory change amplitude is large. If too many discrete points are selected, it may over-smooth the trajectory change of the curve, resulting in an inability to accurately reflect the true change of the curve curvature.

[0021] 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.

[0022] In S1: In some embodiments, 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.

[0023] 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.

[0024] See also Figure 2 , further, the curvature of the reference trajectory The methods of obtaining include: S11. Dynamic selection discrete points to quantify the curvature change of the discrete point trajectory, where is an integer; S12, at discrete points Consider both the left and right sides discrete points, that is , ,in is an integer; S13. Calculation Each discrete point within the range The average value of the coordinate information to get the mean point for: ; wherein, is the abscissa of each discrete point, is the ordinate of each discrete point; S14. Connect and to determine a straight line , and successively calculate , the distance from the discrete point to this straight line and sum them up to obtain the offset of the discrete point trajectory as: ; Considering the size of the robot is , then the relative offset of the discrete point trajectory is: ; wherein, is an integer; is the size of the robot and is a fixed constant; The value range of is ; is the offset of the discrete point trajectory; S15. At the discrete point , the formula for dynamically selecting the discrete point is as follows: wherein, is an integer, is the relative offset of the discrete point trajectory, is the scaling factor, The value range of is , S16. Based on , using the cumulative amount of the distance from a point to a chord, the curvature of the reference trajectory is: ; wherein, is the index value, , is the distance from the discrete point to the chord , and the chord is determined by the two points .

[0025] It can be understood that according to the change of the discrete points of the reference trajectory, dynamically select Quantify the curvature change of the discrete point trajectory with a discrete point.

[0026] It can be understood that when , is the th discrete point to the left of the discrete point; when , , is the th discrete point to the right of the discrete point.

[0027] It can be understood that Round off and take an integer.

[0028] It can be understood that is the scaling factor used to change the influence degree of the relative offset on the dynamically selected number of discrete points .

[0029] It can be understood that from the formula , , , it can be seen that when the robot is moving on a straight reference trajectory, the offset , , and at this time , select the maximum number of discrete points to quantify the curvature change of this point; when the robot is moving on a trajectory with a large curvature bend, , , and at this time , select a smaller number of discrete points to quantify the curvature change of this point.

[0030] In the above-mentioned S2: In some embodiments, the formula for dynamically selecting the prediction step is: ; where is an integer; is an integer; is the dynamically selected number of discrete points, and the value range of is , Round off and take an integer.

[0031] It can be understood that the value of

[0032] is related to the curvature change of the reference trajectory. In some embodiments, the kinematic model of the robot is: ; wherein, is the attitude angle of the robot, is the linear velocity during the movement of the robot, , is the angular velocity of turning, is the robot at direction and the velocity component in the direction, is the coordinate of the robot's center of mass in the moving plane, is the abscissa, is the ordinate, is the control quantity of the model.

[0033] Furthermore, the kinematic model of the robot is discretized by the Euler forward difference method, and the discrete kinematic model of the robot obtained is: ; wherein, is the sampling period, is the state information of the robot at time,<> is the linear velocity of the robot at time, is the angular velocity of turning of the robot at time, is the state information of the robot at time.

[0034] In the said S4: In some embodiments, the trajectory tracking error function is defined as ; wherein, , is at the abscissa of the actual motion trajectory of the robot at the discrete point and the deviation of the abscissa of the reference trajectory ; is at the ordinate of the actual motion trajectory of the robot at the discrete point and the deviation of the ordinate of the reference trajectory ; is at the attitude angle of the actual motion trajectory of the robot at the discrete point and the deviation of the attitude angle of the reference trajectory .

[0035] It can be understood that the reference attitude angle of the reference trajectory at the discrete point is determined by the abscissa and ordinate of the reference trajectory and Calculated.

[0036] Furthermore, Here's how to get it: make: ; 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 but: ; 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.

[0037] Furthermore, 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.

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

[0039] 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 reduces its speed, improves cornering accuracy, and shortens the overall driving time.

[0040] Furthermore, define the actual driving speed of the robot and the expected driving speed deviation : ; Assume that currently at the th discrete point , , and predict points forward, then the trajectory tracking objective function to be optimized is: ; where, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot and the deviation of the expected driving speed , is the weight coefficient; is the index value; is the appropriately selected dynamic prediction step size; is the number of discrete points that make up the reference trajectory; is the Euclidean norm.

[0041] Furthermore, assume that currently at the th discrete point , , and consider points forward, then the trajectory tracking objective function to be optimized is: ; where, is the weight matrix, defined as a diagonal matrix; is the linear velocity of the robot and the deviation of the expected linear velocity , 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; At the th discrete point , , design the robot trajectory tracking controller such that the trajectory tracking objective function to be optimized takes the minimum value.

[0042] It can be understood that is the weight matrix, defined as a diagonal matrix, and the magnitudes of the elements on its diagonal reflect the objective function for the defined trajectory tracking error function in the direction, the sensitivity of the tracking error.

[0043] It can be understood that is the weight coefficient, which reflects the sensitivity of speed control under different curvatures. The larger the value of

[0044] In the above S05: In some embodiments, the method for obtaining a robot trajectory tracking controller includes: S51. Given a reference trajectory of the robot composed of discrete points ; S52. Initialize the parameters involved in obtaining the controller ; S53. Determine the trajectory tracking objective function to be optimized for the robot at time ; S54. Minimize and solve the objective function to obtain the controller ; S55. According to the controller determine the control input of the robot at time , and update the pose state of the robot at S56. Judge whether the robot has reached the last discrete point of the reference trajectory. If "yes", end the loop; if "no", go to the next time and jump to S53 to continue the loop.

[0045] In the above S52: In some embodiments, the initialized parameters include the sampling period for robot control, the reference trajectory of the robot, the initial position of the robot, the initial attitude angle of the robot, the maximum linear velocity of the robot, the maximum angular velocity of turning of the robot, and start the loop.

[0046] In the above S53: In some embodiments, the objective function is: ; wherein, is a weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot and the desired driving speed deviation, is a weight coefficient; is an index value; is a dynamically selected appropriate prediction step size; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; or ; wherein, is a weight matrix, defined as a diagonal matrix; is the linear velocity of the robot and the desired linear velocity deviation, is a weight coefficient; is an index value; is the number of discrete points that make up the reference trajectory; is the Euclidean norm.

[0047] In the S54: In some embodiments, to ensure the safe and controllable trajectory tracking task of the mobile robot, the controller is designed to satisfy the constraint of the value range: ; In the formula, is the maximum driving speed of the robot, is the maximum steering angular velocity of the robot.

[0048] Furthermore, under the constraint of minimize and solve the objective function , to obtain the optimal control sequence at time , select the first two elements of the optimal control sequence at time as the robot trajectory tracking controller at time.

[0049] Second, the embodiments of the present application provide a robot tracking control system adapted to an irregular discrete trajectory, including: A curvature change module for quantifying the reference trajectory, which is used to dynamically select the number of discrete points according to the change of the reference trajectory to quantify the curvature change of the reference trajectory; The prediction step module is used to dynamically select an appropriate prediction step according to the curvature change of the reference trajectory; The robot discrete kinematic model module is used to discretize the robot kinematic model by the forward Euler difference method to obtain the robot discrete kinematic model; The 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 objective module is used to 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 objective.

[0050] In the module for quantifying the curvature change of the reference trajectory: In some embodiments, the curvature of the reference trajectory is: ; where is the index value, , is the distance from the discrete point to the chord , and the chord is determined by the two points and .

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

[0052] Please refer to Figure 2 , further, the method for obtaining the curvature of the reference trajectory includes: S11. Dynamically select discrete points to quantify the curvature change of the discrete point trajectory, where is an integer; S12. Consider discrete points on both the left and right sides of the discrete point , that is, , , where is an integer; S13. Calculate the average value of the coordinate information of each discrete point within the range to obtain the mean point as: ; wherein, is the abscissa of each discrete point, is the ordinate of each discrete point; S14. Connect and to determine a straight line , and calculate , the distance from the discrete point to this straight line and sum them up to obtain the offset of the discrete point trajectory as: ; Considering the size of the robot is , then the relative offset of the discrete point trajectory is: ; wherein, is an integer; is the size of the robot, which is a fixed constant; The value range of is the offset of the discrete point trajectory; S15. At the discrete point , the formula for dynamically selecting the discrete point is as follows: ; wherein, is an integer, is the relative offset of the discrete point trajectory, is the scaling factor, The value range of is , round it to an integer; S16. Based on , using the cumulative amount of the distance from a point to a chord, the curvature of the reference trajectory is: ; wherein, is the index value, , is the distance from the discrete point to the chord , and the chord is determined by the two points .

[0053] It can be understood that according to the change of the discrete points of the reference trajectory, dynamically select Quantify the curvature change of the discrete point trajectory with discrete points.

[0054] It can be understood that when , is the th discrete point to the left of the discrete point; when , , is the th discrete point to the right of the discrete point.

[0055] It can be understood that Round off and take the integer.

[0056] It can be understood that is the scaling coefficient, which is used to change the influence degree of the relative offset on the dynamically selected number of discrete points .

[0057] It can be understood that from the formula , , , it can be seen that when the robot is traveling on a straight reference trajectory, the offset , , and at this time , select the maximum number of discrete points to quantify the curvature change of this point; when the robot is traveling on a large-curvature bend trajectory, , , and at this time , select a smaller number of discrete points to quantify the curvature change of this point.

[0058] In the prediction step module: In some embodiments, the formula for dynamically selecting the prediction step is: ; where is an integer; is an integer; is the dynamically selected number of discrete points, and the value range of is , Round off and take the integer.

[0059] It can be understood that the value of [ is related to the curvature change of the reference trajectory.

[0060] In the robot discrete kinematic model module: In some embodiments, the kinematic model of the robot is:

[0061] In the formula, is the attitude angle of the robot, is the linear velocity during the robot's movement, , is the angular velocity of turning, is the robot at direction and the velocity component in the direction; is the coordinate of the robot's center of mass in the moving plane, is the abscissa, is the ordinate, is the control quantity of the model.

[0062] Furthermore, the kinematic model of the robot is discretized using the Euler forward difference method, and the discrete kinematic model of the robot obtained is: ; In the formula, is the sampling period, is the state information of the robot at time is the linear velocity of the robot at time is the angular velocity of turning of the robot at time is the state information of the robot at time.

[0063] In the trajectory tracking objective function module: In some embodiments, the trajectory tracking error function is defined as ; In the formula, , is the deviation between the abscissa of the actual motion trajectory of the robot at the th discrete point and the abscissa of the reference trajectory; is the deviation between the ordinate of the actual motion trajectory of the robot at the th discrete point and the ordinate of the reference trajectory; is the deviation between the attitude angle of the actual motion trajectory of the robot at the th discrete point and the attitude angle of the reference trajectory.

[0064] It can be understood that the reference attitude angle of the reference trajectory at the th discrete point Calculated from the horizontal and vertical coordinates of the reference trajectory and obtained.

[0065] Furthermore the obtaining method is as follows: Let: ; In the formula is the horizontal coordinate of the reference trajectory of the robot at the th discrete point and the horizontal coordinate of the reference trajectory of the robot at the th discrete point the difference; is the vertical coordinate of the reference trajectory of the robot at the th discrete point and the vertical coordinate of the reference trajectory of the robot at the th discrete point the difference; Then: ; In the formula is the horizontal coordinate of the reference trajectory of the robot at the th discrete point and the horizontal coordinate of the reference trajectory of the robot at the th discrete point the difference; is the vertical coordinate of the reference trajectory of the robot at the th discrete point and the vertical coordinate of the reference trajectory of the robot at the th discrete point the difference.

[0066] Furthermore ; Define the desired speed of the robot: ; where is the desired driving speed of the robot at the discrete point ; is the maximum driving speed of the robot; is the scaling coefficient is the discrete point[[ID=S89]] the curvature of.

[0067] It can be understood that is the scaling coefficient, used to change the influence degree of the curvature change on the desired driving speed.

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

[0069] Furthermore, define the actual driving speed of the robot and the expected driving speed deviation : ; Assume that currently at the th discrete point , , predict points forward, then the trajectory tracking objective function to be optimized is: ; Among them, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot and the deviation between the expected driving speed , is the weight coefficient; is the index value; is the appropriately selected prediction step size dynamically; is the number of discrete points that make up the reference trajectory; is the Euclidean norm.

[0070] Furthermore, assume that currently at the th discrete point , , consider points forward, then the trajectory tracking objective function to be optimized is: ; Among them, is the weight matrix, defined as a diagonal matrix; is the linear velocity of the robot and the deviation between the expected linear velocity , 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; At the th discrete point , , design the robot trajectory tracking controller , such that the trajectory tracking objective function to be optimized takes the minimum value.

[0071] It can be understood that is the weight matrix, defined as a diagonal matrix, and the magnitudes of the elements on its diagonal reflect the sensitivity of the objective function to the defined trajectory tracking error function in the direction of the tracking error.

[0072] It can be understood that is the weight coefficient, which reflects the sensitivity of speed control under different curvatures. The larger the value of

[0073] In the trajectory tracking target module: In some embodiments, the method for obtaining the robot trajectory tracking controller includes: S51. Given the reference trajectory of the robot composed of discrete points ; S52. Initialize the parameters involved in obtaining the controller ; S53. Determine the trajectory tracking objective function to be optimized for the robot at time ; S54. Minimize and solve the objective function to obtain the controller ; S55. According to the controller , determine the control input of the robot at time, and update the pose state of the robot at time using the discrete kinematic model of the robot; S56. Judge whether the robot has reached the last discrete point of the reference trajectory. If "yes", end the loop; if "no", go to the next time

[0074] In S52: In some embodiments, the initialized parameters include the sampling period of the robot control, the reference trajectory of the robot, the initial position of the robot, the initial attitude angle of the robot, the maximum linear velocity of the robot, the maximum angular velocity of steering of the robot, Start the loop.

[0075] In S53: In some embodiments, the objective function is: ; where is the weight matrix, defined as a diagonal matrix; is the deviation between the actual driving speed of the robot and the desired driving speed , is the weight coefficient; is the index value; is a dynamically selected appropriate prediction step; is the number of discrete points constituting the reference trajectory; is the Euclidean norm; or ; where is the weight matrix, defined as a diagonal matrix; is the deviation between the linear velocity of the robot and the desired linear velocity , is the weight coefficient; is the index value; is the number of discrete points constituting the reference trajectory; is the Euclidean norm.

[0076] In S54: In some embodiments, to ensure the safe controllability of the mobile robot trajectory tracking task, the controller is designed to have a value range that satisfies the constraint: ; In the formula, is the maximum driving speed of the robot, is the maximum steering angular velocity of the robot.

[0077] Furthermore, under the constraint of , the objective function is minimized to obtain the optimal control sequence at time , and the first two elements of the optimal control sequence at time are selected as the robot trajectory tracking controller at time.

[0078] In a third aspect, an embodiment of the present application provides a computer device, including a storage and a processor. The storage stores a computer program. When the computer program is executed by the processor, the processor is caused to execute the steps of the robot tracking control method for adapting to an irregular discrete trajectory as described above.

[0079] Among them, the computer device may be a computing device such as a desktop computer, a notebook, a palm computer, and a cloud server. The computer device can perform human-computer interaction with a user through means such as a keyboard, a mouse, a remote control, a touchpad, or a voice control device.

[0080] The memory includes at least one type of readable storage medium. The readable storage medium includes flash memory, a hard disk, a multimedia card, a card-type memory (such as an SD or D interface display memory, etc.), a random access memory (RAM), a static random access memory (SRAM), a read-only memory (ROM), an electrically erasable programmable read-only memory (EEPROM), a programmable read-only memory (PROM), a magnetic memory, a magnetic disk, an optical disk, etc. In some embodiments, the memory may be an internal storage unit of the computer device, such as the hard disk or 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 on 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 and the external storage device of the computer device. In this embodiment, the memory is commonly used to store the operating system installed on the computer device and various application software, such as the program code of the robot tracking control method for adapting to an irregular discrete trajectory. In addition, the memory can also be used to temporarily store various data that have been output or will be output.

[0081] In some embodiments, the processor may be a central processing unit (CPU), a controller, a microcontroller, a microprocessor, or other data processing chips. The processor is generally used to control the overall operation of the computer device. In this embodiment, the processor is used to run the program code stored in the memory or process data, such as running the program code of the robot tracking control method for adapting to an irregular discrete trajectory.

[0082] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, the processor is caused to execute the steps of the robot tracking control method for adapting to an irregular discrete trajectory as described above.

[0083] Among them, the computer-readable storage medium stores an interface display program, and the interface display program can be executed by at least one processor, so that the at least one processor executes the steps of the robot tracking control method for adapting to irregular discrete trajectories as described above.

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

[0085] The present invention will be further described below in conjunction with simulation verification.

[0086] Simulation verification The present invention obtains trajectory tracking simulation results in the environment for two different reference trajectories.

[0087] Curve tracking simulation Reference trajectory: , Initialization parameters: , , , , , , , , , , , , ; The "^—" is the reference trajectory, and the "*--" is the tracking trajectory. The simulation results are as Figure 4 shown It can be seen from Figure 4 that the experiment has achieved good results in the tracking simulation of the curve trajectory, which proves the effectiveness of the algorithm for tracking the curve trajectory.

[0088] Broken line tracking simulation The reference trajectory is a broken line trajectory with irregular changes. Initialization parameters: , , , , , , , , , , , , ; The "^—" is the reference trajectory, and the "*--" is the tracking trajectory. The simulation results are as shown in Figure 5 .

[0089] From Figure 5 it can be seen that the experiment on the tracking simulation of the irregularly changing broken-line trajectory has achieved good results, proving the effectiveness of the algorithm for tracking the broken-line trajectory.

[0090] Figure 4 and Figure 5 The arrows in indicate the local enlarged views of the positions pointed to.

[0091] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A robot tracking control method adapted to irregular discrete trajectories, characterized in that, It includes the following steps: Dynamically select the number of discrete points according to the change of the reference trajectory to quantify the curvature change of the reference trajectory; Dynamically select the prediction step length according to the curvature change of the reference trajectory; Discretize the robot kinematic model using the Euler forward difference method to obtain the discrete robot kinematic model; Define the trajectory tracking error function and construct the trajectory tracking objective function in combination with the discrete robot kinematic model; Solve the objective function to obtain the robot trajectory tracking controller, so that the robot moves along the reference trajectory, thereby achieving the trajectory tracking objective.

2. The robot tracking control method for adapting to irregular discrete trajectories according to claim 1, wherein Curvature of the reference trajectory is as follows: ； wherein, is an index value, , is the dynamically selected number of discrete points, , rounds to an integer, is an integer, is the distance from the discrete point to the chord , and the chord is determined by the two points and ; and / or Prediction step size The formula for dynamically selecting is as follows: ； wherein, is an integer; is an integer; is the discretized number of points dynamically selected, The value range of is rounded to an integer; and / or The robot kinematic model is: ； Wherein, is the attitude angle of the robot, is the linear velocity during the movement of the robot, , is the angular velocity of turning, is the velocity component of the robot in the direction and the direction, is the coordinate of the robot's center of mass in the moving plane, is the abscissa, is the ordinate, is the control quantity of the model. Using the Euler forward difference method to discretize the robot kinematic model, the obtained discrete kinematic model of the robot is: ； Wherein, is the sampling period, is the state information of the robot at time is the linear velocity of the robot at time is the angular velocity of the robot's steering at time is the state information of the robot at time; and / or Define the trajectory tracking error function as ； Wherein, , is the abscissa of the actual motion trajectory of the robot at the th discrete point and the abscissa of the reference trajectory deviation; is the ordinate of the actual motion trajectory of the robot at the th discrete point and the ordinate of the reference trajectory deviation; is the attitude angle of the actual motion trajectory of the robot at the th discrete point and the attitude angle of the reference trajectory deviation.

3. The robot tracking control method for adapting to irregular discrete trajectories according to claim 2, characterized in that, Curvature of the reference trajectory The method for obtaining the same includes: Dynamically select discrete points to quantify the change in the curvature of the discrete point trajectory, where is an integer; At discrete points Consider discrete points on each of the left and right sides, that is , , where is an integer; Calculation For each discrete point within the range Calculate the average value of the coordinate information to obtain the mean point It is: ； Among them, is the abscissa of each discrete point, is the ordinate of each discrete point; Connection With Two points determine a straight line , calculate successively , discrete points to this straight line distance and sum them up to obtain the offset of the discrete point trajectory as follows: ； Considering the size of the robot as , the relative offset of the discrete point trajectory is as follows: ； Among them, is an integer; is the size of the robot and is a fixed constant; The value range of ; is the offset of the discrete point trajectory; At discrete points where the discrete points are dynamically selected, the formula is as follows: ； wherein, is an integer, is the relative offset of the discrete point trajectory, is the scaling factor, The value range of ; Based on , using the cumulative amount of the distance from a point to a chord, referring to the curvature of the trajectory is as follows: ； wherein, is an index value, , is the distance from a discrete point to a chord , and the chord is determined by two points and .

4. The robot tracking control method for adapting to irregular discrete trajectories according to claim 2, characterized in that, Based on the formula ; Define the desired speed of the robot : ； Among them, is the expected traveling speed of the robot at discrete points ; is the maximum traveling speed of the robot; is the scaling coefficient, is the discrete point curvature; Define the actual traveling speed of the robot and the desired traveling speed deviation : ； Assume that the current discrete points , , future prediction points, then the trajectory tracking objective function to be optimized is for: ； Among them, is the weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot and the deviation from the desired driving speed ; 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.

5. The robot tracking control method for adapting to irregular discrete trajectories according to claim 4, characterized in that Assume currently at the th discrete point , , and considering the subsequent points, the trajectory tracking objective function to be optimized is as follows: ； Among them, is the weight matrix, defined as a diagonal matrix; is the linear velocity of the robot and the deviation from the desired linear velocity ; 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; At the discrete point , , design a robot trajectory tracking controller such that the trajectory tracking objective function to be optimized takes the minimum value.

6. The robot tracking control method for adapting to irregular discrete trajectories according to claim 5, characterized in that The method for obtaining the robot trajectory tracking controller includes: Given a reference trajectory of a robot consisting of discrete points ; The controller will be obtained Initialize the parameters involved; Determine the trajectory tracking objective function to be optimized for the robot at moment ; Minimize the solution of the objective function , and obtain a controller ; According to the controller , determine the control input of the robot at the moment, and update the pose state of the robot at the moment using the discrete kinematic model of the robot; Determine whether the robot has reached the last discrete point of the reference trajectory. If "yes", end the loop; if "no", go to the next moment , and jump to S53 to continue the loop.

7. The robot tracking control method for adapting to irregular discrete trajectories according to claim 6, characterized in that, The initialized parameters include the sampling period for robot control , the reference trajectory of the robot , the initial position of the robot , the initial attitude angle of the robot , the maximum linear velocity , the maximum angular velocity of turning , Start the loop; and / or The objective function is as follows: ； Among them, is a weight matrix, defined as a diagonal matrix; is the actual driving speed of the robot and the desired driving speed deviation, is a weight coefficient; is an index value; is a dynamically selected prediction step; is the number of discrete points that make up the reference trajectory; is the Euclidean norm; and / or ； Among them, is the weight matrix, defined as a diagonal matrix; is the linear velocity of the robot and the deviation from the desired linear velocity ; 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 safe and controllable trajectory tracking task of the mobile robot, a controller is designed The value range of satisfies the constraint: ； wherein, is the maximum traveling speed of the robot, is the maximum angular velocity of steering of the robot; under the constraint of , the objective function is minimized to obtain the optimal control sequence at the moment , and the first two elements of the optimal control sequence at the moment are selected as the robot trajectory tracking controller at the moment.

8. A robot tracking control system adapted to an irregular discrete trajectory, characterized in that, It includes: A module for quantifying the curvature change of the reference trajectory, which is used to dynamically select the number of discrete points according to the change of the reference trajectory to quantify the curvature change of the reference trajectory; A prediction step length module, which is used to dynamically select the prediction step length according to the curvature change of the reference trajectory; A discrete robot kinematic model module, which is used to discretize the robot kinematic model using the Euler forward difference method to obtain the discrete robot kinematic model; A trajectory tracking objective function module, which is used to define the trajectory tracking error function and construct the trajectory tracking objective function in combination with the discrete robot kinematic model; A trajectory tracking objective module, which is used to solve the objective function to obtain the robot trajectory tracking controller, so that the robot moves along the reference trajectory, thereby achieving the trajectory tracking objective.

9. A computer device, characterized in that, It includes a memory and a processor. The memory stores a computer program. When the computer program is executed by the processor, the processor executes the steps of the method according to any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, Stores a computer program. When the computer program is executed by the processor, the processor executes the steps of the method according to any one of claims 1-7.

Images