Complex terrain adaptability control system for rescue robot

Abstract

The invention discloses a complex terrain adaptability control system for a rescue robot, and belongs to the technical field of robots. According to the invention, full-closed-loop control is realized through the instruction layer, the hardware layer, the control layer and the monitoring layer. The control layer takes a posture adjusting module as a core, is linked with a PID (Proportion Integration Differentiation) and an impedance controller, and is combined with forward / reverse kinematics and suspension detection to realize accurate correction of a joint angle; the hardware layer feeds back attitude, joint movement and contact force data in real time by means of multiple sensors; meanwhile, a whole vehicle kinetic equation is established based on a spiral theory and a virtual work principle to deduce a contact force component, and a sliding rate is defined to quantify traction characteristics. According to the invention, through fusion of multi-module cooperation and mechanical modeling, organic unification of attitude control, force control flexibility and traction performance monitoring is systematically realized, and a complete attitude-force-traction cooperative control system is constructed.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and more particularly to a complex terrain adaptive control system for a rescue robot. Background Art

[0002] With the increasing frequency of natural disasters, industrial accidents, and urban emergencies, the ability of rescue robots to operate in complex terrain has become a key research topic in the fields of rescue technology and robotics. The complex and ever-changing post-disaster rescue operations pose severe challenges to the stability, mobility, and safety of rescue robots.

[0003] In existing research on robot control in complex terrain, most control systems utilize multi-actuator control coupling, which complicates the control algorithm and affects the system's responsiveness. Some control systems also focus on positioning all legs to achieve the desired effect without considering inter-leg control coupling, resulting in coupled oscillations in the robot's trunk. Furthermore, traditional control systems struggle to accurately track the force required by each leg as the robot traverses complex terrain.

[0004] Therefore, how to propose a complex terrain adaptive control system for a rescue robot to enhance the adaptability of the rescue robot in complex terrain is a problem that technical personnel in this field urgently need to solve. Summary of the Invention

[0005] In view of this, the present invention provides a complex terrain adaptability control system for a rescue robot to enhance the adaptability of the rescue robot in complex terrain.

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

[0007] A complex terrain adaptive control system for a rescue robot, comprising an instruction layer, a hardware layer, a control layer and a monitoring layer;

[0008] The instruction layer is used to set the desired vehicle body posture, which includes the desired pitch angle α d and the desired roll angle β d ;

[0009] The hardware layer includes IMU posture sensor, joint encoder, drive motor encoder and force sensor located on the wheel leg, which are used to obtain the body posture pitch angle α and roll angle β, joint motor angle information θ i , driving motor torque information and force sensor feedback actual support force i F ; i = 1, 2, 3, 4;

[0010] The control layer is used to generate joint control instructions according to the information in the instruction layer and the hardware layer;

[0011] The monitoring layer is used to output vehicle stability and traction performance indicators in real time based on information from the control layer and the hardware layer.

[0012] Preferably, the control layer is used to generate joint control instructions according to the information in the instruction layer and the hardware layer, including:

[0013] The control layer includes a posture adjustment controller, a support contact controller, a forward kinematics calculation module, an inverse kinematics calculation module and a suspension detection module;

[0014] The forward kinematics calculation module is based on the joint motor angle information θ i Get the coordinate transformation matrix and wheel center position (x wi ,y wi ,z wi );

[0015] The attitude adjustment controller includes an attitude adjustment module and a PID controller based on a BP neural network. The attitude adjustment module uses the pitch angle difference Δα, the roll angle difference Δβ and the wheel center position (x wi ,y wi ,z wi ) as input to calculate the attitude displacement compensation The attitude displacement compensation amount is obtained by the PID controller to obtain the attitude adjustment displacement control amount

[0016] The support contact controller includes an expected support force calculation module and an impedance controller. The expected support force calculation module calculates the expected pitch angle α according to the expected pitch angle α. d , expected roll angle β d and wheel center position (x wi ,y wi ,z wi ), calculate the contact force under the desired vehicle body posture The impedance controller is used to determine the contact force under the desired vehicle body posture. And the force sensor feedback actual support force i F Deviation Δ i F is the input, and the support contact displacement control value is obtained

[0017] Adjust the displacement control amount according to the posture and support contact displacement control Calculate the comprehensive displacement control amount of each wheel

[0018] The inverse kinematics module converts the comprehensive displacement control quantity Converted into reverse joint compensation adjustment Δθ ir ;

[0019] The suspension detection module feeds back the actual support force i according to the force sensor. F Perform suspension detection and output the suspension joint compensation adjustment value Δθ id ;

[0020] According to the reverse joint compensation adjustment amount Δθ ir and the suspended joint compensation adjustment amount Δθ id Generate the joint control instruction, that is, the joint adjustment angle Δθ i , based on the joint adjustment angle Δθ i The joint encoder is controlled.

[0021] Preferably, the monitoring layer includes a stability margin monitoring module and a traction characteristic monitoring module;

[0022] The stability margin monitoring module is based on the wheel center position (x wi ,y wi ,z wi ), pitch angle α and roll angle β to calculate the stability margin of the rescue robot;

[0023] The traction characteristic monitoring module is based on the coordinate transformation matrix The pitch angle α, roll angle β and drive motor torque information are combined with the glide rate to monitor the traction performance.

[0024] Preferably, the attitude adjustment module takes the pitch angle difference Δα, the roll angle difference Δβ and the wheel center position as input to calculate the attitude displacement compensation include:

[0025] Compare the pitch angle α with the desired pitch angle α d Subtract the pitch angle difference Δα and compare the roll angle β with the expected roll angle β. d Subtract and get the roll angle difference Δβ;

[0026] The attitude displacement compensation amount According to the pitch displacement compensation and roll displacement compensation The formula is as follows:

[0027]

[0028]

[0029]

[0030] Where,

[0031] The attitude displacement compensation amount The attitude adjustment displacement control amount is obtained by the PID controller The formula is as follows:

[0032]

[0033] Where K p is the proportionality coefficient, K i is the integration constant, K d is the differential coefficient.

[0034] Preferably, the expected support force calculation module is based on the expected pitch angle α d , expected roll angle β d and wheel center position (x wi ,y wi ,z wi ), calculate the contact force under the desired vehicle body posture include:

[0035] The static force analysis of the rescue robot is:

[0036] make

[0037] We get P = QF d ; then F d =Q + P;

[0038] Where Q + is the pseudo-inverse matrix of Q, and Q + =Q T (QQ T ) -1 ;

[0039] The impedance controller is used to determine the contact force under the desired vehicle body posture. And the force sensor feedback actual support force i F Deviation Δ i F is the input, and the support contact displacement control value is obtained include:

[0040]

[0041]

[0042] Where M, B, K are mass matrix, damping matrix and stiffness matrix respectively; Δ i F(t) is the real-time support force tracking error.

[0043] Preferably, the comprehensive displacement control amount The calculation formula is as follows:

[0044]

[0045] Where k1 is the posture adjustment factor; k2 is the support force adjustment factor.

[0046] Preferably, the joint adjustment angle Δθ i The calculation formula is as follows:

[0047] Δθ i =Δθ ir +Δθ id .

[0048] Preferably, the projections of the rescue robot's center of gravity G and the wheel-ground contact points P1, P2, P3, and P4 on the horizontal plane are Q G ,Q1,Q2,Q3,Q4,Q G The distances to the sides of the quadrilateral Q1Q2Q3Q4 are d1, d2, d3, and d4 respectively;

[0049] The stability margin S of the rescue robot in the quadruped support state is:

[0050] S=min(d1,d2,d3,d4).

[0051] Preferably, the calculation process of the glide rate includes:

[0052] Establish wheel-ground contact point P i The local coordinate system P i -T i V i N i , contact force vector f at the contact point i The three coordinate axes of the local coordinate system are decomposed into:

[0053] f i =(f it ,f iv ,f in );

[0054] f it is the tangential force component of the i-th wheel; f iv is the axle direction component of the i-th wheel; f in is the normal component of the i-th wheel perpendicular to the contact plane;

[0055] Define the glide rate S i The traction characteristics are quantified by the ratio of the tangential force to the normal force. The calculation formula is as follows:

[0056]

[0057] Through the above technical solutions, it can be seen that compared with the existing technology, the present invention discloses a complex terrain adaptability control system for a rescue robot, which realizes full closed-loop control through the instruction layer, hardware layer, control layer, and monitoring layer. The control layer takes the posture adjustment module as the core, links the PID and impedance controllers, and combines forward / inverse kinematics with suspension detection to realize precise correction of joint angles; the hardware layer relies on multiple sensors to feedback posture, joint motion and contact force data in real time; at the same time, based on the spiral theory and the principle of virtual work, the whole vehicle dynamics equation is established to derive the contact force component, and the sliding rate is defined to quantify the traction characteristics. The present invention systematically realizes the organic unity of posture control, force control compliance and traction performance monitoring through the integration of multi-module collaboration and mechanical modeling, and constructs a complete posture-force-traction collaborative control system. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are merely embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying any creative work.

[0059] Figure 1 A diagram of the system structure provided by the present invention;

[0060] Figure 2 This is a schematic diagram of the coordinate system of the rescue robot;

[0061] FIG3( a ) is a diagram showing the effect of posture adjustment using a conventional method, and FIG3( b ) is a diagram showing the effect of posture adjustment using the control method of the present invention;

[0062] Figure 4 This is a schematic diagram of the rescue robot driving on complex terrain;

[0063] Figure 5 It is a schematic diagram of PID controller based on BP neural network;

[0064] Figure 6 This is the BP neural network structure diagram. DETAILED DESCRIPTION

[0065] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0066] The embodiment of the present invention discloses a complex terrain adaptive control system for a rescue robot, referring to Figure 1 , including instruction layer, hardware layer, control layer and monitoring layer;

[0067] The instruction layer is used to set the desired vehicle posture, which includes the desired pitch angle α d and the desired roll angle β d .

[0068] The hardware layer includes IMU posture sensor, joint encoder, drive motor encoder and force sensor located on the wheel leg, which are used to obtain the body posture pitch angle α and roll angle β, joint motor angle information θ i , driving motor torque information and force sensor feedback actual support force i F ; i = 1, 2, 3, 4.

[0069] The control layer is used to generate joint control instructions based on the information in the instruction layer and hardware layer.

[0070] Specifically, the control layer includes a posture adjustment controller, a support contact controller, a forward kinematics calculation module, an inverse kinematics calculation module and a suspension detection module.

[0071] The forward kinematics calculation module is based on the joint motor angle information θ i Get the coordinate transformation matrix and wheel center position (x wi ,y wi ,z wi ).

[0072] Figure 2 The coordinate system of the rescue robot in this embodiment is established by taking the geometric center of the rescue robot body as the origin O0 and establishing the body coordinate system O0-XYZ, with the front of the body as the positive X direction, the horizontal left as the positive Y direction, and the upward Z direction.

[0073] The left front wheel leg is recorded as leg1, the right front wheel leg is recorded as leg2, the left rear wheel leg is recorded as leg3, and the right rear wheel leg is recorded as leg4.

[0074] Establish each joint coordinate system at the connection between the wheel leg and the vehicle body: O1-X1Y1Z1 is the left front joint coordinate system; O2-X2Y2Z2 is the right front joint coordinate system; O3-X3Y3Z3 is the left rear joint coordinate system; O4-X4Y4Z4 is the right rear joint coordinate system.

[0075] The three-axis directions of each joint coordinate system are consistent with the three-axis directions of the vehicle body coordinate system.

[0076] Establish the wheel coordinate system at the wheel center W1, W2, W3, and W4: w1 -X w1 Y w1 Z w1is the left front wheel coordinate system; O w2 -X w2 Y w2 Z w2 is the right front wheel coordinate system; O w3 -X w3 Y w3 Z w3 is the left rear wheel coordinate system; O w4 -X w4 Y w4 Z w4 is the right rear wheel coordinate system.

[0077] The three-axis directions of each wheel coordinate system are consistent with the three-axis directions of the vehicle body coordinate system.

[0078] Define the three band direction angles β, α, and γ shown in Figure 3, and use M (β,α,γ) The rotation matrix composed of the combination of roll, pitch and yaw is used to represent the posture of the rescue robot body, that is:

[0079]

[0080] Construct the single-leg forward kinematics model of the rescue robot in this embodiment:

[0081] Establish the homogeneous transformation matrix of the left front, right front, left rear and right rear joint coordinate systems {O1}, {O2}, {O3}, {O4} relative to the body coordinate system {O0}:

[0082]

[0083] L is the distance between the front and rear joints; B is the distance between the left and right joints; θ1 is the rotation angle of the left front wheel leg relative to the body; θ2 is the rotation angle of the right front wheel leg relative to the body; θ3 is the rotation angle of the left rear wheel leg relative to the body; θ4 is the rotation angle of the right rear wheel leg relative to the body.

[0084] The positive and negative angles of this embodiment are defined as follows: When the wheel leg rotates clockwise around the Y axis, θ i Take positive values ​​for rotation counterclockwise and negative values ​​for rotation counterclockwise.

[0085] Due to mechanical structure limitations, to avoid mechanical interference between the wheel legs and the rescue robot body platform, add joint angle limits: θ i ∈(θ min ,θ max ).

[0086] Remember L leg is the wheel leg length, and the wheel coordinate system is transformed relative to the joint coordinate system:

[0087]

[0088] The wheel coordinate system O can be obtained wi -X wi Y wi Z wi Homogeneous transformation matrix relative to the vehicle body coordinate system {O0}:

[0089]

[0090]

[0091] According to the coordinate transformation principle, we have:

[0092]

[0093] r0 is the coordinate of the reference point in the vehicle body coordinate system {O0}; r wi The reference point is in the wheel coordinate system {O wi The coordinates below.

[0094] The attitude adjustment controller includes an attitude adjustment module and a PID controller. The attitude adjustment module uses the pitch angle difference Δα=α-α d , Roll angle difference Δβ=β-β d and wheel center position (x wi ,y wi ,z wi ) as input to calculate the attitude displacement compensation Posture displacement compensation The attitude adjustment displacement control quantity is obtained through the PID controller

[0095] When the rescue robot encounters sudden changes in the terrain and pitches, in order to keep the body in the desired posture (for example, horizontal), it needs to rotate around the center of the body by an angle Δα, and the rear wheel legs 3 / 4 will be raised and suspended in the air, while the front wheel legs 1 / 2 will drop and penetrate the terrain, as shown in state 2 in Figure 3(a). To avoid this phenomenon, the rescue robot is required to raise the front wheel legs 1 / 2 Rear wheel leg 3 / 4 down Reach state 3 in Figure 3(b).

[0096] The pitch displacement compensation of the wheel in the vertical direction caused by the pitch angle The expression is as follows:

[0097]

[0098] Similarly, the Δβ generated by the rolling of the vehicle body due to the undulating terrain can also be converted into the rolling displacement compensation of the wheel in the vertical direction

[0099]

[0100] The attitude adjustment controller compensates for pitch and roll caused by terrain fluctuations for:

[0101]

[0102] In this embodiment, the control system's posture adjustment controller consists of a posture adjustment module and a PID controller based on a BP neural network. The posture adjustment module calculates displacement compensation for vehicle posture adjustment. The BP neural network trains and adjusts PID parameters, with the final output corresponding to the PID parameters under optimal control.

[0103] In order to obtain more effective control effect, the attitude adjustment controller adopts PID controller based on BP neural network. BP neural network PID control system is as follows Figure 5 As shown in the figure, the discrete form formula of the incremental PID control algorithm is:

[0104] u (k) =u (k-1) +K p (e (k) -e (k-1) )+K i e (k) +K d (e (k) -2e (k-1) +e (k-2) );

[0105] Where K p , K i , K d are proportional, integral, and differential coefficients respectively; e (k) is the difference between the expected output and the actual output at the current time; u (k) is the control quantity at the current sampling moment.

[0106] Specifically, the controller adopts a BP neural network with a structure of 4-5-3, as shown in the following figure: Figure 6 As shown. The input of BP neural network is:

[0107] I Ai =x i (i=1,2,3,4);

[0108] The input of input layer A satisfies:

[0109]

[0110] The input and output of hidden layer B are:

[0111]

[0112] Where: wBji is the weighted coefficient of the hidden layer B; f(x) is the hyperbolic tangent function, that is

[0113] The input and output of the output layer C are:

[0114]

[0115] Where: w Cpj is the weight coefficient of the output layer C; since K p , K i , K d It cannot take negative values, so the activation function of the output layer C takes the non-negative Sigmoid function, that is,

[0116] The output of the output layer C satisfies:

[0117]

[0118] Take performance indicators:

[0119]

[0120] The network weight coefficients are modified according to the gradient descent method, that is, the negative gradient direction of the weight coefficients is searched and adjusted according to E(k), and an inertia term is added to enable a quick search to the global minimum. Take the output layer C as an example:

[0121]

[0122] Where: ρ is the learning rate; γ is the inertia coefficient;

[0123] The vehicle body tilt displacement control quantity obtained by the PID controller based on the BP neural network is:

[0124]

[0125] The support contact controller includes an expected support force calculation module and an impedance controller. The expected support force calculation module calculates the expected pitch angle α according to the expected pitch angle α. d , expected roll angle β d and wheel center position (x wi ,y wi ,z wi ), calculate the contact force under the desired vehicle body posture The impedance controller is used to calculate the contact force under the desired vehicle body posture. And the force sensor feedback actual support force i F Deviation Δ i F is the input, and the support contact displacement control value is obtained

[0126] The expected support force calculation module is based on the expected pitch angle α d , expected roll angle β d and wheel center position (x wi ,y wi ,z wi ), calculate the contact force under the desired vehicle body posture include:

[0127] The static force analysis of the rescue robot is:

[0128] make

[0129] We get P = QF d ; then F d =Q + P;

[0130] Where Q + is the pseudo-inverse matrix of Q, and Q + =Q T (QQ T ) -1 ;

[0131] The impedance controller is used to calculate the contact force under the desired vehicle body posture. And the force sensor feedback actual support force i F Deviation Δ i F is the input, and the support contact displacement control value is obtained include:

[0132] Contact force under desired vehicle posture And the force sensor feedback actual support force i F Deviation Δ i F is the force tracking error in the support contact controller, and its value is:

[0133]

[0134]

[0135] Where M, B, K are mass matrix, damping matrix and stiffness matrix respectively; Δ i F(t) is the real-time support force tracking error.

[0136] Adjust the displacement control amount according to the posture and support contact displacement control Calculate the comprehensive displacement control amount of each wheel The formula is as follows:

[0137]

[0138] k1 is the influencing factor for attitude adjustment; k2 is the influencing factor for support force adjustment. Both influencing factors should be adjusted appropriately according to the geometric complexity of the terrain and the wheel-ground contact characteristics.

[0139] The inverse kinematics module integrates the displacement control Converted into reverse joint compensation adjustment Δθ ir .

[0140] The single-leg inverse kinematics model of the rescue robot in this embodiment is:

[0141] When the wheel center position is known, the algebraic solution method can be used to obtain the following values ​​for the joint angles:

[0142]

[0143] The suspension detection module feeds back the actual support force i according to the force sensor F Perform suspension detection and output the suspension joint compensation adjustment value Δθ id .

[0144] According to the inverse joint compensation adjustment amount Δθ ir and the suspended joint compensation adjustment Δθ id Generate joint control instructions, that is, the joint adjusts the angle Δθ i , based on the joint adjustment angle Δθ i Control the joint encoders.

[0145] Joint adjustment angle Δθ i Satisfies the following formula:

[0146] Δθ i =Δθ ir +Δθ id .

[0147] When the actual support force information i obtained by the force sensor F Input to the suspension detection module. When the wheel is detected to be suspended, the joint motor encoder adjusts the rotation angle Δθ according to the joint obtained by the control system. i Adjust the overhanging wheel leg downward until full wheel contact is achieved.

[0148] When all wheels are in contact with the ground, the body posture adjustment controller works together with the support contact controller.

[0149] The monitoring layer is used to output the vehicle stability and traction performance indicators in real time based on the information from the control layer and hardware layer, including the stability margin monitoring module and the traction characteristics monitoring module.

[0150] In this embodiment, the control system uses a stability margin evaluation standard to monitor the stability of the rescue robot vehicle under complex terrain. Figure 4Schematic diagram of the rescue robot driving on complex terrain.

[0151] The stability margin monitoring module is based on the wheel center position (x wi ,y wi ,z wi ), pitch angle α and roll angle β are used to calculate the stability margin of the rescue robot.

[0152] When the rescue robot is traveling on complex terrain, under the action of the terrain adaptability control system, the rescue robot is in a four-legged support state, but the wheel-ground contact points P1, P2, P3, and P4 of the four wheels are often not in the same plane.

[0153] In this embodiment, the projections of the rescue robot's center of gravity G and the wheel-ground contact points P1, P2, P3, and P4 on the horizontal plane are Q G ,Q1,Q2,Q3,Q4,Q G The distances to the sides of the quadrilateral Q1Q2Q3Q4 are d1, d2, d3, and d4 respectively;

[0154] The stability margin S of the rescue robot in the quadruped support state is:

[0155] S=min(d1,d2,d3,d4).

[0156] The traction characteristics monitoring module is based on the coordinate transformation matrix The pitch angle α, roll angle β and drive motor torque information are combined with the glide rate to monitor the traction performance.

[0157] Figure 4 In the process, the wheel-ground contact point P is also established. i The local coordinate system P i -T i V i N i , contact force vector f at the contact point i The three coordinate axes of the local coordinate system are decomposed into:

[0158] f i =(f it ,f iv ,f in );

[0159] f it is the tangential force component of the i-th wheel; f iv is the axle direction component of the i-th wheel; f in is the normal component of the i-th wheel perpendicular to the contact plane.

[0160] This embodiment obtains the vehicle dynamics equation based on the screw theory and the principle of virtual work:

[0161] Kf=W;

[0162] K is a 6×12 position correlation matrix (obtained from the coordinate transformation matrix); f is a 12×1 contact force vector matrix (where f it Corrected by the drive motor torque information); W is the load rotation of 6×1 (when the rescue robot passes through complex terrain at low speed, it depends on the mass of the vehicle).

[0163] The contact force components are obtained according to the vehicle dynamics equations.

[0164] f it ,f iv The size of directly reflects the traction characteristics of the rescue robot, so the glide rate S is defined in this embodiment as i The traction characteristics are quantified by the ratio of the tangential force to the normal force, S i Calculated as:

[0165]

[0166] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. Reference can be made to the common and similar parts between the various embodiments. For the devices disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple, and the relevant parts can be referred to the method description.

[0167] The above description of the disclosed embodiments is intended to enable one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be readily apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention is not limited to the embodiments shown herein but is intended to conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A complex terrain adaptive control system for a rescue robot, characterized in that: Includes instruction layer, hardware layer, control layer and monitoring layer; The instruction layer is used to set the desired vehicle body posture, which includes the desired pitch angle α d and the desired roll angle β d ; The hardware layer includes IMU posture sensor, joint encoder, drive motor encoder and force sensor located on the wheel leg, which are used to obtain the body posture pitch angle α and roll angle β, joint motor angle information θ i , driving motor torque information and force sensor feedback actual support force i F ; i = 1, 2, 3, 4; The control layer is used to generate joint control instructions according to the information in the instruction layer and the hardware layer; The monitoring layer is used to output vehicle stability and traction performance indicators in real time based on information from the control layer and the hardware layer.

2. A complex terrain adaptive control system for a rescue robot according to claim 1, characterized in that: The control layer is used to generate joint control instructions according to the information in the instruction layer and the hardware layer, including: The control layer includes a posture adjustment controller, a support contact controller, a forward kinematics calculation module, an inverse kinematics calculation module and a suspension detection module; The forward kinematics calculation module is based on the joint motor angle information θ i Get the coordinate transformation matrix and wheel center position (x wi ,y wi ,z wi ); The attitude adjustment controller includes an attitude adjustment module and a PID controller based on a BP neural network. The attitude adjustment module uses the pitch angle difference Δα, the roll angle difference Δβ and the wheel center position (x wi ,y wi ,z wi ) as input to calculate the attitude displacement compensation The attitude displacement compensation amount is obtained by the PID controller to obtain the attitude adjustment displacement control amount The support contact controller includes an expected support force calculation module and an impedance controller. The expected support force calculation module calculates the expected pitch angle α according to the expected pitch angle α. d , expected roll angle β d and wheel center position (x wi ,y wi ,z wi ), calculate the contact force under the desired vehicle body posture The impedance controller is used to determine the contact force under the desired vehicle body posture. And the force sensor feedback actual support force i F Deviation Δ i F is the input, and the support contact displacement control value is obtained Adjust the displacement control amount according to the posture and support contact displacement control Calculate the comprehensive displacement control amount of each wheel The inverse kinematics module converts the comprehensive displacement control quantity Converted into reverse joint compensation adjustment Δθ ir ; The suspension detection module feeds back the actual support force i according to the force sensor. F Perform suspension detection and output the suspension joint compensation adjustment value Δθ id ; According to the reverse joint compensation adjustment amount Δθ ir and the suspended joint compensation adjustment amount Δθ id Generate the joint control instruction, that is, the joint adjustment angle Δθ i , based on the joint adjustment angle Δθ i The joint encoder is controlled.

3. A complex terrain adaptive control system for a rescue robot according to claim 2, characterized in that: The monitoring layer includes a stability margin monitoring module and a traction characteristics monitoring module; The stability margin monitoring module is based on the wheel center position (x wi ,y wi ,z wi ), pitch angle α and roll angle β to calculate the stability margin of the rescue robot; The traction characteristic monitoring module is based on the coordinate transformation matrix The pitch angle α, roll angle β and drive motor torque information are combined with the glide rate to monitor the traction performance.

4. The complex terrain adaptive control system of a rescue robot according to claim 2, characterized in that: The attitude adjustment module uses the pitch angle difference Δα, roll angle difference Δβ and wheel center position as input to calculate the attitude displacement compensation include: Compare the pitch angle α with the desired pitch angle α d Subtract the pitch angle difference Δα and compare the roll angle β with the expected roll angle β. d Subtract and get the roll angle difference Δβ; The attitude displacement compensation amount According to the pitch displacement compensation and roll displacement compensation The formula is as follows: Where, The attitude displacement compensation amount The attitude adjustment displacement control amount is obtained by the PID controller The formula is as follows: Where K p is the proportionality coefficient, K i is the integration constant, K d is the differential coefficient.

5. The complex terrain adaptive control system of a rescue robot according to claim 2, characterized in that: The expected support force calculation module is based on the expected pitch angle α d , expected roll angle β d and wheel center position (x wi ,y wi ,z wi ), calculate the contact force under the desired vehicle body posture include: The static force analysis of the rescue robot is: make We get P = QF d ; then F d =Q + P; Where Q + is the pseudo-inverse matrix of Q, and Q + =Q T (QQ T ) -1 ; The impedance controller is used to determine the contact force under the desired vehicle body posture. And the force sensor feedback actual support force i F Deviation Δ i F is the input, and the support contact displacement control value is obtained include: Where M, B, K are mass matrix, damping matrix and stiffness matrix respectively; Δ i F(t) is the real-time support force tracking error.

6. The complex terrain adaptive control system of a rescue robot according to claim 2, characterized in that: The comprehensive displacement control amount The calculation formula is as follows: Where k1 is the posture adjustment factor; k2 is the support force adjustment factor.

7. The complex terrain adaptive control system of a rescue robot according to claim 2, characterized in that: The joint adjusts the rotation angle Δθ i The calculation formula is as follows: Dth i =Δθ ir +Δθ id 。 8. The complex terrain adaptive control system of a rescue robot according to claim 3, characterized in that: The projections of the rescue robot's center of gravity G and the wheel-ground contact points P1, P2, P3, and P4 on the horizontal plane are Q G ,Q1,Q2,Q3,Q4,Q G The distances to the sides of the quadrilateral Q1Q2Q3Q4 are d1, d2, d3, and d4 respectively; The stability margin S of the rescue robot in the quadruped support state is: S=min(d1,d2,d3,d4).

9. The complex terrain adaptive control system of a rescue robot according to claim 3, characterized in that: The calculation process of the glide rate includes: Establish wheel-ground contact point P i The local coordinate system P i -T i V i N i , contact force vector f at the contact point i The three coordinate axes of the local coordinate system are decomposed into: f i =(f it ,f iv ,f in ); f it is the tangential force component of the i-th wheel; f iv is the axle direction component of the i-th wheel; f in is the normal component of the i-th wheel perpendicular to the contact plane; Define the glide rate S i The traction characteristics are quantified by the ratio of the tangential force to the normal force. The calculation formula is as follows:

Images