Six-degree-of-freedom mechanical arm collision-free path planning method based on physical simulation

Abstract

The invention particularly relates to a six-degree-of-freedom mechanical arm collision-free path planning method based on physical simulation, and the method comprises the steps: employing a cascade optimization + random disturbance inverse kinematics solving strategy, integrating the advantages of L-BFGS-B, SLSQP and Powell algorithms, and effectively improving the solving success rate and stability under complex constraints and extreme poses; a smooth track with zero starting and stopping speeds and zero acceleration is generated through a quintic polynomial, so that impact, vibration and mechanical wear are reduced, and the control precision is improved; high-precision collision detection and penetration depth analysis are realized based on MuJoCo, the universality is high, and obstacle avoidance is reliable; and through a lossless rehearsal and state rollback mechanism before action execution, the system safety and the planning result reliability are obviously enhanced.

Description

Technical Field

[0001] This invention relates to the field of robotic arm path planning technology, specifically to a collision-free path planning method for a six-degree-of-freedom robotic arm based on physical simulation. Background Technology

[0002] With the rapid development of space exploration technology, multi-degree-of-freedom robotic arms are widely used in on-orbit servicing (such as satellite capture and repair). These tasks typically require the robotic arm to complete a series of precise movements from an initial state to a target pose, following a specific execution sequence. To ensure mission safety and execution efficiency, motion control and trajectory planning technologies based on physical simulation environments (such as MuJoCo) have become core components of robot development. By performing closed-loop verification of "perception-planning-execution" in a simulation environment, the risk of collisions with physical robots can be effectively avoided, and control parameters can be optimized.

[0003] In the motion control of robotic arms, inverse kinematics (IK) solutions and collision-free trajectory planning are two of the most critical technical challenges. Currently, existing IK solution methods are mainly divided into analytical and numerical methods. While analytical methods are computationally fast, they rely on specific mechanical structures and have poor versatility. Numerical methods, based on Jacobian matrices or optimization algorithms, are applicable to robotic arms of various configurations. To achieve collision-free trajectory planning, existing fundamental techniques for path planning problems mostly employ polynomial interpolation methods, such as linear or cubic polynomial interpolation; while for collision detection, traditional methods are usually performed at the geometric level, using simplified models such as axis-aligned bounding boxes (AABBs) or orientation bounding boxes (OBBs) for intersection testing.

[0004] The aforementioned existing technologies have solved the basic problems of robotic arm control to a certain extent, but when faced with complex tasks requiring high precision and high reliability (such as fine operations in on-orbit servicing scenarios), the following main defects and shortcomings still exist: (1) Existing numerical IK solvers usually only use a single optimization algorithm (such as simple gradient descent or Newton's method), which is very easy to get trapped in local minima, causing the algorithm to converge at the wrong position or directly report no solution, and cannot calculate the accurate joint angle, resulting in poor robustness.

[0005] (2) Many existing trajectory planning schemes only use linear interpolation or cubic polynomial interpolation, resulting in insufficient smoothness of the trajectory planning curve and problems such as mechanical shock and jitter.

[0006] (3) Many existing path planning algorithms use simplified geometric bounding boxes for collision prediction. This simplified model often deviates from the actual physical model and is prone to misjudgment. In addition, the traditional planning process often lacks deep coupling with the state of the physical simulation engine, making it difficult to directly utilize the precise contact mechanics calculations at the engine's underlying level during the planning stage.

[0007] It should be noted that the information disclosed in the background section above is only used to enhance the understanding of the background of the present invention, and therefore may include information that does not constitute prior art known to those skilled in the art. Summary of the Invention

[0008] This invention provides a collision-free path planning method for a six-degree-of-freedom robotic arm based on physical simulation, a computer-readable storage medium, and a computer program product, which can effectively overcome the defects existing in the prior art.

[0009] Other features and advantages of the invention will become apparent from the following detailed description, or may be learned in part by practice of the invention.

[0010] According to a first aspect of the present invention, a collision-free path planning method for a six-degree-of-freedom robotic arm based on physical simulation is provided, the method comprising: Obtain the current joint angles of the robotic arm, the physical limit constraints of each joint, and the current and target positions of the robotic arm's end effector; Based on the inverse kinematics algorithm of cascaded strategy and random perturbation, the target joint angle corresponding to the movement of the end effector of the robotic arm to the target position is solved according to the current position and the target position, combined with the physical limit constraints of each joint. Using a polynomial interpolation algorithm, candidate motion trajectories of the robotic arm are generated based on the current joint angle and the target joint angle. Candidate motion trajectories are loaded into a physical simulation environment for motion pre-play, and collision detection is performed on the candidate motion trajectories based on the contact information returned by the physical simulation environment to obtain the detection results; When the candidate motion trajectory is determined to meet the collision-free requirement based on the collision detection results, the candidate motion trajectory is used as the collision-free path planning result of the robotic arm.

[0011] In some exemplary embodiments, the inverse kinematics algorithm based on cascaded strategies and random perturbations, according to the current position and the target position, and in combination with the physical constraints of each joint, solves for the target joint angle corresponding to the movement of the robotic arm end effector to the target position, including: Construct an inverse kinematics objective function based on the current position and target position of the robotic arm's end effector; Following the cascaded solution mechanism of layered relay, under the physical constraints of each joint, multiple optimization algorithms with different search characteristics are sequentially called to solve the inverse kinematics objective function in a cascaded manner, and the inverse kinematics solution result is obtained. When the inverse kinematics solution does not meet the preset solution requirements, the current joint angle of the robotic arm is randomly perturbed to generate new initial joint values. The cascade solution is re-executed based on the new initial joint values ​​until the inverse kinematics solution meets the preset solution requirements, thus obtaining the target joint angle.

[0012] In some exemplary embodiments, the cascaded solution mechanism following a hierarchical relay, under the physical constraints of each joint, sequentially calls multiple optimization algorithms with different search characteristics to cascade the inverse kinematics objective function to obtain the inverse kinematics solution result, including: A finite-memory quasi-Newton method is used to initially solve the inverse kinematics objective function, obtaining a fast-converging initial joint angle solution; When the joint angle solution does not meet the preset solution requirements, the sequential least squares programming algorithm is used to continue to perform constraint optimization on the inverse kinematics objective function to obtain the secondary joint angle solution. When the solution of the second joint angle still fails to meet the preset solution requirements, the conjugate direction algorithm is used to continue the local supplementary search of the inverse kinematics objective function to obtain the inverse kinematics solution result.

[0013] In some exemplary embodiments, generating candidate motion trajectories for the robotic arm using a polynomial interpolation algorithm based on the current joint angles and target joint angles includes: Based on the current joint angles and target joint angles of the robotic arm, determine the position boundary conditions, velocity boundary conditions, and acceleration boundary conditions for each joint at the start and end times. For each joint, a joint position polynomial trajectory function is constructed, and the joint position polynomial trajectory function is solved based on the position boundary conditions, velocity boundary conditions, and acceleration boundary conditions. Based on the solved joint position polynomial trajectory function, the joint velocity function and joint acceleration function of each joint in the planning time domain are determined. The planning time domain is discretized according to the control period to obtain discrete time points; Based on the joint position polynomial trajectory function, joint velocity function and joint acceleration function of each joint in the planning time domain, the joint position, joint velocity and joint acceleration corresponding to the discrete time are calculated respectively to obtain the joint position sequence, joint velocity sequence and joint acceleration sequence. Candidate motion trajectories for the robotic arm are generated based on the joint position sequence, joint velocity sequence, and joint acceleration sequence.

[0014] In some exemplary embodiments, the collision detection of candidate motion trajectories based on contact information returned from the physical simulation environment to obtain detection results includes: Before performing collision detection on candidate motion trajectories, a copy of the current physical state of the physical simulation environment is made; the current physical state includes: the current joint angle of the robotic arm and the current position of the robotic arm end effector; Load the trajectory points corresponding to the candidate motion trajectory into the physical simulation environment for motion pre-play, and call the geometric calculation interface of the physics engine to obtain contact information; Based on the contact information, it iterates through the contact objects and contact states corresponding to each contact pair, and then uses a contact mask to filter out legitimate contact information based on the contact objects and contact states. Collision detection is performed on the filtered contact information to obtain the collision detection result; Based on the collision determination results, the detection results corresponding to the candidate motion trajectories are obtained.

[0015] In some exemplary embodiments, the step of performing collision determination on the filtered contact information to obtain a collision determination result includes: When the contact pair is the robotic arm and the target satellite, if the contact state indicates that there is penetration between the two, or if there is no penetration between the two but the distance is less than the first preset threshold, it is determined that the robotic arm and the target satellite have collided. When the contact pair is between robotic arm components, if the contact state indicates that the penetration depth between the two is greater than the second preset threshold, it is determined that a collision has occurred between the robotic arm components. When the contact pair is the robotic arm and the service satellite, if the contact status in strict mode indicates that the penetration depth of the corresponding contact pair is greater than the second preset threshold, or if the contact status in normal mode indicates that the penetration depth of the corresponding contact pair is greater than the third preset threshold, it is determined that the robotic arm and the service satellite have collided. When none of the contact pairs are determined to have collided, the candidate motion trajectory is determined to meet the collision-free requirement.

[0016] In some exemplary embodiments, the inverse kinematics objective function is the square of the Euclidean distance between the current position and the target position of the robotic arm end effector.

[0017] According to a second aspect of the present invention, a computer-readable storage medium is provided, the computer-readable storage medium including a stored executable program, wherein, when the executable program is running, the device where the storage medium is located executes the above-described collision-free path planning method for a six-degree-of-freedom robotic arm based on physical simulation.

[0018] According to a third aspect of the present invention, a computer program product is provided, comprising a computer program that, when executed by a processor, implements the above-described collision-free path planning method for a six-degree-of-freedom robotic arm based on physical simulation.

[0019] According to a fourth aspect of the present invention, an electronic device is provided, comprising: Processor; and Memory for storing the executable instructions of the processor; The processor is configured to implement the above-described collision-free path planning method for a six-degree-of-freedom robotic arm based on physical simulation when executing the executable instructions.

[0020] The embodiments of this invention provide a collision-free path planning method for a six-DOF robotic arm based on physical simulation. Firstly, in the inverse kinematics solution stage, a progressive mechanism of "gradient-based fast algorithm priority, complex constraint algorithm relay, and random perturbation restart compensation" is adopted. This comprehensively utilizes the advantages of algorithms such as L-BFGS-B, SLSQP, and Powell in convergence speed, constraint handling, and derivative-free search, effectively overcoming the problems of single algorithms easily getting trapped in local optima and high failure rates in complex scenarios, thus significantly improving the success rate and stability of target joint angle solutions. Secondly, in the trajectory generation stage, by applying zero constraints to the velocity and acceleration at the start and end times, a smooth trajectory with continuously changing position, velocity, and acceleration is generated using a fifth-order polynomial. This reduces rigid impacts and residual vibrations during the robotic arm's movement, improves end-effector accuracy, and reduces joint module wear. Thirdly, the high-precision collision detection capability of the MuJoCo physical simulation engine is directly utilized. Through virtual pre-simulation and penetration depth analysis, accurate obstacle avoidance of complex-shaped obstacles is achieved, avoiding the need to reconstruct complex geometric collision models. Finally, through lossless pre-performance and state rollback mechanisms before action execution, safety verification is completed without changing the current system state. Only trajectories that pass collision detection are executed, further improving the system's safety, reliability, and engineering practicality. This solves the technical problems of six-degree-of-freedom robotic arms easily getting trapped in local extrema, having unsmooth trajectory planning, and inaccurate collision detection in existing technologies.

[0021] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and are not intended to limit the invention. Attached Figure Description

[0022] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention. It is obvious that the drawings described below are merely some embodiments of the invention, and those skilled in the art can obtain other drawings based on these drawings without any inventive effort.

[0023] Figure 1 This schematically illustrates a flowchart of a collision-free path planning method for a six-degree-of-freedom robotic arm based on physical simulation, an exemplary embodiment of the present invention. Figure 2 This schematic diagram illustrates the robotic arm and the positions of each joint in an exemplary embodiment of the present invention. Figure 3 This schematic diagram illustrates the specific process of implementing the hierarchical relay cascade solution mechanism in an exemplary embodiment of the present invention. Figure 4 The diagram illustrates the overall process of a collision-free path planning method for a six-degree-of-freedom robotic arm based on physical simulation, as an exemplary embodiment of the present invention. Figure 5 This diagram illustrates a procedure operation guide for an exemplary embodiment of the present invention. Figure 6 This schematic diagram illustrates the operation of the program in an exemplary embodiment of the present invention under the condition of successful planning; Figure 7 This schematic diagram illustrates the downward operation of the program in an exemplary embodiment of the present invention when the crash test fails. Figure 8 The schematic diagram illustrates the motion trajectory of the robotic arm's front end in a 3D visualization window, as exemplified by an embodiment of the present invention. Figure 9 This schematic diagram illustrates the change process of the angles of six joints during movement in an exemplary embodiment of the present invention. Figure 10 The diagram illustrates the composition of an electronic device according to an exemplary embodiment of the present invention. Detailed Implementation

[0024] Exemplary embodiments will now be described more fully with reference to the accompanying drawings. However, these exemplary embodiments can be implemented in many forms and should not be construed as limited to the examples set forth herein; rather, they are provided so that the invention will be more comprehensive and complete, and will fully convey the concept of the exemplary embodiments to those skilled in the art. The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments.

[0025] Furthermore, the accompanying drawings are merely illustrative of the invention and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and therefore repeated descriptions of them will be omitted. Some block diagrams shown in the drawings are functional entities and do not necessarily correspond to physically or logically independent entities. These functional entities can be implemented in software, in one or more hardware modules or integrated circuits, or in different network and / or processor devices and / or microcontroller devices.

[0026] To address the shortcomings and deficiencies of existing technologies, this example implementation provides a collision-free path planning method for a six-degree-of-freedom robotic arm based on physical simulation. (Reference) Figure 1 As shown, it can specifically include: Step S10: Obtain the current joint angle of the robotic arm, the physical limit constraints of each joint, and the current position and target position of the robotic arm end effector; Step S12: Based on the inverse kinematics algorithm of cascaded strategy and random perturbation, the target joint angle corresponding to the movement of the end effector of the robotic arm to the target position is solved according to the current position and the target position, and combined with the physical limit constraints of each joint. Step S14: Using a polynomial interpolation algorithm, candidate motion trajectories of the robotic arm are generated based on the current joint angle and the target joint angle. Step S16: Load the candidate motion trajectory into the physical simulation environment for motion pre-play, and perform collision detection on the candidate motion trajectory based on the contact information returned by the physical simulation environment to obtain the detection result; Step S18: When the candidate motion trajectory meets the collision-free requirement based on the collision detection result, the candidate motion trajectory is used as the collision-free path planning result of the robotic arm.

[0027] The following will describe in more detail each step of a collision-free path planning method for a six-degree-of-freedom robotic arm based on physical simulation in this example embodiment, with reference to the accompanying drawings and embodiments.

[0028] For example, refer to Figure 2 As shown, Figure 2 This is a schematic diagram of the robotic arm and the positions of its joints. In step S10, the joints of the robotic arm include: joint 1 - shoulder rotation joint, joint 2 - shoulder elevation joint, joint 3 - elbow joint, joint 4 - wrist 1 joint, joint 5 - wrist 2 joint, and joint 6 - wrist 3 joint.

[0029] The physical limit constraints of each joint are the range of joint angle values ​​allowed under the actual mechanical structure, driving capacity and safe operation requirements.

[0030] Specifically, this method relies on a physical simulation environment, such as MuJoCo. When constructing a simulation model environment with physical properties, the system needs to load model files containing robotic arm links, joints, end effectors, and environmental obstacles. The joint state vectors of the robotic arm are then defined. This involves determining the current joint angles of the six joints of the robotic arm and reading the physical limit range of each joint; establishing a world coordinate system with the robotic arm base as the origin, and obtaining the current position of the robotic arm's end effector. And set the target location corresponding to the target task point. .in, This represents the current position of the end effector calculated after substituting the current joint angles of each joint into the forward kinematic model.

[0031] For example, in step S12, the inverse kinematics algorithm based on cascaded strategy and random perturbation, according to the current position and target position, and in combination with the physical constraints of each joint, solves the target joint angle corresponding to the movement of the robotic arm end effector to the target position, including: Step S121: Construct an inverse kinematics objective function based on the current position and target position of the robotic arm end effector; Specifically, the inverse kinematics objective function is the square of the Euclidean distance between the current position and the target position of the robotic arm's end effector, as shown in the following equation:

[0032] in, Current position The target location.

[0033] Step S122: According to the cascaded solution mechanism of the layered relay, under the physical constraints of each joint, multiple optimization algorithms with different search characteristics are called in sequence to solve the inverse kinematics objective function in a cascaded manner to obtain the inverse kinematics solution result. Step S123: When the inverse kinematics solution does not meet the preset solution requirements, the current joint angle of the robotic arm is randomly perturbed to generate new initial joint values. Step S124: Based on the new initial joint values, the cascade solution is re-executed until the inverse kinematics solution results meet the preset solution requirements, and the target joint angle is obtained.

[0034] Specifically, under joint constraint, by combining multi-algorithm relay and random perturbation restart, the error between the end effector position and the target position is continuously reduced, and finally the target joint angle that enables the robotic arm end effector to reach the target position is determined. First, an inverse kinematics objective function is constructed based on the current position and target position of the robotic arm end effector. The inverse kinematics objective function is used to characterize the positional error between the current position and the target position of the robotic arm end effector under a given joint angle vector.

[0035] Then, following a cascaded solution mechanism with layered relays, under the physical constraints of each joint, multiple optimization algorithms with different search characteristics are sequentially invoked to solve the inverse kinematics objective function, yielding the inverse kinematics solution. The physical constraints of each joint limit the range of joint angles, ensuring that the inverse kinematics solution process always occurs within the actual executable joint space of the robotic arm, thus avoiding infeasible solutions exceeding the allowable range of the mechanical structure. By employing multiple optimization algorithms with different search characteristics in a layered relay solution, the advantages of different optimization algorithms in convergence speed, constraint handling capability, and local search capability can be comprehensively utilized, improving the robustness and success rate of the inverse kinematics solution.

[0036] Next, when the inverse kinematics solution does not meet the preset solution requirements, the current joint angles of the robotic arm are randomly perturbed to generate new initial joint values. The preset solution requirements can be set as follows: the inverse kinematics objective function value is less than a preset error threshold, or the distance between the current position of the robotic arm's end effector and the target position is less than a preset position error threshold. Random perturbation refers to applying random changes to one or more joint angles based on the current joint angles to form new initial solution values, thereby changing the initial search starting point of the optimization algorithm and preventing the solution process from getting trapped in local optima.

[0037] Finally, the cascade solution is re-executed based on the new initial joint values ​​until the inverse kinematics solution meets the preset requirements, thus obtaining the target joint angle. If the first cascade solution fails to obtain a joint angle solution that meets the requirements, the system does not terminate directly. Instead, it constructs a new initial joint configuration through random perturbation and calls the cascade solution mechanism again to search again until the target joint angle that enables the robotic arm end effector to reach the target position and satisfies the physical limit constraints of each joint is obtained.

[0038] In this way, on the one hand, the problem of the end effector reaching the target position can be transformed into the problem of minimizing the inverse kinematics error. On the other hand, the combination mechanism of cascaded solution and random perturbation restart can improve the adaptability of inverse kinematics solution to complex target positions and complex joint constraints, thereby improving the accuracy, stability and executability of target joint angle solution, and providing reliable joint target state for subsequent candidate motion trajectory generation and collision-free path planning.

[0039] For example, in step S122, according to the cascaded solution mechanism of layered relay, under the physical constraints of each joint, multiple optimization algorithms with different search characteristics are sequentially called to cascade the solution of the inverse kinematics objective function to obtain the inverse kinematics solution result, including: Step S1221: The finite memory quasi-Newton method algorithm is used to initially solve the inverse kinematics objective function to obtain the fast-converging initial joint angle solution. Step S1222: When the joint angle solution does not meet the preset solution requirements, the sequential least squares programming algorithm is used to continue to perform constraint optimization on the inverse kinematics objective function to obtain the secondary joint angle solution. Step S1223: When the solution of the second joint angle still does not meet the preset solution requirements, the conjugate direction algorithm is used to continue the local supplementary search of the inverse kinematics objective function to obtain the inverse kinematics solution result.

[0040] For details, please refer to Figure 3 As shown, Figure 3 This diagram illustrates the specific process of implementing the hierarchical relay cascaded solution mechanism. The system sequentially employs three optimization algorithms with increasing computational cost and robustness.

[0041] (1) Preferred strategy: L-BFGS-B (Limited Memory Quasi-Newton Method). Rapid iteration is performed using the gradient information of the inverse kinematics objective function, while strictly constraining the solution within the physical limits of the joints; (2) Alternative strategies: If the preferred strategy fails to converge or the residual is too large, the system will automatically switch to the SLSQP (Sequential Least Squares Programming) method to find a feasible solution by handling more complex constraints. (3) Final strategy: If all the above methods fail, switch to the Powell conjugate direction method. This method is a derivative-free algorithm that does not rely on gradient information and can effectively handle non-smooth objective functions near singular points.

[0042] (4) Random perturbation re-search: If the solution obtained by the above cascaded strategy still has a large position error (e.g., greater than 1 mm triggers the re-search logic), then random noise following a normal distribution is superimposed with the current joint angle as the center. Generate new initial guess values The optimization process is then re-executed, repeating the process a maximum of a preset number of times (e.g., 10 times). This mechanism leverages initial value sensitivity to help the algorithm escape local minima.

[0043] (5) Minimum stroke optimization: based on joints The periodicity of rotation can be obtained through The form of the solution The solution is processed from various angles to obtain the solution that is closest to the absolute rotation angle of the current posture, thus avoiding unnecessary large-amplitude rotation of the robotic arm.

[0044] For example, in step S14, generating candidate motion trajectories for the robotic arm using a polynomial interpolation algorithm based on the current joint angles and target joint angles includes: Step S141: Based on the current joint angle and target joint angle of the robotic arm, determine the position boundary conditions, velocity boundary conditions and acceleration boundary conditions of each joint at the start and end times. Step S142: Construct a joint position polynomial trajectory function for each joint, and solve the joint position polynomial trajectory function according to the position boundary conditions, velocity boundary conditions and acceleration boundary conditions; Step S143: Based on the solved joint position polynomial trajectory function, determine the joint velocity function and joint acceleration function of each joint in the planning time domain. Step S144: Discretize the planning time domain according to the control cycle to obtain discrete time intervals; Step S145: Based on the joint position polynomial trajectory function, joint velocity function and joint acceleration function of each joint in the planning time domain, calculate the joint position, joint velocity and joint acceleration corresponding to the discrete time, respectively, to obtain the joint position sequence, joint velocity sequence and joint acceleration sequence. Step S146: Generate candidate motion trajectories for the robotic arm based on the joint position sequence, joint velocity sequence, and joint acceleration sequence.

[0045] Specifically, a fifth-order polynomial interpolation algorithm is used to generate a smooth trajectory. The specific process is as follows: First, the robotic arm is set at the initial time... and termination time The six kinematic boundary conditions: Location boundary conditions: ; Velocity boundary conditions: (Start, stop, stand still); Acceleration boundary conditions: (Shockless start and stop).

[0046] Then, construct the joint position polynomial trajectory function. Based on the above six boundary conditions, a system of linear equations is constructed, and the coefficient vector is analytically obtained. Using the solved fifth-degree polynomial Determine the joint velocity function and joint acceleration function of each joint in the planning time domain.

[0047] Finally, time T is discretized according to the control period dt. Based on the joint position polynomial trajectory function, joint velocity function and joint acceleration function of each joint in the planning time domain, the joint position, joint velocity and joint acceleration corresponding to the discrete time are calculated respectively to obtain continuous joint position sequence, velocity sequence and acceleration sequence, and generate a smooth trajectory with "S" shaped velocity curve.

[0048] For example, in step S16, the collision detection of the candidate motion trajectory based on the contact information returned by the physical simulation environment, and the resulting detection, includes: Step S161: Before performing collision detection on the candidate motion trajectory, copy and back up the current physical state of the physical simulation environment; wherein, the current physical state includes: the current joint angle of the robotic arm and the current position of the robotic arm end effector; Step S162: Load the trajectory points corresponding to the candidate motion trajectory into the physical simulation environment for motion pre-play, and call the geometric calculation interface of the physics engine to obtain contact information; Step S163: Based on the contact information, traverse the contact objects and contact states corresponding to each contact pair, and based on the contact objects and contact states, use a contact mask to filter out legitimate contact information. Step S164: Perform collision determination on the filtered contact information to obtain the collision determination result; Step S165: Based on the collision determination result, obtain the detection result corresponding to the candidate motion trajectory.

[0049] Specifically, to address the insufficient accuracy of traditional detection methods based on geometric bounding boxes, this invention utilizes a physics engine at the underlying level for precise contact mechanics pre-simulation. The specific process is as follows: (1) Status snapshot protection: Before planning the path, copy and back up the current physical state of the simulation environment (including all joint positions and angle information).

[0050] (2) Virtual positioning detection: The simulation model is forcibly set to the calculated target joint angle. It calls the geometry calculation interface of the physics engine, but does not advance the simulation time steps.

[0051] (3) Contact Filtering and Judgment: Traverse the list of contact pairs returned by the physics engine and use contact masks (such as contype and conaffinity) to filter out legitimate contacts such as contact between adjacent links inside the robotic arm. Perform collision detection on each trajectory point generated by interpolation. The collision judgment criteria vary depending on the type and contact method of the colliding parties, and some criteria can be selected by the user. Filter the planning results in turn to find collision-free solutions.

[0052] (4) Lossless rollback of state: Regardless of the detection result, the physical state backed up in the first step is written back to the simulation memory to prevent the loss of the initial state and ensure that the collision detection process does not interfere with the continuous operation of the system.

[0053] For example, in step S164, the collision determination of the filtered contact information to obtain the collision determination result includes: Step S1641: When the contact pair is the robotic arm and the target satellite, if the contact state indicates that there is penetration between the two, or if there is no penetration between the two but the distance is less than the first preset threshold, it is determined that the robotic arm and the target satellite have collided. Step S1642: When the contact pair is between robotic arm components, if the contact state indicates that the penetration depth between the two is greater than the second preset threshold, it is determined that a collision has occurred between the robotic arm components. Step S1643: When the contact pair is the robotic arm and the service satellite, if the contact state in strict mode indicates that the penetration depth of the corresponding contact pair is greater than the second preset threshold, or if the contact state in normal mode indicates that the penetration depth of the corresponding contact pair is greater than the third preset threshold, it is determined that the robotic arm and the service satellite have collided. Step S1644: When none of the contact pairs are determined to have collided, the candidate motion trajectory is determined to meet the collision-free requirement.

[0054] The specific criteria are detailed in the table below: Table 1 Criteria for Determining Collision Occurrence

[0055] If any point of contact is determined to be a "collision," the joint configuration or trajectory is deemed infeasible. Penetration depth refers to how far two objects "press into" each other when they have geometrically overlapped in the simulation; it can be understood as "clipping" between them. Assuming object 1 is the reference, and object 2 undergoes relative motion, the penetration depth... d When the two overlap, the distance between the surface through which object 1 is penetrated and the deepest point reached by object 2 is the distance.

[0056] The method provided in the embodiments of the present invention is referred to Figure 4 As shown, Figure 4This is a schematic diagram illustrating the overall process of the method provided by the present invention. The invention will now be described in further detail with reference to the accompanying drawings and specific embodiments, using a six-degree-of-freedom robotic arm simulation system built on the MuJoCo physics engine as an example.

[0057] System Architecture and Initialization: The control system in this embodiment mainly includes the following functional modules: Environment Construction Module: Used to load XML model files containing the robotic arm, end effector, and environmental obstacles (such as target satellites), and initialize the physics engine data structure. State Awareness Module: Real-time reading of joint angles, end effector Cartesian coordinates, and contact force feedback in the simulation environment. Planning and Calculation Module: Includes an inverse kinematics solver and a trajectory generator. Safety Verification Module: A collision detector based on the physics engine kernel. The method of this invention mainly includes the following steps: Step S1: Target setting and status acquisition The system first obtains the current joint state of the robotic arm. Users input the Cartesian coordinates of the target location through the interactive interface. Alternatively, you can select a target point from a pre-defined list of task points (which can be customized). See reference. Figure 5 As shown, Figure 5 This is a schematic diagram of the program's operation guide. The guide demonstrates how to operate the program after integrating all the modules.

[0058] This diagram illustrates the terminal interface of a space robotic arm control simulation system built using MuJoCo 3.2.5 and Python 3.11.7. The interface provides system function descriptions and user operation guidelines in text format, serving as the human-computer interaction point for driving the robotic arm's path planning and simulation. The interface is mainly divided into three parts: 1. System Title Area: The top displays the system name and operating environment: Space Robotic Arm Control Simulation; Operating Platform: MuJoCo 3.2.5 / Python 3.11.7. This indicates that the system is built based on a physics simulation engine and has dynamics and collision detection capabilities.

[0059] 2. User Guide Area: The middle section contains the operating instructions, listing the main functions supported by the system: (1) Preset target point: Supports input of predefined target points, such as: ['O', 'A', 'B', 'C', 'a1', 'a2', 'b', 'c', 'e'] and prompts for case sensitivity, which is used to quickly call the target location in typical task scenarios.

[0060] (2) Custom target point: Input format: x, y, z. An example '0.65,0,2' is provided, and it is suggested that commas be used to separate the values. Users can directly specify the target point in three-dimensional space to achieve flexible control.

[0061] (3) Trajectory playback: Input replay to replay the historical trajectory (movie-style display) for analyzing path planning results and motion process.

[0062] (4) Clear records: Enter clear to delete all trajectory records and reset the experimental environment.

[0063] (5) Collision test: Enter test to start the collision detection function, which corresponds to the physical simulation collision detection module.

[0064] (6) Exit the program: Enter q; at the same time, the joint angle change curve and time series image are automatically generated. Result analysis and visualization are supported.

[0065] (7) Path visualization: The red sphere represents the motion path, which is used to intuitively show the movement process of the robotic arm.

[0066] 3. Input prompt area The bottom displays the current interaction status: Please enter the next target point: a1, indicating that the system is currently waiting for the user to enter the target point; in the example, the user has already entered the target point a1, and the system will perform path planning and execution based on this target point.

[0067] Step S2: Solving the inverse kinematics of the multi-strategy approach The system calls the solve_ik module to calculate the corresponding target joint angle. To address the problem of numerical optimization easily getting trapped in local minima, the following cascaded strategy is adopted: (1) Define the objective function: construct the cost function That is, to minimize the squared Euclidean distance between the current end position and the target position.

[0068] (2) First-level solution (fast optimization): Using the current joint angle as the initial value, the L-BFGS-B algorithm is used for solution. This algorithm utilizes gradient information to converge quickly and strictly adheres to the physical constraints of the joint.

[0069] (3) Second-level solution (constraint enhancement): If the first-level solution fails (the residual is greater than the threshold, such as 1mm), it will automatically switch to the SLSQP (Sequential Least Squares Programming) algorithm and try again using its powerful constraint processing capabilities.

[0070] (4) Third-level solution (global fallback): If the second-level solution still fails, switch to the Powell algorithm. This algorithm is a derivative-free method that does not rely on gradient information and can effectively escape the traps near singular points.

[0071] (5) Random perturbation retry: If none of the above three strategies converge, the system enters a retry loop (the number of retryes shall not exceed a limit, which can be customized). Each time, Gaussian random noise or uniformly distributed noise is superimposed on the current joint angle as a new initial guess value, and the optimization process is re-executed.

[0072] Step S3: Generating a smooth trajectory based on a fifth-order polynomial Obtain the starting point and target point Then, a continuous trajectory is generated using a fifth-order polynomial interpolation algorithm.

[0073] Step S4: Pre-simulated collision detection Before executing the trajectory, the system performs a virtual rehearsal of each generated waypoint: (1) State backup: Deep copy of the current physical state in memory.

[0074] (2) Forward kinematics calculation: Force the joint positions of the simulation model to be set to the values ​​of the path points, and call mj_ki-nematics to update the spatial position of the geometry.

[0075] (3) Contact detection: Traverse the contact list of the physics engine. Use contact masks to filter out legal contacts of the internal links of the robotic arm. The remaining contacts are judged and eliminated one by one according to the criteria in Table 1 until the collision detection of the entire trajectory is completed.

[0076] (4) State rollback: After the detection is completed, the backed-up physical state is restored immediately. If a collision risk is detected on the path, the system will automatically trigger the replanning logic (such as changing the initial value of IK or adjusting the intermediate transition points) to find a collision-free trajectory planning solution again.

[0077] refer to Figure 6 As shown, Figure 6 This is a schematic diagram illustrating the program's operation under the condition of successful planning.

[0078] "First search successful" indicates that a feasible solution was obtained in the first search. The shortest path solution and the equivalent solution (joint 3 plus 2π) are also provided, showing that the system considers the multiple solutions caused by joint periodicity. The end-path length (0.9026m) and joint motion amplitude (8.123rad) are output to measure the path cost.

[0079] Joint 3 was optimized and adjusted, demonstrating a further optimization process for the joint space. This is the result of step 2. The form of the initially obtained The case study shows that the final solution processed the value of joint 3 from 6.437 to 0.153 rad (optimized to 6.283 rad), resulting in a minimum total motion angle of 8.123 to 1.840 rad for the six joints and a minimum motion trajectory length of 0.9026 m, thus preventing unnecessary 360-degree rotation.

[0080] Subsequently, the system performs rigorous trajectory collision detection, with a focus on collision checks against the target satellite. The first round of rapid detection confirms no collisions, determining the trajectory is executable. Based on this, the system completes trajectory planning and enters the trajectory execution phase.

[0081] During trajectory execution, the system advances step by step according to discrete trajectory points and outputs the end effector position in real time. For example: Track progress 1 / 50 → End position: [0.7953, -0.8172, 0.9471] Track progress 11 / 50 → End position: [0.7437, -0.8244, 0.9453] … Track progress 41 / 50 → End position: [0.0269, -0.5348, 0.9237] Explanation: The robotic arm executes the planned trajectory in a discrete manner, comprising 50 trajectory points, gradually approaching the target position. It eventually enters a stable phase and completes the motion.

[0082] After the exercise is completed, the system outputs key performance indicators: 1. Comparison of target and actual location: Target location: [0, [0.5, 0.9226], actual end position: [8.0 × 10 [6, 0.499999, 0.922596]. This indicates that the actual end position is at the same height as the target position.

[0083] 2. Motion tracking error: 0.000009m, which is about 9 micrometers in error, indicating that the path tracking accuracy is extremely high.

[0084] 3. Cumulative simulation time: 12.14s 4. Trajectory Recording and Visualization: The system records trajectory data; there is currently one trajectory segment available for playback; the path drawing is complete, generating a total of 50 red path points, clearly displaying the motion trajectory.

[0085] refer to Figure 7 As shown, Figure 7This diagram illustrates the program's operation when the collision test fails. It shows the process and output information of the robotic arm performing collision detection on candidate motion trajectories in a physical simulation environment during path planning, specifically including path search results, collision detection process, and trajectory feasibility determination results.

[0086] First, the figure shows "10th search successful", indicating that the system has obtained a set of candidate path solutions through multiple searches, and provides the corresponding end path length (1.4589m) and joint motion amplitude (9.716rad). It also shows the joint optimization results and the final trajectory length information.

[0087] Subsequently, the system performs rigorous trajectory collision detection on the candidate motion trajectories, prioritizing collisions related to the target satellite. During the detection process, the system identifies contact pair information, including the contact between the robotic arm components (such as v_wrist_3_link) and the target satellite base (targetsat_base), and calculates the contact depth information.

[0088] When the detection results indicate that there is penetration between the contact pairs (contact depth of 12.8 mm), the system determines the contact as a collision between the robotic arm and the target satellite, and further marks it as "severe collision with the target satellite," indicating that this type of collision is a direct collision between the robotic arm and the target satellite, which must be avoided.

[0089] Based on the collision detection results, the system determines the candidate trajectory as infeasible and outputs "Trail is infeasible" and "Trail rejected" messages. Simultaneously, the system records the location of the trajectory point where the collision occurred (e.g., the 35th trajectory point) and incorporates constraints to avoid such collisions during subsequent search processes.

[0090] In addition, the system also provides improvement suggestions, including selecting a target point closer to the current position of the robotic arm, selecting a target point farther away from the satellite body, and ensuring that the target point is within the working space of the robotic arm.

[0091] Ultimately, since no trajectory solution satisfying the collision-free constraint was found during multiple searches, the system output a "Automatic search failed" message and suggested that the user change the target point to replan the path.

[0092] This figure clearly demonstrates that the method of the present invention can perform refined collision detection on candidate trajectories based on a physical simulation environment during the path planning process, and promptly determine that the trajectory is infeasible when a collision between the robotic arm and the target satellite is detected, thereby effectively avoiding the execution of dangerous paths and improving the safety and reliability of path planning.

[0093] Step S5: Execution and Feedback Once the trajectory passes safety verification, the system will enter the execution loop: first, it will issue joint position commands point-by-point to the underlying PD controller, and then call `mj_step` to advance the physical simulation time step. During the advancement, the actual position of the end effector will be recorded in real time, and a red trajectory line will be drawn in the 3D visualization window to visually display the motion path. (Reference) Figure 8 As shown, Figure 8 This is a schematic diagram of the movement trajectory of the robotic arm's front end in a 3D visualization window.

[0094] This diagram illustrates the path planning and execution process of a robotic arm moving from the starting point to the ending point in a spatial environment. The robotic arm is positioned on an operating platform, and the target object (such as a target satellite structure) is located in the upper space. The robotic arm must complete the movement task from the starting point to the ending point while avoiding collisions.

[0095] Specifically, the “starting point of the action” marked in the figure is the initial position of the end effector of the robotic arm, corresponding to the initial working point on the operating platform; the “end point of the action” marked in the figure is the target position that the end effector of the robotic arm needs to reach, which is located near the target object.

[0096] The motion trajectory generated by the robotic arm during path planning is discretely marked with red dots, forming a spatial path from the start point to the end point of the action, reflecting the positional changes of the robotic arm's end effector at each discrete moment. Meanwhile, the overall direction of the robotic arm's movement is indicated by arrows in the figure.

[0097] During path planning, the robotic arm determines the changes in joint angles through inverse kinematics and trajectory generation methods. It then performs collision detection and constraint correction on the trajectory using a physical simulation environment, thereby obtaining an executable motion trajectory that satisfies collision-free constraints. This trajectory is a continuous curve in space, effectively avoiding target objects and surrounding structures.

[0098] This figure visually demonstrates that the method of the present invention can generate a smooth and collision-free robotic arm motion trajectory in complex spatial environments, achieving safe and efficient movement from the starting point to the target position.

[0099] After execution, the system will automatically generate images showing the changes in the angles of the six joints during the process. (Reference) Figure 9 As shown, Figure 9 This is a schematic diagram illustrating the changes in the angles of the six joints during the movement.

[0100] This figure illustrates the angle changes of the six joints of the robotic arm throughout the entire motion process, under the motion trajectory generated by the described collision-free path planning method. The horizontal axis represents the simulation time (in seconds), and the vertical axis represents the angle value of the corresponding joint (in radians).

[0101] Specifically, the figure shows the angle-time variation curves of joints 1 to 6, with each sub-figure corresponding to the motion process of different joints, reflecting the joint motion state of the robotic arm when executing the path planning results. Among them, joints 1 to 3 are mainly used to adjust the overall spatial position of the robotic arm, and their angle changes are relatively large, reflecting the process of the robotic arm moving from the starting position to the target position in space; joints 4 to 6 are mainly used for end effector posture adjustment, and their angle changes are relatively smooth, used to achieve fine adjustment of the end effector posture.

[0102] As can be seen from the figure, the angles of each joint change with time in a continuous and smooth curve, and the joint velocity and acceleration tend to zero at the start and end times, indicating that the generated motion trajectory satisfies the smoothness constraint and avoids the impact phenomenon of the robotic arm during the start and stop phases.

[0103] Furthermore, the total simulation time shown in the figure is 48.59 seconds, corresponding to 3783 data points. This indicates that the trajectory, after time discretization, forms a high-resolution joint motion sequence, thereby ensuring the stability and continuity of trajectory execution. The figure clearly demonstrates that, under the premise of satisfying collision-free constraints, the method of this invention can generate coordinated and smooth continuous motion trajectories for each joint, effectively improving the stability and control accuracy of the robotic arm's motion process.

[0104] Key code implementation explanation: The core logic of this embodiment is implemented using Python language combined with the mujoco library. In the IK solving part, the scipy.optimize.minimize interface is used to dynamically switch between various optimization algorithms; in the collision detection part, the model.geom_conaffinity and data.contact data structures are directly accessed to achieve accurate detection based on the physical layer, rather than simplified geometric bounding box detection.

[0105] The beneficial effects of this invention are as follows: (1) This invention prioritizes the use of gradient-based fast algorithms (L-BFGS-B) to ensure fast response under normal conditions; when encountering complex constraints or gradient failure, it automatically switches to SLSQP, which has stronger constraint handling capabilities, or the derivative-free Powell algorithm; if convergence is still not achieved, random noise is introduced to break the deadlock state caused by local extrema. This progressive strategy effectively avoids the limitations of a single algorithm, enabling the system to maintain a very high success rate and stability when facing extreme poses or complex scenarios.

[0106] (2) This invention uses a fifth-order polynomial to generate a trajectory curve (S-shaped velocity curve) where the position, velocity, and acceleration are all continuously differentiable by setting a strong constraint that the velocity and acceleration are both zero at the start and end times. Because the change in acceleration is a smooth transition, rigid impacts and residual vibrations during the motion process are eliminated. This not only significantly improves the accuracy of the robotic arm's end effector operation, but also greatly reduces the mechanical wear of the joint module, thereby extending the service life of the hardware and reducing long-term maintenance costs.

[0107] (3) This invention directly utilizes the existing high-precision collision detection kernel of the physics simulation engine (MuJoCo), without the need to write additional complex geometric intersection algorithms. By "pre-simulating" the target posture and detecting the penetration depth in a virtual environment, it can handle obstacle avoidance problems of objects with arbitrarily complex shapes. This method eliminates the need for the system to remodel for different obstacles, making it highly versatile; at the same time, the detection accuracy based on the physical layer is much higher than that of the simplified bounding box model, ensuring the absolute safety of the planning results.

[0108] (4) Before performing any action, this invention performs a "lossless pre-simulation" in a simulation environment. By copying the initial state in memory, future actions are verified without changing the current system state. Only after the virtual pre-simulation passes all safety checks is the actual control instruction generated. This mechanism fundamentally eliminates the problem of chaotic state of the robotic arm due to planning errors, greatly improving the safety and reliability of the system.

[0109] It should be noted that the above figures are merely illustrative of the processes included in the method according to exemplary embodiments of the present invention, and are not intended to be limiting. It is readily understood that the processes shown in the above figures do not indicate or limit the temporal order of these processes. Furthermore, it is readily understood that these processes may, for example, be executed synchronously or asynchronously in multiple modules.

[0110] It should be noted that although several modules or units of the device for performing actions have been mentioned in the detailed description above, this division is not mandatory. In fact, according to embodiments of the present invention, the features and functions of two or more modules or units described above can be embodied in one module or unit. Conversely, the features and functions of one module or unit described above can be further divided and embodied by multiple modules or units.

[0111] Figure 10 A schematic diagram of an electronic device suitable for implementing embodiments of the present invention is shown.

[0112] It should be noted that, Figure 10 The electronic device 1000 shown is merely an example and should not be construed as limiting the functionality and scope of use of the embodiments of the present invention.

[0113] like Figure 10 As shown, the electronic device 1000 includes a Central Processing Unit (CPU) 1001, which can perform various appropriate actions and processes based on programs stored in Read-Only Memory (ROM) 1002 or programs loaded from storage section 1008 into Random Access Memory (RAM) 1003. The RAM 1003 also stores various programs and data required for system operation. The CPU 1001, ROM 1002, and RAM 1003 are interconnected via a bus 1004. An Input / Output (I / O) interface 1005 is also connected to the bus 1004. Furthermore, the electronic device 1000 also includes an FPGA device and a System-on-a-Chip (SoC) device.

[0114] The following components are connected to I / O interface 1005: an input section 1006 including a keyboard, mouse, etc.; an output section 1007 including a cathode ray tube (CRT), liquid crystal display (LCD), etc., and speakers, etc.; a storage section 1008 including a hard disk, etc.; and a communication section 1009 including a network interface card such as a LAN (Local Area Network) card, modem, etc. The communication section 1009 performs communication processing via a network such as the Internet. A drive 1010 is also connected to I / O interface 1005 as needed. Removable media 1011, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., are installed on drive 1010 as needed so that computer programs read from them can be installed into storage section 1008 as needed.

[0115] In particular, according to embodiments of the present invention, the processes described below with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of the present invention include a computer program product comprising a computer program carried on a storage medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 1009, and / or installed from removable medium 1011. When the computer program is executed by central processing unit (CPU) 1001, it performs various functions defined in the system of this application.

[0116] Specifically, the aforementioned electronic devices can be airborne intelligent electronic devices.

[0117] It should be noted that the storage medium shown in the embodiments of the present invention can be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. A computer-readable storage medium can be, for example,—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), flash memory, optical fiber, portable compact disc read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In the present invention, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device. In the present invention, a computer-readable signal medium can include a data signal propagated in baseband or as part of a carrier wave, wherein computer-readable program code is carried. Such transmitted data signals can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. The computer-readable signal medium can also be any storage medium other than a computer-readable storage medium, which can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the storage medium can be transmitted using any suitable medium, including but not limited to wireless, wired, etc., or any suitable combination thereof.

[0118] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of the present invention. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram or flowchart, and combinations of blocks in a block diagram or flowchart, may be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0119] The units described in the embodiments of the present invention can be implemented in software or hardware, and the described units can also be located in a processor. The names of these units do not necessarily limit the specific unit itself.

[0120] It should be noted that, as another aspect, this application also provides a storage medium, which may be included in an electronic device or may exist independently without being assembled into the electronic device. The aforementioned storage medium carries one or more programs, which, when executed by an electronic device, cause the electronic device to perform the methods described in the following embodiments. For example, the electronic device may perform... Figure 1 The steps of the method shown.

[0121] In one embodiment, this application provides a computer program product including a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0122] Furthermore, the above figures are merely illustrative of the processes included in the method according to exemplary embodiments of the present invention, and are not intended to be limiting. It is readily understood that the processes shown in the above figures do not indicate or limit the temporal order of these processes. Additionally, it is readily understood that these processes may be executed synchronously or asynchronously, for example, in multiple modules.

[0123] Other embodiments of the invention will readily occur to those skilled in the art upon consideration of the specification and practice of the invention herein. This application is intended to cover any variations, uses, or adaptations of the invention that follow the general principles of the invention and include common knowledge or customary techniques in the art not disclosed herein. The specification and embodiments are to be considered exemplary only, and the true scope and spirit of the invention are indicated by the claims.

[0124] It should be understood that the present invention is not limited to the precise structure described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope. The scope of the invention is limited only by the appended claims.

Claims

1. A collision-free path planning method for a six-degree-of-freedom robotic arm based on physical simulation, characterized in that, The method includes: Obtain the current joint angles of the robotic arm, the physical limit constraints of each joint, and the current and target positions of the robotic arm's end effector; Based on the inverse kinematics algorithm of cascaded strategy and random perturbation, the target joint angle corresponding to the movement of the end effector of the robotic arm to the target position is solved according to the current position and the target position, combined with the physical limit constraints of each joint. Using a polynomial interpolation algorithm, candidate motion trajectories of the robotic arm are generated based on the current joint angle and the target joint angle. Candidate motion trajectories are loaded into a physical simulation environment for motion pre-play, and collision detection is performed on the candidate motion trajectories based on the contact information returned by the physical simulation environment to obtain the detection results; When the candidate motion trajectory is determined to meet the collision-free requirement based on the collision detection results, the candidate motion trajectory is used as the collision-free path planning result of the robotic arm.

2. The method according to claim 1, characterized in that, The inverse kinematics algorithm based on cascaded strategies and random perturbations, according to the current position and target position, and in combination with the physical constraints of each joint, solves for the target joint angle corresponding to the movement of the robotic arm end effector to the target position, including: Construct an inverse kinematics objective function based on the current position and target position of the robotic arm's end effector; Following the cascaded solution mechanism of layered relay, under the physical constraints of each joint, multiple optimization algorithms with different search characteristics are sequentially called to solve the inverse kinematics objective function in a cascaded manner, and the inverse kinematics solution result is obtained. When the inverse kinematics solution does not meet the preset solution requirements, the current joint angle of the robotic arm is randomly perturbed to generate new initial joint values. The cascade solution is re-executed based on the new initial joint values ​​until the inverse kinematics solution meets the preset solution requirements, thus obtaining the target joint angle.

3. The method according to claim 2, characterized in that, The cascaded solution mechanism, following a hierarchical relay approach, sequentially calls multiple optimization algorithms with different search characteristics to solve the inverse kinematics objective function under the physical constraints of each joint, yielding the inverse kinematics solution results, including: A finite-memory quasi-Newton method is used to initially solve the inverse kinematics objective function, obtaining a fast-converging initial joint angle solution; When the joint angle solution does not meet the preset solution requirements, the sequential least squares programming algorithm is used to continue to perform constraint optimization on the inverse kinematics objective function to obtain the secondary joint angle solution. When the solution of the second joint angle still fails to meet the preset solution requirements, the conjugate direction algorithm is used to continue the local supplementary search of the inverse kinematics objective function to obtain the inverse kinematics solution result.

4. The method according to claim 1, characterized in that, The process of generating candidate motion trajectories for the robotic arm using a polynomial interpolation algorithm based on the current joint angles and target joint angles includes: Based on the current joint angles and target joint angles of the robotic arm, determine the position boundary conditions, velocity boundary conditions, and acceleration boundary conditions for each joint at the start and end times. For each joint, a joint position polynomial trajectory function is constructed, and the joint position polynomial trajectory function is solved based on the position boundary conditions, velocity boundary conditions, and acceleration boundary conditions. Based on the solved joint position polynomial trajectory function, the joint velocity function and joint acceleration function of each joint in the planning time domain are determined. The planning time domain is discretized according to the control cycle to obtain discrete time points; Based on the joint position polynomial trajectory function, joint velocity function and joint acceleration function of each joint in the planning time domain, the joint position, joint velocity and joint acceleration corresponding to the discrete time are calculated respectively to obtain the joint position sequence, joint velocity sequence and joint acceleration sequence. Candidate motion trajectories for the robotic arm are generated based on the joint position sequence, joint velocity sequence, and joint acceleration sequence.

5. The method according to claim 1, characterized in that, The collision detection of candidate motion trajectories based on contact information returned from the physical simulation environment yields detection results, including: Before performing collision detection on candidate motion trajectories, the current physical state of the physical simulation environment is copied and backed up; the current physical state includes: the current joint angle of the robotic arm and the current position of the robotic arm end effector; Load the trajectory points corresponding to the candidate motion trajectory into the physical simulation environment for motion pre-play, and call the geometric calculation interface of the physics engine to obtain contact information; Based on the contact information, it iterates through the contact objects and contact states corresponding to each contact pair, and then uses a contact mask to filter out legitimate contact information based on the contact objects and contact states. Collision detection is performed on the filtered contact information to obtain the collision detection result; Based on the collision determination results, the detection results corresponding to the candidate motion trajectories are obtained.

6. The method according to claim 5, characterized in that, The process of performing collision determination on the filtered contact information to obtain the collision determination result includes: When the contact pair is the robotic arm and the target satellite, if the contact state indicates that there is penetration between the two, or if there is no penetration between the two but the distance is less than the first preset threshold, it is determined that the robotic arm and the target satellite have collided. When the contact pair is between robotic arm components, if the contact state indicates that the penetration depth between the two is greater than the second preset threshold, it is determined that a collision has occurred between the robotic arm components. When the contact pair is the robotic arm and the service satellite, if the contact status in strict mode indicates that the penetration depth of the corresponding contact pair is greater than the second preset threshold, or if the contact status in normal mode indicates that the penetration depth of the corresponding contact pair is greater than the third preset threshold, it is determined that the robotic arm and the service satellite have collided. When none of the contact pairs are determined to have collided, the candidate motion trajectory is determined to meet the collision-free requirement.

7. The method according to claim 2, characterized in that, The inverse kinematics objective function is the square of the Euclidean distance between the current position and the target position of the robotic arm's end effector.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium includes a stored executable program, wherein, when the executable program is executed, it controls the device on which the storage medium is located to perform the method according to any one of claims 1 to 7.

9. A computer program product, characterized in that, Includes a computer program that, when executed by a processor, implements the method according to any one of claims 1 to 7.

Images