Trajectory tracking control method, system, device and storage medium for improving robot cornering accuracy and inspection efficiency

Abstract

The present invention discloses a trajectory tracking control method, system, device and storage medium for improving the cornering accuracy and inspection efficiency of a robot, belonging to the field of robotics technology. The method includes discretizing the robot kinematic model using the Euler forward difference method 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; solving the objective function to obtain a robot trajectory tracking controller based on predictive control, so that the robot's motion trajectory tracks to a specified reference trajectory. When the robot is traveling on a straight reference trajectory, it is expected that the robot will inspect at the maximum speed; when the robot is traveling on a curve trajectory with a large curvature, it is expected that the robot will reduce the inspection speed to improve cornering accuracy. For discrete reference trajectories that change irregularly, the robot of the present application improves cornering accuracy while also shortening the time of a single inspection.

Description

Technical Field

[0001] The present invention belongs to the field of robotics technology, and in particular relates to a trajectory tracking control method, system, device and storage medium for improving the cornering accuracy and inspection efficiency of a robot. Background Art

[0002] In recent years, with the development of robotics technology, 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, and instead a series of discrete points is often used. Traditional robot trajectory control methods, such as robust control, back-stepping, and PID control, rely on a continuous reference trajectory model during controller design. This results in large trajectory tracking errors in the actual application of substation power inspection robots. This is particularly true when navigating steep curves, where the robot cannot smoothly and stably track the intended trajectory and may even deviate from the trajectory, severely impacting the robot's performance and inspection efficiency. Furthermore, in real-world substations, to ensure safe operation, inspection robots often use low-speed inspections, resulting in long inspection times and low inspection efficiency, adversely impacting substation inspection performance. Summary of the Invention

[0003] The purpose of the present invention is to provide a trajectory tracking control method, system, device and storage medium for improving the robot's cornering accuracy and inspection efficiency, so as to solve the problems of poor cornering accuracy and long single inspection time of substation power inspection robots for irregularly changing discrete reference trajectories.

[0004] The embodiment of the present application is implemented as follows: a trajectory tracking control method for improving the cornering accuracy and inspection efficiency of a robot, comprising:

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

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

[0007] Solve the objective function to obtain a robot trajectory tracking controller based on predictive control, so that the robot's motion trajectory tracks the specified reference trajectory.

[0008] Optionally, in some embodiments of the present application, the robot kinematic model is:

[0009]

[0010] Where θ is the robot's posture angle, v is the robot's linear velocity during motion, is the steering angular velocity, are the velocity components of the robot in the x and y directions, (x, y) are the coordinates of the robot's center of mass in the moving plane, x is the horizontal coordinate, y is the vertical coordinate, and (v, ω) is the control variable of the model; and / or

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

[0012]

[0013] Where T is the sampling period, x(k), y(k), θ(k) are the state information of the robot at time k, v(k) is the linear velocity of the robot at time k, ω(k) is the steering angular velocity of the robot at time k, and x(k+1), y(k+1), θ(k+1) are the state information of the robot at time k+1.

[0014] Optionally, in some embodiments of the present application, a trajectory tracking error function E is defined as xyθ (k)

[0015]

[0016] Where k = 1, 2, ..., N, e x (k) is the horizontal coordinate x(k) of the actual motion trajectory of the robot at the kth discrete point and the horizontal coordinate x of the reference trajectory r Deviation of (k); e y (k) is the vertical coordinate y(k) of the actual motion trajectory of the robot at the kth discrete point and the vertical coordinate y of the reference trajectory r Deviation of (k); e θ (k) is the attitude angle θ(k) of the robot's actual motion trajectory at the kth discrete point and the attitude angle θ of the reference trajectory r (k) Deviation;

[0017] θ r (k) The method of obtaining is as follows:

[0018] make:

[0019]

[0020] Where Δx r is the horizontal coordinate x of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory horizontal coordinate x r The difference between (k-1); Δy r is the ordinate y of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory ordinate yr The difference of (k-1);

[0021] but:

[0022]

[0023] Where Δx r is the horizontal coordinate x of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory horizontal coordinate x r The difference between (k-1); Δy r is the ordinate y of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory ordinate y r (k-1) difference; and / or

[0024] Define the actual robot linear velocity control value v(k) and the desired linear velocity v h (k) Velocity error function E v (k) is:

[0025] E v (k)=v(k)-v h (k)

[0026] discrete point p k The curvature h(k) is:

[0027]

[0028] Where, L≥1 and is an integer, D ik ≥0 is a discrete point p k To chord L i The distance, chord L i From point p k-i and p k-i+L+1 Sure;

[0029] The desired linear speed adjustment is obtained based on the change of curvature h(k) as follows:

[0030]

[0031] Where, v h (k) is the inspection robot at p k The expected linear velocity of the point; V max is the maximum linear speed allowed by the inspection robot; μ>0 is the proportional coefficient, which is used to change the degree of influence of curvature change on the expected inspection speed.

[0032] Optionally, in some embodiments of the present application, it is assumed that at the kth discrete point p k, k=1,2,…,N, and consider one point later, the trajectory tracking objective function J(k) to be optimized is:

[0033]

[0034] Where, E xyθ (k+1) is the trajectory tracking error function at time k+1, is the trajectory tracking error function E at time k+1 xyθ The transpose of (k+1); E v (k+1) is the difference between the robot linear velocity control value v(k+1) and the desired linear velocity v at time k+1 h (k+1) deviation.

[0035] Optionally, in some embodiments of the present application, the method for solving the objective function includes:

[0036] Given a robot's reference trajectory (x r ,y r );

[0037] Initialize the parameters involved in obtaining the controller [v(k),ω(k)];

[0038] Determine the trajectory tracking objective function J(k) to be optimized by the robot at time k;

[0039] Solve the objective function J(k) and obtain the controller [v(k), ω(k)];

[0040] According to the controller [v(k), ω(k)], the control input of the robot at time k is determined, and the robot's discrete kinematic model is used to update the robot's posture state at time k+1;

[0041] 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 k→k+1 and jump to the step of determining the trajectory tracking objective function J(k) to be optimized at moment k to continue the loop.

[0042] Optionally, in some embodiments of the present application, the initialized parameters include the sampling period T of the robot control, the reference trajectory (x r ,y r ), robot initial position (x(1), y(1)), robot initial posture angle θ(1), maximum linear velocity V max , maximum steering angular velocity ω max , k=1 to start the loop; and / or

[0043] The objective function J(k) is:

[0044]

[0045] Where, E xyθ (k+1) is the trajectory tracking error function at time k+1, is the trajectory tracking error function E at time k+1 xyθ The transpose of (k+1); E v (k+1) is the difference between the robot linear velocity control value v(k+1) and the desired linear velocity v at time k+1 h (k+1) deviation.

[0046] Optionally, in some embodiments of the present application, according to the trajectory tracking error function E xyθ (k) and the velocity error function E defined v (k) formula, the objective function J(k) becomes:

[0047]

[0048] Where x(k+1), y(k+1), θ(k+1) are the robot's state information at time k+1;

[0049] x r (k+1),y r (k+1),θ r (k+1) is the reference trajectory state information at time k+1; v(k+1) is the robot linear velocity control value at time k+1; v h (k+1) is the expected linear velocity of the robot at time k+1;

[0050] The specific expression of the predictive controller [v(k), ω(k)] is obtained:

[0051]

[0052] Where T is the sampling period, v h (k+1) is the expected linear velocity of the robot at time k+1, x(k+1), y(k+1), θ(k+1) is the state information of the robot at time k+1; x r (k+1),y r (k+1),θ r (k+1) is the reference trajectory state information at time k+1;

[0053] To ensure the safety and controllability of the mobile robot trajectory tracking task, the value range of the controller [v(k), ω(k)] is designed to meet the constraints:

[0054]

[0055] Where V maxis the maximum speed of the robot, ω max is the maximum steering angular velocity of the robot.

[0056] Accordingly, the embodiment of the present application further provides a trajectory tracking control system for improving the cornering accuracy and inspection efficiency of the robot, including:

[0057] The model discretization module is used to discretize the robot kinematic model using the Euler forward difference method to obtain the robot discrete kinematic model;

[0058] Constructing an objective function module, defining a trajectory tracking error function, and constructing a trajectory tracking objective function in combination with the robot discrete kinematics model;

[0059] The objective function solving module is used to obtain a robot trajectory tracking controller based on predictive control, so that the robot's motion trajectory tracks the specified reference trajectory.

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

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

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

[0063] In this application, the robot's kinematic model is discretized using the Euler forward difference method to obtain a discrete kinematic model. Next, a trajectory tracking error function is defined and combined with model predictive control to construct the objective function of the trajectory control problem. Finally, a trajectory tracking controller is designed based on predictive control to ensure that the robot's motion trajectory tracks the desired reference trajectory. The effectiveness of this method is verified by simulations of a set of irregular reference trajectories. This invention provides a new solution to the trajectory tracking problem of wheeled mobile robots and has important practical value.

[0064] This application uses a novel method for describing the curvature changes of discrete points to describe the curvature changes of a reference trajectory. Based on this method, a speed adjustment strategy is designed to enable the robot to have variable speed during inspections. Specifically, when the robot is traveling on a straight reference trajectory, it is expected to inspect at maximum speed; when traveling on a curve with large curvature, it is expected to reduce its inspection speed to improve cornering accuracy. For irregularly changing discrete reference trajectories, this application's robot improves cornering accuracy while also shortening the time of a single inspection. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 This is a flow chart of a trajectory tracking control method for improving the cornering accuracy and inspection efficiency of a robot according to the present invention;

[0066] Figure 2 This is a schematic diagram of the distance from a point to a chord according to the present invention;

[0067] Figure 3 A flow chart of a method for solving an objective function according to the present invention;

[0068] Figure 4 This is the control effect diagram of the conventional controller;

[0069] Figure 5 This is a control effect diagram of the controller that takes cornering accuracy and inspection efficiency into consideration in the present invention. DETAILED DESCRIPTION

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

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

[0072] First, see Figure 1 The present invention provides a trajectory tracking control method for improving the cornering accuracy and inspection efficiency of a robot, including:

[0073] S1. Discretize the robot kinematic model using the Euler forward difference method to obtain the robot discrete kinematic model;

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

[0075] S3. Solve the objective function to obtain a robot trajectory tracking controller based on predictive control, so that the robot's motion trajectory tracks the specified reference trajectory.

[0076] In this application's technical solution, the robot's kinematic model is discretized using the Euler forward difference method to obtain a discrete kinematic model. Next, a trajectory tracking error function is defined and combined with model predictive control to construct the objective function of the trajectory control problem. Finally, a trajectory tracking controller is designed based on predictive control to ensure that the robot's motion trajectory tracks the desired reference trajectory. The effectiveness of this method was verified by simulations of a set of irregular reference trajectories. This invention provides a new solution to the trajectory tracking problem for wheeled mobile robots and has significant practical value.

[0077] In S1:

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

[0079]

[0080] Where θ is the robot's posture angle, v is the robot's linear velocity during motion, is the steering angular velocity, are the velocity components of the robot in the x and y directions, (x, y) are the coordinates of the robot's center of mass in the moving plane, x is the horizontal coordinate, y is the vertical coordinate, and (v, ω) is the control quantity of the model.

[0081] In some embodiments, the robot kinematics model is discretized using the Euler forward difference method, and the obtained robot discrete kinematics model is:

[0082]

[0083] Where T is the sampling period, x(k), y(k), θ(k) are the state information of the robot at time k, v(k) is the linear velocity of the robot at time k, ω(k) is the steering angular velocity of the robot at time k, and x(k+1), y(k+1), θ(k+1) are the state information of the robot at time k+1.

[0084] In S2:

[0085] In some embodiments, the trajectory tracking error function E is defined as xyθ (k)

[0086]

[0087] Where k = 1, 2, ..., N, e x (k) is the horizontal coordinate x(k) of the actual motion trajectory of the robot at the kth discrete point and the horizontal coordinate x of the reference trajectory r Deviation of (k); e y (k) is the vertical coordinate y(k) of the actual motion trajectory of the robot at the kth discrete point and the vertical coordinate y of the reference trajectory r Deviation of (k); e θ (k) is the attitude angle θ(k) of the robot's actual motion trajectory at the kth discrete point and the attitude angle θ of the reference trajectory r (k) Deviation.

[0088] It can be understood that the reference trajectory at the kth discrete point reference attitude angle θ r (k) The horizontal and vertical coordinates x of the reference trajectory r (k) and y r (k) is calculated.

[0089] Furthermore, θ r (k) The method of obtaining is as follows:

[0090] make:

[0091]

[0092] Where Δx r is the horizontal coordinate x of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory horizontal coordinate x r The difference between (k-1); Δy r is the ordinate y of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory ordinate y r The difference of (k-1);

[0093] but:

[0094]

[0095] Where Δx r is the horizontal coordinate x of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory horizontal coordinate x r The difference between (k-1); Δy r is the ordinate y of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory ordinate y r The difference of (k-1).

[0096] In some embodiments, the actual robot linear velocity control value v(k) and the desired linear velocity v are defined. h (k) Velocity error function E v (k) is:

[0097] E v (k)=v(k)-v h (k)

[0098] It can be understood that in order to describe the change in the curvature of the trajectory of discrete points, the present application proposes a new method for describing the change in the curvature of discrete points by using the accumulated distance from the point to the chord.

[0099] See also Figure 2 , further, the discrete point p k The curvature h(k) is:

[0100]

[0101] Where, L≥1 and is an integer, D ik ≥0 is a discrete point p k To chord L i The distance, chord L i From point p k-i and p k-i+L+1 Sure;

[0102] It can be understood that when i=L, the chord L L From point p k-L and point p k+1 OK, click p k To chord L L The distance is D ik (i=L); if i=1, the chord L1 is from point p k-1 and point p k+L OK, click p k The distance to chord L1 is D ik (i=1); calculate point p in sequence when i=1,2,…,L k To chord L i Distance D ik And sum, use h(k) to describe the discrete point p k The curvature changes.

[0103] It can be understood that in order to improve the cornering accuracy of the inspection robot and shorten the time of a single inspection task, the robot's movement speed is reduced in large curvature curves to improve cornering accuracy; on small curvature trajectories (straight lines), the robot's movement speed is increased, the inspection time is shortened, and the inspection efficiency is improved.

[0104] Furthermore, the desired linear speed adjustment is obtained based on the change of curvature h(k) as follows:

[0105]

[0106] Where, v h (k) is the inspection robot at p k The expected linear velocity of the point; V max is the maximum linear speed allowed by the inspection robot; μ>0 is the proportional coefficient, which is used to change the degree of influence of curvature change on the expected inspection speed.

[0107] It can be understood that when the robot is traveling on a straight reference trajectory, h(k)≈0,v h (k)=v max , it is expected that the robot will patrol at the maximum speed; when the robot is traveling on a curve with large curvature, h(k)>>0,v h (k)<<v max , it is expected that the robot will reduce the inspection speed and improve the cornering accuracy.

[0108] This application uses a novel method for describing the curvature changes of discrete points to describe the curvature changes of a reference trajectory. Based on this method, a speed adjustment strategy is designed to enable the robot to have variable speed during inspections. Specifically, when the robot is traveling on a straight reference trajectory, it is expected to inspect at maximum speed; when traveling on a curve with large curvature, it is expected to reduce its inspection speed to improve cornering accuracy. For irregularly changing discrete reference trajectories, this application's robot improves cornering accuracy while also shortening the time of a single inspection.

[0109] In some embodiments, it is assumed that at the kth discrete point p k , k=1,2,…,N, and consider one point later, the trajectory tracking objective function J(k) to be optimized is:

[0110]

[0111] Where, E xyθ (k+1) is the trajectory tracking error function at time k+1, is the trajectory tracking error function E at time k+1 xyθ The transpose of (k+1); E v (k+1) is the difference between the robot linear velocity control value v(k+1) and the desired linear velocity v at time k+1 h (k+1) deviation;

[0112] At the kth discrete point p k , k=1,2,…,N, design a robot trajectory tracking controller [v(k),ω(k)] based on predictive control so that the trajectory tracking objective function J(k) to be optimized takes the minimum value.

[0113] In the S3:

[0114] See also Figure 3 In some embodiments, the method for solving the objective function includes:

[0115] S31, given a reference trajectory of a robot consisting of N discrete points (x r ,y r );

[0116] S32, initializing the parameters involved in obtaining the controller [v(k), ω(k)];

[0117] S33, determining the trajectory tracking objective function J(k) to be optimized for the robot at time k;

[0118] S34, solving the objective function J(k) to obtain the controller [v(k), ω(k)];

[0119] S35. Determine the control input of the robot at time k according to the controller [v(k), ω(k)], and update the posture state of the robot at time k+1 using the robot discrete kinematic model;

[0120] S36. 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 k→k+1 and jump to S33 to continue the loop.

[0121] It can be understood that the robot in the method for solving the objective function is a wheeled mobile robot.

[0122] In the S32:

[0123] In some embodiments, the initialization parameters include the sampling period T of the robot control, the reference trajectory (x r ,y r ), robot initial position (x(1), y(1)), robot initial posture angle θ(1), maximum linear velocity V max , maximum steering angular velocity ω max , k=1 and the loop starts.

[0124] In said S33:

[0125] In some embodiments, the objective function J(k) is:

[0126]

[0127] Where, E xyθ (k+1) is the trajectory tracking error function at time k+1, is the trajectory tracking error function E at time k+1 xyθ The transpose of (k+1); E v (k+1) is the difference between the robot linear velocity control value v(k+1) and the desired linear velocity v at time k+1 h (k+1) deviation.

[0128] In said S34:

[0129] It can be understood that the trajectory tracking objective function J(k) to be optimized by the robot at time k determined in step S33 is used to calculate the first-order partial derivatives of the control quantities v(k) and ω(k), respectively, and the partial derivatives are set to 0, thereby deriving the specific expression of the robot trajectory tracking controller [v(k), ω(k)] based on predictive control.

[0130] In some embodiments, according to the trajectory tracking error function E xyθ (k) and the defined deviation E v (k) formula, the objective function J(k) becomes:

[0131]

[0132] Where x(k+1), y(k+1), θ(k+1) are the robot's state information at time k+1;

[0133] x r (k+1),y r (k+1),θ r (k+1) is the reference trajectory state information at time k+1; v(k+1) is the robot linear velocity control value at time k+1; v h (k+1) is the expected linear velocity of the robot at time k+1;

[0134] The specific expression of the predictive controller [v(k), ω(k)] is obtained:

[0135]

[0136] Where T is the sampling period, v h (k+1) is the expected linear velocity of the robot at time k+1, x(k+1), y(k+1), θ(k+1) is the state information of the robot at time k+1; x r (k+1),y r (k+1),θ r (k+1) is the reference trajectory state information at time k+1.

[0137] In some embodiments, to ensure the safety and controllability of the mobile robot trajectory tracking task, the value range of the controller [v(k), ω(k)] is designed to satisfy the constraint:

[0138]

[0139] Where V max is the maximum speed of the robot, ω max is the maximum steering angular velocity of the robot.

[0140] In a second aspect, embodiments of the present application provide a trajectory tracking control system that improves the cornering accuracy and inspection efficiency of a robot, including:

[0141] The model discretization module is used to discretize the robot kinematic model using the Euler forward difference method to obtain the robot discrete kinematic model;

[0142] Constructing an objective function module, defining a trajectory tracking error function, and constructing a trajectory tracking objective function in combination with the robot discrete kinematics model;

[0143] The objective function solving module is used to obtain a robot trajectory tracking controller based on predictive control, so that the robot's motion trajectory tracks the specified reference trajectory.

[0144] In the discrete module of the model:

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

[0146]

[0147] Where θ is the robot's posture angle, v is the robot's linear velocity during motion, is the steering angular velocity, are the velocity components of the robot in the x and y directions, (x, y) are the coordinates of the robot's center of mass in the moving plane, x is the horizontal coordinate, y is the vertical coordinate, and (v, ω) is the control quantity of the model.

[0148] In some embodiments, the robot kinematics model is discretized using the Euler forward difference method, and the obtained robot discrete kinematics model is:

[0149]

[0150] Where T is the sampling period, x(k), y(k), θ(k) are the state information of the robot at time k, v(k) is the linear velocity of the robot at time k, ω(k) is the steering angular velocity of the robot at time k, and x(k+1), y(k+1), θ(k+1) are the state information of the robot at time k+1.

[0151] In the target function module:

[0152] In some embodiments, the trajectory tracking error function E is defined as xyθ (k)

[0153]

[0154] Where k = 1, 2, ..., N, e x (k) is the horizontal coordinate x(k) of the actual motion trajectory of the robot at the kth discrete point and the horizontal coordinate x of the reference trajectory r Deviation of (k); e y (k) is the vertical coordinate y(k) of the actual motion trajectory of the robot at the kth discrete point and the vertical coordinate y of the reference trajectory r Deviation of (k); e θ (k) is the attitude angle θ(k) of the robot's actual motion trajectory at the kth discrete point and the attitude angle θ of the reference trajectory r (k) Deviation.

[0155] It can be understood that the reference trajectory at the kth discrete point reference attitude angle θ r (k) The horizontal and vertical coordinates x of the reference trajectory r (k) and yr (k) is calculated.

[0156] Furthermore, θ r (k) The method of obtaining is as follows:

[0157] make:

[0158]

[0159] Where Δx r is the horizontal coordinate x of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory horizontal coordinate x r The difference between (k-1); Δy r is the ordinate y of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory ordinate y r The difference of (k-1);

[0160] but:

[0161]

[0162] Where Δx r is the horizontal coordinate x of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory horizontal coordinate x r The difference between (k-1); Δy r is the ordinate y of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory ordinate y r The difference of (k-1).

[0163] In some embodiments, the actual robot linear velocity control value v(k) and the desired linear velocity v are defined. h (k) Deviation E v (k) is:

[0164] E v (k)=v(k)-v h (k)

[0165] It can be understood that in order to describe the change in the curvature of the trajectory of discrete points, the present application proposes a new method for describing the change in the curvature of discrete points by using the accumulated distance from the point to the chord.

[0166] See also Figure 2 , further, the discrete point p k The curvature h(k) is:

[0167]

[0168] Where, L≥1 and is an integer, D ik ≥0 is a discrete point p k To chord L i The distance, chord L i From point p k-i and p k-i+L+1 Sure;

[0169] It can be understood that when i=L, the chord L L From point p k-L and point p k+1 OK, click p k To chord L L The distance is D ik (i=L); if i=1, the chord L1 is from point p k-1 and point p k+L OK, click p k The distance to chord L1 is D ik (i=1); calculate point p in sequence when i=1,2,…,L k To chord L i Distance D ik And sum, use h(k) to describe the discrete point p k The curvature changes.

[0170] It can be understood that in order to improve the cornering accuracy of the inspection robot and shorten the time of a single inspection task, the robot's movement speed is reduced in large curvature curves to improve cornering accuracy; on small curvature trajectories (straight lines), the robot's movement speed is increased, the inspection time is shortened, and the inspection efficiency is improved.

[0171] Furthermore, the desired linear speed adjustment is obtained based on the change of curvature h(k) as follows:

[0172]

[0173] Where, v h (k) is the inspection robot at p k The expected linear velocity of the point; V max is the maximum linear speed allowed by the inspection robot; μ>0 is the proportional coefficient, which is used to change the degree of influence of curvature change on the expected inspection speed.

[0174] It can be understood that when the robot is traveling on a straight reference trajectory, h(k)≈0,v h (k)=v max , it is expected that the robot will patrol at the maximum speed; when the robot is traveling on a curve with large curvature, h(k)>>0,v h (k)<<v max , it is expected that the robot will reduce the inspection speed and improve the cornering accuracy.

[0175] This application uses a novel method for describing the curvature changes of discrete points to describe the curvature changes of a reference trajectory. Based on this method, a speed adjustment strategy is designed to enable the robot to have variable speed during inspections. Specifically, when the robot is traveling on a straight reference trajectory, it is expected to inspect at maximum speed; when traveling on a curve with large curvature, it is expected to reduce its inspection speed to improve cornering accuracy. For irregularly changing discrete reference trajectories, this application's robot improves cornering accuracy while also shortening the time of a single inspection.

[0176] In some embodiments, it is assumed that at the kth discrete point p k , k=1,2,…,N, and consider one point later, the trajectory tracking objective function J(k) to be optimized is:

[0177]

[0178] Where, E xyθ (k+1) is the trajectory tracking error function at time k+1, is the trajectory tracking error function E at time k+1 xyθ The transpose of (k+1); E v (k+1) is the difference between the robot linear velocity control value v(k+1) and the desired linear velocity v at time k+1 h (k+1) deviation;

[0179] At the kth discrete point p k , k=1,2,…,N, design a robot trajectory tracking controller [v(k),ω(k)] based on predictive control so that the trajectory tracking objective function J(k) to be optimized takes the minimum value.

[0180] In the objective function solving module:

[0181] See also Figure 3 In some embodiments, the method for solving the objective function includes:

[0182] Given a robot's reference trajectory (x r ,y r );

[0183] Initialize the parameters involved in obtaining the controller [v(k),ω(k)];

[0184] Determine the trajectory tracking objective function J(k) to be optimized by the robot at time k;

[0185] Solve the objective function J(k) and obtain the controller [v(k), ω(k)];

[0186] According to the controller [v(k), ω(k)], the control input of the robot at time k is determined, and the robot's discrete kinematic model is used to update the robot's posture state at time k+1;

[0187] 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 k→k+1 and jump to determine the trajectory tracking objective function J(k) to be optimized at moment k to continue the loop.

[0188] It can be understood that the robot in the method for solving the objective function is a wheeled mobile robot.

[0189] In some embodiments, the initialization parameters include the sampling period T of the robot control, the reference trajectory (x r ,y r ), robot initial position (x(1), y(1)), robot initial posture angle θ(1), maximum linear velocity V max , maximum steering angular velocity ω max , k=1 and the loop starts.

[0190] In some embodiments, the objective function J(k) is:

[0191]

[0192] Where, E xyθ (k+1) is the trajectory tracking error function at time k+1, is the trajectory tracking error function E at time k+1 xyθ The transpose of (k+1); E v (k+1) is the difference between the robot linear velocity control value v(k+1) and the desired linear velocity v at time k+1 h (k+1) deviation.

[0193] It can be understood that the trajectory tracking objective function J(k) to be optimized by the robot at time k determined in step S33 is used to calculate the first-order partial derivatives of the control quantities v(k) and ω(k), respectively, and the partial derivatives are set to 0, thereby deriving the specific expression of the robot trajectory tracking controller [v(k), ω(k)] based on predictive control.

[0194] In some embodiments, according to the trajectory tracking error function E xyθ (k) and the velocity error function E defined v (k) formula, the objective function J(k) becomes:

[0195]

[0196] Where x(k+1), y(k+1), θ(k+1) are the robot's state information at time k+1;

[0197] x r (k+1),y r (k+1),θ r (k+1) is the reference trajectory state information at time k+1; v(k+1) is the robot linear velocity control value at time k+1; v h (k+1) is the expected linear velocity of the robot at time k+1;

[0198] The specific expression of the predictive controller [v(k), ω(k)] is obtained:

[0199]

[0200] Where T is the sampling period, v h (k+1) is the expected linear velocity of the robot at time k+1, x(k+1), y(k+1), θ(k+1) is the state information of the robot at time k+1; x r (k+1),y r (k+1),θ r (k+1) is the reference trajectory state information at time k+1.

[0201] In some embodiments, to ensure the safety and controllability of the mobile robot trajectory tracking task, the value range of the controller [v(k), ω(k)] is designed to satisfy the constraint:

[0202]

[0203] Where V max is the maximum speed of the robot, ω max is the maximum steering angular velocity of the robot.

[0204] On the 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 trajectory tracking control method for improving the cornering accuracy and inspection efficiency of the robot as described above.

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

[0206] 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 can 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 can also be an external storage device of the computer device, such as a plug-in hard disk equipped with the computer device, a smart memory card (SMC), a secure digital (SD) card, a flash card, etc. Of course, the memory can 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 trajectory tracking control method for improving the robot's cornering accuracy and inspection efficiency. In addition, the memory can also be used to temporarily store various types of data that have been output or are about to be output.

[0207] 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 typically 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 for a trajectory tracking control method that improves the robot's cornering accuracy and inspection efficiency.

[0208] 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 trajectory tracking control method for improving the cornering accuracy and inspection efficiency of the robot as described above.

[0209] 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 to enable the at least one processor to perform the steps of the trajectory tracking control method for improving the robot's cornering accuracy and inspection efficiency as described above.

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

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

[0212] Simulation Verification

[0213] The present invention adopts a set of closed irregular broken line reference trajectories, conducts comparative simulation experiments between a conventional controller (without considering cornering accuracy and inspection efficiency) and a trajectory tracking controller that takes cornering accuracy and inspection efficiency into consideration, and obtains simulation results in the Matlab environment.

[0214] Conventional controller: does not consider cornering accuracy and inspection efficiency:

[0215] Trajectory tracking objective function: Controller:

[0216] Initialization parameters: T = 0.5, x(1) = 0, y(1) = 1, V max =1.5m / s,ω max =1.57rad / s, “-” is the reference trajectory, “--” is the tracking trajectory, the simulation results are as follows Figure 4 As shown in the figure, when the controller does not consider the cornering accuracy and inspection efficiency, the trajectory tracking control effect at right-angle turns with large curvature is not ideal, and the trajectory tracking error is large.

[0217] Considering cornering accuracy and inspection efficiency:

[0218] Trajectory tracking objective function:

[0219] Controller:

[0220] Initialization parameters: T = 0.5, x(1) = 0, y(1) = 1, V max =1.5m / s,ω max=1.57rad / s, μ=20, “-” is the reference trajectory, “--” is the tracking trajectory, the simulation results are as follows Figure 5 As shown in the figure, the control effect of the controller that takes into account the cornering accuracy and inspection efficiency is significantly improved, especially in large curvature curves. The robot's driving speed is reduced before entering the curve, which improves the accuracy of trajectory tracking; in small curvature trajectories (straight lines), the robot's movement speed is increased, the overall inspection time is shortened, and the inspection efficiency is improved.

[0221] In summary, Figure 4 and Figure 5 The simulation results verify the effectiveness of the trajectory tracking control algorithm proposed in this paper, which takes cornering accuracy and inspection efficiency into consideration.

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

[0223] 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 trajectory tracking control method for improving the robot's cornering accuracy and inspection efficiency, characterized in that: include: The robot kinematic model is discretized using Euler forward difference method to obtain the robot discrete kinematic model; Define the trajectory tracking error function, and construct the trajectory tracking objective function in combination with the robot discrete kinematics model, and define the trajectory tracking error function E xyθ (k) Where k = 1, 2, ..., N, e x (k) is the horizontal coordinate x(k) of the actual motion trajectory of the robot at the kth discrete point and the horizontal coordinate x of the reference trajectory r Deviation of (k); e y (k) is the vertical coordinate y(k) of the actual motion trajectory of the robot at the kth discrete point and the vertical coordinate y of the reference trajectory r Deviation of (k); e θ (k) is the attitude angle θ(k) of the robot's actual motion trajectory at the kth discrete point and the attitude angle θ of the reference trajectory r (k) Deviation; θ r (k) The method of obtaining is as follows: make: Where △x r is the horizontal coordinate x of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory horizontal coordinate x r The difference between (k-1); △y r is the ordinate y of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory ordinate y r The difference of (k-1); but: Where △x r is the horizontal coordinate x of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory horizontal coordinate x r The difference between (k-1); △y r is the ordinate y of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory ordinate y r (k-1) difference; and / or Define the actual robot linear velocity control value v(k) and the desired linear velocity v h (k) Velocity error function E v (k) is: E v (k)=v(k)-v h (to) discrete point p k The curvature h(k) is: Where, L≥1 and is an integer, D ik ≥0 is a discrete point p k To chord L i The distance, chord L i From point p k-i and p k-i+L+1 Sure; The desired linear speed adjustment is obtained based on the change of curvature h(k) as follows: Where, v h (k) is the inspection robot at p k The expected linear velocity of the point; V max is the maximum linear speed allowed by the inspection robot; μ>0 is the proportional coefficient, which is used to change the degree of influence of curvature change on the expected inspection speed; Solve the objective function to obtain a robot trajectory tracking controller based on predictive control, so that the robot's motion trajectory tracks the specified reference trajectory.

2. The trajectory tracking control method for improving the robot's cornering accuracy and inspection efficiency according to claim 1 is characterized in that: The robot kinematic model is: Where θ is the robot's posture angle, v is the robot's linear velocity during motion, is the steering angular velocity, are the velocity components of the robot in the x and y directions, (x, y) are the coordinates of the robot's center of mass in the moving plane, x is the horizontal coordinate, y is the vertical coordinate, and (v, ω) is the control variable of the model; and / or The robot kinematic model is discretized using the Euler forward difference method, and the obtained robot discrete kinematic model is: Where T is the sampling period, x(k), y(k), θ(k) are the state information of the robot at time k, v(k) is the linear velocity of the robot at time k, ω(k) is the steering angular velocity of the robot at time k, and x(k+1), y(k+1), θ(k+1) are the state information of the robot at time k+1.

3. The trajectory tracking control method for improving the robot's cornering accuracy and inspection efficiency according to claim 1, characterized in that: Assume that the current discrete point p is k , k=1,2,…,N, and consider one point later, the trajectory tracking objective function J(k) to be optimized is: Where, E xyθ (k+1) is the trajectory tracking error function at time k+1, is the trajectory tracking error function E at time k+1 xyθ The transpose of (k+1); E v (k+1) is the difference between the robot linear velocity control value v(k+1) and the desired linear velocity v at time k+1 h (k+1) deviation.

4. The trajectory tracking control method for improving the robot's cornering accuracy and inspection efficiency according to claim 3 is characterized in that: The method for solving the objective function includes: Given a robot's reference trajectory (x r ,y r ); Initialize the parameters involved in obtaining the controller [v(k),ω(k)]; Determine the trajectory tracking objective function J(k) to be optimized by the robot at time k; Solve the objective function J(k) and obtain the controller [v(k), ω(k)]; According to the controller [v(k), ω(k)], the control input of the robot at time k is determined, and the robot's discrete kinematic model is used to update the robot's posture state at time k+1; 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 time k→k+1 and jump to the step of determining the trajectory tracking objective function J(k) to be optimized at time k to continue the loop.

5. The trajectory tracking control method for improving the robot's cornering accuracy and inspection efficiency according to claim 4 is characterized in that: The initialization parameters include the sampling period T of the robot control, the reference trajectory of the robot (x r ,y r ), robot initial position (x(1), y(1)), robot initial posture angle θ(1), maximum linear velocity V max , maximum steering angular velocity ω max , k=1 to start the loop; and / or The objective function J(k) is: Where, E xyθ (k+1) is the trajectory tracking error function at time k+1, is the trajectory tracking error function E at time k+1 xyθ The transpose of (k+1); E v (k+1) is the difference between the robot linear velocity control value v(k+1) and the desired linear velocity v at time k+1 h (k+1) deviation.

6. The trajectory tracking control method for improving the robot's cornering accuracy and inspection efficiency according to claim 5, characterized in that: According to the trajectory tracking error function E xyθ (k) and the velocity error function E defined v (k), the objective function J(k) becomes: In the formula, x(k+1), y(k+1), θ(k+1) are the state information of the robot at time k+1; x r (k+1),y r (k+1),θ r (k+1) is the reference trajectory state information at time k+1; v(k+1) is the robot linear velocity control value at time k+1; v h (k+1) is the expected linear velocity of the robot at time k+1; The specific expression of the predictive controller [v(k), ω(k)] is obtained: Where T is the sampling period, v h (k+1) is the expected linear velocity of the robot at time k+1, x(k+1), y(k+1), θ(k+1) is the state information of the robot at time k+1; x r (k+1),y r (k+1),θ r (k+1) is the reference trajectory state information at time k+1; To ensure the safety and controllability of the mobile robot trajectory tracking task, the value range of the controller [v(k), ω(k)] is designed to meet the constraints: Where V max is the maximum speed of the robot, ω max is the maximum steering angular velocity of the robot.

7. A trajectory tracking control system that improves the robot's cornering accuracy and inspection efficiency, characterized by: include: The model discretization module is used to discretize the robot kinematic model using the Euler forward difference method to obtain the robot discrete kinematic model; Construct the objective function module, define the trajectory tracking error function, and construct the trajectory tracking objective function in combination with the robot discrete kinematics model, and define the trajectory tracking error function E xyθ (k) Where k = 1, 2, ..., N, e x (k) is the horizontal coordinate x(k) of the actual motion trajectory of the robot at the kth discrete point and the horizontal coordinate x of the reference trajectory r Deviation of (k); e y (k) is the vertical coordinate y(k) of the actual motion trajectory of the robot at the kth discrete point and the vertical coordinate y of the reference trajectory r Deviation of (k); e θ (k) is the attitude angle θ(k) of the robot's actual motion trajectory at the kth discrete point and the attitude angle θ of the reference trajectory r (k) Deviation; θ r (k) The method of obtaining is as follows: make: Where △x r is the horizontal coordinate x of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory horizontal coordinate x r The difference between (k-1); △y r is the ordinate y of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory ordinate y r The difference of (k-1); but: Where △x r is the horizontal coordinate x of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory horizontal coordinate x r The difference between (k-1); △y r is the ordinate y of the robot reference trajectory of the kth discrete point r (k) and the k-1th discrete point robot reference trajectory ordinate y r (k-1) difference; and / or Define the actual robot linear velocity control value v(k) and the desired linear velocity v h (k) Velocity error function E v (k) is: E v (k)=v(k)-v h (to) discrete point p k The curvature h(k) is: Where, L≥1 and is an integer, D ik ≥0 is a discrete point p k To chord L i The distance, chord L i From point p k-i and p k-i+L+1 Sure; The desired linear speed adjustment is obtained based on the change of curvature h(k) as follows: Where, v h (k) is the inspection robot at p k The expected linear velocity of the point; V max is the maximum linear speed allowed by the inspection robot; μ>0 is the proportional coefficient, which is used to change the degree of influence of curvature change on the expected inspection speed; The objective function solving module is used to obtain a robot trajectory tracking controller based on predictive control, so that the robot's motion trajectory tracks the specified reference trajectory.

8. Computer equipment, characterized in that The device 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 6.

9. 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 6.

Images