A robot control method and robot

Abstract

The present invention discloses a robot control method and a robot thereof, which relates to intelligent control technology. This solution is proposed to address the problem of poor performance of the MPC algorithm in the prior art. The method comprises the following steps: S1. establishing a robot kinematic model; S2. generating an ideal trajectory between two target points; S3. solving the global optimal solution of the nonlinear motion equation; S4. controlling the robot to adaptively track the ideal trajectory according to the result of step S3. The advantage is that the use of bundled gradients instead of the accurate derivatives that need to be solved in linearization, and the use of bundled Jacobian matrices instead of the Hessian matrix and Jacobian matrix with large computational complexity can greatly improve computational efficiency. The bundled gradient solves the problem of the inability to solve the partial derivatives of complex models with the help of reinforcement learning tools. The new algorithm is applied to the differential robot model control, which greatly improves the adaptive ability in the face of external random interference. Compared with the traditional MPC algorithm, the robustness of the robot system is greatly improved.

Description

Technical Field

[0001] The present invention relates to intelligent control technology, and in particular to a robot control method and a robot thereof. Background Art

[0002] Ever since humanity entered the machine age, people have been fascinated by robots, appearing in numerous films and books. With the development of artificial intelligence (AI) and the rise of mechanization, robots that integrate information technology, communications technology, and AI have become a reality. Intelligent robots are increasingly playing a vital role in manufacturing, transportation, the Internet of Things (IoT), healthcare, and automation. Thanks to technological breakthroughs in advanced control theory and the advancement of AI algorithms, intelligent robots hold broad prospects for future development. Robots can be simply categorized into two types based on their mobility: mobile robots and immobile robots. Mobile robots have a wider range of applications, and their trajectory tracking performance is a key indicator of robot performance.

[0003] Trajectory tracking typically involves setting a given ideal optimized trajectory in a planar coordinate system. A robot starts from a certain starting point, gradually approaches that trajectory, and then follows that optimized trajectory to its final destination. Trajectory tracking requires not only that the robot follow the path but also that it maintains the same angular and linear velocity.

[0004] Common trajectory tracking algorithms can be divided into two categories based on their control principles. The first category is geometric tracking algorithms, which use the kinematic geometry of the mobile robot to calculate the required control variables to achieve the tracking target. Common geometric tracking algorithms include pure tracking, Stanley method front axle control, and rear axle control. The latter two are based on the Ackermann model, while pure tracking is applied to two-wheel and four-wheel differential models and the Ackermann model, respectively.

[0005] The second type of tracking algorithm is based on an algorithmic solution model, combining the control laws of the robot's kinematics and dynamics to calculate the tracking control method of the control variable by solving the optimal solution of the control equation or adjusting parameters. There are many common algorithms, such as the classic PID control in control theory, sliding film control, linear quadratic regulator control (LQR), model predictive control (MPC), etc. Different control methods correspond to different model systems, which can be linear or nonlinear, with high model accuracy or without a clear model, showing a variety of modeling methods.

[0006] Research on linear model optimization is well established, and current research on model control algorithms is based on the nonlinear kinematics of robots. As a fundamental nonlinear model, the difference model has been used to improve a large number of mobile robots and has been widely applied.

[0007] Currently, trajectory tracking algorithms for differential robot models are relatively mature. Many products use algorithms based on linearized kinematic models, employing classic PID control algorithms or LQR algorithms to track trajectories. However, these algorithms often suffer from the following two issues:

[0008] (1) In the ideal case where the external interference is small and the difference between the left and right wheels of the differential robot is small, the kinematic model obtained by Taylor approximation for optimization calculation is small compared with the real kinematic model, which can meet the requirements of trajectory tracking. However, in actual applications, there are random interferences in the robot's working scene, and as the robot's working time increases, its mechanical structure will age. These factors will cause the difference between the kinematic model using linear approximation and the real model to increase significantly. When the control algorithm performs trajectory tracking based on this kinematic model, the robot will meet the tracking requirements, but both the spatial error and the temporal error will be too large.

[0009] (2) Since classical control algorithms (PID, LQR, MPC, etc.) must be optimized based on linear motion models, the differential model is a basic nonlinear system. This system can be linearized near its equilibrium point by taking multiple partial derivatives to obtain the Jacobian matrix and the Hessian matrix. However, solving the Hessian matrix requires a lot of computing power to solve the partial derivatives, which may make it impossible to solve the partial derivatives in certain situations.

[0010] Classic MPC algorithms use finite differences to control nonlinear systems. The key is to linearize the dynamics at each time step, integrate the cost function around the current point in state space, and calculate the feedback gain from this. More importantly, for nonsmooth nonlinear systems whose dynamic equations or inertia matrices are difficult and expensive to calculate, the algorithm allows for extensive sampling of existing simulation results to approximate the true dynamic equations and value functions and generate control signals. However, the input to the MPC algorithm is a single state variable at that moment, without considering the impact of future changes on the system. Its optimization assumption is that the linear kinematics at the current time step approximates the nonlinear kinematics of the next step. Therefore, MPC algorithms perform poorly when faced with constantly changing system kinematics. Summary of the Invention

[0011] The object of the present invention is to provide a robot control method and a robot thereof to solve the problems existing in the above-mentioned prior art.

[0012] The robot control method described in the present invention comprises the following steps:

[0013] S1. Establish robot kinematic model;

[0014] S2. Generate an ideal trajectory between two target points;

[0015] S3. Find the global optimal solution to the nonlinear equation of motion;

[0016] S4. Control the robot to adaptively track the ideal trajectory according to the result of step S3;

[0017] Among them, the robot is a differential robot.

[0018] In step S3, the bundled Jacobian matrix A corresponding to the state matrix at time t is calculated. t And the bundled Jacobian matrix B corresponding to the control signal t ;

[0019] Expressed as:

[0020]

[0021] in,

[0022] Denotes the bundled Jacobian matrix A t The bundle gradient of

[0023] Denotes the bundled Jacobian matrix B t The bundle gradient of

[0024] The coefficients δ and μ obey random distribution, and the corresponding multivariate symmetric probability densities are expressed as p(δ), p(μ)∈L 1 ;

[0025] Using the state space expression to get X t+1 =A t X t +B t u t ;X t is the robot state matrix at time t, u t Represents the control input of the system at time t.

[0026] Monte Carlo integration is used to simplify the calculation of the bundle gradient:

[0027]

[0028] Where N is the ratio of the total running time to each time step, n is the number of samples, Represents a Monte Carlo integration operation.

[0029] The following sub-steps are included:

[0030] S41. Given the robot's initial state matrix X0 and the initial control input sequence U = [u0, u1, u2…, u N-1 ];

[0031] S42. Forward transfer process: Obtain the trajectory between two target points based on the initial state matrix and the initial control input sequence;

[0032] S43. Backward transfer process: calculate each sequence (X t ,u t ) The corresponding value equations and system kinematics are expressed in system space and combined to obtain the sequence of control inputs;

[0033] S44. Calculate and update the value of the control input And based on the series Calculate the value of the cost function;

[0034] if The algorithm converges and exits;

[0035] if let And increase the size of the backtracking search parameter α, and return to step S42;

[0036] if let Reduce the size of the backtracking search parameter α and return to step S43;

[0037] The definitions of the cost equation J and the value equation V are given by the following formula:

[0038]

[0039] l(X t ,u t )=QX t +Pu t , l f (X N )=QX N

[0040]

[0041] Q, P are adjustable weight parameters, and the update process of the control input signal is expressed as:

[0042]

[0043] Where k is the feedforward gain coefficient, K is the feedback gain coefficient, and α is the parameter of the backtracking search;

[0044] S45. Combine the bundled Jacobian matrix A t and the bundle Jacobian matrix B t , we get the discrete form of the system kinematics:

[0045]

[0046] c is a constant matrix.

[0047] The robot described in the present invention is controlled using the control method.

[0048] The robot control method and robot described in the present invention have the advantage of using bundled gradients to replace the exact derivatives required for linearization, and using bundled Jacobian matrices to replace the computationally intensive Hessian and Jacobian matrices, which can greatly improve computational efficiency. Bundled gradients, using reinforcement learning tools, solve the problem of inability to solve partial derivatives in complex models. Furthermore, the present invention applies a novel algorithm to the control of differential robot models, which significantly improves the adaptive capability in the face of external random interference. Compared to traditional MPC algorithms, the robustness of the robot system is significantly improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 It is a flow chart of a robot control method described in the present invention. DETAILED DESCRIPTION

[0050] like Figure 1 As shown, the robot control method described in this invention uses a differential robot kinematic model derived from bundled gradient calculations and applies the iMPC algorithm for adaptive trajectory planning. The essence of trajectory planning is to solve nonlinear state-space equations to find the global optimal solution that minimizes the cost function.

[0051] First, establish a nonlinear robot kinematic model

[0052] The kinematic equations for a mobile robot describe the relationship between the motor speed (input) and the robot's state. Typically, the most important parameter for measuring a mobile robot's state is its posture or pose, which includes the robot's position and orientation relative to a reference coordinate system (the world coordinate system). Position is typically expressed using (x,y) coordinates, with θ representing the angle between the robot's geometric centerline and the x-axis. The kinematic equations for a differential robot can be expressed as follows:

[0053]

[0054] where ω R ,ω L It represents the angular velocity of the left and right wheels and can be directly read by the motor with an encoder. r represents the wheel radius, and L represents the distance between the two wheels.

[0055] Then, calculate the bundle Jacobian matrix

[0056] Because the robot controller is a digital controller, the kinematic equations in the continuous time domain need to be converted into the discrete time domain. The discrete time domain form is represented by the update equation. t The robot state matrix at time t includes position and direction, u t Represents the control input of the system at time t:

[0057] X t+1 =f(X t ,u t )

[0058] Expressed in state space, we can get:

[0059] X t+1 =A t X t +B t u t

[0060] Among them A t , B t The present invention uses a bundled Jacobian matrix to replace the traditional Jacobian matrix. The bundled Jacobian matrix can be expressed as follows:

[0061]

[0062] in Indicates that the bundled gradient, δ, μ obeys a random distribution (usually a Gaussian distribution), and the corresponding multivariate symmetric probability density is expressed as p(δ), p(μ)∈L 1 In order to improve computational efficiency, Monte Carlo integration can be used to simplify the calculation of bundled gradients:

[0063]

[0064] N is the ratio of the total running time to each time step, n is the number of samples, Represents a Monte Carlo integration operation.

[0065] Finally, the following algorithm is applied for adaptive trajectory tracking

[0066] Step S41. Given the robot initial state matrix X0(0,0,0) and the initial control input sequence U=[u 0, u1,u2…,u N-1 ].

[0067] Step S42. Forward transfer process: obtain the trajectory between the two target points based on the initial state and initial input.

[0068] Step S43. Backward transfer process: Calculate each (X t ,ut ) and the system kinematics, expressed in system space. Combining them yields the sequence of control inputs.

[0069] Step S44. Calculate and update the value of the control input Based on Calculate the value of the cost function. If The algorithm converges and exits. let And increase the size of the backtracking search parameter α, and return to step S42. The size of the backtracking search parameter α is reduced and the process returns to step S43.

[0070] The definitions of the cost equation J and the value equation V are given by the following formula:

[0071]

[0072] l(X t ,u t )=QX t +Pu t ,l f (X N )=QX N

[0073]

[0074] Q and P are adjustable weight parameters. The size of Q / P affects the control effect of the controller on different signals. The update process of the control input signal can be expressed as:

[0075]

[0076] Where k is the feedforward gain coefficient, K is the feedback gain coefficient, and α is the parameter of the backtracking search.

[0077] Add linear inequality restrictions: C, d are the coefficient matrices of linear restrictions in state space form

[0078]

[0079] Considering the bundle Jacobian matrix calculated in the previous step, the discrete form of the system kinematics can be expressed as:

[0080]

[0081] c represents a constant matrix.

[0082] The robot described in the present invention is controlled using the control method.

[0083] Those skilled in the art can make various other corresponding changes and deformations based on the technical solutions and concepts described above, and all of these changes and deformations should fall within the scope of protection of the claims of the present invention.

Claims

1. A robot control method, characterized in that: The following steps are involved: S1. Establish robot kinematic model; S2. Generate an ideal trajectory between two target points; S3. Find the global optimal solution to the nonlinear equation of motion; S4. Control the robot to adaptively track the ideal trajectory according to the result of step S3; Among them, the robot is a differential robot; In step S3, the bundled Jacobian matrix A corresponding to the state matrix at time t is calculated. t And the bundled Jacobian matrix B corresponding to the control signal t ; Expressed as: in, Denotes the bundled Jacobian matrix A t The bundle gradient of Denotes the bundled Jacobian matrix B t The bundle gradient of The coefficients δ and μ obey random distribution, and the corresponding multivariate symmetric probability densities are expressed as p(δ), p(μ)∈L 1 ; Using the state space expression to get X t+1＝ A t X t +·B t u t ;X t is the robot state matrix at time t, u t Represents the control input of the system at time t.

2. A robot control method according to claim 1, characterized in that: Monte Carlo integration is used to simplify the calculation of the bundle gradient: Where N is the ratio of the total running time to each time step, n is the number of samples, Represents a Monte Carlo integration operation.

3. A robot control method according to claim 2, characterized in that: In step S4, the following sub-steps are included: S41. Given the robot's initial state matrix X0 and the initial control input sequence U = [u0, u1, u2…, u N-1 ]; S42. Forward transfer process: Obtain the trajectory between two target points based on the initial state matrix and the initial control input sequence; S43. Backward transfer process: calculate each sequence (X t ,u t ) The corresponding value equations and system kinematics are expressed in system space and combined to obtain the sequence of control inputs; S44. Calculate and update the value of the control input And based on the series Calculate the value of the cost function; if The algorithm converges and exits; if let And increase the size of the backtracking search parameter α, and return to step S42; if let Reduce the size of the backtracking search parameter α and return to step S43; The definitions of the cost equation J and the value equation V are given by the following formula: Q, P are adjustable weight parameters, and the update process of the control input signal is expressed as: Where k is the feedforward gain coefficient, K is the feedback gain coefficient, and α is the parameter of the backtracking search; S45. Combine the bundled Jacobian matrix A t and the bundle Jacobian matrix B t , we get the discrete form of the system kinematics: c is a constant matrix, and N is the ratio of the total running time to each time step.

4. A robot controlled by a robot control method according to any one of claims 1 to 3.

Images