Multi-joint coordinated control method of self-reconfigurable star wheel

Abstract

The application provides a multi-joint coordinated control method of a self-reconfigurable star car, and relates to the technical field of star cars, and the method comprises the following steps: acquiring vehicle body state data, including the rotation angle of a driving rocker arm, the wheel base reduction caused by the rotation of the driving rocker arm, the angular velocity of the driving rocker arm and the rolling angular velocity of the wheel; establishing a multi-joint motion constraint model according to the vehicle body state data, including a vehicle body centroid position constraint model, a multi-joint action coordination constraint model and a vehicle body differential steering motion stability constraint model; and performing multi-joint motion control on the self-reconfigurable star car according to the multi-joint motion constraint model. By establishing the multi-joint motion constraint model on the three levels of the vehicle body centroid position, the multi-joint action coordination and the vehicle body differential steering motion stability, the motion coordinated control of the multi-joints in the vehicle body during the reconfiguration process is realized, so that the mechanical mode conversion function and the multi-motion mode switching function of the self-reconfigurable wheeled mobile star car are realized.

Description

Multi-joint coordinated control method for self-reconfigurable planetary rovers Technical Field

[0001] This invention relates to the field of planetary rover technology, and in particular to a multi-joint coordinated control method for a self-reconfigurable planetary rover. Background Technology

[0002] As planetary exploration missions advance, planetary rovers, as effective carrier platforms for scientific instruments, play a crucial role in planetary exploration projects. Among them, wheeled mobile robots are widely used as the main body of planetary rovers due to their advantages of easy control, flexible movement, modularity, and strong expandability. In the process of self-reconfigurable planetary rovers performing exploration missions, multi-joint motion control is the key to improving the exploration capabilities of planetary rovers in deformable and rough terrain, especially in planetary environments.

[0003] Current research on reconfigurable planetary rovers focuses on the diversification of planetary rover configurations, neglecting the definition of multi-joint motion coordination strategies in control mode switching strategies. This fails to prevent coordination failures between multiple joints within the vehicle body during the reconfiguration process, resulting in the planetary rover lacking the complex operational functions of multi-joint coordinated mechanical mode conversion and motion mode switching control, thus affecting its practical application value. Summary of the Invention

[0004] The problem addressed by this invention is how to coordinate and control the multi-joint motion of a self-reconfigurable planetary rover, thereby improving the practical application value of the self-reconfigurable planetary rover.

[0005] To address the above problems, this invention provides a multi-joint coordinated control method for a self-reconfigurable planetary rover, comprising the following steps:

[0006] Acquire vehicle body status data, which includes the rotation angle of the main rocker arm, the reduction in wheelbase caused by the rotation of the main rocker arm, the angular velocity of the main rocker arm, and the rolling angular velocity of the wheel;

[0007] Based on the vehicle body state data, a multi-joint motion constraint model of the self-reconfigurable planetary rover is established. The multi-joint motion constraint model includes a vehicle body center of mass position constraint model, a multi-joint motion coordination constraint model, and a vehicle body differential steering motion stability constraint model.

[0008] The self-reconfigurable planetary rover is subjected to multi-joint motion control based on the multi-joint motion constraint model.

[0009] Optionally, establishing the multi-joint motion constraint model of the self-reconfigurable planetary rover based on the vehicle body state data includes:

[0010] A vehicle center of gravity position constraint model is established based on the rotation angle of the active rocker arm and the reduction in wheelbase caused by the rotation of the active rocker arm. A multi-joint motion coordination constraint model is established based on the reduction in wheelbase caused by the rotation of the active rocker arm. A vehicle differential steering motion stability constraint model is established based on the angular velocity of the active rocker arm and the rolling angular velocity of the wheel.

[0011] Optionally, the vehicle body center of gravity position constraint model includes a first constraint, a second constraint, and a third constraint, wherein the first constraint includes:

[0012] The reduction in wheelbase caused by the rotation of the active rocker arm is less than or equal to a first preset threshold, and the first constraint is used to avoid wheel collisions.

[0013] The second constraint includes:

[0014] The roll angle of the vehicle body caused by the rotation of the active rocker arm is less than or equal to the second preset threshold. The second constraint is used to prevent the vehicle body from generating abnormal roll angles.

[0015] The third constraint includes:

[0016] The vehicle body steering angle caused by the rotation of the active rocker arm is less than or equal to a third preset threshold. The third constraint is used to avoid excessive deviation in the vehicle body steering angle.

[0017] Optionally, the multi-joint motion coordination constraint model includes:

[0018] The forward speed difference of the wheels is equal to a first preset multiple of the angular velocity of the active rocker arm, and the multi-joint coordinated motion mode constraint model is used to avoid vehicle rollover.

[0019] Optionally, the vehicle body differential steering motion stability constraint model includes a fourth constraint, a fifth constraint, a sixth constraint, and a seventh constraint, wherein the fourth constraint includes:

[0020] The angular velocity of the active rocker arm is less than or equal to a second preset multiple of the initial velocity of the vehicle body before the suspension attitude adjustment, and the fourth constraint is used to prevent the wheel merging speed from zero.

[0021] The fifth constraint includes:

[0022] The forward velocity deviation of the self-reconfigurable planetary rover is less than the fourth preset threshold, and the fifth constraint is used to avoid the forward velocity of the vehicle body becoming unbalanced.

[0023] The sixth constraint includes:

[0024] The deviation of the vehicle body's steering angular velocity of the self-reconfigurable planetary rover is less than the fifth preset threshold, and the sixth constraint is used to avoid the vehicle body's steering angular velocity from becoming unbalanced.

[0025] The seventh constraint includes:

[0026] The roll rate of the self-reconfigurable planetary rover is less than the sixth preset threshold, and the seventh constraint is used to avoid roll rate misalignment.

[0027] Optionally, it also includes a quantity constraint model for wheel-to-ground contact, the quantity constraint model for wheel-to-ground contact including an eighth constraint, a ninth constraint, and a tenth constraint, wherein the eighth constraint includes:

[0028] The number of wheels simultaneously off the ground is less than or equal to the seventh preset threshold;

[0029] The ninth constraint includes:

[0030] The number of wheels on one side that are off the ground is less than or equal to the eighth preset threshold.

[0031] The tenth constraint includes:

[0032] At least one of the front and rear wheels is in contact with the ground.

[0033] Optionally, the multi-joint coordinated control method for the self-reconfigurable planetary rover also includes:

[0034] A kinematic model of the vehicle body is established based on the variable position of the wheel-ground contact point and the three-dimensional linear velocity of the vehicle body, and a dynamic model of the vehicle body is established based on the vehicle body properties.

[0035] Optionally, the vehicle kinematic model includes a dynamic movement kinematic model considering the wheel-ground contact point and a differential steering kinematic model with vertical degrees of freedom.

[0036] Optionally, the process of establishing the dynamic kinematic model considering the wheel-ground contact point and the differential steering kinematic model with vertical degrees of freedom includes:

[0037] A dynamic kinematic model considering the wheel-to-ground contact point is obtained based on the offset wheel matrix of the vehicle coordinate system, the angular velocity of the suspension articulation joint, the linear velocity of the wheel-to-ground contact point relative to the wheel center, the angular velocity of the wheel support arm articulation joint, and the three-dimensional linear velocity of the vehicle body.

[0038] Based on the angular velocity of the active rocker arm and the vehicle body attitude angle, a differential steering kinematic model with vertical degrees of freedom is obtained.

[0039] Optionally, establishing the vehicle dynamics model based on vehicle body attributes includes:

[0040] The vehicle dynamics model is established based on the joint angles, the number of joints, and the driving torque of the active rocker arm.

[0041] Compared to existing technologies, the multi-joint coordinated control method for the self-reconfigurable planetary vehicle described in this invention acquires vehicle body state data and establishes multi-joint motion constraint models on three levels: vehicle body center of mass position, multi-joint motion coordination, and vehicle body differential steering motion stability. This enables coordinated motion control of multiple joints within the vehicle body during the reconfiguration process, ensuring vehicle body motion safety. Consequently, it realizes the mechanical mode conversion function and multi-motion mode switching function of the self-reconfigurable wheeled mobile planetary vehicle, improving the practical application value of the self-reconfigurable wheeled mobile planetary vehicle. Attached Figure Description

[0042] Figure 1 is a control flowchart of the multi-joint coordinated control method of the self-reconfigurable planetary rover according to an embodiment of the present invention.

[0043] Figure 2 is a schematic diagram of the wheel lifting off the ground according to an embodiment of the present invention. Detailed Implementation

[0044] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Although some embodiments of the present invention are shown in the drawings, it should be understood that the present invention can be implemented in various forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of the present invention. It should be understood that the accompanying drawings and embodiments of the present invention are for illustrative purposes only and are not intended to limit the scope of protection of the present invention.

[0045] It should be understood that the various steps described in the method embodiments of the present invention may be performed in different orders and / or in parallel. Furthermore, the method embodiments may include additional steps and / or omit the steps shown. The scope of the present invention is not limited in this respect.

[0046] The term "comprising" and its variations as used herein are open-ended inclusions, meaning "including but not limited to". The term "based on" means "at least partially based on". The term "one embodiment" means "at least one embodiment"; the term "another embodiment" means "at least one additional embodiment"; the term "some embodiments" means "at least some embodiments"; the term "optionally" means "optional embodiments". Definitions of other terms will be given in the description below.

[0047] It should be noted that the terms "a" and "a plurality of" used in this invention are illustrative rather than restrictive. Those skilled in the art should understand that, unless otherwise expressly indicated in the context, they should be understood as "one or more".

[0048] An embodiment of the present invention provides a multi-joint coordinated control method for a self-reconfigurable planetary rover, as shown in Figure 1, including:

[0049] Step S100: Obtain vehicle body status data, which includes the rotation angle of the main rocker arm, the reduction in wheelbase caused by the rotation of the main rocker arm, the angular velocity of the main rocker arm, and the rolling angular velocity of the wheel.

[0050] Step S200: Establish a multi-joint motion constraint model for the self-reconfigurable planetary rover based on the vehicle body state data. The multi-joint motion constraint model includes a vehicle body center of mass position constraint model, a multi-joint motion coordination constraint model, and a vehicle body differential steering motion stability constraint model.

[0051] Step S300: Perform multi-joint motion control on the self-reconfigurable planetary rover according to the multi-joint motion constraint model.

[0052] Specifically, the self-reconfiguration capability of the wheeled mobile planetary vehicle can be achieved through the coordinated sequential action of the rocker arm joints or through connection / disconnection. By acquiring vehicle body state data, rotational, linear displacement, and contact constraints are established between the rocker arm bogies and between the rocker arm and the wheels to ensure the stable attitude of the vehicle body and safe contact with the terrain.

[0053] Based on the vehicle's status data, a constraint system is established covering three levels: center of gravity position, multi-joint motion coordination, and vehicle differential steering motion stability. After the self-reconfigurable wheeled mobile planetary vehicle switches forms, a new motion plan is realized based on the actual motion state between the joints. According to certain control objectives, the relative speed and position of multiple joints are coordinated and changed, such as the stable posture of the vehicle body and the limited range of movement of the center of gravity position, thereby steadily realizing the reconfiguration.

[0054] In this embodiment, by establishing a multi-joint motion constraint model of the planetary rover based on the vehicle body state data, the planetary rover is equipped with the control function of internal multi-joint movements and the mechanical mode conversion function, thereby realizing the coordinated motion control of multiple joints inside the vehicle body.

[0055] Preferably, establishing a multi-joint motion constraint model for the self-reconfigurable planetary rover based on the vehicle body state data includes:

[0056] A vehicle center of gravity position constraint model is established based on the rotation angle of the active rocker arm and the reduction in wheelbase caused by the rotation of the active rocker arm; a multi-joint motion coordination constraint model is established based on the reduction in wheelbase caused by the rotation of the active rocker arm; and a vehicle differential steering motion stability constraint model is established based on the angular velocity of the active rocker arm and the rolling angular velocity of the wheel, including:

[0057] Specifically, multi-joint motion data is acquired, and a mapping between different data classes and vehicle motion states is established. A vehicle center of gravity position constraint model is established based on the mapping between the active rocker arm rotation angle and the reduction in wheelbase caused by the active rocker arm rotation and the vehicle motion state. A multi-joint motion coordination constraint model is established based on the mapping between the reduction in wheelbase caused by the active rocker arm rotation and the active rocker arm rollover angle. A vehicle differential steering motion stability constraint model is established based on the mapping between the angular velocity of the active rocker arm and the rolling angular velocity of the wheel and the vehicle stability.

[0058] In this embodiment, by establishing a vehicle body center of mass position constraint model, a multi-joint motion coordination constraint model, and a vehicle body differential steering motion stability constraint model, the constraint control of the multi-joints can coordinate the motion speed and position of the multi-joints according to the goals such as vehicle body posture stability and center of mass position limitation range, thereby realizing the motion interaction between the internal structures of the robot and the interaction with the terrain.

[0059] Preferably, the vehicle body center of gravity position constraint model includes a first constraint, a second constraint, and a third constraint, wherein the first constraint includes:

[0060] The reduction in wheelbase caused by the active rocker arm rotation is less than or equal to a first preset threshold, and the first constraint is used to avoid wheel collisions.

[0061] The second constraint includes:

[0062] The roll angle of the vehicle body caused by the rotation of the active rocker arm is less than or equal to the second preset threshold. The second constraint is used to prevent the vehicle body from generating abnormal roll angles.

[0063] The third constraint includes:

[0064] The vehicle body steering angle caused by the rotation of the active rocker arm is less than or equal to the third preset threshold. The third constraint is used to avoid excessive deviation in the vehicle body steering angle.

[0065] Specifically, the active suspension generates additional motion during the differential steering drive of the vehicle body. This additional motion will affect the position of the vehicle body's center of gravity and the vehicle body's speed. In order to prevent the rotation angle of the two active rocker arms from affecting the motion safety of the planetary rover, the range of rocker arm suspension motion is limited to ensure the steady state of the vehicle body's center of gravity position. The roll angle and steering angle of the vehicle body caused by the rotation of the active rocker arms are obtained based on the rotation angle of the two active rocker arms. The steady-state constraint of the center of gravity position is established by setting thresholds for the front and center wheel spacing, the roll angle, and the steering angle.

[0066] For example, in one embodiment, the first constraint includes:

[0067] D s1 =d r11 (1-coss θ s11 )+d r12 (cos o θ o10 -cos s θ s12 )≤d wt -2r,

[0068] D s2 =d r21 (1-cos s θ s21 )+d r22 (cos o θ o20 -cos s θ s22 )≤d wt -2r,

[0069] Among them, D S1 D represents the decrease in the wheelbase of the left front and center wheels. S2 This indicates the decrease in the wheelbase of the right front and center wheels, d. r11 dr 21 d represents the equivalent length of the front half of the main rocker arms on the left and right sides of the vehicle body, respectively. r12 dr 22 These represent the equivalent lengths of the rear half of the main rocker arms on the left and right sides of the vehicle body, respectively. s θ s12 , s θ s22 d represents the rotation angle of the rear half of the left and right active rocker arms, respectively. wt This indicates the lateral distance between the centerlines of the two wheels, where r represents the wheel radius. o θ o10， o θ o20 This indicates the angle between the initial position of the active suspension secondary rocker arm and the horizontal direction. s θ s11 , s θ s21 These represent the rotation angles of the front half of the left and right active rocker arms, respectively.

[0070] The second constraint includes:

[0071]

[0072] in, Indicates the vehicle roll angle threshold;

[0073] The third constraint includes:

[0074]

[0075] in, This indicates the threshold for deviation of the vehicle's steering angle.

[0076] In addition, one embodiment of the present invention provides a metric for measuring the steady-state margin of the center of mass position, used to verify the comprehensive feasibility of constraining the rotation angles of the two active rocker arms, including:

[0077]

[0078] Where, γ p C represents the steady-state margin of the center of mass position. pi ( s θ s12 , s θ s22 ) represents the constraint equation. s ω s12 , s ω s22 Indicates the angular velocity of the active rocker arm. s θ s12 , s θ s22 Indicates the rotation angle of the active rocker arm. o θ o10 , o θ o20 d represents the angle between the initial position of the active suspension secondary rocker arm and the horizontal direction. r12 ,d r22 δ is the equivalent length of the rear half of the main rocker arms on both sides of the vehicle body. pi The threshold value is the centroid position constraint threshold.

[0079] In this embodiment, the variables affecting the steady state of the vehicle's center of mass are obtained by measuring the rotation angle of the active rocker arms on both sides of the rover, and the constraint effect is ensured by setting thresholds for the variables affecting the steady state of the vehicle's center of mass.

[0080] Preferably, the multi-joint motion coordination constraint model includes:

[0081] The forward speed difference of the wheels is equal to a first preset multiple of the angular velocity of the active rocker arm, and the multi-joint motion coordination constraint model is used to prevent the vehicle body from overturning.

[0082] Specifically, the movement of the rocker arm suspension will cause a decrease in the wheelbase. The forward velocity difference of the wheel is obtained based on the amount of wheelbase reduction caused by the rotation of the active rocker arm. The range of the active rocker arm angular velocity is limited by setting a multiple threshold between the forward velocity difference of the wheel and the angular velocity of the active rocker arm.

[0083] For example, in one embodiment, the relationship between the vehicle body's active rocker arm rotation angle, the reduction in wheelbase, and the robot's fixed length design parameters is expressed as follows:

[0084]

[0085] Where, d s2 ,d s4 d represents the equivalent length of the front and middle wheel support arms, respectively. s2 =d s4 z represents the length of the vehicle body;

[0086] Differentiate the above expression and apply the relation. The following relationship can be derived: The rotational speed of the active rocker arm and the difference in forward speed between the front and center wheels.

[0087] Δv aRx =v a4x -v a2x =zL d2 | s ω s22 |,

[0088] Where, Δv aRx This indicates the difference in forward speed between the front and middle wheels. s ω s22 This indicates the angular velocity of the second half of the right-side active rocker arm.

[0089] In this embodiment, a multi-joint motion coordination constraint model is established by mapping the wheelbase reduction caused by the active rocker arm movement to the active rocker arm rotation speed and the forward angular velocity of the wheel, so as to avoid additional slippage and robot rollover caused by motion incoordination between the wheel and the suspension rocker arm.

[0090] Preferably, the vehicle body differential steering motion stability constraint model includes a fourth constraint, a fifth constraint, a sixth constraint, and a seventh constraint, wherein the fourth constraint includes:

[0091] The angular velocity of the active rocker arm is less than or equal to a second preset multiple of the initial velocity of the vehicle body before the suspension attitude adjustment, and the fourth constraint is used to prevent the wheel merging speed from zero.

[0092] The fifth constraint includes:

[0093] The forward velocity deviation of the self-reconfigurable planetary rover is less than the fourth preset threshold, and the fifth constraint is used to avoid the forward velocity of the vehicle body becoming unbalanced.

[0094] The sixth constraint includes:

[0095] The deviation of the vehicle body's steering angular velocity of the self-reconfigurable planetary rover is less than the fifth preset threshold, and the sixth constraint is used to avoid the vehicle body's steering angular velocity from becoming unbalanced.

[0096] The seventh constraint includes:

[0097] The roll rate of the self-reconfigurable planetary rover is less than the sixth preset threshold, and the seventh constraint is used to avoid roll rate misalignment.

[0098] Specifically, a mapping is established between the rotational angular velocity of the active rocker arm and the initial velocity of the vehicle body before suspension attitude adjustment, and the wheel merging velocity. Based on the mapping relationship, a threshold value for the numerical multiple of the rotational angular velocity of the active rocker arm and the initial velocity of the vehicle body before suspension attitude adjustment is set. A mapping is established between the rolling angular velocity of the wheel and the rotational angular velocity of the active rocker arm and the forward velocity of the vehicle body. Based on the mapping relationship, a threshold value for the forward velocity of the vehicle body is set. A mapping is established between the rolling angular velocity of the wheel and the rotational angular velocity of the active rocker arm and the steering angular velocity of the vehicle body. Based on the mapping relationship, a threshold value for the steering angle of the vehicle body is set. A mapping is established between the rotational angular velocity of the active rocker arm and the roll angular velocity of the vehicle body. Based on the mapping relationship, a threshold value for the roll angle of the vehicle body is set.

[0099] For example, in one embodiment, the fourth constraint includes:

[0100]

[0101] Among them, v 0x This indicates the initial speed of the vehicle body before the suspension attitude is adjusted. s W s12 This indicates the angular velocity of the latter half of the left active rocker arm;

[0102] The fifth constraint includes:

[0103]

[0104] Among them, W L W R These represent the angular velocities of the left and right wheels during differential steering. This indicates the threshold for forward speed deviation of the vehicle body;

[0105] The sixth constraint includes:

[0106]

[0107] Where, d b Indicates the width of the vehicle body. The threshold value for the vehicle's steering angular velocity deviation is represented by d. b This indicates the vehicle body width; the sixth constraint is used to prevent changes in the vehicle body's steering angular velocity.

[0108] The seventh constraint includes:

[0109]

[0110] in, This indicates the threshold value for the vehicle's roll rate.

[0111] In addition, one embodiment of the present invention provides a metric for measuring the steady-state margin of the center of gravity velocity, used to verify the feasibility of constraining the speeds of the active rocker arm bogie and wheels, including:

[0112]

[0113] Where, γ v C represents the steady-state margin of the center of mass velocity. vi ( s ω s12 , s ω s22 , s θ s12 , s θ s22 ,ω L ,ω R ) represents the constraint equation. s ω s12 , s ω s22 Indicates the angular velocity of the active rocker arm. s θ s12 , s θ s22 ω represents the rotation angle of the active rocker arm. L ,ω R v is the angular velocity of the left and right wheels during differential steering. 0x Let r be the initial velocity of the vehicle body before the suspension attitude is adjusted, and d be the wheel radius. wt d represents the lateral distance between the centerlines of the two wheels. b d is the width of the vehicle body. r12 ,d r22 δ is the equivalent length of the rear half of the main rocker arms on both sides of the vehicle body. vi This represents the centroid velocity constraint threshold.

[0114] In this embodiment, a stability constraint model for the differential steering motion of the vehicle body is established by mapping the rolling angular velocity of the active rocker arm bogie and wheels to the suspension-wheel speed, the forward speed of the vehicle body, the steering angular velocity of the vehicle body, and the roll angular velocity of the vehicle body. This model enables the stable change of the instantaneous turning radius of the planetary rover during differential steering, thereby ensuring the stability of the planetary rover during differential steering motion.

[0115] Preferably, it also includes a quantity constraint model for wheel-to-ground contact, which includes an eighth constraint, a ninth constraint, and a tenth constraint, wherein the eighth constraint includes:

[0116] The number of wheels simultaneously off the ground is less than or equal to the seventh preset threshold;

[0117] The ninth constraint includes:

[0118] The number of wheels on one side that are off the ground is less than or equal to the eighth preset threshold.

[0119] The tenth constraint includes:

[0120] At least one of the front and rear wheels is in contact with the ground.

[0121] Specifically, when the active suspension of the planetary rover is stationary, the adaptive restricted direction movement constraints of the wheels and the automatic adjustment of the passive suspension to terrain undulations can meet the wheel-ground contact requirements in most cases. However, there are still situations where the passive suspension causes the wheels to temporarily lift off the ground due to adapting to terrain undulations. Based on the driving performance of the main robot of the self-weight wheeled planetary rover, a threshold for the number of wheels lifting off the ground is established. For example, in a six-wheeled active and passive suspension planetary rover embodiment, its driving performance determines that a maximum of two wheels can be lifted simultaneously. Based on the control principle of stable planetary rover movement and the vehicle's driving performance, the motion constraints are defined as follows: the number of wheels lifting off the ground simultaneously is less than or equal to 2, and the number of wheels lifting off the ground on one side is less than or equal to 1, as shown in Figures 2(a), (b), and (c). Lifting any one of the six wheels off the ground includes six cases. The second constraint is shown in Figures 2(d), (e), (g), (h), and (f), where two symmetrical wheels on both sides of the six wheels lift off the ground, or two asymmetrical wheels on both sides lift off the ground, including seven cases.

[0122] In this embodiment, a quantity constraint model for wheel-to-ground contact is established by setting a threshold for the number of wheels off the ground to limit the range of different wheel lifting quantity / position combinations. This ensures safe vehicle movement while improving trajectory tracking accuracy and optimizing energy consumption.

[0123] Preferably, the multi-joint coordinated control method for the self-reconfigurable planetary rover further includes:

[0124] A vehicle kinematic model is established based on the variable position of the wheel-to-ground contact point and the three-dimensional linear velocity of the vehicle body. A vehicle dynamics model is established based on the vehicle body properties. The vehicle kinematic model includes a dynamic movement kinematic model considering the wheel-to-ground contact point and a differential steering kinematic model with vertical degrees of freedom. The process of establishing the dynamic movement kinematic model considering the wheel-to-ground contact point and the differential steering kinematic model with vertical degrees of freedom includes:

[0125] Based on the offset wheel matrix of the vehicle coordinate system, the angular velocity of the suspension articulation joints, the linear velocity of the wheel-to-ground contact point relative to the wheel center, the angular velocity of the wheel support arm articulation joints, and the three-dimensional linear velocity of the vehicle body, a dynamic kinematic model considering the wheel-to-ground contact point is obtained; based on the angular velocity of the active rocker arm and the vehicle body attitude angle, a differential steering kinematic model with vertical degrees of freedom is obtained; and a vehicle dynamics model is established based on vehicle body properties, including:

[0126] A vehicle dynamics model is established based on joint angles, number of joints, and driving torque of the active rocker arm.

[0127] Specifically, the linear drive model defines the vehicle body coordinate system, the center difference coordinate system, the wheel-to-ground contact point coordinate system, the support coordinate system, the active suspension articulation coordinate system, the passive suspension articulation coordinate system, and the wheel-axle coordinate system. The steering drive model reflects the instantaneous steering center of the vehicle body. By combining the linear drive model and the steering drive model, a kinematic model that reflects the number of wheel-to-ground contact points and the vertical degrees of freedom of the vehicle body is constructed.

[0128] For example, in one embodiment, the vehicle coordinate system, center difference coordinate system, wheel-to-ground contact point coordinate system, support coordinate system, active suspension articulation coordinate system, passive suspension articulation coordinate system, and wheel-axle coordinate system are iteratively transformed to obtain a dynamic kinematic model considering the wheel-to-ground contact point. Based on the instantaneous center of velocity method, a differential steering motion model with variable instantaneous curvature during wheel movement is established, which can achieve the desired effect by changing the angle and lateral attitude of the active rocker arm bogie.

[0129] The dynamic kinematic model considering the wheel-ground contact point includes:

[0130]

[0131] in, Let represent the offset wheel matrices in coordinate system Σb, respectively. This indicates that the robot's linear velocity and angular velocity are combined. This indicates the angular velocity of the active suspension articulation joint. This represents the angular velocity of the passive suspension articulation joint. This indicates the angular velocity of the hinge joint of the wheel support arm. ε represents the linear velocity of the wheel-to-ground contact point relative to the wheel center, and ε represents the rotation angle of the wheel support arm in the wheel center coordinate system.

[0132] Based on the wheel radius, the lateral distance between the wheel centerlines, the vehicle width, the equivalent length of the rear half of the main rocker arms on the left and right sides of the vehicle, the rotational angular velocity of the rear half of the main rocker arms on the left and right sides of the vehicle, and the attitude angle of the vehicle in the world coordinate system, a differential steering kinematic model with vertical degrees of freedom is obtained.

[0133] Specifically, the differential steering kinematic model with vertical degrees of freedom includes:

[0134]

[0135]

[0136]

[0137]

[0138]

[0139]

[0140]

[0141]

[0142]

[0143]

[0144]

[0145]

[0146]

[0147]

[0148]

[0149] Where r represents the wheel radius, d wt d represents the lateral distance between the centerlines of the two wheels. b Indicates the width of the vehicle body, d r12 d r22 This indicates the equivalent length of the rear half of the main rocker arms on both the left and right sides of the vehicle body. s θ s12 , s θ s22The values ​​represent the rotational angular velocities of the rear half of the left and right main rocker arms, respectively. φ, θ, and ψ represent the roll angle, pitch angle, and yaw angle of the vehicle body in the world coordinate system, respectively.

[0150] The dynamic equations of the variable linkage are established based on the Newton-Euler dynamic equilibrium method:

[0151]

[0152] Where W represents the work done by external forces, K and P represent the kinetic and potential energy of the planetary rover, respectively, D represents energy loss, and q i represents the joint angle, and n represents the number of joints.

[0153] kinetic energy:

[0154]

[0155] Potential energy:

[0156] P = m²g(d r21 sin s θ s21 +d s2 )+(m4+m6+m o2 )gd r22 sin s θ s22 +m4g[d r23 sin( o θ so2 - s θ s22 )+d s4 ]+m5g[d r24 sinθ ∑ +d s6 ],

[0157] Energy loss:

[0158]

[0159] Work done by external force:

[0160] W = T1 s θ s21 +T2 s θ s22 +T3 o θ so2 ,

[0161] Where, d r21 d r22 d r23 and d r24d represents the equivalent lengths of the front and rear halves of the main rocker arm and the front and rear halves of the auxiliary rocker arm on the right side of the vehicle body, respectively. s2 d s4 d s6 These represent the equivalent lengths of the right front, middle, and rear wheel support arms, respectively; g is the acceleration due to gravity; m2, m4, and m6 represent the masses of the right front, middle, and rear wheels, respectively; and m... o2 This indicates the mass of the passive suspension, including the center and rear wheel control arms. s θ s21 , s θ s22 This indicates the angle between the front and rear halves of the right main rocker arm and the horizontal direction. o θ so2 θ represents the angle between the rear half of the right main rocker arm and the front half of the secondary rocker arm. ∑ =π+ s θ s22 - o θ so2 - o θ o22 - o θ o21 This indicates the combined angles in the derivation process. o θ o21 , o θ o22 The angle between the front and rear halves of the secondary rocker arm and the vertical direction is represented by C1, C2, and C3; the centripetal acceleration coefficients are represented by C1, C2, and C3; and the driving torques at the front and rear halves of the main rocker arm and the hinge of the secondary rocker arm are represented by T1, T2, and T3, respectively.

[0162] When the rear wheel of the planetary rover is connected to the rear half of the main rocker arm via a single linkage through the secondary rocker arm,

[0163]

[0164] When the rear wheel of the planetary rover is connected to the rear half of the main rocker arm via a double-link configuration through the secondary rocker arm, then q2 = o θ so2 ,

[0165]

[0166] in, s θ s21 =z s θ s22 , z represents the mechanical power transmission ratio coefficient of the front and rear halves of the main rocker arm.

[0167] Based on the above relationships, the planetary rover dynamics model is obtained:

[0168]

[0169] Among them, the matrix is ​​defined as:

[0170]

[0171]

[0172]

[0173]

[0174] V 11 =C1z 2 +C2,V 12 =0,V 21 =0,V 22 = C 3

[0175] G1=2m4d r22 d r23 sin o i so2 +2m6d r22 d r24 sin( o i so2 + o i o22 + o i o21 ), G2=0

[0176] Q 11 =0,Q 22 =0

[0177] Q 12 =-m4d r22 d r23 sin o i so2 -m6d r22 d r24 sin( o i so2 + o i o22 + o i o21 )

[0178] Q 21 =-m4d r22 d r23 sin o i so2

[0179] Γ d1 =m2gzd r21 cos(z s θ s22 )+(m4+m6+m o2 )gd r22 cos s θ s22 -m4gd r23 cos( o θ so2 - s θ s22 )-m6gd r24 cos( o θ so2 - o θ s22 + o θ o22 + o θ o21 )

[0180] Γ d2 =m4gd r23 coS( o θ so2 - s θ s22 )+m6gd r24 coS( o θ so2 - o θ s22 + o θ o22 + o θ o21 ).

[0181] In another embodiment of the present invention, the influencing factors of the contact force at the wheel-ground contact point and the amount of change of these factors are obtained, and the variation law of the contact force at the wheel-ground contact point is determined according to the mapping relationship between the influencing factors, the amount of change of these factors and the contact force.

[0182] For example, as the distance between the wheel edge and the wheel center decreases, the size of the obstacle increases, and the hardness of the terrain increases, the hook traction, lateral force, and normal force at the wheel-to-ground contact point all tend to increase; as the amount of obstacle sinking increases, the hook traction and lateral force at the wheel-to-ground contact point tend to decrease; and as the speed of the planetary vehicle increases, the lateral force tends to increase.

[0183] The linear motion function of the planetary rover is realized by establishing a dynamic kinematic model considering the wheel-ground contact point based on the linear drive model. The steering motion function of the planetary rover is realized by establishing a differential steering kinematic model with vertical degrees of freedom based on the steering drive model. The dynamic prediction of the wheel-ground contact point position movement is provided based on the variation law of the contact force at the wheel-ground contact point, which is conducive to improving the model's adaptability to terrain undulations and achieving more accurate modeling.

[0184] While the present invention has been disclosed above, its scope of protection is not limited thereto. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention, and all such changes and modifications will fall within the scope of protection of the present invention.

Claims

1. A multi-joint coordinated control method for a self-reconfigurable planetary rover, characterized in that, include: Acquire vehicle body state data, including the rotation angle of the active rocker arm, the reduction in wheelbase caused by the rotation of the active rocker arm, the angular velocity of the active rocker arm, and the rolling angular velocity of the wheels; establish a multi-joint motion constraint model for the self-reconfigurable planetary rover based on the vehicle body state data, including a vehicle body center of mass position constraint model, a multi-joint motion coordination constraint model, and a vehicle body differential steering motion stability constraint model; perform multi-joint motion control on the self-reconfigurable planetary rover based on the multi-joint motion constraint model.

2. The multi-joint coordinated control method for a self-reconfigurable planetary rover according to claim 1, characterized in that, The step of establishing the multi-joint motion constraint model of the self-reconfigurable planetary rover based on the vehicle body state data includes: establishing a vehicle body center of gravity position constraint model based on the rotation angle of the active rocker arm and the reduction in wheelbase caused by the rotation of the active rocker arm; establishing a multi-joint motion coordination constraint model based on the reduction in wheelbase caused by the rotation of the active rocker arm; and establishing a vehicle body differential steering motion stability constraint model based on the angular velocity of the active rocker arm and the rolling angular velocity of the wheels.

3. The multi-joint coordinated control method for a self-reconfigurable planetary rover according to claim 2, characterized in that, The vehicle body center of gravity position constraint model includes a first constraint, a second constraint, and a third constraint. The first constraint includes: the reduction in wheelbase caused by the rotation of the active rocker arm is less than or equal to a first preset threshold, and the first constraint is used to avoid wheel collisions. The second constraint includes: the vehicle body roll angle caused by the rotation of the active rocker arm is less than or equal to a second preset threshold, and the second constraint is used to avoid abnormal roll angles of the vehicle body. The third constraint includes: the vehicle body steering angle caused by the rotation of the active rocker arm is less than or equal to a third preset threshold, and the third constraint is used to avoid excessive deviation in the vehicle body steering angle.

4. The multi-joint coordinated control method for a self-reconfigurable planetary rover according to claim 2, characterized in that, The multi-joint motion coordination constraint model includes: the forward speed difference of the wheels is equal to a first preset multiple of the angular velocity of the active rocker arm, and the multi-joint motion coordination constraint model is used to avoid vehicle rollover.

5. The multi-joint coordinated control method for a self-reconfigurable planetary rover according to claim 2, characterized in that, The vehicle body differential steering motion stability constraint model includes a fourth constraint, a fifth constraint, a sixth constraint, and a seventh constraint. The fourth constraint includes: the angular velocity of the active rocker arm is less than or equal to a second preset multiple of the vehicle body's initial velocity before suspension attitude adjustment; this fourth constraint is used to prevent the wheel merging velocity from zero. The fifth constraint includes: the forward velocity deviation of the self-reconfigurable planetary vehicle is less than a fourth preset threshold; this fifth constraint is used to prevent forward velocity misalignment. The sixth constraint includes: the vehicle body steering angular velocity deviation of the self-reconfigurable planetary vehicle is less than a fifth preset threshold; this sixth constraint is used to prevent vehicle body steering angular velocity misalignment. The seventh constraint includes: the vehicle body roll angular velocity of the self-reconfigurable planetary vehicle is less than a sixth preset threshold; this seventh constraint is used to prevent vehicle body roll angular velocity misalignment.

6. The multi-joint coordinated control method for a self-reconfigurable planetary rover according to claim 1, characterized in that, It also includes a constraint model for the number of wheels in contact with the ground, which includes an eighth constraint, a ninth constraint, and a tenth constraint. The eighth constraint includes: the number of wheels simultaneously off the ground is less than or equal to a seventh preset threshold; the ninth constraint includes: the number of wheels on one side off the ground is less than or equal to the eighth preset threshold; and the tenth constraint includes: at least one of the front wheels and the rear wheels is in contact with the ground.

7. The multi-joint coordinated control method for a self-reconfigurable planetary rover according to claim 1, characterized in that, Also includes: A kinematic model of the vehicle body is established based on the variable position of the wheel-ground contact point and the three-dimensional linear velocity of the vehicle body, and a dynamic model of the vehicle body is established based on the vehicle body properties.

8. The multi-joint coordinated control method for a self-reconfigurable planetary rover according to claim 7, characterized in that, The vehicle kinematics model includes a dynamic movement kinematics model considering the wheel-ground contact point and a differential steering kinematics model with vertical degrees of freedom.

9. The multi-joint coordinated control method for a self-reconfigurable planetary rover according to claim 8, characterized in that, The process of establishing the dynamic kinematic model considering the wheel-to-ground contact point and the differential steering kinematic model with vertical degrees of freedom includes: obtaining the dynamic kinematic model considering the wheel-to-ground contact point based on the offset wheel matrix of the vehicle coordinate system, the angular velocity of the suspension articulation joint, the linear velocity of the wheel-to-ground contact point relative to the wheel center, the angular velocity of the wheel support arm articulation joint, and the three-dimensional linear velocity of the vehicle body; and obtaining the differential steering kinematic model with vertical degrees of freedom based on the angular velocity of the active rocker arm and the vehicle body attitude angle.

10. The multi-joint coordinated control method for a self-reconfigurable planetary rover according to claim 7, characterized in that, The process of establishing a vehicle dynamics model based on vehicle attributes includes: establishing the vehicle dynamics model based on joint angles, number of joints, and driving torque of the active rocker arm.

Images