Complex terrain trajectory tracking control method for a six-wheel independent drive and independent steering robot
By combining model predictive control algorithm and sliding mode approach law, the trajectory tracking deviation problem of a six-wheeled independently driven and independently steering robot in complex terrain was solved, and high-precision and stable trajectory tracking control was achieved.
Patent Information
- Application Number
- CN202210915063.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-08-01
- Publication Date
- 2024-11-01
- Estimated Expiration
- 2042-08-01
AI Technical Summary
Existing technologies are insufficient to effectively address the issues of large trajectory tracking deviations and low tracking accuracy in six-wheeled independently driven and independently steering robots on complex terrains, especially sloping terrains. Particularly in environments with alternating flat and sloping surfaces, the robot is prone to problems such as idle spinning of the middle wheels, energy loss, and rollover.
By employing a model predictive control algorithm combined with a sliding mode approach law, and by real-time monitoring of the robot's pose and tire forces, the control input information is optimized. Combined with an outdoor positioning system and sensor feedback, trajectory tracking control is achieved.
It improves the accuracy and stability of robot trajectory tracking in complex terrain, reduces the phenomenon of intermediate wheel idling, ensures that the robot travels along the given trajectory, and reduces control difficulty and error.
Smart Images

Figure CN115268450B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the technical field of trajectory tracking control of wheeled mobile robots, and in particular relates to a trajectory tracking control method of an unmanned autonomous robot. Background Art
[0002] With the development of science and technology, intelligent robots have played an increasingly important role in human production and life, greatly improving the production efficiency of industrial production. In the rapid development of mobile robot technology, wheeled mobile robots have important applications in industrial production, national defense and military, and multi-machine coordination due to their high mobility and strong load capacity.
[0003] The model prediction algorithm is an optimization control algorithm used in the industrial field that has emerged in Europe and the United States. After years of development, it has been applied in the industrial field and intelligent control field. With the improvement of the algorithm theory, it has become a classic algorithm frequently used in the industrial field. Although there are differences in the application of algorithms in various fields, they all follow the basic principles of prediction model, rolling optimization, and feedback correction.
[0004] The trajectory tracking control technology of wheeled mobile robots is the basis for mobile robots to perform various tasks through motion control. The trajectory tracking technology based on model predictive control makes full use of the rolling optimization and feedback correction links in the model prediction process, greatly improving the accuracy of the trajectory tracking process, achieving precise tracking under complex curvature and broken line trajectory conditions, and improving the motion accuracy and control efficiency of the wheeled autonomous mobile robot platform.
[0005] Compared with other common mobile robot platforms such as leg-type mobile robots and tracked mobile robots, wheeled mobile robots are more adaptable to a variety of terrains and complex environmental conditions, and have greater advantages in rapid movement, flexible turning and crossing slopes. They can also cooperate with other wheeled mobile robots to build a multi-machine collaborative platform, have higher robustness, and are more efficient in task completion and team collaboration. Therefore, the research and further development of wheeled mobile robots is one of the important directions in the current and future research field of mobile robots.
[0006] The six-wheeled independent driving and independent steering robot is a new model in the field of wheeled mobile robots. Compared with traditional two-wheeled and four-wheeled robots, the six-wheeled independent driving and independent steering robot has a greater improvement in single load capacity, moving speed, climbing ability, driving ability and steering accuracy. Therefore, the six-wheeled independent driving and independent steering robot is more suitable for the execution of special environment tasks in complex terrain and unknown situations, and is of great significance in the transportation of goods, rescue materials, building materials, etc. in special environments. First, due to its greatly enhanced load capacity, various platforms, various sensors or materials can be installed on it, which is suitable for tasks in different situations; secondly, the six-wheeled independent driving and independent steering robot is driven by an electric motor, which greatly reduces noise, and can also adjust the number of driving wheels according to working conditions to improve endurance. Thirdly, due to its high flexibility and high maneuverability, it can adapt to a variety of extreme working conditions and has high applicability in disaster relief, geological exploration and other special fields.
[0007] However, in actual tasks, the robot's movement environment is often more complex. It is usually not just a single flat road, but an environment where multiple complex roads are combined. When a wheeled robot, especially a six-wheel independent drive and independent steering robot, moves on complex terrain, such as moving on a slope or moving from a flat ground to a slope, due to its multi-wheel characteristics, it is necessary to consider the movement of each wheel at the same time and perform appropriate control, so the control difficulty increases significantly. In addition, the six-wheel independent drive and independent steering robot will also face the problem of middle wheel idling when going uphill (such as Figure 3 In this case, if certain adaptive control is not added, the driving ability of the six-wheel independent drive and independent steering robot will be reduced. When faced with a large slope, a low road friction coefficient or a heavy robot load, the robot will not be able to travel along the normal route and may slip, deviate from the route or even roll over. Therefore, solving the trajectory tracking problem of the six-wheel independent drive and independent steering robot under such terrain conditions has great application value.
[0008] Current research focuses on the trajectory tracking control of four-wheeled robots / vehicles and tracked robots on flat ground. Usually, the experimental environment is based on flat roads with high friction coefficients and other good terrain conditions. In actual field operation environments, we usually face terrain environments such as the above, and there are various factors that lead to inaccurate tracking problems. Summary of the invention
[0009] Based on the high performance characteristics of the six-wheeled mobile robot, the purpose of the present invention is to propose a trajectory tracking control method based on a model predictive control algorithm in a special environment with a slope terrain as a typical terrain feature. It is mainly aimed at the scenario where a six-wheel independently driven and independently steered robot moves in an alternating environment of flat ground and slopes and on a slope surface, to solve the problems of large trajectory tracking deviation and low tracking accuracy in this scenario.
[0010] The complex terrain trajectory tracking control method of the six-wheel independently driven and independently steered robot of the present invention comprises the following steps:
[0011] S1: Collect the posture information and physical parameters of the six-wheel robot, establish a spatial coordinate system and a kinematic model of the robot according to the robot model; convert the given trajectory into state information (coordinate points) in the coordinate system, filter the robot's posture information (state quantity X, Y coordinates and yaw angle), robot parameters and state information of the given trajectory, and remove coordinate points with excessive offset and parameter information that does not conform to the actual situation;
[0012] S2: Take the first and second derivatives of the processed given trajectory coordinate points and calculate the curvature as the given trajectory related information input of the linear model predictive control algorithm; analyze the robot's heading angle deviation based on the processed robot posture information and trajectory state information, and track the heading angle before trajectory tracking to make the robot's orientation consistent with the orientation of the trajectory starting point;
[0013] S3: Establish a state space equation with the processed given trajectory state information and robot posture information (state quantity X, Y coordinates and yaw angle), discretize and linearize it, input it into the linear model predictive control controller, convert the trajectory tracking problem into a quadratic programming optimal solution problem, and obtain the output control quantity information (robot moving speed and steering angle) by setting constraints;
[0014] S4: Filter the control information to remove steering angles that deviate too much and speed values that surge; dynamically optimize the processed control information and send it to the robot single-wheel controller to perform the first trajectory tracking control;
[0015] S5: The actual moving speed of the robot's single wheel, the tire force, and the offset between the robot's center of mass and the trajectory under the outdoor positioning system are sent to the controller as feedback through the robot's single wheel sensor for optimization control;
[0016] S6: When the force on some wheels is significantly different from that on other wheels or the deviation between the actual moving speed of a single wheel and the theoretical speed exceeds a threshold, it is determined that the force on the robot is abnormal (the tire is suspended at the junction of the slope and the plane or other situations), and the center of mass and trajectory offset and the deviation between the single wheel moving speed and the theoretical speed are input into the optimization module;
[0017] S7: The optimization module performs error optimization through the exponential reaching law in the sliding mode control algorithm, and sends the calculated single-wheel moving speed and steering angle to the single-wheel controller;
[0018] S8: Update the robot position to realize the robot's trajectory tracking control.
[0019] The process of establishing the robot kinematic model in S1 is:
[0020] Assume that the wheel axle spacing of the six-wheeled robot is L, {O} is the world coordinate system, {R} is the robot coordinate system, its origin is the center of the robot's rear wheel, the X-axis is the robot's forward direction, the Y-axis is perpendicular to the X-axis, the dark-colored wheel in the middle is the equivalent wheel of the robot's left and right wheels, and the robot speed in the robot coordinate system is:
[0021] V xR =v,V yR =0;
[0022] The intersection of the dotted lines is the instantaneous center of rotation (P). The reference point on the robot (the geometric center of the middle wheel) moves in an arc around the instantaneous center. θ is the angle between the forward direction and the world coordinate system, and its angular velocity is:
[0023]
[0024] Turning radius:
[0025]
[0026] γ is the steering angle of the front and rear wheels (the steering angles of the front and rear wheels are the same in magnitude and opposite in direction);
[0027] In the world coordinate system, the kinematic equation of the robot is written as:
[0028]
[0029]
[0030]
[0031] The standard simplified model is:
[0032]
[0033] This kinematic equation is the state quantity of the model predictive control;
[0034] In addition, in the world coordinate system, the velocity constraint in the Y direction is:
[0035]
[0036] The other constraints are:
[0037]
[0038] In the actual model, the control is usually the robot speed v and the steering angle γ, so the control quantity is:
[0039]
[0040] Therefore, the nonlinear state-space equations defining the model are:
[0041]
[0042] The given trajectory is the reference trajectory, that is, after the model is processed, the kinematic equation of the model is equal to the kinematic equation of the trajectory. Let the kinematic equation of the given trajectory be the reference quantity (ξ r ,U r ), the nonlinear state equation is Taylor expanded at the reference point, that is, the model is approximately linearized:
[0043]
[0044] To facilitate calculation, the error is defined as but:
[0045]
[0046] Therefore, the rearranged linear state space equation is:
[0047]
[0048] in,
[0049] The linear state space equations are discretized using the forward Euler method:
[0050]
[0051] Wherein, T is the sampling time, k = 1, 2, 3, ...;
[0052] After simplifying the equation, we get:
[0053]
[0054] Where I is the identity matrix,
[0055] The process of the linear model predictive control algorithm in S2 is as follows:
[0056] The three steps of the model predictive control algorithm are: prediction model, rolling optimization and feedback correction; through the definition in the six-wheel robot kinematic model, the prediction model is obtained as follows:
[0057]
[0058]
[0059]
[0060] ……;
[0061]
[0062] After sorting:
[0063]
[0064] The variables are:
[0065] Therefore, the trajectory tracking problem is converted into a quadratic programming optimization problem. The control objective is to minimize U'(k) and make Y approach Y. r , so the design cost J is:
[0066] J=[(YY r ) T Q(YY r )+U'(k) T RU'(k)];
[0067] Among them, the prediction step size is Np, and the control step size is Nc;
[0068] Weight matrix Q = Qc*eye(Nx*Np,Nx*Np), R = Rc*eye(Nc*Nu), where Qc and Rc are matrix weight coefficients;
[0069] To simplify the calculation, let:
[0070]
[0071] so:
[0072] YY r =E+ΦU′(k);
[0073] After importing:
[0074] J=[E T QE+(ΦU'(k)) T Q(ΦU'(k))+2E T Q(ΦU'(k))+U'(k) T RU'(k)];
[0075] Since the main control variable is U'(k), the irrelevant terms can be ignored, and after approximate processing, we can get:
[0076] J=[U'(k) T (Φ T QΦ+R)U'(k)+2E 2 QΦU'(k)];
[0077] After designing the cost function, in order to ensure that the calculation results are consistent with the actual situation and avoid errors caused by the steering angle or speed surge, it is necessary to define constraints, Umin≤U'(k)≤Umax, including steering angle constraints and speed constraints;
[0078] Therefore, the problem of model predictive control is finally converted into the optimal solution of quadratic programming problem, that is:
[0079] minJ=min([U'(k) T (Φ T QΦ+R)U'(k)+2E 2 QΦU'(k)]);
[0080] stUmin≤U′(k)≤Umax;
[0081] In the process of model predictive control, it is necessary to select reasonable prediction step size and control step size to ensure the rationality of rolling optimization. The selected prediction step size Np = 60 and control step size Nc = 30;
[0082] The feedback correction link in model predictive control is to detect the actual output of the object in the new sampling link, and use the real-time information to optimize the model-based results, so the robot position information needs to be updated in real time and input into the model;
[0083] The robot position is updated as:
[0084] Discretize the input trajectory curve into coordinate points (cx, cy). Calculate the first-order derivative P of the trajectory according to the discrete coordinate points d and the second-order derivative P dd ;
[0085] Calculate the curvature of the trajectory curve:
[0086]
[0087] Calculate the angle between the trajectory curve and the coordinate axis:
[0088] cyaw=atan(Pd);
[0089] The tracking error is calculated by the trajectory coordinate point (cx(i), cy(i)) and the robot position coordinate (x(i), y(i)); the angle cyaw is used to calculate the orientation update, and the orientation angle yaw input at the beginning is used to calculate the orientation of the robot at the beginning. From the next calculation, the size of the yaw angle is determined by the orientation update of the following formula;
[0090] Facing angle update:
[0091]
[0092] Through the above operation, the speed v and the front wheel steering angle γ required for the robot to track the trajectory are calculated.
[0093] The process of error optimization by the optimization module in S7 through the exponential reaching law in the sliding mode control algorithm is:
[0094] Define the threshold function:
[0095] P = P1 + P2;
[0096] P1=v a -v r ,P2=e;
[0097] Among them, v a is the actual speed of the robot, v r is the robot's calculated speed, and e is the tracking error, which is the Euclidean distance between the robot's current position and the calculated position;
[0098] Tracking Error:
[0099]
[0100] When P1 or P2 exceeds the set threshold, set the speed optimization function to enter the speed optimization function:
[0101] V n =c 4k *(v a ±c 1k (c 2k *s+c 3k *sign(s));
[0102] Where s = c 0k *(v a -v r ), sign is the sign function, c 0k 、c 1k 、c2k 、c 3k 、c 4k is the proportional coefficient. The speed calculated by the speed optimization function is the single-wheel speed optimized based on the first calculation of the model predictive control. The motion state is judged according to the robot's single-wheel speed error and torque output, and the appropriate proportional coefficient is defined.
[0103] The trajectory tracking control method proposed in the present invention solves the trajectory tracking control problem of a six-wheel independently driven and independently steered robot in special environments such as flat terrain, sloping terrain, alternating environments and other complex terrains. The present invention is aimed at the real motion environment of a six-wheel independently driven and independently steered robot, and the proposed invention content has been verified on an actual six-wheel independently driven and independently steered robot. In addition, the present invention further expands the model and application environment of the trajectory tracking control field, mainly based on the model predictive control algorithm for trajectory tracking and the sliding mode convergence law for speed optimization, which is a brand-new control method in the field of mobile robot trajectory tracking.
[0104] Compared with the prior art, the innovation of the present invention is mainly reflected in the following aspects:
[0105] 1. Use a new model of a six-wheeled independently driven and independently steered robot. When the six-wheeled independently driven and independently steered robot moves in complex terrain, compared with two-wheeled and four-wheeled robots, the six-wheeled robot has a larger turning radius at the same steering angle, and the amount of calculation is greatly increased. Due to its multi-wheel characteristics, it is necessary to consider the movement of each wheel at the same time and perform appropriate control to avoid mutual antagonism between the wheels, which greatly increases the difficulty of control. By adding force sensors and encoders to the drive shafts of each wheel to feedback the real-time running speed value to judge the current wheel running status, and combining the six-wheeled robot position control method under the outdoor positioning system, the robot's stability and accuracy in the trajectory tracking process are greatly improved;
[0106] 2. Trajectory tracking for special terrain environments. The present invention is mainly aimed at the movement of a six-wheel independently driven and independently steered robot in a special terrain environment with a flat ground environment as the typical feature of going uphill and on a slope. In this environment, the middle wheel is prone to idling, and the resulting problems include energy loss, mechanism damage, robot rollover and other problems. The present invention solves the various problems caused by the idling of the middle wheel during the uphill process and the speed mismatch caused by uneven force on each wheel when moving on a slope. An error detection and optimization module is also provided. If the above phenomenon occurs, the wheel with the problem is automatically detected and the speed is optimized and matched in combination with the current movement state of the wheel to ensure that the robot travels along a given trajectory;
[0107] 3. Model predictive control is combined with sliding mode approach rate for optimization. The current model predictive control algorithm is slow to adjust when receiving target feedback. The optimization function proposed in the speed optimization module of the present invention is combined with the sliding mode adaptive approach law to optimize the speed. The error value between the actual speed and the theoretical speed is used as input to set the optimization function, which can enable the robot to optimize the speed when there is a slight deviation, thus avoiding the situation where the robot deviates from the trajectory to a large extent.
[0108] 4. In current research, the combination of model predictive control algorithm and sliding mode control usually takes model predictive control as the first step of trajectory tracking, and then uses sliding mode control to perform yaw moment control, position control or center of mass sideslip angle control, etc. The control is difficult and the process is cumbersome. The present invention calculates the robot's running speed and deflection angle through the model predictive control algorithm, uses the sliding mode reaching law to perform speed control, and performs speed compensation, which reduces the control difficulty and greatly speeds up the calculation process of the control algorithm;
[0109] In addition, the present invention also provides a new model of a six-wheel independently driven and independently steering robot in the field of mobile robot trajectory tracking and a tracking method in a complex terrain environment with sloping terrain as a typical feature. Based on this, we can further explore the trajectory tracking research content of more terrains and new models, and have a high promotion value. BRIEF DESCRIPTION OF THE DRAWINGS
[0110] Figure 1 It is the overall flow chart of the present invention.
[0111] Figure 2 It is a kinematic model of a six-wheel independently driven and independently steering robot.
[0112] Figure 3 It is the block diagram of model predictive control operation.
[0113] Figure 4 This is a schematic diagram of wheel suspension.
[0114] Figure 5 This is the robot trajectory tracking result diagram.
[0115] Figure 6 It is the error diagram of the robot’s center of mass position and tracking trajectory. DETAILED DESCRIPTION
[0116] The present invention proposes a trajectory tracking control method for a six-wheeled independently driven and independently steered robot based on a model predictive control algorithm. By real-time tracking and feedback of the robot position and real-time monitoring of the robot motion state, the robot trajectory tracking capability is greatly improved and the error is greatly reduced (the error is controlled within 5% of the robot length). The model predictive control algorithm adopted has good control effect and strong robustness. The optimization algorithm can be used within an effective time to optimize the control sequence of the input model, thereby generating a new control sequence. However, the controller only optimizes the first set of control inputs of the control sequence at a time, and then repeats this step, iteratively calculates the new controlled sequence, and greatly improves the control accuracy. The robot position deviation and speed deviation are used as input quantities, and the sliding mode approach rate is used to optimize the speed to ensure the real-time matching of the speed.
[0117] The specific implementation modes of the present invention are further described below in combination with the embodiments and the accompanying drawings to make the technical solutions of the present invention easier to understand and grasp.
[0118] The overall process of the complex terrain trajectory tracking control method of the six-wheel independently driven and independently steered robot of the present invention is as follows: Figure 1 As shown, the following modules are needed:
[0119] Environmental perception module: Due to the limitations of the robot itself, it is usually necessary to add various sensors to increase the robot's perception of the surrounding environment, including position information, robot status information and environmental information, etc., and it is necessary to fuse various types of information and send them to the processor for processing. The sensors used in the present invention mainly include: RTK (real-time kinematic measurement), IMU (inertial measurement unit), torque sensor, etc.
[0120] Tracking control module: The tracking control module is the most important part of the present invention and is also the output part of the robot. The robot running speed and steering angle obtained by solving the quadratic programming problem through model predictive control are used to track the trajectory to ensure stable and accurate tracking of the robot.
[0121] Error detection optimization module: In the present invention, the robot operates on a sloped road in addition to the conventional flat road, so the robot's single wheel may idle or the speed may not match. By designing an error detection optimization module, the robot's output torque, position and single wheel speed are detected, and corrections are made in time when errors occur, which greatly improves the tracking accuracy.
[0122] The trajectory tracking control method of the six-wheel independent driving and independent steering robot proposed in this invention is shown in Figure 1 , including the following steps:
[0123] (1) The position information and physical parameters of the six-wheeled robot are collected through the outdoor positioning system, and the spatial coordinate system and the robot's kinematic model are established according to the robot model; the given trajectory is converted into state information (coordinate points) in the coordinate system, and the robot's position information (state quantity X, Y coordinates and yaw angle), robot parameters measured by sensors, and state information of the given trajectory are filtered to remove coordinate points with excessive offset and parameter information that does not conform to the actual situation;
[0124] (2) Taking the first and second derivatives of the processed given trajectory coordinate points and calculating the curvature as the given trajectory related information input of the linear model predictive control algorithm; analyzing the robot's heading angle deviation through the outdoor positioning system based on the processed robot posture information and trajectory state information, and tracking the heading angle before trajectory tracking so that the robot's orientation is consistent with the orientation of the trajectory starting point;
[0125] (3) The state space equation is established based on the processed given trajectory state information, robot posture information (state quantity X, Y coordinates and yaw angle), and then discretized and linearized. The equation is input into the linear model predictive control controller, and the trajectory tracking problem is converted into a quadratic programming optimal solution problem. The output control quantity information (robot moving speed and steering angle) is obtained by setting constraints.
[0126] (4) Filtering the control information to remove steering angles that deviate too much and speed values that surge; dynamically optimizing the processed control information and sending it to the robot's single-wheel controller for initial trajectory tracking control;
[0127] (5) The actual moving speed of the robot's single wheel, the force on the tire, and the offset between the robot's center of mass and the trajectory under the outdoor positioning system are sent to the controller as feedback through the robot's single wheel sensor for optimization control;
[0128] (6) When the force on one or more wheels differs greatly from that on other wheels or the deviation between the actual moving speed of a single wheel and the theoretical speed exceeds a threshold, it is determined that the force on the robot is abnormal (the tire is suspended at the junction of the slope and the plane or other situations), and the center of mass and trajectory offset and the deviation between the single wheel moving speed and the theoretical speed are input into the optimization module;
[0129] (7) The optimization module performs error optimization through the exponential reaching law in the sliding mode control algorithm and sends the calculated single-wheel moving speed and steering angle to the single-wheel controller;
[0130] (8) Update the robot position to realize the robot’s trajectory tracking control.
[0131] The specific implementation steps are as follows.
[0132] First, the kinematic model of the six-wheel robot is established.
[0133] like Figure 2 As shown in the figure, the kinematic relationship of the six-wheel independent drive and independent steering robot is as follows. Assume that the wheel axle spacing of the robot is L, {O} is the world coordinate system, {R} is the robot coordinate system, its origin is the center of the robot's rear wheel, the X-axis is the robot's forward direction, the Y-axis is perpendicular to the X-axis, the dark-colored wheel in the middle is the equivalent wheel of the robot's left and right wheels, and the robot speed in the robot coordinate system is:
[0134] V xR =v,V yR =0
[0135] The intersection of the dotted lines is the instantaneous center of rotation (P). The reference point on the robot (the geometric center of the middle wheel) moves in an arc around the instantaneous center. θ is the angle between the forward direction and the world coordinate system, and its angular velocity is:
[0136]
[0137] Turning radius:
[0138]
[0139] γ is the steering angle of the front and rear wheels (the steering angles of the front and rear wheels are the same in magnitude but opposite in direction).
[0140] In the world coordinate system, the kinematic equation of the robot can be written as:
[0141]
[0142]
[0143]
[0144] The standard simplified model is:
[0145]
[0146] This kinematic equation is the state variable of model predictive control.
[0147] In addition, in the world coordinate system, the velocity constraint in the Y direction is:
[0148]
[0149] The other constraints are:
[0150]
[0151] In the actual model, the control is usually the robot speed v and the steering angle γ, so the control quantity is:
[0152]
[0153] Therefore, the nonlinear state space equation of the model can be defined as:
[0154]
[0155] The given trajectory is the reference trajectory, that is, after the model is processed, the kinematic equation of the model is equal to the kinematic equation of the trajectory. Let the kinematic equation of the given trajectory be the reference quantity (ξ r ,U r ), the nonlinear state equation is Taylor expanded at the reference point, and the model can be approximately linearized:
[0156]
[0157] To facilitate calculation, the error is defined as but:
[0158]
[0159] Therefore, the rearranged linear state space equation is:
[0160]
[0161] in,
[0162] The linear state space equations are discretized using the forward Euler method:
[0163]
[0164] Wherein, T is the sampling time, k=1, 2, 3, ...
[0165] After simplifying the equation, we can get:
[0166]
[0167] Where I is the identity matrix,
[0168] At this point, after designing the robot's kinematic model and linearizing and discretizing the given reference trajectory, the model predictive control algorithm process can be input to perform operations to calculate the robot's speed and steering angle.
[0169] Figure 3 The model predictive control operation block diagram is given. The model predictive control algorithm process is as follows:
[0170] The three key steps of the model predictive control algorithm are: prediction model, rolling optimization and feedback correction. According to the above definition, the prediction model is:
[0171]
[0172]
[0173]
[0174] …
[0175]
[0176] After sorting:
[0177]
[0178] The variables are:
[0179] Therefore, the trajectory tracking problem can be converted into a quadratic programming optimization problem. The control goal is to minimize U'(k) and make Y approach Y. r , so the design cost J is:
[0180] J=[(YY r ) T Q(YY r )+U'(k) T RU'(k)]
[0181] Among them, the prediction step size is Np, and the control step size is Nc.
[0182] The weight matrix Q=Qc*eye(Nx*Np,Nx*Np), R=Rc*eye(Nc*Nu), where Qc and Rc are matrix weight coefficients.
[0183] To simplify the calculation, let:
[0184]
[0185] so:
[0186] YY r =E+ΦU′(k)
[0187] After importing, you can get:
[0188] J=[E T QE+(ΦU'(k)) T Q(ΦU'(k))+2E T Q(ΦU'(k))+U'(k)T RU'(k)]
[0189] Since the main control variable is U'(k), the irrelevant terms can be ignored, and after approximate processing, we can get:
[0190] J=[U'(k) T (Φ T QΦ+R)U'(k)+2E 2 QΦU'(k)]
[0191] After designing the cost function, in order to ensure that the calculation results are consistent with the actual situation and avoid errors caused by steering angle or speed surges, it is necessary to define constraints, Umin≤U'(k)≤Umax, including steering angle constraints and speed constraints.
[0192] It can be concluded that the problem of model predictive control is ultimately converted into the optimal solution of the quadratic programming problem, namely:
[0193] minJ=min([U'(k) T (Φ T QΦ+R)U'(k)+2E 2 QΦU'(k)])
[0194] stUmin≤U′(k)≤Umax
[0195] The above process is the main part of the prediction model in model predictive control.
[0196] In the process of model predictive control, it is necessary to select reasonable prediction step size and control step size to ensure the rationality of rolling optimization. In the present invention, the selected prediction step size Np=60 and the control step size Nc=30 are used. Experiments show that this selection is reasonable.
[0197] The feedback correction link in model predictive control is a new sampling link that detects the actual output of the object and uses real-time information to optimize the model-based results. Therefore, the robot position information needs to be updated in real time and input into the model.
[0198] The robot position is updated as:
[0199] Discretize the input trajectory curve into coordinate points (cx, cy). Calculate the first-order derivative P of the trajectory according to the discrete coordinate points d and the second-order derivative P dd .
[0200] Calculate the curvature of the trajectory curve:
[0201]
[0202] Calculate the angle between the trajectory curve and the coordinate axis:
[0203] cyaw=atan(Pd)
[0204] The robot position coordinates (x(i), y(i)) and orientation angle yaw, trajectory coordinate points (cx(i), cy(i)) and angle cyaw, curvature k and weight matrix, step size and other variables are input into the controller, and the tracking error is calculated by the trajectory coordinate points and the robot position coordinates, and the orientation angle update is calculated by the angle cyaw. The orientation angle yaw input at the beginning is used to calculate the orientation of the robot at the beginning. From the next operation, the size of the yaw angle is determined by the orientation angle update of the following formula. In addition, the trajectory coordinate points and the angle cyaw are also the above ξ r The weight matrix (Q and R in the above) and the step size (Np and Nc in the above) are required for the actual calculation in the input controller. The curvature k is the operation of the first-order term vector and the second-order term matrix in the quadratic programming matrix of the above model predictive control process.
[0205] Facing angle update:
[0206]
[0207] Through the operation of the above three parts, the speed v and the front wheel steering angle γ required for the robot trajectory tracking can be calculated.
[0208] At this point, the calculation of the model predictive control part is completed.
[0209] However, during the operation of the robot, when it runs at the intersection of a flat road and a slope (such as Figure 4 As shown in the figure, or because of the uphill slope, the robot load is unbalanced or there is a speed mismatch when tracking the slope surface trajectory. An error detection module is added and a speed optimization function is designed to ensure the accuracy of trajectory tracking. The specific optimization function is the following V n , other functions (such as P and e, etc.) are the judgment conditions for entering the error optimization process.
[0210] Define the threshold function:
[0211] P=P1+P2
[0212] P1=v a -v r ,P2=e
[0213] Among them, v a is the actual speed of the robot, v r is the calculated speed of the robot, and e is the tracking error, which is the Euclidean distance between the current position of the robot and the calculated position.
[0214] Tracking Error:
[0215]
[0216] When P1 or P2 exceeds the set threshold, set the speed optimization function to enter the speed optimization function:
[0217] V n =c 4k *(v a ±c 1k (c 2k *s+c 3k *sign(s)))
[0218] Where s = c 0k *(v a -v r ), sign is the sign function, c 0k 、c 1k 、c 2k 、c 3k 、c 4k is the proportional coefficient. The speed calculated by the speed optimization function is the single-wheel speed optimized based on the first calculation of the model predictive control. The motion state is judged according to the robot's single-wheel speed error and torque output, and the appropriate proportional coefficient is defined.
[0219] To verify the effectiveness of the invention, simulation was carried out in MATLAB software in conjunction with CoppeliaSim, and verification was carried out in an actual robot model. The tracking effect was verified by designing a trajectory function and the error was output, such as Figure 5 and Figure 6 As shown, it can be seen through verification that the error of the algorithm is within 0.06m when moving in a straight line on a slope, and within 0.08m when turning on a slope. The tracking effect is accurate, which also shows that the control algorithm designed in the present invention is more stable.
[0220] The experiment shows that the model predictive controller proposed in the present invention can quickly and accurately track the reference trajectory, and has a smaller fluctuation error during the tracking process. It also proves that it can adapt to complex environments such as slopes, thereby verifying the effectiveness and accuracy of the controller of the present invention, improving the driving stability and safety of the vehicle, and having important significance for trajectory tracking control applied to wheeled mobile robots.
Claims
1. A complex terrain trajectory tracking control method for a six-wheel independently driven and independently steered robot, characterized in that: The following steps are involved: S1: Collect the posture information and physical parameters of the six-wheel robot, establish a spatial coordinate system and a kinematic model of the robot according to the robot model; convert the given trajectory into state information in the coordinate system, filter the robot's posture information, robot parameters and state information of the given trajectory, and remove coordinate points with excessive offset and parameter information that does not conform to the actual situation; S2: Take the first and second derivatives of the processed given trajectory coordinate points and calculate the curvature as the given trajectory related information input of the linear model predictive control algorithm; analyze the robot's heading angle deviation based on the processed robot posture information and trajectory state information, and track the heading angle before trajectory tracking to make the robot's orientation consistent with the orientation of the trajectory starting point; S3: Establish state space equations with the processed given trajectory state information and robot posture information, discretize and linearize them, input them into the linear model predictive control controller, convert the trajectory tracking problem into a quadratic programming optimal solution problem, and obtain the output control quantity information by setting constraint calculations; S4: Filter the control information to remove steering angles that deviate too much and speed values that surge; dynamically optimize the processed control information and send it to the robot single-wheel controller to perform the first trajectory tracking control; S5: The actual moving speed of the robot's single wheel, the tire force, and the offset between the robot's center of mass and the trajectory are sent to the controller as feedback through the robot's single wheel sensor for optimization control; S6: When the force on some wheels is significantly different from that on other wheels or the deviation between the actual moving speed of a single wheel and the theoretical speed of the robot exceeds a threshold, it is determined that the force on the robot is abnormal at this time, and the center of mass and trajectory offset and the deviation between the single wheel moving speed and the theoretical speed are input into the optimization module; S7: The optimization module performs error optimization through the exponential reaching law in the sliding mode control algorithm, and sends the calculated single-wheel moving speed and steering angle to the single-wheel controller; S8: Update the robot position to realize the robot's trajectory tracking control.
2. The complex terrain trajectory tracking control method of the six-wheel independently driven and independently steered robot according to claim 1 is characterized in that: The process of establishing the kinematic model of the six-wheeled robot in S1 is: Assume that the wheel axle spacing of the six-wheeled robot is L, {O} is the world coordinate system, {R} is the robot coordinate system, its origin is the center of the robot's rear wheel, the X-axis is the robot's forward direction, the Y-axis is perpendicular to the X-axis, the dark-colored wheel in the middle is the equivalent wheel of the robot's left and right wheels, and the robot speed in the robot coordinate system is: V xR =v,V yR =0; The intersection of the dotted lines is the instantaneous center of rotation (P). The reference point on the robot moves in an arc around the instantaneous center. θ is the angle between the forward direction and the world coordinate system. Its angular velocity is: Turning radius: γ is the steering angle of the front and rear wheels; In the world coordinate system, the kinematic equation of the robot is written as: The standard simplified model is: This kinematic equation is the state quantity of the model predictive control; In addition, in the world coordinate system, the velocity constraint in the Y direction is: The other constraints are: In the actual model, the control is usually the robot speed v and the steering angle γ, so the control quantity is: Therefore, the nonlinear state-space equations defining the model are: The given trajectory is the reference trajectory, that is, after the model is processed, the kinematic equation of the model is equal to the kinematic equation of the trajectory. Let the kinematic equation of the given trajectory be the reference quantity (ξ r ,U r ), the nonlinear state equation is Taylor expanded at the reference point, that is, the model is approximately linearized: To facilitate calculation, the error is defined as but: Therefore, the rearranged linear state space equation is: in, The linear state space equations are discretized using the forward Euler method: Wherein, T is the sampling time, k = 1, 2, 3...; After simplifying the equation, we get: Where I is the identity matrix, 3. The complex terrain trajectory tracking control method of the six-wheel independently driven and independently steered robot according to claim 1 is characterized in that: The process of the linear model predictive control algorithm in S2 is as follows: The three steps of the model predictive control algorithm are: prediction model, rolling optimization and feedback correction; through the definition in the six-wheel robot kinematic model, the prediction model is obtained as follows: After sorting: The variables are: Therefore, the trajectory tracking problem is converted into a quadratic programming optimization problem. The control objective is to minimize U′(k) and make Y approach Y. r , so the design cost J is: J=[(Y-Y r ) T Q(Y-Y r )+U′(k) T RU′(k)]; Among them, the prediction step size is Np, and the control step size is Nc; Weight matrix Q = Qc*eye(Nx*Np,Nx*Np), R = Rc*eye(Nc*Nu), where Qc and Rc are matrix weight coefficients; To simplify the calculation, let: so: Y-Y r =E+ΦU′(k); After importing: J=[E T QE+(ΦU′(k)) T Q(ΦU′(k))+2E T Q(ΦU′(k))+U′(k) T RU′(k)]; Since the main control variable is U′(k), the irrelevant terms can be ignored, and after approximate processing, we can get: J=[U′(k) T (F T QΦ+R)U′(k)+2E 2 QΦU′(k)]; After designing the cost function, in order to ensure that the calculation results are consistent with the actual situation and avoid errors caused by the steering angle or speed surge, it is necessary to define constraints, Umin≤U′(k)≤Umax, including steering angle constraints and speed constraints; Therefore, the problem of model predictive control is finally converted into the optimal solution of quadratic programming problem, that is: minJ=min([U′(k) T (Φ T QΦ+R)U′(k)+2E 2 QΦU′(k)]); stUmin≤U′(k)≤Umax; In the process of model predictive control, it is necessary to select reasonable prediction step size and control step size to ensure the rationality of rolling optimization; The feedback correction link in model predictive control is to detect the actual output of the object in the new sampling link, and use the real-time information to optimize the model-based results, so the robot position information needs to be updated in real time and input into the model; The robot position is updated as: Discretize the input trajectory curve into coordinate points (cx, cy), and calculate the first-order derivative P of the trajectory according to the discrete coordinate points d and the second-order derivative P dd ; Calculate the curvature of the trajectory curve: Calculate the angle between the trajectory curve and the coordinate axis: cyaw=atan(Pd); The tracking error is calculated by the trajectory coordinate point (cx(i), cy(i)) and the robot position coordinate (x(i), y(i)); the angle cyaw is used to calculate the orientation update, and the orientation angle yaw input at the beginning is used to calculate the orientation of the robot at the beginning. From the next calculation, the size of the yaw angle is determined by the orientation update of the following formula; Facing angle update: Through the above operation, the speed v and the front wheel steering angle γ required for the robot to track the trajectory are calculated.
4. The complex terrain trajectory tracking control method of a six-wheel independently driven and independently steered robot according to claim 3 is characterized in that: The selected prediction step size Np=60 and control step size Nc=30.
5. The complex terrain trajectory tracking control method of a six-wheel independently driven and independently steered robot according to claim 1 is characterized in that: The process of error optimization by the optimization module in S7 through the exponential reaching law in the sliding mode control algorithm is: Define the threshold function: P = P1 + P2; P1=v a -v r ,P2=e; Among them, v a is the actual speed of the robot, v r is the robot's calculated speed, and e is the tracking error, which is the Euclidean distance between the robot's current position and the calculated position; Tracking Error: When P1 or P2 exceeds the set threshold, set the speed optimization function to enter the speed optimization function: V n =c 4k *(v a ±c 1k (c 2k *s+c 3k *sign(s))); Where s = c 0k *(v a -v r ), sign is the sign function, c 0k 、c 1k 、c 2k 、c 3k 、c 4k is the proportional coefficient. The speed calculated by the speed optimization function is the optimized single-wheel speed based on the first calculation of the model predictive control. The motion state is judged according to the single-wheel speed error and torque output of the robot, and the appropriate proportional coefficient is defined.
