A stability hierarchical control method for multi-configuration motion of unmanned transformable vehicle

Abstract

The application discloses a kind of for unmanned transform car multi-configuration motion stability layered control method, comprising:1 the electromechanical dynamics model of establishment adjustment centroid mechanism,2 upper layer decision controller is established,3 lower layer car state steady steering stability controller and humanoid state walking stability controller are established, the control voltage required for the displacement of adjustment centroid slider is calculated and output,4 the control voltage obtained is input to the electromechanical dynamics model of adjustment centroid mechanism to obtain the displacement of adjustment centroid slider,5 the stability factor of unmanned transform car car state or the zero moment point of humanoid state is calculated using the displacement of adjustment centroid slider and is fed back to lower layer stability controller to constitute closed loop control.The application can determine the working condition according to the motion parameters of unmanned transform car, convert into corresponding working mode to control the movement of slider in adjustment centroid mechanism, and then change the centroid position of unmanned transform car, so as to improve the stability of unmanned transform car multi-configuration motion.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of robot stability control, and particularly relates to a stability hierarchical control method for multi-configuration motion of an unmanned transformable vehicle. BACKGROUND

[0002] The unmanned transformable vehicle is a wheel-foot robot capable of transforming between a car configuration and a humanoid configuration, and has the advantages of wheel movement and foot movement, strong flexibility and environmental suitability, and is one of the research hotspots in the field of robots. At present, the wheel-foot robot mainly includes the following research aspects: design and optimization of mechanical structure, nonlinear motion planning and control, perception and navigation, and artificial intelligence and machine learning. The walking stability control method of the wheel-foot robot is mostly based on the zero moment point method to readjust and plan the motion trajectory of the robot leg joint, redesign the motion law of the ankle joint and the hip joint according to the three-dimensional inverted pendulum model, and control the motion posture of the robot in real time, so as to improve the walking stability of the robot. However, the robot walking is a multi-degree-of-freedom and strongly coupled motion process, and each joint of the robot leg needs to perform its own walking motion control while considering the real-time control of walking stability, so the whole walking control process has strong dependence on the kinematics and dynamics model, and the stability control effect in a complex environment is limited. In addition, in the research on the stability control of the wheel movement of the wheel-foot robot, the speed, acceleration and inclination angle of the robot are basically collected by the sensor, and the rotation speed and motion direction of the wheel are controlled through the decision module and the control module, and few studies use the automobile theory to control the stability of the wheel movement. At present, the steering stability control of the automobile is mostly realized by differential braking or active front wheel steering technology, and the steady-state steering control of the vehicle is rarely studied. SUMMARY

[0003] The application is to solve the problems existing in the prior art, and provides a stability hierarchical control method for multi-configuration motion of an unmanned transformable vehicle, so as to determine the working condition according to the motion parameters of the unmanned transformable vehicle, convert into the corresponding working mode to control and adjust the motion of the sliding block in the mass center mechanism, change the mass center position of the unmanned transformable vehicle, and improve the stability of the multi-configuration motion of the unmanned transformable vehicle.

[0004] In order to achieve the above application purposes, the application adopts the following technical scheme:

[0005] The application is characterized in that the unmanned transformable vehicle has two configurations of a car configuration and a humanoid configuration, and the whole vehicle structure comprises a front vehicle body, a rear vehicle body and a mass center adjusting mechanism; two mechanical legs and a lifting mechanism are arranged on the bottom plate of the rear vehicle body; each mechanical leg comprises a thigh, a lower leg, a supporting foot, an ankle joint, a knee joint and a hip joint; the mass center adjusting mechanism comprises a slide rail, a mass center adjusting slider moving in a horizontal and vertical direction with two degrees of freedom, a ball screw and a driving motor; the stability hierarchical control method is performed according to the following steps:

[0006] Step one, establishing a coordinate system of the unmanned transformable vehicle;

[0007] Step 1.1, establishing a basic coordinate system on the horizontal plane where the supporting foot of the unmanned transformable vehicle is located , the origin of the basic coordinate system is arranged at the middle position of the supporting foot, the positive direction of the axis of the basic coordinate system is the forward direction of the unmanned transformable vehicle, the positive direction of the axis of the basic coordinate system is upward perpendicular to the horizontal plane, and the positive direction of the axis of the basic coordinate system is perpendicular to the plane where the axis and the axis are located and points to the outside of the vehicle;

[0008] Step 1.2, sequentially establishing the body coordinate system of the right leg ankle joint, the body coordinate system of the knee joint and the body coordinate system of the hip joint, the body coordinate system of the left leg ankle joint, the body coordinate system of the knee joint and the body coordinate system of the hip joint and the body coordinate system of the lifting mechanism, and marking any one of the body coordinate systems as the first body coordinate system , ;

[0009] Step two, establishing the electromechanical dynamics model between the displacement of the mass center adjusting slider and the control voltage of the driving motor according to formula (1);

[0010] (1)

[0011] In formula (1), represents the equivalent current constant of the mass center adjusting mechanism; represents the equivalent torque constant of the mass center adjusting mechanism; is the transmission ratio of the ball screw, and , is the lead of the ball screw; and are the equivalent moment of inertia and the equivalent viscous damping coefficient of the mass center adjusting mechanism, respectively;​​​ to adjust the acceleration of the mass center slider, to adjust the velocity of the mass center slider;

[0012] Step three, establish the upper decision control strategy according to formula (2) ;

[0013] (2)

[0014] In formula (2), is the motion parameter of the unmanned transformable vehicle in each configuration, and wherein, indicates the vehicle speed of the unmanned transformable vehicle, is the front wheel steering angle of the unmanned transformable vehicle, is the velocity of the mass center slider, is the angular velocity of the three joints; is a set of discrete working modes, and wherein, is the working mode of the unmanned transformable vehicle in the automobile state steering driving state, is the working mode of the unmanned transformable vehicle in the humanoid state walking state, is the working mode of the unmanned transformable vehicle in the straight line driving state, is the working mode of the unmanned transformable vehicle in the parking static state; is a finite set of input variables, and , indicates the state of the front wheel steering angle , when , let , when , let ; indicates the state of the vehicle speed , when , let , when , let ; indicates the state of the joint angular velocity , when , let , when , let ; indicates the state of the mass center slider velocity , when , let , when , let ; indicates that indicates the transition between two working modes;

[0015] Step 4: Determine the working condition of the unmanned transforming vehicle based on its motion parameters and perform stability control under the corresponding working mode;

[0016] Make the unmanned transforming car in working mode initially ;

[0017] When the motion parameters of the unmanned transformable vehicle are obtained When the unmanned transforming car is switched to working mode ;

[0018] When the motion parameters of the unmanned transformable vehicle are obtained When the unmanned transforming car is switched to working mode ;

[0019] When the motion parameters of the unmanned transformable vehicle are obtained When the unmanned transforming car is switched to working mode ;

[0020] When the motion parameters of the unmanned transformable vehicle are obtained When the unmanned transforming car is switched to working mode ;

[0021] Step 5: Establish the lower layer stability control strategy and adjust the working mode or working mode Stability control of the unmanned transformable vehicle under the vehicle;

[0022] Step 5.1, in working mode Stability control of the unmanned transformable vehicle under the vehicle;

[0023] Step 5.1.1: Turn the unmanned transformable vehicle into a car-like state. The actual stability factor at the moment and expected stability factor The deviation between and the rate of change of deviation The input is processed by the Fuzzy-PID controller and the output is used to adjust the center of mass slider. Control voltage at the moment ;

[0024] Step 5.1.2, Substituting into formula (1), we can get the adjustment of the center of mass slider at After the displacement of time, Substitute the displacement at the moment into formula (3) to obtain The unmanned transforming car is in the basic coordinate system at all times The centroid coordinates of ;

[0025] (3)

[0026] In formula (3), is the mass of any e-th component of the lifting mechanism, the centroid adjustment slider, the thigh of the two mechanical legs, the lower leg of the two mechanical legs, and the supporting foot of the two mechanical legs, m is the total mass of the unmanned transformable vehicle, num is the number of components, and num = 8, is the centroid of the e-th component of the unmanned transformable vehicle at time t; is the axis coordinate of the centroid of the e-th component of the unmanned transformable vehicle at time t;

[0027] Step 5.1.3, obtaining the actual stability factor of the unmanned transformable vehicle at time t in the automobile-state turning driving state by using formula (4)

[0028] (4)

[0029] In formula (4), are the cornering stiffness of the front wheel and the rear wheel, respectively, is the distance from the centroid of the whole vehicle to the front axle at time t, and ; is the distance from the centroid of the whole vehicle to the rear axle at time t, and ; is the wheelbase of the unmanned transformable vehicle;

[0030] Step 5.1.4, assigning + 1 to , and then sequentially executing step 5.1.1 until the turning driving state of the unmanned transformable vehicle in the automobile state changes or the deviation is zero, so as to realize the stability control of the unmanned transformable vehicle in the working mode ;

[0031] Step 5.2, performing the stability control on the unmanned transformable vehicle in the working mode based on the K-means clustering algorithm;

[0032] Step 5.2.1, establishing the stability level of the K-means clustering algorithm ;

[0033] Step 5.2.2, judging the stability level corresponding to the actual zero moment point position of the unmanned transformable vehicle at time t in the humanoid-state walking state based on the stability level , and inputting it into the stretch factor fuzzy controller for processing, so as to output ​​​​​Deviation scaling factor at the moment 、 The time deviation change rate scaling factor 、 The expected displacement expansion factor at time As the control parameters of the basic fuzzy controller;

[0034] Step 5.2.3: Make the unmanned transformable car walk in a human-like state The expected zero moment point position at time and the actual zero moment point position Deviation and the rate of change of deviation The input is processed into the basic fuzzy controller and the output is used to adjust the centroid slider. Expected displacement at time ;

[0035] Step 5.2.4, adjust the center of mass slider to Expected displacement at time The input is processed by the ADRC and the output is the center of mass slider of the unmanned vehicle in the human-like walking state. Control voltage at the moment ;

[0036] Step 5.2.5, Substituting into formula (1), we can get the adjustment of the center of mass slider at After the displacement of time, Substitute the displacement at the moment into formula (7) to calculate the displacement of the unmanned deformable vehicle in the basic coordinate system. Moment Towards the zero moment point and Towards the zero moment point , thus obtaining the unmanned transformable vehicle in a human-like walking state The actual zero moment point position at time ;

[0037] (7)

[0038] In formula (7), They are The e-th component of the unmanned transforming car at the moment Towards, Xianghe The acceleration toward the center of mass, is the acceleration due to gravity, They are The center of mass of the e-th component of the unmanned transforming vehicle at the moment is in the basic coordinate system Axis coordinates, axis coordinates and axis coordinates;

[0039] Step 5.2.6, assigning to After that, return to step 5.2.2 for sequential execution until the walking state of the unmanned transformable vehicle in the humanoid state changes or the deviation is zero, thereby realizing the stability control of the unmanned transformable vehicle in the working mode .

[0040] The stability hierarchical control method for the multi-configuration motion of the unmanned transformable vehicle according to the present application is also characterized in that the step 5.2.1 comprises:

[0041] Step 5.2.1.1, calculating the deviation sequence and the deviation change rate sequence between the j actual zero moment point positions in the humanoid state walking state of the unmanned transformable vehicle and the expected zero moment point positions, to obtain the stability clustering data set in the humanoid state walking state of the unmanned transformable vehicle;

[0042] Step 5.2.1.2, defining the maximum iteration number , the current iteration number is R, and initializing R = 1, randomly selecting N data points in the stability clustering data set as initial cluster center points | }, wherein, represents the n-th initial cluster center point in the R-th iteration; represents the deviation of the n-th cluster center point in the R-th iteration; represents the deviation change rate of the n-th cluster center point in the R-th iteration; Step 5.2.1.3, defining and initializing the variable i = 1; Step 5.2.1.4, calculating the distance

[0043] between the i-th data point in the stability clustering data set and the n-th cluster center point

[0044] in the R-th iteration according to formula (6), so as to assign the i-th data point to the nearest cluster center point;

[0045] (6)

[0046] In formula (6), represents the deviation weight factor, represents the deviation change rate weight factor;​​​

[0047] In step 5.2.1.5, after assigning the value i+1 to i, return to step 5.2.1.4 and repeat the process until i > j. This assigns all data points in the stability clustering dataset to the nearest cluster center, resulting in N clusters at the Rth iteration.

[0048] Step 5.2.1.6. Calculate the average value of the deviation and the rate of change of deviation of the data points contained in the nth cluster at the Rth iteration, and use them as the center point of the nth cluster at the R+1th iteration. ;in, Indicates the R+1th iteration Deviation of cluster centers; Indicates the R+1th iteration The deviation change rate of the cluster center points;

[0049] Step 5.2.1.7: After assigning R+1 to R, return to step 5.2.1.3 and execute sequentially until R > So far, we get N clusters and their cluster centers at the first iteration; The cluster center point at the iteration is used as the stability level of the unmanned transformable vehicle in the walking state .

[0050] An electronic device of the present invention includes a memory and a processor, and is characterized in that the memory is used to store a program that supports the processor to execute the stability stratification control method, and the processor is configured to execute the program stored in the memory.

[0051] The present invention provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program executes the steps of the stability stratification control method when executed by a processor.

[0052] Compared with the prior art, the present invention has the following beneficial effects:

[0053] 1. This invention uses the center of mass adjustment mechanism installed on the rear body of an unmanned transforming vehicle as the control target and designs a hierarchical control method for the multi-configuration motion stability control of the unmanned transforming vehicle. The upper-level decision controller, based on hybrid theory, makes control decisions based on the motion parameters of the unmanned transforming vehicle to select the appropriate operating mode. The lower-level stability controller, based on the control decisions issued by the upper-level controller, uses a fuzzy-PID controller for steady-state steering control in the vehicle state or variable universe fuzzy control and active disturbance rejection control for humanoid walking stability control, thereby improving the stability of the unmanned transforming vehicle in various motion states.

[0054] 2. The application innovatively fuses the K-means clustering method with the zero moment point stability criterion method, realizes the stability quantitative rating of the unmanned transformable vehicle in the walking process, further improves the control accuracy and adaptability of the variable universe fuzzy controller, and improves the accuracy and real-time performance of the walking stability control.

[0055] 3. The control system of the application is completely independent of the power system of the unmanned transformable vehicle, the human-like walking and turning walking are completed by the mechanical legs, the steering driving of the car state is completed by the steer-by-wire mechanism, and the stability control of the system is completed by the adjustment mass mechanism. Two functions are completed by two execution mechanisms respectively, the stability control does not affect the normal motion planning of the unmanned transformable vehicle, not only widens the control method of the steering stability of the car state, but also improves the stability and flexibility of the human-like walking and turning. BRIEF DESCRIPTION OF DRAWINGS

[0056] Figure 1 The figure is a structure and configuration schematic diagram of the unmanned transformable vehicle involved in the application;

[0057] Figure 2 The figure is a structure schematic diagram of the adjustment mass mechanism of the unmanned transformable vehicle involved in the application;

[0058] Figure 3 The figure is a stability hierarchical control principle diagram of the multi-configuration motion of the unmanned transformable vehicle based on the adjustment mass mechanism;

[0059] Figure 4 The figure is a coordinate system schematic diagram of the unmanned transformable vehicle involved in the application;

[0060] Figure 5 The figure is a stability factor simulation curve diagram of the system before and after the steering stability control of the car state of the unmanned transformable vehicle involved in the application;

[0061] Figure 6a The figure is a stability factor simulation curve diagram of the system before and after the walking stability control of the human-like state of the unmanned transformable vehicle involved in the application; to the zero moment point;

[0062] Figure 6b The figure is a stability factor simulation curve diagram of the system before and after the walking stability control of the human-like state of the unmanned transformable vehicle involved in the application; to the zero moment point. DETAILED DESCRIPTION

[0063] The application will be further described below in combination with the drawings and specific embodiments:

[0064] The unmanned transformable vehicle is a multi-component motion coupling nonlinear mechanical system, has two configurations of a car state and a humanoid state, and has different motion modes and stability characteristics in different configurations. Figure 1 The unmanned transformable vehicle includes a front vehicle body, a rear vehicle body and a mass center adjusting mechanism, two mechanical legs and a lifting mechanism are arranged on the bottom plate of the rear vehicle body, wherein each mechanical leg includes a thigh, a shank, a supporting foot, an ankle joint, a knee joint and a hip joint.

[0065] The front vehicle body and the rear vehicle body of the unmanned transformable vehicle have a hub motor, a steering trapezoidal mechanism, a disc brake and a MacPherson suspension, so that the unmanned transformable vehicle can run at high speed, steer and brake in the car state; the front vehicle body is connected with the rear vehicle body through the lifting mechanism, when the unmanned transformable vehicle is reconfigured into the humanoid state, the lifting mechanism is driven by the motor and the electric push rod to realize the horizontal lifting of the front vehicle body. The mechanical leg is installed on the bottom plate of the rear vehicle body, is folded in the bottom of the vehicle body in the car state, is unfolded to the ground to support the whole vehicle body in the humanoid state, and realizes the step walking and steering.

[0066] The mass center adjusting mechanism includes a slide rail, a mass center adjusting slider moving in the horizontal and vertical directions with two degrees of freedom, a ball screw and a driving motor, and the structure and arrangement of the mass center adjusting mechanism are as shown in Figure 2 The main working principle is that the rotation of the driving motor is converted into the movement of the mass center adjusting slider on the slide rail through the ball screw, and then the mass center position of the unmanned transformable vehicle is changed, so that the stability of the multi-configuration motion of the unmanned transformable vehicle is improved.

[0067] In this embodiment, a stability hierarchical control method for multi-configuration motion of the unmanned transformable vehicle is provided, and the method is as shown in Figure 3 The method is composed of an upper decision controller and a lower motion stability controller. The upper decision controller is based on the hybrid theory, judges the motion working condition of the unmanned transformable vehicle according to the motion parameters of the unmanned transformable vehicle, and converts it into the corresponding working mode for motion stability control. The lower controller is composed of a steady-state steering stability control module in the car state and a walking stability control module in the humanoid state, and the motion stability of the unmanned transformable vehicle is realized by controlling the motion of the mass center adjusting slider. The stability hierarchical control method is performed according to the following steps:

[0068] Step one, establishing a coordinate system of the unmanned transformable vehicle;

[0069] Step 1.1, establishing a basic coordinate system on the horizontal plane where the supporting foot of the unmanned transformable vehicle is located The origin of the basic coordinate system is arranged at the middle position of the supporting foot, the positive direction of the axis of the basic coordinate system is the forward direction of the unmanned transformable vehicle, and the The positive direction of the axis is perpendicular to the horizontal plane upward, and the base coordinate system is The positive direction of the axis is perpendicular to the horizontal plane upward, and the base coordinate system is The positive direction of the axis is perpendicular to the horizontal plane upward, and the base coordinate system is The positive direction of the axis is perpendicular to the horizontal plane upward, and the base coordinate system is

[0070] Step 1.2, in turn, establish the body coordinate system of the right ankle joint, the body coordinate system of the knee joint, and the body coordinate system of the hip joint, the body coordinate system of the left ankle joint, the body coordinate system of the knee joint, and the body coordinate system of the hip joint, and the body coordinate system of the lifting mechanism, and any one of the body coordinate systems is marked as the wth body coordinate system , Specifically, as shown in Figure 4 ;

[0071] Define the body coordinate system Relative to The rotation angle and the translation distance of the axis, The rotation angle and the translation distance of the axis, The rotation angle and the translation distance of the axis, , , , If the rotation angle and the translation distance of the axis, The homogeneous transformation matrix between adjacent body coordinate systems can be expressed as:

[0072]

[0073] The homogeneous transformation matrix of any body coordinate system relative to the base coordinate system can be expressed as: ;

[0074] Step 2, according to formula (1) to establish the electromechanical dynamics model between the displacement of the adjustment mass slider And the control voltage of the driving motor ;

[0075] (1)

[0076] In formula (1), Indicates the equivalent current constant of the adjustment mass mechanism; Indicates the equivalent torque constant of the adjustment mass mechanism; Is the transmission ratio of the ball screw, and , Is the lead of the ball screw; And Is the equivalent rotational inertia and the equivalent viscous damping coefficient of the adjustment mass mechanism, respectively; Is the acceleration of the adjustment mass slider, Is the speed of the adjustment mass slider;

[0077] Step three, establish the upper decision control strategy according to formula (2) ;

[0078] (2)

[0079] In formula (2), is the motion parameter of the unmanned transformable vehicle in each configuration, and , wherein, indicates the vehicle speed of the unmanned transformable vehicle, is the front wheel steering angle of the unmanned transformable vehicle, is the speed of the mass center slider, is the angular velocity of the three joints; is a set of discrete working modes, and , wherein, is the working mode of the unmanned transformable vehicle in the automobile state steering driving state, is the working mode of the unmanned transformable vehicle in the humanoid state walking state, is the working mode of the unmanned transformable vehicle in the straight line driving state, is the working mode of the unmanned transformable vehicle in the parking stationary state; is a finite set of input variables, and , indicates the state of the front wheel steering angle , when , let , when , let ; indicates the state of the vehicle speed , when , let , when , let ; indicates the state of the joint angular velocity , when , let , when , let ; indicates the state of the mass center slider speed , when , let , when , let ; indicates that, and indicates the conversion between two working modes.

[0080] Step four, determine the working condition according to the motion parameter of the unmanned transformable vehicle, convert to the corresponding working mode for stability control;

[0081] For the multi-configuration motion of the unmanned transformable vehicle, on the one hand, the motion parameters (vehicle speed , front wheel steering angle , velocity of the mass center adjusting slider , joint angular velocity ) of the unmanned transformable vehicle in each configuration change with time. On the other hand, the motions in different configurations constitute a series of discrete motion events. Therefore, the motion of the unmanned transformable vehicle in multiple configurations is a hybrid dynamic system. When the unmanned transformable vehicle detects changes in the motion parameters, the upper-level decision controller selects the corresponding working mode according to the characteristics of the discrete events to perform motion stability control in the corresponding working mode.

[0082] Let the unmanned transformable vehicle initially be in working mode ;

[0083] When the motion parameter input of the unmanned transformable vehicle is , let the unmanned transformable vehicle switch to working mode ;

[0084] When the motion parameter input of the unmanned transformable vehicle is , let the unmanned transformable vehicle switch to working mode ;

[0085] When the motion parameter input of the unmanned transformable vehicle is , let the unmanned transformable vehicle switch to working mode ;

[0086] When the motion parameter input of the unmanned transformable vehicle is , let the unmanned transformable vehicle switch to working mode .

[0087] Step five, establish a lower-level stability control strategy and perform stability control on the unmanned transformable vehicle in working mode or working mode ;

[0088] Step 5.1, perform stability control on the unmanned transformable vehicle in working mode ;

[0089] Step 5.1.1, input the deviation between the actual stability factor of the unmanned transformable vehicle at time in the automobile state steering driving state and the expected stability factor and the rate of change of the deviation to the Fuzzy-PID controller for processing, and output the control voltage of the mass center adjusting slider at time In the steady-state steering stability control of automobiles, the control parameters of conventional PID controllers are fixed and have poor robustness, so fuzzy PID controllers are used as the working mode. The stability controller below.

[0090] The actual stability factor and expected stability factor The deviation between and the rate of change of deviation As the input of the fuzzy controller, the output is 、 、 First, put and Fuzzy processing, and the processed data is input into the fuzzy controller; after fuzzification, fuzzy reasoning and clarity, 、 、 , and then get the new PID control parameter value.

[0091] Step 5.1.1.1. Determine the domain of the fuzzy controller: Input variables The domain of is [-0.1, 0.1], The domain is [-0.02, 0.02], and the output variable The domain of is [-15, 15], The domain of is [-1, 1], The domain of is [-15, 15];

[0092] Step 5.1.1.2, determine the fuzzy set: the fuzzy set of fuzzy control input and output is , representing {negative large, negative medium, negative small, zero, positive small, positive medium, positive large} respectively;

[0093] Step 5.1.1.3. Determine the membership function: triangular membership function.

[0094] Step 5.1.1.4, establish fuzzy rules;

[0095] 、 、 The fuzzy rules are shown in Table 1;

[0096] Table 1 、 、 Fuzzy rule table

[0097]

[0098] Step 5.1.2, Substituting into formula (1), we can get the adjustment of the center of mass slider at After the displacement of time, Substitute the displacement at the moment into formula (3) to obtain The unmanned transforming car is in the basic coordinate system at all times The centroid coordinates of ;

[0099] (3)

[0100] In formula (3), is the mass of any e-th component among the lifting mechanism, the center of mass adjustment slider, the thighs of the two robotic legs, the calves of the two robotic legs, and the supporting feet of the two robotic legs. m is the total mass of the unmanned transformable vehicle. num is the number of components, and num=8. for The center of mass of the e-th component of the unmanned transformable vehicle at time Axis coordinates.

[0101] Step 5.1.3: Use formula (4) to get the unmanned transformable vehicle in the car state turning state The actual stability factor at the moment ;

[0102] (4)

[0103] In formula (4), are the cornering stiffness of the front and rear wheels, yes The distance from the center of mass of the vehicle to the front axle at the moment, and ; yes The distance from the center of mass of the vehicle to the rear axle at the moment, and ; It is the wheelbase of the unmanned transforming car;

[0104] Step 5.1.4, +1 assigned to After that, return to step 5.1.1 and execute in sequence until the steering state of the unmanned transforming vehicle in the car state changes or the deviation is zero, thereby realizing the unmanned transforming vehicle in the working mode. Stability control under.

[0105] Step 5.2: Based on the K-means clustering algorithm, Stability control of the unmanned transformable vehicle under the vehicle;

[0106] Step 5.2.1. Establish the stability level of the K-means clustering algorithm ;

[0107] Step 5.2.1.1, calculate the stability clustering data set of the humanoid walking state of the unmanned transformable vehicle the deviation sequence between the actual zero moment point position and the expected zero moment point position and the deviation change rate sequence , to obtain the stability clustering data set of the humanoid walking state of the unmanned transformable vehicle;

[0108] Step 5.2.1.2, define the maximum number of iterations as 1000 times, the current iteration number as R, and initialize R = 1, and randomly select 5 data points in the stability clustering data set as initial cluster center points | }, wherein represents the n-th initial cluster center point in the R-th iteration; represents the deviation of the n-th cluster center point in the R-th iteration; represents the deviation change rate of the n-th cluster center point in the R-th iteration. Step 5.2.1.3, define and initialize the variable i = 1; Step 5.2.1.4, calculate the distance between the i-th data point in the stability clustering data set

[0109] ) and the n-th cluster center point in the R-th iteration according to formula (6)

[0110] , so as to assign the i-th data point to the nearest cluster center point; (6)

[0111] (6)

[0112] In formula (6), represents the deviation weight factor, represents the deviation change rate weight factor.

[0113] Step 5.2.1.5, after assigning i+1 to i, return to step 5.2.1.4 for sequential execution until i > ; so as to assign all data points in the stability clustering data set to the nearest cluster center point, to obtain 5 clusters in the R-th iteration;

[0114] Step 5.2.1.6, calculate the average value of the deviation and the deviation change rate of the data points contained in the n-th cluster in the R-th iteration, respectively, and take it as the n-th cluster center point in the R+1-th iteration ; wherein ​​Indicates the R+1th iteration Deviation of cluster centers; Indicates the R+1th iteration The deviation change rate of the cluster center.

[0115] Step 5.2.1.7: After assigning R+1 to R, return to step 5.2.1.3 and execute sequentially until R > So far, we get The five clusters and their cluster centers at the first iteration; The cluster center point at the iteration is used as the stability level of the unmanned transformable vehicle in the walking state ,Specifically, as shown in Table 2;

[0116] Table 2 Cluster center points and stability levels during walking

[0117]

[0118] Step 5.2.2: The domain range and the number of fuzzy quantity levels significantly influence the control performance of the fuzzy controller. The greater the number of fuzzy quantity levels and the smaller the domain range, the finer the level divisions, and the higher the control accuracy of the controller. However, the finer the level divisions, the slower the controller's solution speed and the greater the negative impact of measurement errors on the classification. To improve the adaptability and accuracy of the fuzzy controller without increasing the number of fuzzy levels, a variable domain fuzzy controller is used to control the driving stability of an unmanned transformable vehicle.

[0119] According to stability level Determine whether the unmanned transformable car is in a human-like walking state The stability level corresponding to the actual zero moment point position at the moment , which is input into the scaling factor fuzzy controller for processing and output Deviation scaling factor at the moment 、 The time deviation change rate scaling factor 、 The expected displacement expansion factor at time As the control parameters of the basic fuzzy controller;

[0120] Step 5.2.2.1. Determine the fuzzy domain: Input variables of the scaling factor fuzzy controller The domain is [0, 1], and the output variable is the deviation scaling factor , Deviation change rate scaling factor , expected displacement expansion factor , the output domain is [0.5, 1.5];

[0121] Step 5.2.2.2, determining fuzzy sets: the input fuzzy sets of the scale factor fuzzy controller are , representing {very stable, stable, sub-stable, critical stable, unstable} respectively, and the output fuzzy sets are , representing {very small, small, medium, large, very large} respectively;

[0122] Step 5.2.2.3, determining membership functions: triangular membership functions;

[0123] Step 5.2.2.4, establishing fuzzy rules of scale factor, specifically as shown in Table 3;

[0124] Table 3 Fuzzy rule table of scale factor

[0125]

[0126] Step 5.2.3, inputting the deviation of the desired zero moment point position of the unmanned transformable vehicle in the human-like walking state at time t from the actual zero moment point position and the rate of change of the deviation to the basic fuzzy controller for processing, and outputting the desired displacement of the adjustment centroid slider at time t .

[0127] Step 5.2.3.1, determining fuzzy domains: the domain of the input variable deviation is [- , ]; the domain of the input variable rate of change of the deviation is [- ], and the domain of the output variable desired slider displacement is [- , ];

[0128] Step 5.2.3.2, determining fuzzy sets: the input and output fuzzy sets of the basic fuzzy controller are , representing {negative large, negative medium, negative small, zero, positive small, positive medium, positive large} respectively;

[0129] Step 5.2.3.3, determining membership functions: triangular membership functions;

[0130] Step 5.2.3.4, establishing fuzzy rules of desired slider displacement, specifically as shown in Table 4;

[0131] Table 4 Fuzzy rule table of desired slider displacement

[0132] ​​​

[0133] Step 5.2.4, the adjustment of the center of mass slider at the expected displacement of the time is input into the active disturbance rejection controller for processing, and the control voltage of the adjustment of the center of mass slider at the time is output.

[0134] Step 5.2.4.1, design a linear extended observer;

[0135] Definition is the total disturbance of the system and is differentiable, ; is the input coefficient of the control system, , the electromechanical dynamics model established according to formula (1) is rewritten according to formula (7);

[0136] (7)

[0137] In formula (7), is the control voltage of the driving motor, is the acceleration of the adjustment of the center of mass slider.

[0138] According to formula (8), the augmented state space expression of the electromechanical dynamics model is obtained:

[0139] (8)

[0140] In formula (8), represents the augmented state variable, and , wherein ; represents the differential of the state variable, represents the displacement of the adjustment of the center of mass slider; represents the augmented coefficient matrix, and , represents the augmented input matrix and , represents the augmented disturbance matrix, and , represents the differential of the total disturbance of the system, represents the augmented output matrix, and .

[0141] According to formula (9), the Luenberger state observer is established;

[0142] (9)

[0143] In formula (9), is the observer vector, and ; is the observation error feedback gain matrix, and ; represents the estimated adjustment centroid slider displacement.

[0144] Define the observation error Subtracting equation (8) from equation (9) gives:

[0145] (10)

[0146] According to modern control theory, the observation error is the condition that the eigenvalues of the matrix have negative real parts. The expression of the matrix is:

[0147] (11)

[0148] The characteristic polynomial of the matrix is:

[0149] (12)

[0150] According to the bandwidth method, define the observer bandwidth as Configure all three poles of the observer at , that is:

[0151] (13)

[0152] According to equation (14), the observer error feedback gain is obtained when , ;

[0153] (14)

[0154] Step 5.2.4.2, design the tracking differentiator.

[0155] In the process of stability adjustment, the expected displacement of the base fuzzy controller output to adjust the centroid slider is a very complex nonlinear curve, and there will be a large overshoot in the process of rapid tracking. In order to suppress the overshoot of control, the steepest tracking differentiator is used to smooth the expected displacement. The steepest tracking differentiator can well balance the contradiction between overshoot and rapidity, and enhance the robustness and noise suppression ability of the system.

[0156] Therefore, the steepest tracking differentiator is used to optimize the tracking effect of the active disturbance rejection controller on the input signal, and the discrete form of the steepest tracking differentiator is obtained according to equation (15):

[0157] (15)

[0158] is the tracking signal of the desired displacement of the slider, is the approximation differential signal of the desired displacement of the slider, is the approximation differential signal of the desired displacement of the slider; is the discrete point of the desired displacement of the slider, is the discrete sampling period; is the steepest tracking function; is the velocity factor; is the filtering factor.

[0159] Step 5.2.4.3, the active compensation of the total disturbance estimated by the state observer is carried out by the error feedback control law of the active disturbance rejection controller, not only the anti-interference effect is achieved, but also the controlled object is transformed into the integral series structure. Therefore, the control voltage is designed according to the state error feedback control law of formula (16):

[0160] (16)

[0161] in formula (16), is the proportional control parameter, is the differential control parameter.

[0162] The desired displacement of the slider output by the variable universe fuzzy controller is smoothed by using the tracking differentiator, and the control voltage without disturbance is output according to formula (17) after the operation of the state observer and the error feedback control law, so as to complete the real-time tracking of the desired displacement of the slider;

[0163] (17)

[0164] Step 5.2.5, substitute into formula (1) to obtain the displacement of the slider at time , and then substitute the displacement at time into formula (18), so as to calculate the zero moment point and the zero moment point of the unmanned transformable vehicle in the basic coordinate system at time , so as to obtain the actual zero moment point position of the unmanned transformable vehicle at time in the humanoid walking state;

[0165] (18)

[0166] ​​​In formula (18), respectively, at the moment of the e th component of the unmanned transformable vehicle, to, to and to the centroid acceleration, is the gravitational acceleration, respectively, at the moment of the e th component of the unmanned transformable vehicle in the base coordinate system, axis coordinates, axis coordinates and axis coordinates.

[0167] Step 5.2.6, assign to , and then return to step 5.2.2 for sequential execution until the walking state of the unmanned transformable vehicle in the humanoid state changes or the deviation is zero, thereby realizing the stability control of the unmanned transformable vehicle in the working mode .

[0168] In this embodiment, an electronic device includes a memory for storing a program supporting a processor to execute the above method, and the processor is configured to execute the program stored in the memory.

[0169] In this embodiment, a computer readable storage medium has a computer program stored thereon, and the computer program is executed by a processor to perform the steps of the above method.

[0170] The change curve of the stability factor before and after the adjustment of the centroid mechanism for the steady-state stability control of the automobile state is shown in Figure 5 . Without stability control, the slider of the centroid self-adjusting mechanism remains stationary at the central position of the slide rail, and since the centroid position of the vehicle does not change, the stability factor is always 0.00097. When stability control is performed, both PID control and fuzzy PID control can make the stability factor curve converge to the expected value 0.0024, but the three control coefficients of fuzzy PID control are variable, so compared with PID control, fuzzy PID has better control effect, as shown in Table 5.

[0171] Table 5 Control system performance index

[0172]

[0173] The change curves of the zero moment point of the unmanned transformable vehicle in the direction and direction before and after the adjustment of the centroid mechanism for the humanoid state walking stability control are shown inFigure 6a and Figure 6b as shown. Figure 6a PID control algorithm in tracking the desired trajectory of zero moment point, not only there is a more obvious control mutation, and tracking effect is poor, the maximum tracking error can reach 0.037 m. So in the PID stability control algorithm, the walking stability margin of unmanned transformable car is insufficient, which is easy to fall instability under external disturbance. In contrast, the two kinds of Fuzzy-ADRC control algorithm(with K-means clustering algorithm and without K-means clustering algorithm) can track the desired zero moment point more quickly and smoothly, and show better control effect. But in the single-double foot support period conversion stage, after the K-means clustering algorithm quantifies the stability level, the overshoot of Fuzzy-ADRC control algorithm is reduced by 14.86%, and the steady-state error is also reduced by 3.6%. Therefore, K-means clustering algorithm can not only more accurately quantify the stability change of unmanned transformable car in the walking process, but also further improve the control effect of Fuzzy-ADRC control algorithm. Figure 6b show similar control results, and in tracking the desired trajectory of zero moment point, the control effect of PID is the worst, and there is a large mutation, while the Fuzzy-ADRC control algorithm with K-means clustering algorithm shows the least overshoot and steady-state error. Therefore, the three control strategies can ensure the stability of the unmanned transformable car in the walking process, but the Fuzzy-ADRC control algorithm with K-means clustering algorithm performs best in the control of zero moment point, thereby ensuring the walking stability of the unmanned transformable car to be optimal.

Claims

1. A stability hierarchical control method for multi-modal motion of an unmanned transformable vehicle, characterized in that: The unmanned transformable vehicle has two configurations: a car and a humanoid. The vehicle structure includes a front body, a rear body, and a mass adjustment mechanism. Two mechanical legs and a lifting mechanism are arranged on the floor of the rear body. Each mechanical leg includes a thigh, a calf, a supporting foot, an ankle joint, a knee joint, and a hip joint. The mass adjustment mechanism includes a slide rail, a mass adjustment slider that moves in two degrees of freedom in the horizontal and vertical directions, a ball screw, and a drive motor. The stability hierarchical control method is performed according to the following steps: Step 1: Establish the coordinate system of the unmanned transformable vehicle; Step 1.1: Establish a basic coordinate system on the horizontal plane where the support foot of the unmanned transforming vehicle is located The origin of the basic coordinate system is set at the middle position of the support foot. The positive direction of the axis is the forward direction of the unmanned transforming vehicle, and the basic coordinate system The positive direction of the axis is perpendicular to the horizontal plane and upward. The positive direction of the axis is perpendicular to Axis and The axis is in the plane and points to the outside of the vehicle; Step 1.2: Establish the appendage coordinate system of the right leg ankle joint, the appendage coordinate system of the knee joint, the appendage coordinate system of the hip joint, the appendage coordinate system of the left leg ankle joint, the appendage coordinate system of the knee joint, the appendage coordinate system of the hip joint, and the appendage coordinate system of the lifting mechanism in sequence, and record any one of the appendage coordinate systems as the wth appendage coordinate system , ; Step 2: Establish the displacement of the adjustment center of mass slider according to formula (1) The control voltage of the drive motor The electromechanical dynamics model between; (1) In formula (1), represents the equivalent current constant of the mass center adjustment mechanism; represents the equivalent torque constant of the mass center adjustment mechanism; is the transmission ratio of the ball screw, and , is the lead of the ball screw; and are the equivalent moment of inertia and equivalent viscous damping coefficient of the adjusted center of mass mechanism respectively; To adjust the acceleration of the center of mass slider, To adjust the speed of the center of mass slider; Step 3: Establish upper-level decision-making control strategy according to formula (2) ; (2) In formula (2), are the motion parameters of the unmanned transformable vehicle in each configuration, and ,in, Indicates the speed of the unmanned transforming car, The front wheel angle of the unmanned transforming car, To adjust the speed of the center of mass slider, is the angular velocity of the three joints; is a discrete set of operating modes, and ,in, This is the working mode of the unmanned transformable car in the car-like steering state. This is the working mode of the unmanned transformable vehicle in a human-like walking state. This is the working mode of the unmanned transformable vehicle in a straight-line driving state. This is the working mode of the unmanned transformable vehicle when it is parked and stationary; is a finite set of input variables, and , Indicates the front wheel angle state, when season ,when season ; Indicates vehicle speed state, when season ,when season ; Represents the joint angular velocity state, when season ,when season ; Indicates adjusting the center of mass slider speed state, when season ,when season ; Indicates and; Indicates the transition between two working modes; Step 4: Determine the working condition of the unmanned transforming vehicle based on its motion parameters and perform stability control under the corresponding working mode; Make the unmanned transforming car in working mode initially ; When the motion parameters of the unmanned transformable vehicle are obtained When the unmanned transforming car is switched to working mode ; When the motion parameters of the unmanned transformable vehicle are obtained When the unmanned transforming car is switched to working mode ; When the motion parameters of the unmanned transformable vehicle are obtained When the unmanned transforming car is switched to working mode ; When the motion parameters of the unmanned transformable vehicle are obtained When the unmanned transforming car is switched to working mode ; Step 5: Establish the lower layer stability control strategy and adjust the working mode or working mode Stability control of the unmanned transformable vehicle under the vehicle; Step 5.1, in working mode Stability control of the unmanned transformable vehicle under the vehicle; Step 5.1.1: Turn the unmanned transformable vehicle into a car-like state. The actual stability factor at the moment and expected stability factor The deviation between and the rate of change of deviation The input is processed by the Fuzzy-PID controller and the output is used to adjust the center of mass slider. Control voltage at the moment ; Step 5.1.2, Substituting into formula (1), we can get the adjustment of the center of mass slider at After the displacement of time, Substitute the displacement at the moment into formula (3) to obtain The unmanned transforming car is in the basic coordinate system at all times The centroid coordinates of ; (3) In formula (3), is the mass of any e-th component among the lifting mechanism, the center of mass adjustment slider, the thighs of the two robotic legs, the calves of the two robotic legs, and the supporting feet of the two robotic legs. m is the total mass of the unmanned transformable vehicle. num is the number of components, and num=8. for The center of mass of the e-th component of the unmanned transformable vehicle at time axis coordinates; Step 5.1.3: Use formula (4) to get the unmanned transformable vehicle in the car state turning state The actual stability factor at the moment ; (4) In formula (4), are the cornering stiffness of the front and rear wheels, yes The distance from the center of mass of the vehicle to the front axle at the moment, and ; yes The distance from the center of mass of the vehicle to the rear axle at the moment, and ; It is the wheelbase of the unmanned transforming car; Step 5.1.4, +1 assigned to After that, return to step 5.1.1 and execute in sequence until the steering state of the unmanned transforming vehicle in the car state changes or the deviation is zero, thereby realizing the unmanned transforming vehicle in the working mode. stability control under Step 5.2: Based on the K-means clustering algorithm, Stability control of the unmanned transformable vehicle under the vehicle; Step 5.2.

1. Establish the stability level of the K-means clustering algorithm ; Step 5.2.2: According to the stability level Determine whether the unmanned transformable car is in a human-like walking state The stability level corresponding to the actual zero moment point position at the moment , and input into the scaling factor fuzzy controller for processing, thus outputting Deviation scaling factor at the moment 、 The time deviation change rate scaling factor 、 The expected displacement expansion factor at time As the control parameters of the basic fuzzy controller; Step 5.2.3: Make the unmanned transformable car walk in a human-like state The expected zero moment point position at time and the actual zero moment point position Deviation and the rate of change of deviation The input is processed into the basic fuzzy controller and the output is used to adjust the centroid slider. Expected displacement at time ; Step 5.2.4, adjust the center of mass slider to Expected displacement at time The input is processed by the ADRC and the output is the center of mass slider of the unmanned vehicle in the human-like walking state. Control voltage at the moment ; Step 5.2.5, Substituting into formula (1), we can get the adjustment of the center of mass slider at After the displacement of time, Substitute the displacement at the moment into formula (7) to calculate the displacement of the unmanned deformable vehicle in the basic coordinate system. Moment Towards the zero moment point and Towards the zero moment point , thus obtaining the unmanned transformable vehicle in a human-like walking state The actual zero moment point position at time ; (7) In formula (7), They are The e-th component of the unmanned transforming car at the moment Towards, Xianghe The acceleration toward the center of mass, is the acceleration due to gravity, They are The center of mass of the e-th component of the unmanned transforming vehicle at the moment is in the basic coordinate system Axis coordinates, Axis coordinates and axis coordinates; Step 5.2.6, Assign to After that, return to step 5.2.2 and execute sequentially until the walking state of the unmanned transformable vehicle in the human-like state changes or the deviation is zero, thereby realizing the unmanned transformable vehicle in the working mode. Stability control under.

2. A stability hierarchical control method for multi-modal motion of an unmanned transformable vehicle according to claim 1, characterized in that: The step 5.2.1 includes: Step 5.2.1.

1. Calculate the deviation sequence between the j actual zero-moment point positions and the expected zero-moment point positions of the unmanned transformable vehicle in the humanoid walking state and deviation change rate series , we obtain the stability clustering dataset of the unmanned deformable vehicle in the human-like walking state; Step 5.2.1.

2. Define the maximum number of iterations , the current number of iterations is R, and R=1 is initialized, and N data points are randomly selected from the stability clustering data set as the initial cluster center points { | },in, Indicates the Rth iteration Initial cluster centers; Indicates the Rth iteration Deviation of cluster centers; Indicates the Rth iteration The deviation change rate of the cluster center points; Step 5.2.1.

3. Define and initialize the variable i = 1; Step 5.2.1.4, calculate the stability of the i-th data point in the clustering data set according to formula (6) ) and the nth cluster center point at the Rth iteration distance , so that the i-th data point ( ) is assigned to the nearest cluster center; (6) In formula (6), represents the bias weight factor, represents the weight factor of the deviation change rate; In step 5.2.1.5, after assigning the value i+1 to i, return to step 5.2.1.4 and repeat the process until i > j. This assigns all data points in the stability clustering dataset to the nearest cluster center, resulting in N clusters at the Rth iteration. Step 5.2.1.

6. Calculate the average value of the deviation and the rate of change of deviation of the data points contained in the nth cluster at the Rth iteration, and use them as the center point of the nth cluster at the R+1th iteration. ;in, Indicates the R+1th iteration Deviation of cluster centers; Indicates the R+1th iteration The deviation change rate of the cluster center points; Step 5.2.1.7: After assigning R+1 to R, return to step 5.2.1.3 and execute sequentially until R > So far, we get N clusters and their cluster centers at the first iteration; The cluster center point at the iteration is used as the stability level of the unmanned transformable vehicle in the walking state .

3. An electronic device comprising a memory and a processor, characterized in that: The memory is used to store a program that supports the processor to execute the stability stratification control method according to claim 1 or 2, and the processor is configured to execute the program stored in the memory.

4. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the stability stratification control method according to claim 1 or 2 are executed.

Images