Cubic spline trajectory planning and MPC tracking control method for optimizing hydraulic mechanical arm based on genetic algorithm

Abstract

The invention relates to a cubic spline trajectory planning and MPC tracking control method for optimizing a hydraulic mechanical arm based on a genetic algorithm, and belongs to the field of trajectory gauges of hydraulic mechanical arms. According to the method, the minimum time of cubic spline trajectory planning is iteratively solved by taking the time of the whole trajectory as a target function and the kinematics constraint of the mechanical arm as a boundary condition through a genetic algorithm, so that the trajectory planning is optimized, and the grabbing efficiency is improved. Meanwhile, MPC control rates with constraints are designed according to the identification models of all the shafts of the hydraulic mechanical arm, corresponding constraints are designed according to the upper and lower limits of input signals of all the shafts and the upper and lower limits of input variable quantities, and accurate trajectory tracking is achieved. Compared with a traditional PID control rate and a composite PID-DOB control rate, the method has remarkable advantages in the aspects of static errors and dynamic errors, the debugging process is simpler, parameters are fewer, and application and popularization are easier.

Description

Technical Field

[0001] The invention belongs to the field of trajectory planning of hydraulic mechanical arms, and relates to a cubic spline curve trajectory planning and MPC tracking control method based on genetic algorithm optimization of hydraulic mechanical arms. Background Art

[0002] For the trajectory planning of the scrap steel grabbing hydraulic robot arm, although the cubic spline curve trajectory planning can plan a complete trajectory based on a few key points, and the positions of the key points can be determined according to the actual scrap steel grabbing scene, the time information of each key point, that is, when to reach the key point, cannot be easily obtained. This not only reduces its applicability, but also causes the time information of the entire trajectory to be fixed, and it is impossible to optimize the time.

[0003] At the same time, for the tracking control of the scrap steel grabbing hydraulic mechanical arm, the traditional PID control rate and the composite PID-DOB control rate can also play a good effect on the trajectory tracking of the hydraulic unmanned steel grabber, but the debugging process is relatively complicated, involving too many intermediate parameters and PID parameters. Once the grabbing scene of the unmanned hydraulic steel grabber changes, the debugging is too complicated. In addition, due to the characteristics of the hydraulic steel grabber, the input signal of the third braking section of the control cannot be quickly reduced from the dead zone to zero, otherwise the hydraulic oil cannot be supplied in time, resulting in a very drastic deceleration process of the steel grabber's mechanical arm.

[0004] PID (Proportional-Integral-Differential): A PID controller is an automatic control system that measures the error between the set value and the actual value and uses three methods: proportional (P), integral (I), and differential (D) to adjust the output to ensure that the system is stable and achieves the desired performance. The proportional part handles the current error, the integral part solves the accumulated error, and the differential part predicts the future error, working together to optimize the control effect.

[0005] PID-DOB: PID-DOB control is a control strategy that combines the traditional PID controller and the disturbance observer (DOB). It uses the PID controller to handle conventional regulation tasks, while using the disturbance observer to estimate and compensate for external disturbances and uncertainties within the system. This combination can improve the system's resistance to external disturbances and overall performance, making the control more stable and accurate.

[0006] MPC (Model Predictive Control): An advanced control strategy that uses a mathematical model of the system to predict future behavior and achieve the desired system performance by optimizing control inputs. MPC continuously calculates control actions to minimize future prediction errors and take into account possible operating constraints. This approach is particularly suitable for handling multivariable systems and constraints, which are common in complex process control in industrial and high-tech fields.

[0007] LQR (Linear Quadratic Regulator); an optimization control strategy that designs the optimal state feedback control law for a linear system by minimizing a quadratic cost function to balance the state deviation and control energy to achieve the best performance of the system. Summary of the invention

[0008] In view of this, the object of the present invention is to provide a cubic spline curve trajectory planning and MPC tracking control method for a hydraulic manipulator based on genetic algorithm optimization.

[0009] A genetic algorithm is used to iteratively solve the minimum time of cubic spline trajectory planning, taking the time of the entire trajectory as the optimization goal and the kinematic constraints of the robot as the restriction conditions.

[0010] According to the identification model of each axis of the unmanned hydraulic manipulator, the constrained MPC control rate is designed respectively, and the corresponding constraints are designed according to the upper and lower limits of the input signal of each axis and the upper and lower limits of the input variation.

[0011] In order to achieve the above object, the present invention provides the following technical solutions:

[0012] A cubic spline curve trajectory planning method for optimizing a hydraulic manipulator based on a genetic algorithm includes:

[0013] Determine the key point positions and the points that the trajectory needs to pass through based on the actual scrap steel grabbing scenario;

[0014] Original key time setting, setting time information for each key point, that is, the time when the end of the robot arm reaches the key point;

[0015] Encoding, encode the variables of the planning time optimization problem;

[0016] Population generation, through the fitness function screening, generates a population containing multiple variable individuals;

[0017] Optimize the objective function and boundary constraint settings, taking the time of the entire trajectory as the objective function and the kinematic constraints of the robot as the boundary conditions;

[0018] Fitness function calculation,Calculate the fitness value based on the key point sequence, arrival time sequence, trajectory interpolation period and other information, and add the constraints of joint angle and joint speed into the fitness function as the cost function.

[0019] Further, the fitness function calculation process includes:

[0020] Cartesian space trajectory generation: Generate the Cartesian space trajectory based on DH parameters, key point sequence, arrival time sequence, trajectory interpolation period and other information, and calculate the position sequence, velocity sequence and acceleration sequence of the trajectory interpolation point in Cartesian space;

[0021] Calculation of joint angle and joint speed limits: Calculate the maximum joint angle and maximum rotation speed of each joint;

[0022] Trajectory time calculation: Calculate the time taken for the entire trajectory as part of the optimization function;

[0023] Inverse solution calculation: According to the Cartesian space trajectory, the position of each joint is obtained by inverse solution;

[0024] Jacobian matrix calculation: Calculate the Jacobian matrix to obtain the joint space velocity;

[0025] Joint maximum velocity calculation: Calculate the maximum velocity of the three joints in the entire trajectory;

[0026] Constraints are added: The constraints of joint angles and joint velocities are added to the fitness function as cost functions.

[0027] The control method based on the planning method includes:

[0028] Reference angle acquisition: obtain the reference arc corresponding to each axis of the hydraulic manipulator at the current moment from the cubic spline curve trajectory planning based on genetic algorithm optimization;

[0029] Real-time angle acquisition: obtain the measured arc of each axis from the sensor installed on the hydraulic manipulator, and perform compensation conversion to obtain the real-time arc of each axis;

[0030] Rolling optimization: perform rolling optimization on the reference arc and real-time arc of each axis to obtain a control signal vector with a dimension of ;

[0031] The control signal is output, and the first value of the control signal vector is taken as the current control input signal, and it is transmitted to the hydraulic drive system to drive the hydraulic manipulator.

[0032] Further, the rolling optimization process includes:

[0033] Model establishment, based on the identification model of each axis of the hydraulic manipulator, establish the MPC control model;

[0034] Constraint setting: set the constraints of the MPC control model according to the upper and lower limits of the input signals of each axis and the upper and lower limits of the input variation;

[0035] Objective function setting: setting the objective function of rolling optimization to minimize the deviation between the predicted output and the reference signal;

[0036] Control signal calculation, through the optimization algorithm, calculates the optimal control signal vector.

[0037] Furthermore, the constraint setting includes:

[0038] Input constraints, set upper and lower limits on the increment of the control signal;

[0039] Output constraints: set upper and lower limits on the output of the control system.

[0040] Furthermore, the input constraints and output constraints are set in a "soft" constraint manner, which is achieved by adding relaxation factors to the constraints.

[0041] A hydraulic mechanical arm control device, comprising:

[0042] Trajectory planning module: responsible for planning the motion trajectory of the hydraulic manipulator, generating key points and time information on the trajectory; outputting the position and arrival time of each key point on the trajectory, as well as the reference arc of each point on the trajectory; connecting with the MPC control module, using the generated reference arc as the input of the MPC control module to guide the movement of the manipulator;

[0043] MPC control module: responsible for real-time control based on the reference arc provided by the trajectory planning module and the real-time collected robot arm joint angle information, so that the actual motion trajectory of the robot arm tracks the reference trajectory; inputs the reference arc provided by the trajectory planning module and the real-time collected robot arm joint angle information; outputs the control signal of the hydraulic drive system to drive the movement of each joint of the hydraulic robot arm; connects with the hydraulic drive system, outputs the calculated control signal to the hydraulic drive system, and controls the movement of the hydraulic robot arm;

[0044] Hydraulic drive system: According to the control signal output by the MPC control module, drive the movement of each joint of the hydraulic robotic arm so that the actual movement trajectory of the robotic arm tracks the reference trajectory; input the control signal output by the MPC control module; output the movement of each joint of the robotic arm.

[0045] The beneficial effects of the present invention are:

[0046] (1) Not only can the scenario applicability of cubic spline trajectory planning be increased, but the time required for the entire trajectory can also be reduced through iterative optimization of genetic algorithms, thereby improving the grasping efficiency.

[0047] (2) Field experimental results have proved that the static error and dynamic error of the MPC control method for single-axis motion and compound motion tracking are smaller than the PID control rate, and are similar to or even slightly better than the compound PID-DOB control rate. In addition, the debugging process is simpler and the debugging parameters are much smaller than the above two control methods.

[0048] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

[0050] Figure 1 Flowchart of the algorithm for optimizing cubic spline trajectory planning based on genetic algorithm;

[0051] Figure 2 This is the flow chart of the MPC tracking control algorithm;

[0052] Figure 3 This is a flow chart of the MPC control algorithm for a single axis of the boom. DETAILED DESCRIPTION

[0053] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0054] Among them, the drawings are only used for illustrative explanations, and they only represent schematic diagrams rather than actual pictures, and should not be understood as limitations on the present invention. In order to better illustrate the embodiments of the present invention, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the size of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted.

[0055] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right", "front", "rear", etc. indicate the orientation or position relationship, they are based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the terms describing the position relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0056] The algorithm flow chart of optimizing cubic spline trajectory planning based on genetic algorithm is as follows: Figure 1 shown.

[0057] 1) Key point position: The points that a given trajectory needs to pass through can be determined based on the actual scrap steel grabbing scenario.

[0058] 2) Original key time: the time information of each key point, that is, when the end of the robot arm reaches the key point

[0059] 3) Encoding: variables for planning time optimization problems

[0060] 4) Population: The variable individuals selected after each iteration through the fitness function

[0061] 5) Optimization objective function and boundary constraints:

[0062]

[0063] i is the spline trajectory number, n is the number of key points, f(t) is the optimization objective function, q jmax is the maximum joint angle reached by each joint. Similarly, v jmax is the maximum rotation speed achieved by each joint.

[0064] 6) Fitness function pseudo code:

[0065]

[0066]

[0067] The flow chart of MPC tracking control algorithm is as follows Figure 2 shown.

[0068] The MPC control rate first obtains the reference radian corresponding to each axis of the unmanned hydraulic steel grabber at the current moment from the trajectory planning of the cubic spline curve optimized by the genetic algorithm. If it is planned in Cartesian space, the inverse solution is first obtained to obtain the reference radian of each axis; then the measured radian of each axis is obtained from the sensor installed on the hydraulic unmanned steel grabber, and then the sensor compensation conversion is performed according to the position relationship to obtain the real-time radian of each axis; then the reference radian and the real-time radian of each axis are N-thumbnailed p The step size is optimized to obtain a dimension of N c The control signal vector is taken, and its first value is taken as the current control input signal, and it is transmitted to the hydraulic drive system to drive the hydraulic manipulator.

[0069] Rolling Optimization: MPC constructs a dynamic optimization problem and calculates an optimal control sequence at each sampling moment based on the current state and future prediction information. The goal of this optimization problem is to minimize the deviation between the predicted output and the reference signal. In this way, MPC can not only take into account the current state of the system, but also predict and optimize the system behavior in the future, thereby achieving accurate tracking of the reference trajectory.

[0070] The flowchart of the MPC control algorithm for a single-axis arm is as follows: Figure 3 shown.

[0071] Constraint Control:

[0072] For a single-input single-output (SISO) system, assume that the upper limit of the control input signal increment is Δu max , the lower limit is Δu min , then the incremental constraint of the control signal is:

[0073] Δu min ≤Δu(k)≤Δu max

[0074] The constraints on the control signal can be used to deal with the situation where the input change range and upper and lower limits are limited. The input signal of each axis of the unmanned hydraulic steel grabber control system has upper and lower limits and the change range of the input signal is also limited during the final braking. These constraints can be added to the MPC control rate design. Since Δu(k) is the input increment at the current moment, the actual control input variable is the increment variable plus its steady-state value:

[0075] -u min +u(k-1)≤u(k)≤u max +u(k-1)

[0076] In addition to constraining the input signal, the MPC algorithm can also set the upper and lower amplitudes of the output. Assume that the upper limit of the output y(k) of the entire control system is y max , the lower limit is y min , then the output constraints are:

[0077] y min ≤y(k)≤y max

[0078] The constraints on the output of MPC are not mandatory, but a flexible and effective method is adopted by adding a relaxation factor s to the constraints. v (s v >0) is implemented in the form of "soft" constraints. On the one hand, if the output constraint is enforced, the input constraint of the entire system may become invalid, and the control input variable will violate the input constraint and change greatly, resulting in constraint conflict. Generally, when the input variable is more important to the entire system, a larger slack variable is selected to relax the output constraint to resolve the conflict.

[0079] y min -s v ≤y(k)≤y max +s v

[0080] The MPC algorithm pseudo code is as follows

[0081]

[0082]

[0083] The genetic algorithm optimization of spline curve trajectory not only does not require the time information of key points, which enhances its versatility, but also shortens the time used for the entire trajectory and increases efficiency.

[0084] In the case of descending, which is a relatively easy-to-control action, MPC control is not much different from compound PID-DOB control, but both static and dynamic errors are better than PID control; while for ascending, which is an action to overcome gravity, MPC is better than the other two methods in both dynamic and static errors. Moreover, MPC requires fewer adjustment parameters in actual work, is simpler in design, and can be more widely used in a variety of scenarios.

[0085] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A cubic spline curve trajectory planning method for optimizing a hydraulic manipulator based on a genetic algorithm, characterized in that: The method includes: Determine the key point positions and the points that the trajectory needs to pass through based on the actual scrap steel grabbing scenario; Original key time setting, setting time information for each key point, that is, the time when the end of the robot arm reaches the key point; Encoding, encode the variables of the planning time optimization problem; Population generation, through the fitness function screening, generates a population containing multiple variable individuals; Optimize the objective function and boundary constraint settings, taking the time of the entire trajectory as the objective function and the kinematic constraints of the robot as the boundary conditions; Fitness function calculation,Calculate the fitness value based on the key point sequence, arrival time sequence, trajectory interpolation period and other information, and add the constraints of joint angle and joint speed into the fitness function as the cost function.

2. The cubic spline curve trajectory planning method based on genetic algorithm optimization of hydraulic mechanical arm according to claim 1 is characterized by: The fitness function calculation process includes: Cartesian space trajectory generation: Generate the Cartesian space trajectory based on DH parameters, key point sequence, arrival time sequence, trajectory interpolation period and other information, and calculate the position sequence, velocity sequence and acceleration sequence of the trajectory interpolation point in Cartesian space; Joint angle and joint speed limit calculation: calculate the maximum joint angle and maximum rotation speed of each joint; Trajectory time calculation: Calculate the time taken for the entire trajectory as part of the optimization function; Inverse solution calculation: According to the Cartesian space trajectory, the position of each joint is obtained by inverse solution; Jacobian matrix calculation: Calculate the Jacobian matrix to obtain the joint space velocity; Joint maximum velocity calculation: Calculate the maximum velocity of the three joints in the entire trajectory; Constraints are added: The constraints of joint angles and joint velocities are added to the fitness function as cost functions.

3. A control method based on the planning method according to claim 1 or 2, characterized in that: include: Reference angle acquisition: obtain the reference arc corresponding to each axis of the hydraulic manipulator at the current moment from the cubic spline curve trajectory planning based on genetic algorithm optimization; Real-time angle acquisition: obtain the measured arc of each axis from the sensor installed on the hydraulic manipulator, and perform compensation conversion to obtain the real-time arc of each axis; Rolling optimization: perform rolling optimization on the reference arc and real-time arc of each axis to obtain a control signal vector with a dimension of ; The control signal is output, and the first value of the control signal vector is taken as the current control input signal, and it is transmitted to the hydraulic drive system to drive the hydraulic manipulator.

4. The control method according to claim 3, characterized in that: The rolling optimization process includes: Model establishment, based on the identification model of each axis of the hydraulic manipulator, establish the MPC control model; Constraint setting: set the constraints of the MPC control model according to the upper and lower limits of the input signals of each axis and the upper and lower limits of the input variation; Objective function setting: setting the objective function of rolling optimization to minimize the deviation between the predicted output and the reference signal; Control signal calculation, through the optimization algorithm, calculates the optimal control signal vector.

5. The control method according to claim 4, characterized in that: The constraint settings include: Input constraints, set upper and lower limits on the increment of the control signal; Output constraints: set upper and lower limits on the output of the control system.

6. The control method according to claim 5, characterized in that: The input constraints and output constraints are set in a "soft" constraint manner by adding relaxation factors to the constraints.

7. A hydraulic mechanical arm control device, characterized in that: include: Trajectory planning module: responsible for planning the motion trajectory of the hydraulic manipulator and generating key points and time information on the trajectory; Output the position and arrival time of each key point on the trajectory, as well as the reference arc of each point on the trajectory; connect with the MPC control module, and use the generated reference arc as the input of the MPC control module to guide the movement of the robot arm; MPC control module: responsible for real-time control based on the reference arc provided by the trajectory planning module and the real-time collected robot arm joint angle information, so that the actual motion trajectory of the robot arm tracks the reference trajectory; Input the reference arc provided by the trajectory planning module and the real-time collected robot arm joint angle information; output the control signal of the hydraulic drive system to drive the movement of each joint of the hydraulic robot arm; connect with the hydraulic drive system, output the calculated control signal to the hydraulic drive system, and control the movement of the hydraulic robot arm; Hydraulic drive system: According to the control signal output by the MPC control module, it drives the movement of each joint of the hydraulic manipulator, so that the actual movement trajectory of the manipulator tracks the reference trajectory; Input the control signal output by the MPC control module; Output the motion of each joint of the robot arm.

Images