Single-side step obstacle crossing attitude control method for wheel-track combined type mobile system

Abstract

The invention provides a unilateral step obstacle crossing attitude control method for a wheel-track combined type mobile system, which relates to the field of mechanical engineering control and comprises the following steps of: establishing an initial coordinate system of a robot, and defining space attitude parameters of the robot in an obstacle crossing attitude adjustment process; representing topographic conditions by using the wheel center position, representing a vehicle body attitude by using the spatial attitude parameters, and performing spatial transformation on the initial coordinate system to obtain a kinematic model about the wheel center position and the spatial attitude parameters; on the basis of kinematics modeling, obstacle crossing stability of the robot is used as a control target, the swing angle of a rear swing arm of the robot and the length of an electric push rod are used as control variables, the control variables are used as an initial population of an improved genetic algorithm, and the improved genetic algorithm is used for carrying out iterative solution on the control variables until an optimal solution is obtained. The obstacle crossing posture of the single-side step obstacle of the robot is adjusted by controlling the optimal solution, then the pitch angle and the roll angle are cooperatively adjusted, and the obstacle crossing stability is improved.

Description

Technical Field

[0001] The present invention belongs to the field of mechanical engineering control, and particularly relates to a method for controlling the obstacle-crossing attitude of a unilateral step obstacle of a wheel-track composite mobile system. Background Art

[0002] With the development of agricultural mechanization and intelligence, the application demand of mobile robots in complex terrains such as hilly mountains, orchards and tea gardens is increasing day by day. About 43% of the total land area of our country is hilly mountains, and there are widely distributed unilateral step obstacles such as ridge dikes and ditches in the terrain, which poses a severe challenge to the obstacle-crossing ability of mobile robots. Although traditional wheeled robots have a high moving speed, they are prone to tipping over when facing vertical obstacles; although tracked robots have strong passability, they are inconvenient to turn and have high energy consumption. The wheel-track composite mobile robot combines the advantages of wheels and tracks, and has both high speed and high passability, becoming an ideal platform for operating in complex terrains.

[0003] In the field of mobile robot obstacle-crossing research, Li Yunwang et al. designed a four-track two-swing-arm robot and analyzed the motion mechanism of the robot to overcome typical obstacles such as steps, slopes, and channels and its maximum obstacle-crossing ability. Cui Jintao et al. took a four-track robot with a differential mechanism as the research object and completed the obstacle-crossing performance analysis of step-type obstacles through the kinematic analysis of the robot. Wang Chuanwei et al. designed a four-swing-arm six-track robot, carried out kinematic analysis on the robot, established the center-of-gravity coordinate equation of the robot, and analyzed the obstacle-crossing stability of the robot under unilateral obstacle terrains.

[0004] However, existing research mostly focuses on the aspect of crossing step-like vertical obstacles, and controls the robot to cross obstacles smoothly by studying the change of the pitch angle during the obstacle-crossing process. However, there is less research on the comprehensive regulation of the vehicle body pitch angle and roll angle during the process of crossing a unilateral step obstacle terrain, especially. During the process of crossing a unilateral step, when the obstacle-crossing side swing arm of the robot contacts and climbs the obstacle, the vehicle body attitude will change both in pitch and roll. Due to the double-angle coupling relationship between the pitch angle and the roll angle, if the strategy of independently adjusting the swing arm or the leg to stepwise adjust the vehicle body attitude is adopted, it will lead to the deterioration of one angle while optimizing a certain angle, resulting in problems such as insufficient vehicle body stability and low obstacle-crossing efficiency. In addition, the agricultural terrain has the characteristics of being unstructured and asymmetric, and the traditional control strategies based on bilateral steps or fixed parameters are difficult to adapt to the diversity of unilateral obstacles such as ridge dikes and ditches. Summary of the Invention

[0005] In order to overcome the deficiency that the vehicle body cannot maintain stability during the process of crossing a unilateral step obstacle by the robot, the present invention provides a method for controlling the obstacle-crossing attitude of a unilateral step obstacle of a wheel-track composite mobile system, including the following steps:

[0006] Establish the initial coordinate system of the robot and define the spatial attitude parameters characterizing the obstacle-crossing attitude of the robot;

[0007] Based on the initial coordinate system and the spatial attitude parameters, perform homogeneous transformation on the initial coordinate system by using the method of kinematic pose transformation to obtain a kinematic model regarding the wheel center position and the spatial attitude parameters;

[0008] Based on the kinematic model, with the stability of the robot body as the control objective, take the swing angle of the rear swing arm of the robot and the length of the electric push rod as control variables, use the control variables as the initial population of the improved genetic algorithm, and use the improved genetic algorithm to iteratively solve the control variables until the optimal solution of the control variables is obtained;

[0009] According to the optimal solution of the control variables, perform coordinated control on the swing arm-leg to adjust the obstacle-crossing attitude of the body and enhance the obstacle-crossing stability.

[0010] Preferably, the initial coordinate system includes the spatial absolute coordinate system OX O Y O Z O 、the main body coordinate system DX of the robot D Y D Z D 、the right swing arm coordinate system AX A Y A Z A 、the left swing arm coordinate system HX H Y H Z H 、the balance link coordinate system EX E Y E Z E 、the right front swing arm coordinate system KX K Y K Z K 、the right rear swing arm coordinate system MX M Y M Z M 、the left front swing arm coordinate system NX N Y N Z N 、the left rear swing arm coordinate system PX P Y P Z P ;The origin D of the DX D Y D Z D is the midpoint of the main body central axis, and Y D is along the axis direction of the central axis, and Y D represents perpendicular to the central axis and pointing to the forward direction of the robot; AX A Y A Z A rotates around the J A joint, and HXH Y H Z H Rotate around J H joint rotation, EX E Y E Z E Rotate around J E joint rotation, KX K Y K Z K Rotate around J K joint rotation, MX M Y M Z M Rotate around J M joint rotation, NX N Y N Z N Rotate around J N joint rotation, PX P Y P Z P Rotate around J P joint rotation.

[0011] Preferably, the steps for defining the spatial attitude parameters characterizing the obstacle-crossing attitude of the robot are as follows:

[0012] Translation DX D Y D Z D to the origin so that DX D Y D Z D the origin coincides with the origin of OX O Y O Z O Then, rotate DX D Y D Z D around the X-axis and Y-axis of OX O Y O Z O respectively for rotation transformation so that DX D Y D Z D coincides completely with OX O Y O Z O Rotate DX D Y D Z D around the X-axis of OX O Y O Z O The rotation angle is defined as the roll angle β. Rotate DX D Y D Z D around OC O Y O Z OThe angle of rotation about the Y-axis is defined as the pitch angle α;

[0013] Rotate AX A Y A Z A about joint J A The rotation angle is defined as γ1. Rotate HX H Y H Z H about joint J H The rotation angle is defined as γ2, where γ2 = -γ1. Rotate KX K Y K Z K about joint J K The rotation angle is defined as θ K , rotate MX M Y M Z M about joint J M The rotation angle is defined as θ M , rotate NX N Y N Z N about joint J N The rotation angle is defined as θ N , rotate PX P Y P Z P about joint J P The rotation angle is defined as θ P ; The mobile robot adjusts the included angle θ el and θ er (the length of the left electric push rod is represented by L el , and the length of the right electric push rod is represented by L er ) of the two side legs by controlling the lengths L L and θ R to change its roll angle.

[0014] Preferably, the homogeneous transformation of the initial coordinate system is performed by using the method of kinematic pose transformation, including the following steps:

[0015] If the origin of DX D Y D Z D coincides with the origin of OX O Y O Z O , the homogeneous translation transformation matrix is:

[0016]

[0017] where x OD , y OD , z ODis the three-dimensional coordinate difference between the origin D of the vehicle body coordinate system and the origin O of the absolute coordinate system in the absolute coordinate system at the initial attitude.

[0018] After the coincidence of the coordinate system origins, rotate the main vehicle body coordinate system DX D Y D Z D First rotate by β around the X-axis of the space absolute coordinate system, and then rotate by α around the Y-axis to make the main vehicle body coordinate system completely coincide with the space absolute coordinate system. Then the homogeneous rotation transformation matrix is:

[0019]

[0020] where s represents sin and c represents cos, which is expressed in this way in the subsequent matrices; α and β respectively represent the rotation angles of the coordinate system DX D Y D Z D in the Y-axis and X-axis directions, and are defined as the pitch angle and roll angle of the robot;

[0021] According to DX D Y D Z D respectively with AX A Y A Z A and HX H Y H Z H the positional relationship between them, AX A Y A Z A rotates around joint J A by an angle of γ1, and HX H Y H Z H rotates around joint J H by an angle of γ2. Then their translation transformation matrix and rotation transformation matrix relative to the main vehicle body coordinate system DX D Y D Z D are respectively:

[0022]

[0023] where, (x DA , y DA , z DA ) —— the coordinate difference between AX A Y A Z A and DX D Y D Z D in the main vehicle body coordinate system, (x DH , y DH , z DH)——HX H Y H Z H and DX D Y D Z D The coordinate difference in the main vehicle body coordinate system;

[0024] Due to the right front swing arm rotating around joint J K rotating, KX K Y K Z K relative to AX A Y A Z A The translation transformation matrix and rotation transformation matrix are respectively:

[0025]

[0026] where, (x AK , y AK , z AK )——KX K Y K Z K and AX A Y A Z A The coordinate difference in the right swing arm coordinate system;

[0027] The transformation at the other three swing arm joints is the same. Therefore, the coordinates of the wheel centers w of the 8 wheels i in their respective coordinate systems are respectively:

[0028]

[0029] where, w′ i is the coordinate of each wheel center in its respective coordinate system, x′ i is the X-direction coordinate of each wheel center in its respective coordinate system, y′ i is the Y-direction coordinate of each wheel center in its respective coordinate system, z′ i is the Z-direction coordinate of each wheel center in its respective coordinate system;

[0030] According to the robot kinematics spatial attitude equation, the kinematic matrix equation about the relationship between the wheel center position and the spatial attitude parameters is:

[0031]

[0032] Preferably, the kinematic model of the wheel center position and the spatial attitude parameters includes a forward kinematic model and an inverse kinematic model. The forward kinematic model is:

[0033] w i = f i(α, β, γ1, θ K , θ M , θ N , θ P , L el , L er )

[0034] Wherein, i is the i-th round;

[0035] The inverse kinematics model is:

[0036] (α, β, γ1) = f -1 (w1, w2, w3, w4, w5, w6, w7, w8, θ K , θ M , θ N , θ P , L el , L er )

[0037] Preferably, the improved genetic algorithm is used to iteratively solve the kinematics model until the optimal solution of the control target is obtained, including the following steps:

[0038] Set the population range with the design adjustment range of the robot as the limiting condition, and use the range of the swing angle of the rear swing arm and the length of the electric push rod as the population boundary of the improved genetic algorithm. Generate new individuals within the limited interval to form the initial population;

[0039] Design a fitness function to improve the obstacle-crossing stability of the robot;

[0040] According to the fitness function, perform selection, crossover, mutation, and periodic particle swarm optimization (PSO) on the initial population, and perform population update and iteration to finally obtain the optimal solution that meets the control target.

[0041] Preferably, the population boundary range of the improved genetic algorithm is: the change range of the rear swing arm angle is 13° ≤ θ P ≤ 90°, and the change range of the length of the electric push rod is 170 mm ≤ L e ≤ 230 mm.

[0042] Preferably, the fitness function is:

[0043]

[0044] In the formula, α is the pitch angle, β is the roll angle, and in the formula, Δθ P is the swing angle of the rear swing arm, and ΔL el , ΔL er are the change amounts of the lengths of the electric push rods of the attitude adjustment structures on the left and right sides, and n is the influence factor of the change amount of the control variable on the stability of the robot.

[0045] Preferably, after performing selection, crossover, and mutation operations on the initial population, periodic particle swarm optimization (PSO) is carried out. The periodic particle swarm optimization (PSO) includes the following steps:

[0046] Trigger particle swarm optimization once every N generations;

[0047] The particle swarm size for PSO optimization is 20% of the current population size;

[0048] By comparing the fitness of the population after PSO optimization with the global optimal solution, the global optimal solution is updated. When the fitness of the current solution is better, the global optimal solution and the corresponding individual are updated.

[0049] The single-sided step obstacle crossing attitude control method for the wheel-track composite mobile system provided by the present invention has the following beneficial effects:

[0050] By establishing a kinematic model that relates the wheel center height to the spatial attitude parameters, the present invention can characterize the relationship between the body attitude of the wheeled robot and the terrain conditions; based on kinematic modeling and with the stability of the robot body as the control objective, by taking the swing angle of the rear swing arm of the robot and the length of the electric push rod as control variables, an attitude control method that combines the traditional genetic algorithm and the particle swarm optimization algorithm is designed; by using this attitude control method, the maximum roll angle and the maximum pitch angle of the robot during the single-sided step obstacle crossing process can be reduced by 12.8° and 4.5° respectively, greatly improving the stability of the robot body during the single-sided step obstacle crossing process. Brief Description of the Drawings

[0051] In order to more clearly illustrate the embodiments of the present invention and their design schemes, the drawings required for this embodiment will be briefly introduced below. The drawings in the following description are only partial embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0052] Figure 1 It is a flowchart of a single-sided step obstacle crossing attitude control method for a wheel-track composite mobile system according to an embodiment of the present invention;

[0053] Figure 2 It is a schematic diagram of a wheel-track composite mobile robot;

[0054] Figure 3 It is a spatial coordinate system of a wheel-track composite mobile robot;

[0055] Figure 4 It is the relative position relationship between the main body coordinate system and the spatial absolute coordinate system;

[0056] Figure 5 It is the structural dimensions of a wheel-track composite mobile robot;

[0057] Figure 6 is the attitude control block diagram;

[0058] Figure 7 is the flow chart of the improved genetic algorithm;

[0059] Figure 8 is the comprehensive adjustment simulation result;

[0060] Figure 9 is the comparison of algorithm response time; Specific implementation manner

[0061] In order to enable those skilled in the art to better understand the technical solution of the present invention and be able to implement it, the present invention will be described in detail below in conjunction with the accompanying drawings and specific embodiments. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and cannot be used to limit the protection scope of the present invention.

[0062] In the description of the present invention, it should be understood that the orientation or positional relationship indicated by the terms "center", "longitudinal", "transverse", "length", "width", "thickness", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", "axial", "radial", "circumferential", etc. is based on the orientation or positional relationship shown in the accompanying drawings. It is only for the convenience of describing the technical solution of 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 operated in a specific orientation, and therefore cannot be understood as a limitation to the present invention.

[0063] In addition, the terms "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying relative importance. In the description of the present invention, it should be noted that unless otherwise clearly specified or limited, the terms "connected" and "connected" should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be directly connected or indirectly connected through an intermediate medium. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances. In the description of the present invention, unless otherwise stated, the meaning of "plurality" is two or more, and details are not described herein again.

[0064] Embodiment 1

[0065] The present invention provides a method for controlling the attitude of a wheel-track composite mobile system to cross a single-sided step obstacle, specifically as Figure 1 shown, including the following steps:

[0066] Step 1: Establish the initial coordinate system of the robot and define the spatial attitude parameters of the robot during attitude adjustment.

[0067] Establishing the robot coordinate system and defining the spatial attitude parameters are the prerequisites for establishing the kinematic model of the wheel-track compound mobile robot. As the robot performs obstacle-crossing actions, the attitude of the robot body will change. Therefore, define the state of the robot on a horizontal ground when the front and rear swing arms and the left and right electric push rods do not change their attitudes as the initial state. In this state, establish the initial coordinate system as shown in Figure 3 . The initial coordinate system includes the spatial absolute coordinate system OX O Y O Z O , the main body coordinate system DX D Y D Z D , the right swing arm coordinate system AX A Y A Z A , the left swing arm coordinate system HX H Y H Z H , the balance link coordinate system EX E Y E Z E , the right front swing arm coordinate system KX K Y K Z K , the right rear swing arm coordinate system MX M Y M Z M , the left front swing arm coordinate system NX N Y N Z N , the left rear swing arm coordinate system PX P Y P Z P ; the origin D of the DX D Y D Z D is the midpoint of the main body central axis. Y D is along the axis direction of the central axis, and X D represents the direction perpendicular to the central axis and pointing to the forward direction of the robot; AX A Y A Z A rotates around the J A joint, HX H Y H Z H rotates around the J H joint, EX E Y E Z E rotates around the J E joint, KX K Y KZ K Rotate around J K the joint rotates, MX M Y M Z M Rotate around J M the joint rotates, NX N Y N Z N Rotate around J N the joint rotates, PX P Y P Z P Rotate around J P the joint rotates. Define the coordinates of the centers of the 8 wheels of the robot relative to the spatial absolute coordinate system OX O Y O Z O as follows:

[0068] w i =(x i , y i , z i , 1), (i = 1, 2, 3, 4, 5, 6, 7, 8)

[0069] where i is the i-th wheel;

[0070] As the robot moves continuously, the body attitude will also change relative to the initial state. The transformation process from the main body coordinate system DX D Y D Z D to the spatial absolute coordinate system OX O Y O Z O is as shown Figure 4 First, translate the main body coordinate system DX of the robot D Y D Z D so that it coincides with the origin of the spatial absolute coordinate system OX O Y O Z O (the coincident origin is denoted as O). At this time, the main body coordinate system will form a certain angle with the spatial absolute coordinate system. Through the main body coordinate system DX D Y D Z D first rotate by β around the X-axis of the spatial absolute coordinate system, and then rotate by α around the Y-axis to make the main body coordinate system completely coincide with the spatial absolute coordinate system, and define the roll angle β and the pitch angle α to characterize the body attitude of the robot. Define the right rocker arm coordinate system AX A Y A Z A the rotation angle around joint J A is γ1, HX H Y H Z H Rotate around joint JH The rotation angle of K Y K Z K about joint J K is γ2, where γ2 = -γ1, and define KX K MX M Y M Z M The rotation angle of M about joint J M is θ, NX N Y N Z N The rotation angle of N about joint J N is θ, PX P Y P Z P The rotation angle of P about joint J P is θ; The mobile robot adjusts the included angle θ el and θ er (the length of the left electric push rod is represented by L el , and the length of the right electric push rod is represented by L er ) of the two side legs by controlling the lengths L L and L R to change its roll angle, as shown in Figure 2 . In summary, define the 9 spatial attitude parameters of this robot, namely (α, β, γ1, θ K , θ M , θ N , θ P , L el , L er )

[0071] Step 2: Use the wheel center position to characterize the terrain conditions and the spatial attitude parameters to characterize the vehicle body attitude, and use the method of kinematic pose transformation to perform a spatial transformation on the initial coordinate system to obtain a kinematic model of the wheel center position and the spatial attitude parameters.

[0072] The spatial transformation of the initial coordinate system includes the following steps:

[0073] Coincide the origin of DX D Y D Z D with the origin of OX O Y O Z O . Then the homogeneous translation transformation matrix is:

[0074]

[0075] where x OD and yOD , z OD is the three - dimensional coordinate difference between the origin D of the vehicle body coordinate system and the origin O of the absolute coordinate system in the absolute coordinate system when in the initial attitude.

[0076] After the coincidence of the coordinate system origins, rotate the main vehicle body coordinate system DX D Y D Z D First rotate by β around the X - axis of the space absolute coordinate system, and then rotate by α around the Y - axis to make the main vehicle body coordinate system completely coincide with the space absolute coordinate system. Then the homogeneous rotation transformation matrix is:

[0077]

[0078] Among them, s represents sin, c represents cos, and this representation is used in all subsequent matrices; α and β respectively represent the rotation angles in the Y - axis and X - axis directions of the rotating coordinate system DX D Y D Z D and are defined as the pitch angle and roll angle of the robot.

[0079] According to the position relationship between DX D Y D Z D and AX A Y A Z A and HX H Y H Z H AX A Y A Z A rotates around joint J A with a rotation angle of γ1, and HX H Y H Z H rotates around joint J H with a rotation angle of γ2. Then their translation transformation matrix and rotation transformation matrix relative to the main vehicle body coordinate system DX D Y D Z D are respectively:

[0080]

[0081]

[0082] Among them, (x DA , y DA , z DA ) —— the position of AX A Y A Z A relative to DX D Y D ZD Coordinate difference in the main body coordinate system, (x DH , y DH , z DH )——HX H Y H Z H and DX D Y D Z D Coordinate difference in the main body coordinate system;

[0083] Due to the right front swing arm rotating around joint J K KX K Y K Z K with respect to AX A Y A Z A The translation transformation matrix and rotation transformation matrix are respectively:

[0084]

[0085] where, (x AK , y AK , z AK )——KX K Y K Z K and AX A Y A Z A Coordinate difference in the right swing arm coordinate system;

[0086] The transformation at the other three swing arm joints is the same. Therefore, the coordinates of the wheel centers w i of the 8 wheels in their respective coordinate systems are respectively:

[0087]

[0088] where, w′ i is the coordinate of each wheel center in its respective coordinate system, x′ i is the X-direction coordinate of each wheel center in its respective coordinate system, y′ i is the Y-direction coordinate of each wheel center in its respective coordinate system, z′ i is the Z-direction coordinate of each wheel center in its respective coordinate system;

[0089] According to the robot kinematics spatial attitude equation, the kinematic matrix equation about the relationship between the wheel center position and the spatial attitude parameters is:

[0090]

[0091] Table 1 gives a set of body structure parameters of the wheel-track compound mobile robot, Figure 5It is a schematic diagram of the body structure dimensions. Substituting the parameters in Table 1 into the kinematic matrix equation, specific equations regarding the body attitude parameters and the wheel center position coordinates are obtained, and the equations representing the wheel center height z i are extracted.

[0092] Given the known body structure parameters, the forward kinematic model and the inverse kinematic model of the wheel-track composite mobile robot can be obtained. Through the forward kinematic model, for any set of attitude parameters (α, β, γ1, θ K , θ M , θ N , θ P , L el , L er ), the corresponding wheel center position coordinates w i can be calculated.

[0093] Through the inverse kinematic model, the corresponding body attitude parameters can be calculated from any wheel center position coordinates. The forward kinematic model of the wheel-track composite mobile robot is:

[0094] w i = f i (α, β, γ1, θ K , θ M , θ N , θ P , L el , L er )

[0095] The inverse kinematic model is:

[0096] (α, β, γ1) = f -1 (w1, w2, w3, w4, w5, w6, w7, w8, θ K , θ M , θ N , θ P , L el , L er )

[0097] Table 1 Structural dimensions of the mobile robot and related parameters between coordinates

[0098]

[0099] Establishing the robot kinematic model can accurately characterize the relationship between the body attitude and the wheel center position, and can be used for the improved design and performance analysis of the robot. The solution of the inverse kinematics provides a solution for the attitude control of the robot, that is, when the active adjustment angle is unknown, the corresponding body attitude can be deduced according to the current wheel center position of the robot, and then the input quantity of the active control angle can be obtained using the control algorithm to complete the attitude control of the robot.

[0100] Step 3: Determine the control objectives and control variables for the robot's motion according to the kinematic model. Use the control variables as the initial population of the improved genetic algorithm, and use the improved genetic algorithm to iteratively solve the kinematic model to obtain the optimal solution of the control objective.

[0101] When the robot passes through the unilateral step obstacle terrain, the vehicle body stability becomes worse compared to the flat terrain. To improve the obstacle-crossing stability of the robot, it is necessary to actively adjust the swing angle of the swing arm and the included angle of the legs to control the change of the vehicle body attitude. During this process, the magnitude of the roll angle and pitch angle of the robot is used to evaluate the quality of the vehicle body stability. Therefore, the control process of the mobile robot is actually a process of actively controlling and adjusting the angle through the swing arm track mechanism and the leg attitude adjustment mechanism to reduce the pitch angle and roll angle of the vehicle body.

[0102] During the obstacle-crossing process, the active attitude control of the robot needs to comprehensively consider the influence of factors such as the pitch angle and roll angle of the vehicle body. In essence, it is a multi-objective optimization control process. For such problems, the genetic algorithm is more in line with the requirements. The population generated by the genetic algorithm is evaluated by designing a reasonable fitness function, and genetic operations such as selection, crossover, and mutation are performed based on this fitness function. After several genetic iterations, the optimal solution is generated. In this process, the fitness function is designed under the influence of various factors, so the genetic algorithm has a significant effect on multi-objective optimization problems.

[0103] The control variable in attitude control is the active control angle θ of the rear swing arm P and the length L of the electric push rod e . First, obtain the current vehicle body attitude angle through the attitude sensor installed on the vehicle body. Combine the current attitude of the robot as the input, calculate the relative position of the wheel center of the robot in the current attitude through the forward kinematic model, input the wheel center position information into the control algorithm, the algorithm outputs the active control variable, then adjust the rear swing arm and the electric push rod according to the active control amount, and repeat the above process according to the real-time attitude change during the obstacle-crossing process to realize the real-time control of the vehicle body attitude during the obstacle-crossing process. The specific attitude control process is as Figure 6 shown.

[0104] In the selection of the control algorithm, the genetic algorithm has great advantages in multi-objective control and global search, but the traditional genetic algorithm has defects and its local search ability is insufficient, and it is easy to fall into the local optimal solution. Therefore, it needs to be improved. The specific improvement method is to use an optimization algorithm with strong local search ability to further optimize the results of the traditional genetic algorithm.

[0105] With its parallel search mechanism based on swarm intelligence, the particle swarm optimization algorithm performs outstandingly in terms of local optimization efficiency and convergence speed. Therefore, by combining the traditional genetic algorithm and the particle swarm optimization algorithm, an improved genetic algorithm for the obstacle-crossing attitude control of the unilateral step obstacle of the wheel-track compound mobile system is proposed, taking into account the advantages of both algorithms.

[0106] The improved genetic algorithm takes the genetic algorithm as the main framework and nests the particle swarm optimization algorithm. Therefore, the design of the improved genetic algorithm is specifically divided into the design of these two parts. The algorithm flow is as Figure 7 shown. First, the population is initialized. An initial population composed of a certain number of active control angle individuals is generated within the limited range. By inputting the wheel center position information, the corresponding roll angle and pitch angle values unique to each individual can be obtained through the inverse kinematics model. Substitute the roll angle and pitch angle into the fitness function to calculate the fitness value of the corresponding individual. Based on the fitness values of different individuals, genetic operator operations such as selection, crossover, and mutation are performed. At the same time, after such operations, the particle swarm optimization operation is performed every N generations. Finally, the population is updated, and the new population is evolved and iterated according to the above steps for several times to finally obtain the optimal solution. The specific steps are as follows:

[0107] (1) Initialization of the genetic algorithm population

[0108] To enable the algorithm to have a faster response speed and adjustment accuracy, the present invention sets the population range with the reasonable variation ranges of the electric push rod length and the swing angle of the rear swing arm of the wheel-track compound mobile robot as the limiting conditions. And the length variation range is 170mm ≤ L e ≤ 230mm, and the angle variation range is 13° ≤ θ P ≤ 90°. Taking this value range as the population boundary of the genetic algorithm, new individuals are generated within the limited interval to form the initial population.

[0109] (2) Design of the fitness function

[0110] This study aims to improve the obstacle-crossing stability of the robot. By measuring the pitch angle and roll angle of the mobile robot, the body stability is judged. The smaller the two angles are, the more stable the body is and the better the control effect is. At the same time, starting from the stability of the mobile robot, it is also necessary to ensure that the change amounts of the control parameters Δθ P , ΔL el and ΔL er are as small as possible. Therefore, on the premise of meeting the continuity and non-negativity of the fitness function, the fitness function is designed as:

[0111]

[0112] In the formula, α is the pitch angle, β is the roll angle. In the formula, Δθ P is the swing angle of the rear swing arm, ΔL el , ΔLer is the change in the length of the electric push rod of the attitude adjustment structure on both the left and right sides, and n is the influence factor of the change in the control variable on the stability of the robot. In this fitness function, the smaller the pitch angle and roll angle, the larger the corresponding individual fitness F, and the more it meets the final requirements.

[0113] (3) Main loop of genetic algorithm

[0114] Calculate the fitness values of the individuals in the population according to the fitness function, and perform certain processing on the initial population based on the individual fitness values, that is, operations such as selection, crossover, mutation, and reinsertion into the population to obtain the final result that meets the requirements. The steps for performing this operation in the algorithm are called genetic operator operations.

[0115] The selection operation uses the stochastic universal sampling method improved from the roulette wheel method to select individuals. The specific idea is to calculate the sum of the fitness values of the population, and the probability of each individual being selected is:

[0116]

[0117] In the formula: F is the fitness value of the i-th individual, and N is the number of individuals.

[0118] Perform a crossover operation on the individuals after the selection operation to enable the offspring individuals to inherit the excellent characteristics of the parent individuals, generate excellent individuals, increase the diversity of the population, and accelerate the convergence of the optimization process. Use the single-point crossover method to generate offspring. The specific crossover rule is:

[0119]

[0120] In the formula: m is a random number in the interval [0,1].

[0121] To maintain the diversity of the population, perform a mutation operation on some individuals in the new population after selection and crossover with a relatively small probability. While maintaining the stability of the population, increase the diversity of categories and have a probability of generating better individuals. The mutation rule for individual a3 is:

[0122] a3′ = a3 + (1 - g / g max ) 2 × η

[0123] In the formula: g is the current iteration number, g max is the maximum iteration number, and η is a random number. It can be seen that the mutation amplitude decays with the increase of the iteration number, the population gradually stabilizes, the proportion of excellent individuals increases, and the mutation amplitude of individuals becomes smaller.

[0124] (4) Particle swarm optimization algorithm nesting

[0125] To overcome the defects of the traditional genetic algorithm, after the mutation operation is completed, the particle swarm optimization algorithm is triggered every 5 generations. The top 20% of individuals with the highest fitness in the current population are selected as the initial particle swarm of the particle swarm optimization algorithm, and the movement of the particle swarm is used to search the solution space. During the iteration process, the fitness of the particles is calculated, and the individual best position (p best ) and the global best position (g best ) are updated, and the velocity and position of each particle are updated according to the following formula:

[0126] v i (t + 1) = w·v i (t) + c1·r1·(p best - x i (t)) + c2·r2·(g best - x i (t))

[0127] x i (t + 1) = x i (t) + v i (t + 1)

[0128] In the formula: w is the inertia weight, c1 and c2 are learning factors, and r1 and r2 are random numbers between [0, 1].

[0129] This step will utilize the fast convergence ability of the particle swarm optimization algorithm to perform a fine search on the area near the global optimal solution generated by the genetic algorithm, thereby improving the accuracy of the solution. After each generation ends, the algorithm checks whether the optimal solution in the current population is better than the existing global optimal solution. If a better solution is found, the global optimal solution (gbest) and the corresponding individual are updated.

[0130] Since the selection operation will reduce the population size, while the crossover, mutation, and periodically executed particle swarm optimization operations do not change the number of individuals. To ensure that the population size is consistent with the initial size during iteration, after the above operations, individuals with larger fitness values are selected from the initial population for supplementation, and the number of supplemented individuals is the number of individuals reduced by the selection operation.

[0131] Repeat the above operations for a certain number of iterations until the algorithm terminates, and the finally obtained solution is the optimal solution of the improved genetic algorithm.

[0132] Example 2

[0133] To verify the improvement of the above method on the obstacle-crossing stability of the mobile system over a unilateral stepped obstacle, it is necessary to conduct obstacle-crossing control simulation. By building a simplified model of the robot and a unilateral stepped obstacle with a vertical obstacle height of 200 mm, and jointly using MATLAB to conduct a simulation experiment on the control results of the algorithm. Based on multiple simulation experiments, the parameter settings of the motion control algorithm experiment are as follows: Set the population size of the genetic algorithm part to 30, the algorithm iterates 50 times, the selection probability is 0.9, the crossover probability is 0.8, the individual mutation probability is 0.05, and the influence factor n of the control variable change amount on the robot stability is 0.01; Set to perform periodic particle swarm optimization every 5 generations, and set the maximum number of iterations of the particle swarm optimization algorithm part to 10, the inertia weight to 0.5, the personal learning factor to 1.5, and the social learning factor to 1.5.

[0134] Under the above conditions, a simulation is carried out. Figure 8 (a) shows the image of the roll angle changing with time when the mobile robot crosses the unilateral stepped obstacle. Figure 8 (b) shows the image of the pitch angle changing with time during this process. It can be seen from the simulation results that compared with not using the motion control algorithm, the maximum roll angle during the entire obstacle-crossing process is reduced by 12.8°, and the maximum pitch angle is reduced by 4.5°. It can be seen that using the motion control algorithm can effectively reduce the body inclination angle during the obstacle-crossing process and improve the stability of the body during the obstacle-crossing process. At the same time, after using the control algorithm, the obstacle-crossing time of the robot is shortened by 14%, and the obstacle-crossing efficiency is improved.

[0135] Under the condition that other conditions remain unchanged, the genetic algorithm and the improved genetic algorithm are respectively used to conduct 100 experiments under each population size, and their response times are statistically analyzed and the average value is taken. The experimental results are as Figure 9 shown. It can be seen from Figure 9 that the algorithm response time is related to the population size. The expansion of the population size increases the calculation amount and increases the algorithm response time. Since the improved genetic algorithm combines the genetic algorithm and the particle swarm optimization algorithm, it combines the fast convergence ability of the particle swarm optimization algorithm and conducts fine search in the area near the globally good solution obtained by the genetic algorithm. Therefore, combined with the simulation experiment results, the improved genetic algorithm is superior to the genetic algorithm in terms of response time. In addition, this experiment is carried out under a single terrain condition. When facing complex terrains, the advantage of the improved genetic algorithm in terms of response time will be further amplified.

[0136] The beneficial effects produced by the present invention are as follows:

[0137] (1) A kinematic model of the wheel-track composite mobile robot is established, and a mathematical model characterizing the relationship between the wheel center height and the spatial attitude parameters is obtained. The correctness and accuracy of the kinematic model are verified through example calculations, and the forward and inverse kinematic models are obtained, laying a foundation for further motion control.

[0138] (2) Aiming at improving the stability of a robot when crossing a single-sided step obstacle, a motion control method based on an improved genetic algorithm is proposed. The improved algorithm combines the traditional genetic algorithm with the particle swarm optimization algorithm, and has the advantages of multi-objective control, strong global search ability and local search ability, etc. On the basis of achieving the expected control objectives, the stability and response speed of the algorithm are improved.

[0139] (3) By establishing a robot and obstacle road surface model and conducting simulations in Matlab, the adjustment effect of the algorithm on the obstacle-crossing attitude is verified. The experimental results show that compared with not using the control algorithm, the maximum roll angle and maximum pitch angle of the robot are reduced by 12.8° and 4.5° respectively; by adopting this attitude control method, the obstacle-crossing time is shortened by 14%, greatly improving the body stability of the robot when crossing the single-sided step obstacle terrain, and at the same time, the obstacle-crossing time is shortened and the obstacle-crossing efficiency is improved.

[0140] (4) Comparing with the traditional genetic algorithm to verify the performance improvement of the improved genetic algorithm, the results show that compared with the traditional genetic algorithm, the response time of the improved genetic algorithm is shortened by 60%, and on the premise of ensuring the adjustment accuracy and stability, the efficiency of the algorithm is greatly improved.

[0141] The above-described embodiments are only preferred specific implementation manners of the present invention, and the protection scope of the present invention is not limited thereto. Any simple changes or equivalent replacements of the technical solutions that can be obviously obtained by those skilled in the art within the technical scope disclosed by the present invention all belong to the protection scope of the present invention.

Claims

1. A method for controlling the obstacle-crossing attitude of a single-side step obstacle of a wheel-track composite mobile system, characterized in that It includes the following steps: Establish the initial coordinate system of the robot and define the spatial attitude parameters representing the obstacle-crossing attitude of the robot; Based on the initial coordinate system and the spatial attitude parameters, use the method of kinematic pose transformation to perform homogeneous transformation on the initial coordinate system to obtain the kinematic model regarding the wheel center position and the spatial attitude parameters; Based on the kinematic model, with the stability of the robot body as the control target, take the swing angle of the rear swing arm of the robot and the length of the electric push rod as control variables, use the control variables as the initial population of the improved genetic algorithm, and use the improved genetic algorithm to iteratively solve the control variables until the optimal solution of the control variables is obtained; According to the optimal solution of the control variables, perform coordinated control on the swing arm-leg to adjust the obstacle-crossing attitude of the vehicle body and enhance the obstacle-crossing stability.

2. A unilateral step obstacle crossing attitude control method for a wheel-track composite mobile system according to claim 1, characterized in that The initial coordinate system includes Spatial absolute coordinate system OX O Y O Z O 、Robot main body coordinate system DX D Y D Z D 、Right rocker arm coordinate system AX A Y A Z A 、Left rocker arm coordinate system HX H Y H Z H 、Balance link coordinate system EX E Y E Z E 、Right front swing arm coordinate system KX K Y K Z K 、Right rear swing arm coordinate system MX M Y M Z M 、Left front swing arm coordinate system NX N Y N Z N 、Left rear swing arm coordinate system PX P Y P Z P ; The said DX D Y D Z D The origin D of which is the midpoint of the main body central axis, Y D is along the central axis direction, X D represents perpendicular to the central axis and pointing to the forward direction of the robot; AX A Y A Z A rotates around the J A joint, HX H Y H Z H rotates around the J H joint, EX E Y E Z E rotates around the J E joint, KX K Y K Z K rotates around the J K joint, MX M Y M Z M rotates around the J M joint, NX N Y N Z N rotates around the J N joint, PX P Y P Z P rotates around the J P joint.

3. A method for controlling the obstacle-crossing attitude of a single-side step obstacle of a wheel-track compound mobile system according to claim 2, characterized in that, Defining the spatial attitude parameters representing the obstacle-crossing attitude of the robot includes the following steps: Translation DX D Y D Z D Origin, making DX D Y D Z D The origin coincides with OX O Y O Z O Then translate DX D Y D Z D Rotate respectively around the X-axis and Y-axis of OX O Y O Z O to make DX D Y D Z D completely coincide with OX O Y O Z O Rotate DX D Y D Z D around the X-axis of OX O Y O Z O The rotation angle is defined as the roll angle β, and rotate DX D Y D Z D around the Y-axis of OX O Y O Z O The rotation angle is defined as the pitch angle α; Rotate AX A Y A Z A about joint J A The rotation angle is defined as γ1. Rotate HX H Y H Z H about joint J H The rotation angle is defined as γ2, where γ2 = -γ1. Rotate KX K Y K Z K about joint J K The rotation angle is defined as θ K , rotate MX M Y M Z M about joint J M The rotation angle is defined as θ M , rotate NX N Y N Z N about joint J N The rotation angle is defined as θ N , rotate PX P Y P Z P about joint J P The rotation angle is defined as θ P ; The mobile robot adjusts the included angle θ el and θ er (The length of the left electric push rod is represented by L el , and the length of the right electric push rod is represented by L er ) of the two side legs by controlling the lengths L L and θ R to change its roll angle.

4. A unilateral step obstacle crossing attitude control method for a wheel-track compound mobile system according to claim 3, characterized in that Using the method of kinematic pose transformation to perform homogeneous transformation on the initial coordinate system includes the following steps: Translate DX D Y D Z D If the origin coincides with OX O Y O Z O the homogeneous translation transformation matrix is: where x OD , y OD , z OD is the three-dimensional coordinate difference between the origin D of the vehicle body coordinate system and the origin O of the absolute coordinate system in the absolute coordinate system when getting off the initial attitude. After the coordinate system origins coincide, rotate the main vehicle body coordinate system DX D Y D Z D First rotate by β about the X-axis of the space absolute coordinate system, and then rotate by α about the Y-axis to make the main vehicle body coordinate system completely coincide with the space absolute coordinate system. Then the homogeneous rotation transformation matrix is: where s represents sin and c represents cos, and this notation will be used in matrices hereafter; α and β respectively represent the rotation angles in the Y-axis and X-axis directions, and are defined as the pitch angle and roll angle of the robot. D Y D Z D in the Y-axis and X-axis directions, and are defined as the pitch angle and roll angle of the robot. According to DX D Y D Z D are respectively related to AX A Y A Z A and HX H Y H Z H in terms of the positional relationship. AX A Y A Z A rotates around joint J A by an angle of γ1, and HX H Y H Z H rotates around joint J H by an angle of γ2. Then their translation transformation matrix and rotation transformation matrix with respect to the main vehicle body coordinate system DX D Y D Z D are respectively as follows: where, (x DA , y DA , z DA )——the coordinate difference between AX A Y A Z A and DX D Y D Z D in the main vehicle body coordinate system, and (x DH , y DH , z DH )——the coordinate difference between HX H Y H Z H and DX D Y D Z D in the main vehicle body coordinate system; Due to the right front swing arm rotating around joint J K rotates, the translation transformation matrix and rotation transformation matrix of KX K Y K Z K relative to AX A Y A Z A are respectively as follows: Among them, (x AK , y AK , z AK )——the coordinate difference between KX K Y K Z K and AX A Y A Z A in the right rocker arm coordinate system; The transformation at the other three swing arm joints is the same. Therefore, the wheel centers w of the eight wheels are defined as follows: i The coordinates in their respective coordinate systems are respectively: where w′ i is the coordinate of the center of each wheel in its respective coordinate system, x′ i is the X-direction coordinate of the center of each wheel in its respective coordinate system, y′ i is the Y-direction coordinate of the center of each wheel in its respective coordinate system, z′ i is the Z-direction coordinate of the center of each wheel in its respective coordinate system; According to the robot kinematic spatial attitude equation, the kinematic matrix equation regarding the relationship between the wheel center position and the spatial attitude parameters is:

5. A method for controlling the obstacle-crossing attitude of a single-side step obstacle by a wheel-track composite mobile system according to claim 4, characterized in that The kinematic model of the wheel center position and the spatial attitude parameters includes a forward kinematic model and an inverse kinematic model. The forward kinematic model is: w i = f i (α, β, γ1, θ K , θ M , θ N , θ P , L el , L er ) where i is the i-th wheel; The inverse kinematic model is: (α, β, γ1) = f -1 (w1, w2, w3, w4, w5, w6, w7, w8, θ K , θ M , θ N , θ P , L el , L er ) 6. A method for controlling the obstacle-crossing attitude of a single-side step obstacle by a wheel-track composite mobile system according to claim 1, characterized in that Using the improved genetic algorithm to iteratively solve the inverse kinematic model until the optimal solution of the control target is obtained includes the following steps: Set the population range with the design adjustment range of the robot as the limit condition, use the ranges of the swing angle of the rear swing arm and the length of the electric push rod as the population boundaries of the improved genetic algorithm, and generate new individuals within the limited interval to form the initial population; Design a fitness function to improve the obstacle-crossing stability of the robot; According to the fitness function, perform selection, crossover, mutation, and periodic particle swarm optimization (PSO) on the initial population, and perform population update and iteration to finally obtain the optimal solution that meets the control target.

7. A method for controlling the over-obstacle attitude of a single-side step obstacle of a wheel-track composite mobile system according to claim 6, characterized in that, The population boundary range of the improved genetic algorithm is as follows: the change range of the rear swing arm angle is 13° ≤ θ P ≤ 90°, and the change range of the length of the electric push rod is 170 mm ≤ L e ≤ 230 mm.

8. A method for controlling the obstacle-crossing attitude of a single-side step obstacle by a wheel-track composite mobile system according to claim 6, characterized in that The fitness function is: Where α is the pitch angle, β is the roll angle, and in the formula, Δθ P is the swing angle of the rear swing arm, and ΔL el , ΔL er are the change amounts of the lengths of the electric push rods of the attitude adjustment structures on the left and right sides, and n is the influence factor of the change amount of the control variable on the stability of the robot.

9. A unilateral step obstacle crossing attitude control method for a wheel-track composite mobile system according to claim 6, characterized in that After performing selection, crossover, and mutation operations on the initial population, perform periodic particle swarm optimization (PSO). The periodic particle swarm optimization (PSO) includes the following steps: Trigger particle swarm optimization once every N generations; The particle swarm size of the PSO optimization is 20% of the current population size; Update the global optimal solution by comparing the fitness of the population after PSO optimization with the global optimal solution. When the fitness of the current solution is better, update the global optimal solution and the corresponding individual.

Images