Trajectory tracking algorithm for asteroid surface space equipment based on sparrow search algorithm

Abstract

The application discloses a kind of asteroid surface space equipment trajectory tracking algorithm based on sparrow search algorithm, including space mining car system, wheel-soil interaction force estimation model is established according to the soil condition of asteroid surface by space mining car system, wheel dynamics analysis model and whole vehicle dynamics analysis model are respectively established by kinematics analysis and dynamics analysis by space mining car system, the motion trajectory of space mining car under existing state is solved by SSA-PID control model solution of space mining car system, and finally output.The beneficial effects of the present application are: in the process of dynamics modeling, according to the complex surface environment of asteroid, the change factor of effective radius of wheel is considered, the influence of possible motion error is avoided, the PID control optimized by sparrow search algorithm is used, the local optimal problem is avoided, and the trajectory tracking effect is enhanced.

Description

Technical Field

[0001] This invention relates to a trajectory tracking algorithm, specifically a trajectory tracking algorithm for asteroid surface space equipment based on the sparrow search algorithm, belonging to the field of aerospace technology. Background Technology

[0002] Research in the field of space mining necessitates the design of highly reliable intelligent mining robot platforms to achieve the intelligence and automation of space mining robots. Drive control technology is a crucial element in achieving high-precision operation of space mining equipment. It involves controlling and rationally distributing the drive torque of the six wheels and matching it to the surface environment of outer space asteroids, thereby avoiding unnecessary energy loss and mechanical wear, and improving the equipment's operational accuracy and lifespan.

[0003] Currently, my country's control algorithms for tracking the trajectory of space equipment have the following drawbacks when applied to near-Earth asteroids:

[0004] (1) In the process of establishing the dynamic model of space equipment, the wheels are generally idealized as rigid bodies. However, for some near-Earth asteroids with high hardness surfaces, the change in the effective radius of the wheels can easily cause a certain degree of error in the equipment control.

[0005] (2) At present, there are few control algorithms for space mining equipment, and the overall calculation process is extremely complex and the workload is large, making it impossible to effectively control the entire operation process of space mining equipment. Summary of the Invention

[0006] The purpose of this invention is to provide a trajectory tracking algorithm for asteroid surface space equipment based on the sparrow search algorithm to solve at least one of the above-mentioned technical problems. This algorithm addresses the errors caused by changes in the effective radius of the wheels during the establishment of wheel dynamics in existing space mining equipment trajectory tracking control, as well as the problem that existing control algorithms have a large workload and cannot effectively monitor the entire operation process of the equipment.

[0007] The present invention achieves the above objectives through the following technical solution: a trajectory tracking algorithm for asteroid surface space equipment based on the sparrow search algorithm, including a space mining vehicle system. The space mining vehicle system establishes a wheel-soil interaction force estimation model based on the soil conditions of the asteroid surface. The space mining vehicle system establishes a wheel dynamics analysis model and a whole vehicle dynamics analysis model through kinematic analysis and dynamic analysis, respectively. The space mining vehicle system solves the motion trajectory of the space mining vehicle under the current state through an SSA-PID control model and finally outputs the result.

[0008] As a further aspect of the present invention: the space mining vehicle system takes a single wheel of the space mining vehicle as the research object and studies the wheel-soil relationship between the space vehicle and the surface of the asteroid. Considering that some asteroid surfaces are composed of soft soil and rocks, the objects acted upon by the wheels of the space mining equipment during operation exhibit alternating soft and hard properties. Therefore, an estimation model for the wheel-soil interaction force is established.

[0009] The soil resistance of the wheels consists of two parts:

[0010] F R =F b +F c

[0011] Among them, F R For the soil resistance of the wheel, F b and F c These are bulldozing resistance and compaction resistance, respectively.

[0012] Based on the forces acting on the wheels, the effective tire radius of the space mining vehicle under actual working conditions can be calculated as follows:

[0013]

[0014] Where, r e r is the effective tire radius of the space mining vehicle. s r0 is the tire radius when the vehicle is compressed against the rocky road surface, and r0 is the tire radius when the vehicle is unloaded.

[0015] The rolling resistance torque experienced by the wheel on the asteroid surface is derived as follows:

[0016]

[0017] Among them, T R This is the rolling resistance torque experienced by the wheel on the surface of the asteroid.

[0018] As a further aspect of the present invention: for the established wheel dynamics analysis model, when the wheel is in pure rolling, all velocity components of the wheel in its plane are equal to the wheel's traveling speed, that is...

[0019]

[0020] Among them, V W For the longitudinal speed of the wheel, Let be the velocity of the wheel in the X direction of the wheel coordinate system. Let be the velocity of the wheel in the Y direction of the wheel coordinate system, and let c be the distance from the center of the vehicle body to the center of the wheel. The rotational speed of the wheel around the center of the vehicle body.

[0021] As a further aspect of the present invention: when establishing the wheel dynamics analysis model of the space mining vehicle system, it is necessary to establish a local coordinate system for the space mining vehicle, which specifically includes:

[0022] Let the position matrix of the space mining vehicle be ξ. R =[X R Y R ,θ] T Then we get:

[0023]

[0024] Based on the geometry and positional relationships of the space mining vehicle, the formulas for calculating the turning angles of the front and rear wheels at the left and right ends are as follows:

[0025]

[0026]

[0027]

[0028]

[0029] The turning radius of the space mining vehicle is R. t The deflection angles of the front and rear wheels at the left and right ends are δ1, δ2, δ3, and δ4, respectively. The position of the center of the space mining vehicle chassis in this coordinate system is S(x, y). The steering azimuth angle of the vehicle body is α. The distance between the front and rear axles is l. The width of the vehicle body is d.

[0030] During the operation of the space mining vehicle, the rotation angle of each wheel is calculated using the above formula.

[0031] As a further aspect of this invention: for the established wheel dynamics analysis model, when all six wheels of the space mining vehicle are under pure rolling conditions, the constraint equations are as follows:

[0032]

[0033] As a further aspect of the present invention: the space mining vehicle system is a nonholonomically constrained system. According to the Routh equation under nonholonomically constrained systems, its dynamic equation is:

[0034]

[0035] Where, q j Q j Let λ represent the generalized coordinates and generalized forces of the space mining vehicle; T represents the kinetic energy of the space mining vehicle; λ represents the kinetic energy of the space mining vehicle. k Powering the space mining vehicle; B kj The coefficients are determined by the constraints;

[0036] Among them, B kj for:

[0037] B k1 =sinδ k (k = 1 to 6)

[0038] B 12 =l1cos(β1-δ1), B 22 =l2cos(β2), B 32 =l3cos(β3-δ2)

[0039] B 42 = -l4cos(β4-δ3), B 52 = -l5cos(β5), B 62 = -l6cos(β6-δ4)

[0040] B kj =-r(k=1~6, j=k+2)

[0041] As a further aspect of the present invention: the overall kinetic energy of the space mining vehicle system consists of three parts: the movement of the vehicle body itself, its own rotation, and the rotation of its six wheels, as represented as:

[0042]

[0043] Where m represents the mass of the space mining vehicle; J represents the moment of inertia of the space mining vehicle; J w This represents the moment of inertia of the space mining vehicle's wheels; v represents the speed of the space mining vehicle.

[0044] During its operation on the asteroid surface, the space mining vehicle traverses alternating layers of soft soil and hard rock, thus its potential energy is defined as:

[0045] V = mg a Z

[0046] Among them, g a This represents the gravitational acceleration at the asteroid.

[0047] With generalized coordinates q j The corresponding generalized force Q j for:

[0048]

[0049]

[0050] Q j =T ai (j = 3~8, i = 1~6)

[0051] Among them, T i The driving torque for wheel i.

[0052] As a further aspect of the present invention: when establishing the overall dynamic analysis model of the space mining vehicle system, a force analysis is performed on the space mining vehicle, and the dynamic equations are as follows:

[0053]

[0054] We can obtain:

[0055]

[0056]

[0057]

[0058] Using the previously obtained T Ri From the equations, we can obtain the dynamic equations of the space mining vehicle as follows:

[0059]

[0060]

[0061] As a further aspect of the present invention, the SSA-PID control model specifically includes:

[0062] Imagine the PID control parameters as a sparrow population, divided into three categories: discoverers, followers, and watchdogs. Let the number of sparrows in the population be n. Based on the number of PID control parameters, let the dimension of the variable to be optimized be d. Then, the population of n sparrows can be represented as:

[0063]

[0064] The fitness values ​​of all sparrows can be represented as:

[0065]

[0066] In the sparrow search algorithm, the position of the discoverer is updated in each iteration, as described below:

[0067] Where t represents the current iteration parameter, j = 1, 2, 3, ..., d represents the dimension of the problem being computed, and item max x is a constant representing the maximum number of iterations. i，jLet represent the position information of the i-th sparrow in the j-th dimension. α (α∈(0,1]) is a random number, R2 (R2∈[0,1]) and ST (ST∈[0.5,1]) represent the warning value and the safety value, respectively. Q is a random number that follows a normal distribution, and L represents a 1×d matrix where each element has a value of 1.

[0068] The SSA-PID control model algorithm includes the following steps:

[0069] Step 1: Randomly generate the PID parameter K for the group of space mining vehicles. P K I K D The control parameters are then substituted into the fitness function to calculate the target fitness value.

[0070] Step 2: Determine whether the calculated fitness value exceeds the maximum number of iterations. If yes, output the optimal solution for the parameters. If no, arrange the parameters according to the fitness value.

[0071] Step 3: Select the top 20% of PID parameters with the best fitness values ​​as discoverers, and update the position of the discoverers according to the discoverer formula;

[0072] Step 4: Select the bottom 80% of PID parameters with the worst fitness values ​​as followers, and update the positions of the followers according to the follower formula;

[0073] Step 5: Randomly select 10%-20% of the PID parameters as vigilants, and update the vigilant's position according to the vigilant formula;

[0074] Step 6: Update the PID parameters of each group to the optimal solution based on the fitness value, and compare the updated PID parameters of the n groups, retaining the optimal solution.

[0075] Step 7: Re-evaluate the calculated optimal solution and determine whether the calculated fitness value exceeds the maximum number of iterations. If yes, output the optimal solution. If no, return to step 3 to recalculate until the optimal solution can be output.

[0076] As a further aspect of the present invention: the algorithm, based on the trajectory of the space mining vehicle, plans the desired displacement X of the space mining vehicle in the X-axis direction. d Expected speed and expected acceleration As the space mining vehicle moves in a straight line, let θ be the angle between this line and its X-axis.

[0077]

[0078] As the space mining vehicle moves clockwise along a circular arc trajectory, with the center of the circle located at (c, 0) on the X-axis, the central angle corresponding to the circular arc trajectory is... but

[0079]

[0080] By performing proportional and differential calculations on the angular and rotational deviations, and compensating for the desired angular acceleration, then...

[0081]

[0082] In the formula, K P and K D These are the proportional and derivative coefficients of the SSA-PID controller.

[0083] The effective driving torque of the wheel is

[0084]

[0085] The relationship between the effective driving torque of wheel i and the wheel speed can be simplified to an inertial element, namely:

[0086]

[0087] In the formula, k and T are the proportional coefficient and time constant of the inertial element, respectively.

[0088] The torque required to be distributed to the wheels can be obtained from the above formula. Finally, the trajectory of the space mining vehicle is corrected by updating the driving torque so that the space mining vehicle moves according to the predetermined trajectory.

[0089] The beneficial effects of this invention are: it enables stable operation of space mining equipment in rugged environments and ensures good robustness of the control system. In the dynamic modeling process, the trajectory tracking control algorithm of this invention considers the variation of the effective wheel radius based on the complex surface environment of the asteroid, avoiding potential motion errors. This invention employs a sparrow search algorithm-optimized PID control, optimizing the PID control parameters to avoid local optima and enhance trajectory tracking performance. Attached Figure Description

[0090] Figure 1 This is a three-dimensional model of the six-wheeled space mining vehicle of the present invention;

[0091] Figure 2 This is a diagram showing the wheel-soil interaction force of the space mining vehicle of the present invention;

[0092] Figure 3 This is a kinematic representation of the wheels of the space mining vehicle of the present invention;

[0093] Figure 4 This is a kinematic representation of the space mining vehicle of the present invention;

[0094] Figure 5 This is a flowchart of the SSA-PID control method for the space mining vehicle of the present invention;

[0095] Figure 6 This is a block diagram of the trajectory control of the space mining vehicle of the present invention.

[0096] In the diagram: 1. Right front wheel, 2. Right middle wheel, 3. Right rear wheel, 4. Left front wheel, 5. Left middle wheel, 6. Left rear wheel, 7. Main rocker arm, 8. Bogie, 9. Car body, 10. Functional module group. Detailed Implementation

[0097] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0098] Example 1

[0099] like Figures 1 to 6 As shown, a trajectory tracking algorithm for asteroid surface space equipment based on the sparrow search algorithm includes a space mining vehicle system. The space mining vehicle system establishes a wheel-soil interaction force estimation model based on the soil conditions of the asteroid surface. The space mining vehicle system establishes a wheel dynamics analysis model and a whole vehicle dynamics analysis model through kinematic analysis and dynamic analysis, respectively. The space mining vehicle system solves the motion trajectory of the space mining vehicle under the current state through the SSA-PID control model and finally outputs the result.

[0100] The space mining vehicle system comprises a vehicle body 9. Main rocker arms 7 are connected to the left and right sides of the vehicle body 9. A right rear wheel 3 is mounted on the rear end of the right main rocker arm 7 via a bogie 8. A secondary rocker arm is connected to the front end of the right main rocker arm 7, and a right front wheel 1 and a right middle wheel 2 are respectively mounted on the two ends of the secondary rocker arm via the bogie 8. A left rear wheel 6 is mounted on the rear end of the left main rocker arm 7 via the bogie 8. A secondary rocker arm is connected to the front end of the left main rocker arm 7, and a left front wheel 4 and a left middle wheel 5 are respectively mounted on the two ends of the secondary rocker arm via the bogie 8. The vehicle body 9 is equipped with a functional module group 10 including a sampling and grasping module, a drilling module, and an environmental perception module.

[0101] Example 2

[0102] like Figures 1 to 6As shown, in addition to all the technical features included in Embodiment 1, this embodiment also includes:

[0103] The space mining vehicle system takes a single wheel of the space mining vehicle as the research object to study the wheel-soil relationship between the space vehicle and the surface of an asteroid. Considering that some asteroid surfaces are composed of soft soil and rocks, the objects that the wheels act on during the operation of the space mining equipment are alternating between soft and hard materials. Therefore, an estimation model of wheel-soil interaction force is established.

[0104] The soil resistance of the wheels consists of two parts:

[0105] F R =F b +F c

[0106] Among them, F R For the soil resistance of the wheel, F b and F c These are bulldozing resistance and compaction resistance, respectively.

[0107] Based on the forces acting on the wheels, the effective tire radius of the space mining vehicle under actual working conditions can be determined as follows:

[0108]

[0109] Where, r e r is the effective tire radius of the space mining vehicle. s r0 is the tire radius when the vehicle is compressed against a rocky road surface, and r0 is the tire radius when the vehicle is unloaded.

[0110] The rolling resistance torque experienced by the wheel on the asteroid surface can be derived as follows:

[0111]

[0112] Among them, T R This is the rolling resistance torque experienced by the wheel on the surface of the asteroid.

[0113] For the established wheel dynamics analysis model, when the wheel is in pure rolling, all velocity components of the wheel in its plane are equal to the wheel's traveling velocity, that is...

[0114]

[0115] Among them, V W For the longitudinal speed of the wheel, Let be the velocity of the wheel in the X direction of the wheel coordinate system. Let be the velocity of the wheel in the Y direction of the wheel coordinate system, and let c be the distance from the center of the vehicle body to the center of the wheel. The rotational speed of the wheel around the center of the vehicle body.

[0116] When establishing the wheel dynamics analysis model for the space mining vehicle system, it is necessary to establish the local coordinate system of the space mining vehicle, which specifically includes:

[0117] Let the position matrix of the space mining vehicle be ξ. R =[X R Y R ,θ] T Then we can get:

[0118]

[0119] Based on the geometry and positional relationships of the space mining vehicle, the formulas for calculating the turning angles of the front and rear wheels at the left and right ends are as follows:

[0120]

[0121]

[0122]

[0123]

[0124] The turning radius of the space mining vehicle is R. t The deflection angles of the front and rear wheels at the left and right ends are δ1, δ2, δ3, and δ4, respectively. The position of the center of the space mining vehicle chassis in this coordinate system is S(x, y). The steering azimuth angle of the vehicle body is α. The distance between the front and rear axles is l. The width of the vehicle body is d.

[0125] During the operation of the space mining vehicle, the rotation angle of each wheel can be calculated using the above formula.

[0126] For the established wheel dynamics analysis model, when all six wheels of the space mining vehicle are in pure rolling condition, the constraint equations are as follows:

[0127]

[0128] Example 3

[0129] like Figures 1 to 6 As shown, in addition to all the technical features included in Embodiment 1, this embodiment also includes:

[0130] The space mining vehicle system is a nonholonomically constrained system. According to the Routh equations for nonholonomically constrained systems, its dynamic equations are:

[0131]

[0132] Where, q jQ j Let λ represent the generalized coordinates and generalized forces of the space mining vehicle; T represents the kinetic energy of the space mining vehicle; λ represents the kinetic energy of the space mining vehicle. k Powering the space mining vehicle; B kj The coefficients are determined by the constraints;

[0133] Among them, B kj for:

[0134] B k1 =sinδ k (k = 1 to 6)

[0135] B 12 =l1cos(β1-δ1), B 22 =l2cos(β2), B 32 =l3cos(β3-δ2)

[0136] B 42 = -l4cos(β4-δ3), B 52 = -l5cos(β5), B 62 = -l6cos(β6-δ4)

[0137] B kj =-r(k=1~6, j=k+2)

[0138] The overall kinetic energy of the space mining vehicle system consists of three parts: the movement of the vehicle body itself, its own rotation, and the rotation of its six wheels, which can be expressed as:

[0139]

[0140] Where m represents the mass of the space mining vehicle; J represents the moment of inertia of the space mining vehicle; J w This represents the moment of inertia of the space mining vehicle's wheels; v represents the speed of the space mining vehicle.

[0141] During its operation on the asteroid surface, the space mining vehicle traverses alternating layers of soft soil and hard rock, thus its potential energy is defined as:

[0142] V = mg a Z

[0143] Among them, g a This represents the gravitational acceleration at the asteroid.

[0144] With generalized coordinates q j The corresponding generalized force Q j for:

[0145]

[0146]

[0147] Q j =T ai (j = 3~8, i = 1~6)

[0148] Among them, T i The driving torque for wheel i.

[0149] When establishing the overall dynamic analysis model of the space mining vehicle system, a force analysis is performed on the space mining vehicle, and the dynamic equations are as follows:

[0150]

[0151] We can obtain:

[0152]

[0153]

[0154]

[0155] Using the previously obtained T Ri From the equations, we can obtain the dynamic equations of the space mining vehicle as follows:

[0156]

[0157]

[0158] Example 4

[0159] like Figures 1 to 6 As shown, in addition to all the technical features included in Embodiment 1, this embodiment also includes:

[0160] The SSA-PID control model specifically includes:

[0161] Imagine the PID control parameters as a sparrow population, divided into three categories: discoverers, followers, and watchdogs. Let the number of sparrows in the population be n. Based on the number of PID control parameters, let the dimension of the variable to be optimized be d. Then, the population of n sparrows can be represented as:

[0162]

[0163] The fitness values ​​of all sparrows can be represented as:

[0164]

[0165] In the sparrow search algorithm, the position of the discoverer is updated in each iteration, as described below:

[0166]

[0167] Where t represents the current iteration parameter, j = 1, 2, 3, ..., d represents the dimension of the problem being computed, and item max x is a constant representing the maximum number of iterations. i，j Let represent the position information of the i-th sparrow in the j-th dimension. α (α∈(0,1]) is a random number, R2 (R2∈[0,1]) and ST (ST∈[0.5,1]) represent the warning value and the safety value, respectively. Q is a random number that follows a normal distribution, and L represents a 1×d matrix where each element has a value of 1.

[0168] When R2 < ST, it means there are no predators in the foraging environment, and the finder can perform extensive search operations. If R2 ≥ ST, it means that some sparrows in the population have discovered the predator and alerted the other sparrows in the population. At this time, all sparrows need to quickly fly to other safe places to forage.

[0169] During the foraging process, some participants will constantly monitor the discoverer. The location update description of the participants is as follows:

[0170]

[0171] Among them, X p This is currently the optimal position occupied by the discoverer, X. worst This represents the current worst position globally. A represents a 1×d matrix where each element is randomly assigned a value of 1 or -1, and A... + =A T (AA T ) -1 ;

[0172] In a sparrow population, 10% to 20% of sparrows are randomly selected as watchmen. The initial positions of these sparrows are randomly generated within the population, and their mathematical expression can be represented as follows:

[0173]

[0174] Among them, X best This is the current global optimal position. β, as the step size control parameter, is a random number following a normal distribution with a mean of 0 and a variance of 1. K∈[-1, 1] is a random number, f i This is the fitness value of the current individual sparrow. g and f w These are the current best and worst fitness values ​​globally, respectively. ε is a constant to avoid zero in the denominator.

[0175] For simplicity, when f i >fg This indicates that the sparrows are currently on the periphery of the population and are extremely vulnerable to predators. When f i >f g This indicates that sparrows in the middle of the population are aware of the danger and need to move closer to other sparrows to minimize their risk of being preyed upon. When f i =f g This indicates that the sparrows are in the optimal position.

[0176] When optimizing PID parameters using the Sparrow Search algorithm, the objective function is set as follows:

[0177]

[0178] Where e(t) represents the input-output error, u(t) is added to avoid excessive control amplitude, and ω1 and ω2 are weights. In this invention, ω1 = 0.999, ω2 = 0.001, ω3 = 2, and ω4 = 1000 are selected.

[0179] The SSA-PID control method includes the following steps:

[0180] Step 1: Randomly generate the PID parameter K for the group of space mining vehicles. P K I K D The control parameters are then substituted into the fitness function to calculate the target fitness value.

[0181] Step 2: Determine whether the calculated fitness value exceeds the maximum number of iterations. If yes, output the optimal solution for the parameters. If no, arrange the parameters according to the fitness value.

[0182] Step 3: Select the top 20% of PID parameters with the best fitness values ​​as discoverers, and update the position of the discoverers according to the discoverer formula;

[0183] Step 4: Select the bottom 80% of PID parameters with the worst fitness values ​​as followers, and update the positions of the followers according to the follower formula;

[0184] Step 5: Randomly select 10%-20% of the PID parameters as vigilants, and update the vigilant's position according to the vigilant formula;

[0185] Step 6: Update the PID parameters of each group to the optimal solution based on the fitness value, and compare the updated PID parameters of the n groups, retaining the optimal solution.

[0186] Step 7: Re-evaluate the calculated optimal solution and determine whether the calculated fitness value exceeds the maximum number of iterations. If yes, output the optimal solution. If no, return to step 3 to recalculate until the optimal solution can be output.

[0187] Example 5

[0188] like Figures 1 to 6 As shown, in addition to all the technical features included in Embodiment 1, this embodiment also includes:

[0189] Based on the trajectory of the space mining vehicle, the algorithm plans the desired displacement X of the space mining vehicle in the X-axis direction. d Expected speed and expected acceleration

[0190] As the space mining vehicle moves in a straight line, let the angle between this line and its X-axis be θ. but

[0191]

[0192] As the space mining vehicle moves clockwise along a circular arc trajectory, with the center of the circle located at (c, 0) on the X-axis, the central angle corresponding to the circular arc trajectory is... but

[0193]

[0194] By performing proportional and differential calculations on the angular and rotational deviations, and compensating for the desired angular acceleration, then...

[0195]

[0196] In the formula, K P and K D These are the proportional and derivative coefficients of the SSA-PID controller.

[0197] The effective driving torque of the wheel is

[0198]

[0199] The relationship between the effective driving torque of wheel i and the wheel speed can be simplified to an inertial element, namely:

[0200]

[0201] In the formula, k and T are the proportional coefficient and time constant of the inertial element, respectively.

[0202] The torque required to be distributed to the wheels can be obtained from the above formula. Finally, the trajectory of the space mining vehicle is corrected by updating the driving torque so that the space mining vehicle moves according to the predetermined trajectory.

[0203] Working principle: The space mining vehicle system establishes a wheel-soil interaction force model based on the soil conditions on the asteroid surface. Through kinematic and dynamic analysis, wheel dynamics models and whole vehicle dynamics models are established respectively. The corrected torque under the existing state is solved to correct the trajectory of the space mining vehicle. In the process of dynamic modeling, the algorithm considers the variation of the effective radius of the wheel according to the complex surface environment of the asteroid, avoiding the influence of possible motion errors.

[0204] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other specific forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

[0205] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A trajectory tracking algorithm for asteroid surface spacecraft based on the sparrow search algorithm, characterized in that: Including space mining vehicle systems; The trajectory tracking algorithm specifically includes: Step 1: The space mining vehicle system establishes a model for estimating the wheel-soil interaction force based on the soil conditions on the asteroid surface; Step 2: The space mining vehicle system establishes wheel dynamics analysis models and whole vehicle dynamics analysis models through kinematic and dynamic analysis, respectively; Step 3: The space mining vehicle system solves the motion trajectory of the space mining vehicle under the current state through the SSA-PID control model; Step 4: Final Output; In step four, based on the trajectory of the space mining vehicle, the desired displacement X of the space mining vehicle in the X-axis direction is planned. d Expected speed and expected acceleration As the space mining vehicle moves in a straight line, let the angle between the line and its X-axis be θ. but: As the space mining vehicle moves clockwise along a circular arc trajectory, with the center of the circle located at (c, 0) on the X-axis, the central angle corresponding to the circular arc trajectory is β. but: By performing proportional and differential calculations on the angular deviation and speed deviation, and compensating for the desired angular acceleration, then: In the formula, K P and K D These are the proportional and derivative coefficients of the SSA-PID controller; The effective driving torque of the wheel is: The relationship between the effective driving torque of wheel i and the wheel speed can be simplified to an inertial element, namely: In the formula, k and T are the proportionality coefficient and time constant of the inertial element, respectively; The torque required to be distributed to the wheels can be obtained from the above formula. Finally, the trajectory of the space mining vehicle is corrected by updating the driving torque so that the space mining vehicle moves according to the predetermined trajectory.

2. The trajectory tracking algorithm according to claim 1, characterized in that, In step one: The soil resistance of a wheel consists of two parts: F R =F b +F c Among them, F R For the soil resistance of the wheel, F b and F c These are bulldozing resistance and compaction resistance, respectively. Based on the forces acting on the wheels, the effective tire radius of the space mining vehicle under actual working conditions is: Where, r e r is the effective tire radius of the space mining vehicle. s r0 is the tire radius when the vehicle is compressed against the rocky road surface, and r0 is the tire radius when the vehicle is unloaded. The rolling resistance torque experienced by the wheel on the asteroid surface is derived as follows: Among them, T R This is the rolling resistance torque experienced by the wheel on the surface of the asteroid.

3. The trajectory tracking algorithm according to claim 1, characterized in that, In step two: For the established wheel dynamics analysis model, when the wheel is in pure rolling, all velocity components of the wheel in its plane are equal to the wheel's traveling speed, that is: Among them, V W For the longitudinal speed of the wheel, Let be the velocity of the wheel in the X direction of the wheel coordinate system. Let be the velocity of the wheel in the Y direction of the wheel coordinate system, and let c be the distance from the center of the vehicle body to the center of the wheel. The rotational speed of the wheel around the center of the vehicle body.

4. The trajectory tracking algorithm according to claim 1, characterized in that, In step two: When establishing the wheel dynamics analysis model of the space mining vehicle system, it is necessary to establish the local coordinate system of the space mining vehicle, which specifically includes: Let the position matrix of the space mining vehicle be ξ. R =[X R ,Y R ,θ] T Then we get: Based on the geometry and position of the space mining vehicle, the formulas for calculating the turning angles of the front and rear wheels at the left and right ends are as follows: The turning radius of the space mining vehicle is R. t The deflection angles of the front and rear wheels at the left and right ends are δ1, δ2, δ3, and δ4, respectively. The position of the center of the space mining vehicle chassis in this coordinate system is S(x,y). The steering azimuth angle of the vehicle body is α. The distance between the front and rear axles is l. The width of the vehicle body is d.

5. The trajectory tracking algorithm according to claim 4, characterized in that, In step two: The wheel dynamics analysis model, assuming all six wheels of the space mining vehicle are in pure rolling conditions, yields the following constraint equations:

6. The trajectory tracking algorithm according to claim 1, characterized in that, In step three, the space mining vehicle system is a nonholonomically constrained system. According to the Routh equation for nonholonomically constrained systems, its dynamic equation is: Where, q j Q j Let λ represent the generalized coordinates and generalized forces of the space mining vehicle; T represent the kinetic energy of the space mining vehicle; λ represent the kinetic energy of the space mining vehicle. k Powering the space mining vehicle; B kj The coefficients are determined by the constraints; Among them, B kj for: B k1 =sinδ k ,k=1~6 b 12 =l1cos(β1-δ1),B 22 =l2cos(β2),B 32 =l3cos(β3-δ2) B 42 =-l4cos(β4-δ3),B 52 =-l5cos(β5),B 62 =-l6cos(β6-δ4) B kj =-r,k=1~6,j=k+2。 7. The trajectory tracking algorithm according to claim 1, characterized in that: The overall kinetic energy of the space mining vehicle system consists of three parts: the movement of the vehicle body itself, its own rotation, and the rotation of its six wheels, represented as follows: Where m represents the mass of the space mining vehicle; J represents the moment of inertia of the space mining vehicle; J w This represents the moment of inertia of the space mining vehicle's wheels; v represents the speed of the space mining vehicle. During its operation on the asteroid surface, the space mining vehicle traverses alternating layers of soft soil and hard rock, thus its potential energy is defined as: V=mg a From Among them, g a This represents the gravitational acceleration at the asteroid. With generalized coordinates q j The corresponding generalized force Q j for: Q j =T ai ,j=3~8,i=1~6 Among them, T i The driving torque for wheel i.

8. The trajectory tracking algorithm according to claim 1, characterized in that, In step two When establishing the overall dynamic analysis model of the space mining vehicle system, the force analysis of the space mining vehicle is performed, and the dynamic equations are as follows: We can obtain: Using the previously obtained T Ri From the equations, we can obtain the dynamic equations of the space mining vehicle as follows:

9. The trajectory tracking algorithm according to claim 1, characterized in that, In step three The SSA-PID control model specifically includes: treating the PID control parameters as a sparrow population, categorizing them into three types: discoverers, followers, and watchdogs; setting the number of sparrows in the population as n; and setting the dimension of the variable to be optimized as d based on the number of PID control parameters. Then, the population of n sparrows can be represented as: The fitness values ​​of all sparrows are represented as follows: In the sparrow search algorithm, the position of the discoverer is updated in each iteration, as described below: Where t represents the current iteration parameter, j = 1, 2, 3, ..., d represents the dimension of the problem being computed, and item max x is a constant representing the maximum number of iterations. i,j Let represent the position information of the i-th sparrow in the j-th dimension, α, α∈(0,1] is a random number, R2, R2∈[0,1] and ST, ST∈[0.5,1] represent the warning value and the safety value respectively, Q is a random number that follows a normal distribution, and L represents a 1×d matrix in which each element has a value of 1; The SSA-PID control method includes the following steps: 1) Randomly generate the PID parameter K of the space mining vehicle group P K I K D The control parameters are then substituted into the fitness function to calculate the target fitness value; 2) Determine whether the calculated fitness value exceeds the maximum number of iterations. If yes, output the optimal solution for the parameters. If no, arrange the parameters according to the fitness value. 3) Select the top 20% of PID parameters with the best fitness values ​​as discoverers, and update the position of the discoverers according to the discoverer formula; 4) Select the bottom 80% of PID parameters with the worst fitness values ​​as followers, and update the position of the followers according to the follower formula; 5) Randomly select 10%-20% of the PID parameters as vigilants, and update the vigilant's position according to the vigilant formula; 6) Update the PID parameters of each group to the optimal solution based on the fitness value, and compare the updated PID parameters of n groups, retaining the optimal solution; 7) Re-evaluate the calculated optimal solution and determine whether the calculated fitness value exceeds the maximum number of iterations. If yes, output the optimal solution. If no, return to step three to recalculate until the optimal solution can be output.

Images