Joint space planning method, device, equipment and medium based on gradient projection optimization

Abstract

An embodiment of the present invention discloses a joint space planning method, device, equipment and medium based on gradient projection optimization; the joint space planning method includes: establishing an initial collision detection kinematic model corresponding to a multi-arm spacecraft according to multi-body dynamics; using a collision detection algorithm to detect the initial collision detection kinematic model; when a collision occurs between the robotic arms in the initial collision detection kinematic model, using a gradient projection method to plan the joint motion trajectory of the robotic arms to obtain a corrected collision detection kinematic model; when no collision occurs between the robotic arms in the corrected collision detection kinematic model and the multi-arm spacecraft can reach the set target location, outputting the center of mass position posture of the multi-arm spacecraft and the joint motion trajectory of the end position posture of the robotic arms.

Description

Technical Field

[0001] Embodiments of the present invention relate to the technical field of joint space planning, and in particular to a joint space planning method, apparatus, device, and medium based on gradient projection optimization. Background Art

[0002] Collision detection for multi-arm spacecraft is achieved by changing the trajectory of the manipulator arm joint angles in joint space, thereby achieving positional changes between the various mechanisms of the multi-arm spacecraft in three-dimensional space and further preventing collisions between the mechanisms themselves. Currently, mainstream manipulator arm joint space trajectory planning is mainly divided into six-degree-of-freedom manipulator planning and redundant manipulator planning, depending on the type of manipulator. Six-degree-of-freedom manipulator planning technology is relatively mature and currently uses deep learning or neural networks to train the manipulator's object manipulation performance. Redundant manipulator planning technology has a wider range of research directions, such as optimization space due to its redundant degrees of freedom, singularity avoidance, and collision detection technology. Currently, mainstream collision detection technology is mostly focused on obstacle avoidance between a single manipulator arm or the entire robot and its surrounding environment, while research on collision detection technology for multi-arm robots is relatively limited. Summary of the Invention

[0003] In view of this, the embodiments of the present invention hope to provide a joint space planning method, device, equipment and medium based on gradient projection optimization; in the collision detection process of multi-arm spacecraft, the gradient projection method can be used to optimize the joint space motion trajectory according to the stability and configuration uniformity requirements, thereby realizing collision-free motion trajectory planning of multi-arm spacecraft.

[0004] The technical solution of the embodiment of the present invention is achieved as follows:

[0005] In a first aspect, an embodiment of the present invention provides a joint space planning method based on gradient projection optimization, the joint space planning method comprising:

[0006] Establish the initial collision detection kinematic model corresponding to the multi-arm spacecraft based on multi-body dynamics;

[0007] Using a collision detection algorithm to detect the initial collision detection kinematic model;

[0008] When a collision occurs between the robotic arms in the initial collision detection kinematic model, a gradient projection method is used to plan the joint motion trajectories of the robotic arms to obtain a revised collision detection kinematic model;

[0009] When no collision occurs between the robotic arms in the corrected collision detection kinematic model and the multi-arm spacecraft can reach the set target location, the joint motion trajectory of the center of mass position posture of the multi-arm spacecraft and the position posture of the end of the robotic arm is output.

[0010] In a second aspect, an embodiment of the present invention provides a joint space planning device based on gradient projection optimization, the joint space planning device comprising: an establishment part, a detection part, a planning part and an output part; wherein,

[0011] The establishing part is configured to establish an initial collision detection kinematic model corresponding to the multi-arm spacecraft according to multi-body dynamics;

[0012] The detection part is configured to detect the initial collision detection kinematic model using a collision detection algorithm;

[0013] The planning part is configured to plan the joint motion trajectories of the robotic arms using a gradient projection method when a collision occurs between the robotic arms in the initial collision detection kinematic model to obtain a revised collision detection kinematic model;

[0014] The output part is configured to output the joint motion trajectory of the center of mass position posture of the multi-arm spacecraft and the end position posture of the robotic arm when no collision occurs between the robotic arms in the corrected collision detection kinematic model and the multi-arm spacecraft can reach the set target location.

[0015] In a third aspect, an embodiment of the present invention provides a joint space planning device based on gradient projection optimization, characterized in that the joint space planning device includes: a communication interface, a memory and a processor; each component is coupled together through a bus system; wherein,

[0016] The communication interface is used to receive and send signals when sending and receiving information with other external network elements;

[0017] The memory is used to store a computer program that can be run on the processor;

[0018] The processor is used to execute the steps of the joint space planning method based on gradient projection optimization described in the first aspect when running the computer program.

[0019] In a fourth aspect, an embodiment of the present invention provides a medium storing a program for joint space planning based on gradient projection optimization, wherein the program for joint space planning based on gradient projection optimization, when executed by at least one processor, implements the steps of the joint space planning method based on gradient projection optimization described in the first aspect.

[0020] The embodiment of the present invention provides a joint space planning method, device, equipment and medium based on gradient projection optimization; by establishing an initial collision detection kinematic model corresponding to a multi-arm spacecraft according to multi-body dynamics; using a collision detection algorithm to detect the initial collision detection kinematic model; when a collision occurs between the manipulators in the initial collision detection kinematic model, the gradient projection method is used to plan the joint motion trajectory of the manipulator to obtain a revised collision detection kinematic model; when no collision occurs between the manipulators in the revised collision detection kinematic model and the multi-arm spacecraft can reach the set target location, the joint motion trajectory of the center of mass position posture of the multi-arm spacecraft and the position posture of the end of the manipulator is output. Through the joint space planning method based on gradient projection optimization provided by the embodiment of the present invention, a collision-free multi-arm spacecraft joint motion trajectory is generated. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 A schematic structural diagram of a multi-arm spacecraft provided in an embodiment of the present invention;

[0022] Figure 2 A schematic diagram of a three-dimensional model of a robotic arm provided in an embodiment of the present invention;

[0023] Figure 3 A schematic flow chart of a joint space planning method based on gradient projection optimization provided by an embodiment of the present invention;

[0024] Figure 4 A schematic diagram of the coordinate system of the robot arm DH model provided in an embodiment of the present invention;

[0025] Figure 5 A schematic diagram of a collision detection kinematic model provided by an embodiment of the present invention;

[0026] Figure 6 A schematic diagram of a distribution uniformity index of a robotic arm configuration provided by an embodiment of the present invention;

[0027] Figure 7 A schematic diagram of a distribution uniformity index of a robotic arm configuration provided by another embodiment of the present invention;

[0028] Figure 8 A flowchart of collision detection provided by an embodiment of the present invention;

[0029] Figure 9 A schematic diagram of the Cartesian space trajectory of a robotic arm provided in an embodiment of the present invention;

[0030] Figure 10 A schematic diagram of a joint angle curve provided by an embodiment of the present invention;

[0031] Figure 11A schematic diagram of a joint angular velocity curve provided by an embodiment of the present invention;

[0032] Figure 12 A schematic diagram of the positional relationship of geometric bodies calculated by a collision detection algorithm provided by an embodiment of the present invention;

[0033] Figure 13 A schematic diagram of a joint angle curve after improved obstacle avoidance provided by an embodiment of the present invention;

[0034] Figure 14 A schematic diagram of a joint angular velocity curve after improved obstacle avoidance provided by an embodiment of the present invention;

[0035] Figure 15 A schematic diagram of a curve showing the difference in joint angles before and after the improved obstacle avoidance provided by an embodiment of the present invention;

[0036] Figure 16 A schematic diagram of a curve showing the difference in joint angular velocity before and after the improved obstacle avoidance provided by an embodiment of the present invention;

[0037] Figure 17 The embodiment of the present invention provides Schematic diagram of the joint angle curve;

[0038] Figure 18 The embodiment of the present invention provides Schematic diagram of joint angular velocity curve;

[0039] Figure 19 The embodiment of the present invention provides and Schematic diagram of the angle difference curve;

[0040] Figure 20 The embodiment of the present invention provides and Schematic diagram of angular velocity difference curve;

[0041] Figure 21 The embodiment of the present invention provides and of Change curve;

[0042] Figure 22 A schematic diagram of the composition of a joint space planning device based on gradient projection optimization provided by an embodiment of the present invention;

[0043] Figure 23 A schematic diagram of the specific hardware structure of a joint space planning device based on gradient projection optimization provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0044] The technical solutions in the embodiments of the present invention will be described clearly and completely below with reference to the accompanying drawings in the embodiments of the present invention.

[0045] Before elaborating on the embodiments of the present invention in detail, it should be noted that in the embodiments of the present invention, only a multi-arm spacecraft comprising four robotic arms is used as an example to elaborate on the gait planning technical solution for space movement. However, in the specific implementation process, the number of robotic arms in the multi-arm spacecraft is not specifically limited.

[0046] See also Figure 1 , which shows a schematic structural diagram of a multi-arm spacecraft 1 capable of implementing an embodiment of the present invention, such as Figure 1 As shown, the multi-arm spacecraft 1 consists of a cubic base 11 and four centrally symmetrically distributed robotic arms 12. The three-dimensional model of the robotic arms 12 is specifically referred to Figure 2 .Depend on Figure 1 and Figure 2 As can be seen, each robotic arm 12 includes seven rotary joints 121, an arm rod 122, and an end 123. It should be noted that in the embodiments of the present invention, the technical solutions involved are described in detail based on the fact that each robotic arm 12 includes seven rotary joints 121, but the number of joints of the robotic arm 12 is not limited in the embodiments of the present invention.

[0047] See also Figure 3 , which shows a joint space planning method based on gradient projection optimization provided by an embodiment of the present invention, the method specifically includes:

[0048] S301. Establish an initial collision detection kinematic model corresponding to the multi-arm spacecraft based on multi-body dynamics;

[0049] S302, using a collision detection algorithm to detect the initial collision detection kinematic model;

[0050] S303: When a collision occurs between the manipulators in the initial collision detection kinematic model, a gradient projection method is used to plan the joint motion trajectories of the manipulators to obtain a revised collision detection kinematic model;

[0051] S304. When no collision occurs between the robotic arms in the corrected collision detection kinematic model and the multi-arm spacecraft can reach the set target location, output the center of mass position posture of the multi-arm spacecraft and the joint motion trajectory of the robotic arm end position posture.

[0052] for Figure 3The technical solution shown is to establish an initial collision detection kinematic model corresponding to a multi-arm spacecraft based on multi-body dynamics; use a collision detection algorithm to detect the initial collision detection kinematic model; when a collision occurs between the manipulators in the initial collision detection kinematic model, use a gradient projection method to plan the joint motion trajectory of the manipulators to obtain a revised collision detection kinematic model; when no collision occurs between the manipulators in the revised collision detection kinematic model and the multi-arm spacecraft can reach the set target location, output the joint motion trajectory of the center of mass position posture of the multi-arm spacecraft and the position posture of the end of the manipulator. Through the joint space planning method based on gradient projection optimization provided by the embodiment of the present invention, a collision-free multi-arm spacecraft joint motion trajectory is generated.

[0053] for Figure 3 In some possible implementations of the technical solution shown, before establishing the initial collision detection kinematic model corresponding to the multi-arm spacecraft according to multi-body dynamics, the joint space planning method further includes:

[0054] Based on the inverse kinematics calculation method of the manipulator arm of the multi-arm spacecraft, the joint angles and joint angular velocities of the manipulator arm are obtained.

[0055] For the above-mentioned implementation, in some examples, the inverse kinematics calculation method of the manipulator arm of the multi-arm spacecraft to obtain the joint angle and joint angular velocity of the manipulator arm includes:

[0056] According to the first pseudo-inverse solution method of the Jacobian matrix shown in formula (1), the first solution formula of the inverse kinematics of the manipulator shown in formula (2) is calculated:

[0057] (1)

[0058] (2)

[0059] in, represents the angular velocity vector of the manipulator joint; represents the manipulator task space angular velocity vector; represents the Jacobian matrix; represents the pseudo-inverse of the Jacobian matrix, and ; represents the identity matrix; represents any angular velocity vector in the joint space of the manipulator, optimized based on the current motion of the manipulator;

[0060] The variable damping coefficient shown in formula (3) βSubstituting into equation (1), we can obtain the second pseudo-inverse solution method of the Jacobian matrix shown in equation (4) after the singularity avoidance process. And according to the second pseudo-inverse solution method of the Jacobian matrix shown in equation (4), we can calculate the second solution formula of the inverse kinematics of the manipulator shown in equation (5):

[0061] (3)

[0062] (4)

[0063] (5)

[0064] in, represents the maximum damping coefficient; represents the minimum singular value; represents a boundary value of the minimum singular value; represents the singular robust inverse of the Jacobian matrix, and .

[0065] It can be understood that in the specific implementation process, when the posture path of the center of mass of the above-mentioned multi-arm spacecraft and the posture path of the end of each robotic arm are known, the pseudo-inverse solution method of the Jacobian matrix based on the inverse kinematics of the redundant robotic arm can obtain the robotic arm joint angular velocity and joint angle vector.

[0066] It should be noted that the inverse kinematics of the manipulator based on the pseudo-inverse method needs to be processed to avoid singularity, otherwise the planned joint angular velocity will change suddenly at the singular position of the manipulator. Therefore, a variable damping coefficient is further introduced. β To adjust the tracking error and joint angular velocity changes of the end of the robot arm, so as to avoid the sudden change of the joint angular velocity during the planning process that does not comply with the safety constraints of the mechanism.

[0067] In addition, by introducing the variable damping coefficient β The pseudo-inverse solution method is optimized so that the joint angular velocity will not show abnormal mutations when tracking the end error of the robot arm, so that the solution obtained by the inverse kinematics of the redundant robot arm is physically feasible and has practical significance.

[0068] for Figure 3 In some possible implementations of the technical solution shown, establishing an initial collision detection kinematic model corresponding to a multi-arm spacecraft based on multi-body dynamics includes:

[0069] Assuming that the base of the multi-arm spacecraft and the joints, arm rods, and ends of each robotic arm are rigid bodies, a DH (Denavit-Hartenberg) model is established for each robotic arm from the base.

[0070] Based on the DH model, multi-body dynamics is used to envelop the joints, arm rods and ends of the base and each robotic arm using cubes and cylinders to obtain the collision detection kinematic model.

[0071] Specifically, based on Figure 2 The three-dimensional model of the robot arm shown in FIG. 1 and the DH model coordinate system of the robot arm 12 are established as shown in FIG. Figure 4 The parameters of each rod of the robotic arm are shown in Table 1 and Table 2.

[0072] Table 1 Dimensional parameters of each arm of the robotic arm

[0073]

[0074] Table 2 Parameters of the robotic arm DH model

[0075]

[0076] It should be noted that Figure 4 middle x EE axis 、y EE Axis and z EE The axes represent the three coordinate axes of the coordinate system established at the end of the robotic arm.

[0077] For the above-mentioned embodiments, in some examples, in the DH model, the joint coordinate system Relative to the origin coordinate system The transformation matrix for:

[0078] (6)

[0079] in, Indicates the number of joints, ; Represents the coordinate system from the origin Origin to Axis and The offset distance of the intersection of the axes; Indicates winding The axis is determined by the right-hand rule Axle steering The angle of the axis; Represents the coordinate system from the joint Origin to Axis and The intersection of the axes along distance between axes; Indicates winding The axis is determined by the right-hand rule Axle steering Joint angle of the axis; r Indicates the number of joints of each robotic arm;

[0080] The matrix change of the end of the manipulator relative to the connection between the manipulator and the base is:

[0081] (7)

[0082] in, A transformation matrix representing the end of the manipulator relative to the connection between the manipulator and the base; Represents the rotational transformation of the end of the manipulator relative to the connection between the manipulator and the base; Represents the translation transformation of the end of the manipulator relative to the connection between the manipulator and the base.

[0083] It should be noted that, in the embodiment of the present invention, the base 11 of the multi-arm spacecraft is enclosed by a cube, and the joints, arm rods and ends of the manipulator 12 are respectively enclosed by multiple cylinders; it can be understood that the size of the collision detection kinematic model is determined according to the maximum size of the base and the manipulator. The established collision detection kinematic model of the multi-arm spacecraft 1 is as follows: Figure 5 shown.

[0084] for Figure 3 In some possible implementations of the technical solution shown, the use of a collision detection algorithm to detect the initial collision detection kinematic model includes:

[0085] Based on the known joint angles and joint angular velocities, the position of each rigid body in the collision detection kinematic model is obtained using the forward dynamics of the manipulator. and posture ;

[0086] Based on the position of each rigid body in the collision detection kinematic model and posture , get the minimum distance between each rigid body in the collision detection kinematic model , if the minimum distance If it is less than 0, it is determined that a collision will occur between the two rigid bodies; Represents different rigid bodies.

[0087] for Figure 3 In some possible implementations of the technical solution shown, if a collision occurs between the robotic arms, a gradient projection method is used to perform gradient optimization planning on the joint angular velocity, including:

[0088] Based on the gradient projection method, the relationship between the joint angle and the performance index is obtained by substituting formula (8) into formula (5) as shown in formula (9):

[0089] (8)

[0090] (9)

[0091] in, represents the optimization coefficient; Indicates performance indicators Relative to joint angle The gradient of ; express r The joint angles of the robotic arm with degrees of freedom; r Indicates the number of joints of each robotic arm;

[0092] Based on the dynamic coupling characteristics of the manipulator and the base and the parameters of the uniformity of the manipulator configuration distribution, the performance index is optimized according to equations (10) and (11) respectively, so as to obtain the expression of the performance index as shown in equation (12):

[0093] (10)

[0094] (11)

[0095] (12)

[0096] in, Represents the performance index of dynamic coupling characteristics; Indicates the coupling relationship between the end of the robotic arm and the base; It represents the performance index of uniformity of the manipulator configuration distribution; Indicates the fourth joint position of two adjacent robotic arms In multi-arm spacecraft The angle of the projection in the plane; Indicates the position of the end of the robotic arm and the fourth joint of the robotic arm In multi-arm spacecraft The angle of the projection in the plane; represents the optimization coefficient of dynamic coupling characteristics; represents the optimization coefficient of the uniformity of the manipulator configuration distribution;

[0097] If during the planning process the rigid body and A collision occurred between and The vector between the two before the collision is , after the collision occurs, the rigid body and The manipulator modifies the direction gradient according to formula (13) during planning:

[0098] (13)

[0099] in, Represents the collision avoidance coefficient, which increases as the number of iterations in the collision detection loop increases.

[0100] It is understandable that the dynamic coupling between the manipulator and the base is closely related to the trajectory planning of the multi-arm spacecraft, so it is necessary to derive the position-level and velocity-level coupling relationship between the manipulator and the base through momentum conservation. We can get:

[0101] (14)

[0102] in, Indicates the speed of the robotic arm; Represents the Jacobian matrix of the robot arm rotation; represents the Jacobian matrix of the robot base; Indicates the base speed.

[0103] Therefore, the coupling relationship between the end of the robot arm and the base can be expressed as:

[0104] (15)

[0105] in, Represents the coupling matrix from the end of the manipulator to the base; represents a third-order real symmetric coupled matrix, and .

[0106] Assumptions for The eigenvalues of a square matrix have the following relationship based on the properties of the second-order norm:

[0107] (16)

[0108] Coupling Matrix It is related to the mass characteristics of the base, each arm of the robot, the joint angle of the robot, the posture base, etc., which can be achieved by changing the matrix The distribution of singular values can reduce the coupling characteristics of dynamics. Therefore, this coefficient is introduced into the performance index, so that the robot arm is more inclined to the joint space path with less impact on the base during movement, as shown in the above formula (10).

[0109] On the other hand, for the planning of multi-arms, the coordination between the various manipulators is also very important. The uniformity of the layout of each manipulator relative to the entire multi-arm spacecraft during movement is an important indicator for measuring the stability of the multi-arm spacecraft. If the result of joint space planning causes multiple manipulators to lean towards one side of the multi-arm spacecraft, the center of mass of the entire multi-arm spacecraft will deviate from the center of the multi-arm spacecraft structure, and the torque generated by the movement of each joint relative to the multi-arm spacecraft base will also increase. This uneven result is obviously not conducive to the control of the multi-arm spacecraft. Therefore, the parameter of the uniformity of the manipulator configuration distribution is introduced in the planning process of the joint space as one of the evaluation indicators of multi-arm coordination, as shown in formula (17):

[0110] (17)

[0111] in, Indicates the fourth joint position of two adjacent robotic arms In multi-arm spacecraft The angle of projection in the plane is as follows: Figure 6 shown.

[0112] At the same time, in order to avoid the middle joint of the manipulator rotating clockwise or counterclockwise relative to its end around the center of the base while ensuring uniformity, Equation (17) is improved to Equation (11). It should be noted that the angle in Equation (11) is Specific as Figure 7 shown.

[0113] Therefore, the performance indicators in the embodiment of the present invention are It is defined as formula (12).

[0114] See also Figure 8 , which shows a collision detection flow chart provided by an embodiment of the present invention. Specifically:

[0115] S801, obtaining the angles of each joint in the robotic arm according to an inverse kinematics calculation formula, and obtaining the position of a collision detection kinematic model to perform collision detection on the collision detection model using a collision detection algorithm;

[0116] S802: If the detection result determines that a collision has occurred, the planned joint space path is modified in terms of directional gradient using the gradient projection method, and the position of the modified collision detection kinematic model is calculated again using the forward kinematics of the manipulator;

[0117] S803, using a collision detection algorithm to detect whether a collision detection occurs in the modified collision detection kinematic model; if a collision still occurs in the modified collision detection model, continue to step S802; if no collision occurs in the modified collision detection model, execute step S804;

[0118] S804, detecting whether the multi-arm spacecraft has reached the target point; if the multi-arm spacecraft has not reached the target point, continuing to execute step S801; if the multi-arm spacecraft has reached the target point, outputting the joint motion trajectory.

[0119] The technical solution provided by the embodiment of the present invention will be described in detail below with reference to specific simulation examples.

[0120] Self-collision detection for multi-arm spacecraft is a safety constraint requirement in path planning tasks and a key technology for ensuring the safe and successful completion of ground experiments and space mobility missions. First, without applying any constraints, the manipulator joint space planning is performed directly based on the gradient projection method. The simulation parameters are set as follows:

[0121] The simulation results are as follows Figures 9 to 11 As shown. Figure 9 The unit in is meter (m). Figures 9 to 11 It can be seen that the initial states and target position settings of the four robotic arms 1 to 4 are the same, so the joint angles and joint angular velocity curves of the four robotic arms are the same. After that, joint 1 collides with arm 4, and the position relationship of the corresponding geometric bodies is calculated through the collision detection algorithm, such as Figure 12 As shown, it is verified that a collision occurs between the geometric bodies corresponding to the arm 1 and the arm 4. Therefore, the collision detection method is proven to be reasonable and effective, and the simulation verification is correct.

[0122] The improved obstacle avoidance gradient projection method based on the above collision detection technology is simulated, and the results are as follows Figures 13 to 20 shown.

[0123] Analysis of the joint angular velocity curves after the improved obstacle avoidance showed that after the collision detection technology was adopted, the joint angular velocity changed suddenly at around 0, 4, 8, and 9 seconds compared to when the collision detection technology was not adopted. This indicates that some parts of the robot arm collided at these moments, so a maneuvering strategy was adopted during the planning process to avoid collisions.

[0124] Respectively Figure 13 and Figure 14 The joint angle and joint angular velocity results in the difference are subtracted to obtain the joint angle and joint angular velocity difference curve before and after the improved obstacle avoidance. Figure 15 and Figure 16 It can be seen that during the obstacle avoidance process, all joint angles have changed compared to before the improved obstacle avoidance. The difference in some joints reaches a peak of 0.3281° around 10 seconds. After 10 seconds, there are basically no major maneuvers, indicating that no collisions have occurred, and the angle difference has remained at 0°. This shows that if no collision occurs after the collision detection technology is adopted, the method can converge according to the previous gradient and reach the target position. After verification, after adopting the improved obstacle avoidance gradient projection method based on collision detection technology to plan the joint space, no collisions occur between the envelopes of the multi-arm spacecraft, proving the effectiveness of this method.

[0125] pass Figure 17 and Figure 18 It can be seen that the joint space path of the robot arm has changed significantly, and the joint angular velocity of the robot arm has changed suddenly at around 0, 5, 6, 12, and 14 seconds, indicating that the robot arm judged that a collision had occurred at these moments and took obstacle avoidance measures. and The time and location of the collision are also different in different situations.

[0126] Respectively Figure 17 and Figure 13 、 Figure 18 and Figure 14 The result of the difference is Figure 19 、 Figure 20 The joint angle and joint angular velocity difference curves show that The joint space path of the robot arm is The final planning results of most joints have deviations ranging from 5 to 10 degrees, and the largest joint angle deviation reaches 14.65 degrees. The joint angular velocity difference has a large mutation at around 3 seconds, 13 seconds, and 17 seconds, indicating that the performance index for The change is discontinuous.

[0127] Respectively and Substitute the joint angle simulation results into formula (12) to calculate the performance index The time-varying curve, such as Figure 21 As shown by Figure 21 It can be seen that the performance indicators of the planned joint space path are higher throughout the entire process, with an improvement of 14.31% to 30.50%. This shows that the joint space trajectory has a trend of gradient planning with a smaller dynamic coupling coefficient and a more uniform configuration distribution, which also verifies the effectiveness of the algorithm.

[0128] Based on the same inventive concept as the above technical solution, see Figure 22, which shows a joint space planning device 220 based on gradient projection optimization provided by an embodiment of the present invention, the joint space planning device 220 includes: an establishment part 2201, a detection part 2202, a planning part 2203 and an output part 2204; wherein,

[0129] The establishing part 2201 is configured to establish an initial collision detection kinematic model corresponding to the multi-arm spacecraft according to multi-body dynamics;

[0130] The detection part 2202 is configured to detect the initial collision detection kinematic model using a collision detection algorithm;

[0131] The planning part 2203 is configured to plan the joint motion trajectories of the robotic arms using a gradient projection method when a collision occurs between the robotic arms in the initial collision detection kinematic model to obtain a revised collision detection kinematic model;

[0132] The output part 2204 is configured to output the center of mass position posture of the multi-arm spacecraft and the joint motion trajectory of the end position posture of the robotic arm when no collision occurs between the robotic arms in the corrected collision detection kinematic model and the multi-arm spacecraft can reach the set target location.

[0133] It should be noted that for the specific implementation methods or implementation examples of the functions configured by the above-mentioned components, please refer to the corresponding steps, implementation methods and examples of the aforementioned technical solutions, and the embodiments of the present invention are not described in detail here. It is understood that in this embodiment, "part" can be a part of a circuit, a part of a processor, a part of a program or software, etc., and of course it can also be a unit, a module, or a non-modular device.

[0134] In addition, the components in this embodiment may be integrated into a single processing unit, or each unit may exist physically separately, or two or more units may be integrated into a single unit. The above-mentioned integrated units may be implemented in the form of hardware or software functional modules.

[0135] If the integrated unit is implemented as a software functional module and not sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this embodiment, or the portion that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, server, or network device, etc.) or a processor to execute all or part of the steps of the method described in this embodiment. The aforementioned storage medium includes various media capable of storing program code, such as a USB flash drive, a mobile hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0136] Therefore, this embodiment provides a computer storage medium, which stores a program for joint space planning based on gradient projection optimization. When the program for joint space planning based on gradient projection optimization is executed by at least one processor, the steps of the joint space planning method based on gradient projection optimization described in the above technical solution are implemented.

[0137] According to the joint space planning device 220 and computer storage medium based on gradient projection optimization, see Figure 23 , which shows the specific hardware structure of a computing device 230 provided by an embodiment of the present invention that can implement the above-mentioned joint space planning device 220 based on gradient projection optimization. The computing device 230 can be a wireless device, a mobile or cellular phone (including a so-called smart phone), a personal digital assistant (PDA), a video game console (including a video display, a mobile video game device, a mobile video conferencing unit), a laptop computer, a desktop computer, a TV set-top box, a tablet computing device, an e-book reader, a fixed or mobile media player, etc. The computing device 230 includes: a communication interface 2301, a memory 2302 and a processor 2303; the various components are coupled together through a bus system 2304. It can be understood that the bus system 2304 is used to realize the connection and communication between these components. In addition to the data bus, the bus system 2304 also includes a power bus, a control bus and a status signal bus. However, for the sake of clarity, in Figure 23 In FIG, various buses are labeled as bus system 2304. Among them,

[0138] The communication interface 2301 is used to receive and send signals during the process of sending and receiving information between other external network elements;

[0139] The memory 2302 is used to store computer programs that can be run on the processor 2303;

[0140] The processor 2303 is used to execute the steps of the joint space planning method based on gradient projection optimization described in the aforementioned technical solution when running the computer program.

[0141] It is understood that the memory 2302 in the embodiment of the present invention can be a volatile memory or a non-volatile memory, or can include both volatile and non-volatile memories. Among them, the non-volatile memory can be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), or a flash memory. The volatile memory can be a random access memory (RAM), which is used as an external cache. By way of example and not limitation, many forms of RAM are available, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate synchronous DRAM (DDRSDRAM), enhanced synchronous DRAM (ESDRAM), synchronous link DRAM (SLDRAM), and direct RAM bus random access memory (DRRAM). The memory 2302 of the systems and methods described herein is intended to include, but is not limited to, these and any other suitable types of memory.

[0142] Processor 2303 may be an integrated circuit chip with signal processing capabilities. During implementation, each step of the above method may be completed by hardware integrated logic circuits or software instructions in processor 2303. The above-mentioned processor 2303 may be a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. It may implement or execute the various methods, steps, and logic block diagrams disclosed in the embodiments of the present invention. A general-purpose processor may be a microprocessor or any conventional processor. The steps of the method disclosed in conjunction with the embodiments of the present invention may be directly implemented and executed by a hardware decoding processor, or by a combination of hardware and software modules in the decoding processor. The software module may be located in a storage medium well-known in the art, such as random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, or the like. The storage medium is located in the memory 2302, and the processor 2303 reads the information in the memory 2302 and completes the steps of the above method in combination with its hardware. It is understandable that the embodiments described herein can be implemented using hardware, software, firmware, middleware, microcode or a combination thereof. For hardware implementation, the processing unit can be implemented in one or more application specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DSP devices, DSPDs), programmable logic devices (PLDs), field programmable gate arrays (FPGAs), general-purpose processors, controllers, microcontrollers, microprocessors, other electronic units for performing the functions described in this application or a combination thereof.

[0143] For software implementation, the technology described herein can be implemented by a module (e.g., a procedure, a function, etc.) that performs the functions described herein. The software code can be stored in a memory and executed by a processor. The memory can be implemented in the processor or outside the processor. Specifically, the processor 2303 is also configured to execute the steps of the joint space planning method based on gradient projection optimization described in the aforementioned technical solution when running the computer program, which will not be repeated here.

[0144] It should be noted that the technical solutions described in the embodiments of the present invention can be arbitrarily combined without conflict.

[0145] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any modifications or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A joint space planning method based on gradient projection optimization, characterized in that: The joint space planning method comprises: Establish the initial collision detection kinematic model corresponding to the multi-arm spacecraft based on multi-body dynamics; Using a collision detection algorithm to detect the initial collision detection kinematic model; When a collision occurs between the robotic arms in the initial collision detection kinematic model, a gradient projection method is used to plan the joint motion trajectories of the robotic arms to obtain a revised collision detection kinematic model; When no collision occurs between the manipulator arms in the modified collision detection kinematic model and the multi-arm spacecraft can reach the set target location, output the joint motion trajectory of the center of mass position and posture of the multi-arm spacecraft and the position and posture of the manipulator end arms; When a collision occurs between the robotic arms in the initial collision detection kinematic model, a gradient projection method is used to plan the joint motion trajectory of the robotic arms to obtain a modified collision detection kinematic model, including: Based on the gradient projection method, formula (1) is substituted into the second solution formula of the inverse kinematics of the manipulator shown in formula (2) to obtain the relationship between the joint angle and the performance index as shown in formula (3): （1） （2） （3） in, represents the optimization coefficient; Indicates performance indicators Relative to joint angle The gradient of ; represents the angular velocity vector of the manipulator joint; represents the manipulator task space angular velocity vector; A Jacobian matrix representing the inverse kinematics of the manipulator; represents the pseudo-inverse of the Jacobian matrix, and ; represents the identity matrix; represents any angular velocity vector in the joint space of the manipulator; represents the singular robust inverse of the Jacobian matrix, and ; Based on the dynamic coupling characteristics of the manipulator and the base and the parameters of the uniformity of the manipulator configuration distribution, the performance index is optimized according to equations (4) and (5) respectively, so as to obtain the expression of the performance index as shown in equation (6): （4） （5） （6） in, Represents the performance index of dynamic coupling characteristics; Indicates the coupling relationship between the end of the robotic arm and the base; It represents the performance index of uniformity of the manipulator configuration distribution; Indicates the fourth joint position of two adjacent robotic arms In multi-arm spacecraft The angle of the projection in the plane; Indicates the position of the end of the robotic arm and the fourth joint of the robotic arm In multi-arm spacecraft The angle of the projection in the plane; represents the optimization coefficient of dynamic coupling characteristics; represents the optimization coefficient of the uniformity of the manipulator configuration distribution; If the rigid body and A collision occurred between and The vector between the two before the collision is , after the collision occurs, the rigid body and The manipulator modifies the direction gradient according to formula (7) during planning: （7） in, Represents the collision avoidance coefficient, which increases as the number of iterations in the collision detection loop increases.

2. The joint space planning method according to claim 1, characterized in that: Before establishing the initial collision detection kinematic model corresponding to the multi-arm spacecraft according to multi-body dynamics, the joint space planning method further includes: Based on the inverse kinematics calculation method of the manipulator arm of the multi-arm spacecraft, the joint angles and joint angular velocities of the manipulator arm are obtained.

3. The joint space planning method according to claim 2, characterized in that: The inverse kinematics calculation method of the manipulator arm based on the multi-arm spacecraft, for obtaining the joint angle and joint angular velocity of the manipulator arm, includes: According to the first pseudo-inverse solution method of the Jacobian matrix shown in formula (8), the first solution formula of the inverse kinematics of the manipulator shown in formula (9) is calculated: （8） （9） in, represents the angular velocity vector of the manipulator joint; represents the manipulator task space angular velocity vector; represents the Jacobian matrix; represents the pseudo-inverse of the Jacobian matrix, and ; represents the identity matrix; represents any angular velocity vector in the joint space of the manipulator, optimized based on the current motion of the manipulator; The variable damping coefficient shown in formula (10) β Substituting into equation (8), we can obtain the second pseudo-inverse solution method of the Jacobian matrix shown in equation (11) after the singularity avoidance process. And according to the second pseudo-inverse solution method of the Jacobian matrix shown in equation (11), we can calculate the second solution formula of the inverse kinematics of the manipulator shown in equation (5): （10） （11） （2） in, represents the maximum damping coefficient; represents the minimum singular value; represents a boundary value of the minimum singular value; represents the singular robust inverse of the Jacobian matrix, and .

4. The joint space planning method according to claim 1, characterized in that: The initial collision detection kinematic model corresponding to the multi-arm spacecraft is established according to multi-body dynamics, including: Assuming that the base of the multi-arm spacecraft and the joints, arm rods and ends of each robotic arm are rigid bodies, a DH model is established for each robotic arm from the base; Based on the DH model, multi-body dynamics is used to envelop the joints, arm rods and ends of the base and each robotic arm using cubes and cylinders to obtain the collision detection kinematic model.

5. The joint space planning method according to claim 4, characterized in that: In the DH model, the joint coordinate system Relative to the origin coordinate system The transformation matrix for: （12） in, Indicates the number of joints, ; Represents the coordinate system from the origin Origin to Axis and The offset distance of the intersection of the axes; Indicates winding The axis is determined by the right-hand rule Axle steering The angle of the axis; Represents the coordinate system from the joint Origin to Axis and The intersection of the axes along Axis distance; Indicates winding The axis is determined by the right-hand rule Axle steering Joint angle of the axis; The matrix change of the end of the manipulator relative to the connection between the manipulator and the base is: （13） in, A transformation matrix representing the end of the manipulator relative to the connection between the manipulator and the base; Represents the rotational transformation of the end of the manipulator relative to the connection between the manipulator and the base; Represents the translation transformation of the end of the manipulator relative to the connection between the manipulator and the base.

6. The joint space planning method according to claim 1, characterized in that: The adopting of a collision detection algorithm to detect the initial collision detection kinematic model includes: Based on the known joint angles and joint angular velocities, the position of each rigid body in the collision detection kinematic model is obtained using the forward dynamics of the manipulator. and posture ; Based on the position of each rigid body in the collision detection kinematic model and posture , get the minimum distance between each rigid body in the collision detection kinematic model , if the minimum distance If it is less than 0, it is determined that a collision will occur between the two rigid bodies; Represents different rigid bodies.

7. A joint space planning device based on gradient projection optimization, characterized in that: The joint space planning device includes: an establishment part, a detection part, a planning part and an output part; wherein, The establishing part is configured to establish an initial collision detection kinematic model corresponding to the multi-arm spacecraft according to multi-body dynamics; The detection part is configured to detect the initial collision detection kinematic model using a collision detection algorithm; The planning part is configured to plan the joint motion trajectories of the robotic arms using a gradient projection method when a collision occurs between the robotic arms in the initial collision detection kinematic model to obtain a revised collision detection kinematic model; The output part is configured to output the center of mass position and posture of the multi-arm spacecraft and the joint motion trajectory of the position and posture of the end positions of the manipulator arms when no collision occurs between the manipulator arms in the modified collision detection kinematic model and the multi-arm spacecraft can reach the set target location; Wherein, the planning part is configured as follows: Based on the gradient projection method, formula (1) is substituted into the second solution formula of the inverse kinematics of the manipulator shown in formula (2) to obtain the relationship between the joint angle and the performance index as shown in formula (3): （1） （2） （3） in, represents the optimization coefficient; Indicates performance indicators Relative to joint angle The gradient of ; represents the angular velocity vector of the manipulator joint; represents the manipulator task space angular velocity vector; A Jacobian matrix representing the inverse kinematics of the manipulator; represents the pseudo-inverse of the Jacobian matrix, and ; represents the identity matrix; represents any angular velocity vector in the joint space of the manipulator; represents the singular robust inverse of the Jacobian matrix, and ; Based on the dynamic coupling characteristics of the manipulator and the base and the parameters of the uniformity of the manipulator configuration distribution, the performance index is optimized according to equations (4) and (5) respectively, so as to obtain the expression of the performance index as shown in equation (6): （4） （5） （6） in, Represents the performance index of dynamic coupling characteristics; Indicates the coupling relationship between the end of the robotic arm and the base; It represents the performance index of uniformity of the manipulator configuration distribution; Indicates the fourth joint position of two adjacent robotic arms In multi-arm spacecraft The angle of the projection in the plane; Indicates the position of the end of the robotic arm and the fourth joint of the robotic arm In multi-arm spacecraft The angle of the projection in the plane; represents the optimization coefficient of dynamic coupling characteristics; represents the optimization coefficient of the uniformity of the manipulator configuration distribution; If during the planning process the rigid body and A collision occurred between and The vector between the two before the collision is , after the collision occurs, the rigid body and The manipulator modifies the direction gradient according to formula (7) during planning: （7） in, Represents the collision avoidance coefficient, which increases as the number of iterations in the collision detection loop increases.

8. A joint space planning device based on gradient projection optimization, characterized in that: The joint space planning device includes: a communication interface, a memory and a processor; each component is coupled together through a bus system; wherein, The communication interface is used to receive and send signals when sending and receiving information with other external network elements; The memory is used to store a computer program that can be run on the processor; The processor is configured to execute the steps of the joint space planning method based on gradient projection optimization according to any one of claims 1 to 6 when running the computer program.

9. A medium, characterized in that The medium stores a program for joint space planning based on gradient projection optimization, and when the program for joint space planning based on gradient projection optimization is executed by at least one processor, the steps of the joint space planning method based on gradient projection optimization described in any one of claims 1 to 6 are implemented.

Images