Obstacle avoidance and trajectory planning method for super-redundant series-parallel manipulator based on null space

Abstract

The zero space-based obstacle avoidance and trajectory planning method for super-redundant series-parallel manipulator belongs to the technical field of robot motion planning. In order to solve the problem of effectively avoiding obstacles for the super-redundant series-parallel manipulator during operation in a complex environment, the present application comprises setting various parameters of a six-degree-of-freedom parallel mechanism module; adopting a pseudo-distance to represent the distance from a certain point to the obstacles in the operation environment; updating the corresponding Jacobian matrix based on the current actual manipulator configuration; constructing the total series Jacobian matrix of the overall mechanism through velocity recursion and vector product method; constructing a criterion function for evaluating the proximity of obstacles to the manipulator and a criterion function for evaluating the proximity of the joint to the stroke limit; performing numerical solution and discrete integration of the velocity-level inverse kinematics of the overall mechanism, thereby updating the joint variables; using the forward kinematics solution of the overall mechanism to update the homogeneous transformation matrix of the upper and lower hinge point coordinate systems of each module in the base coordinate system and the end position and posture and performing iterative calculation.

Description

Technical Field

[0001] This invention belongs to the field of robot motion planning technology, specifically relating to a method for obstacle avoidance and trajectory planning of a super-redundant serial-parallel robotic arm based on zero space. Background Technology

[0002] Robotic arms are the most common manipulators in the field of robotics, widely used in industrial production, aerospace exploration, medical surgery, and other areas. Traditional robotic arms consist of multiple rigid links connected in series, offering a large workspace, but their load-bearing capacity and stiffness are relatively poor, making them unsuitable for high-precision applications. Parallel robots, also composed of multiple rigid links connected in parallel, offer higher load-bearing capacity and stiffness, enabling high precision, but their workspace is smaller. Hyperredundant serial-parallel robotic arms, composed of multiple six-degree-of-freedom parallel mechanism modules connected in series, not only combine the advantages of both but also achieve greater dexterity and other optimization goals based on redundant degrees of freedom, such as obstacle avoidance based on self-motion and rational allocation of actuator drive lengths to avoid drive limits.

[0003] Trajectory planning for a hyper-redundant serial-parallel robotic arm under end-effector trajectory constraints is essentially a redundant inverse kinematics problem. This involves determining the position of each joint at every moment within the operational space, given a specified 3D trajectory for the end-effector. Since the degrees of freedom in the joint space are far greater than those in the operational space, the inverse kinematics has infinitely many solutions, requiring the addition of optimization indices or constraints to find a unique solution. In complex environments, hyper-redundant serial-parallel robotic arms need to avoid collisions with obstacles without affecting the end-effector trajectory. Currently, several inverse kinematics solutions have been proposed for such robotic arms, including pseudo-inverse methods, ridge line methods, deep reinforcement learning methods, and optimization methods. However, these methods only apply to obstacle-free environments and lack effective methods for obstacle avoidance under end-effector trajectory constraints. Summary of the Invention

[0004] The problem this invention aims to solve is to enable ultra-redundant serial-parallel robotic arms to effectively avoid obstacles during operation in complex environments. It proposes a zero-space-based method for obstacle avoidance and trajectory planning of ultra-redundant serial-parallel robotic arms.

[0005] To achieve the above objectives, the present invention provides the following technical solution:

[0006] A method for obstacle avoidance and trajectory planning of a super-redundant serial-parallel robotic arm based on null space includes the following steps:

[0007] Step 1: Set the structural parameters of the six-DOF parallel mechanism module, the coordinate system of the upper and lower hinge points of the module, the number of modules, the base coordinate system, the initial joint variables, the relative poses of adjacent modules and the desired trajectory of the end effector of the robotic arm. Then, use global forward kinematics to calculate the homogeneous transformation matrix of the coordinate system of the upper and lower hinge points of each module in the base coordinate system and the end effector pose.

[0008] Step 2: Identify obstacles in the work environment and enclose them with regular shapes; the shape and surface of the obstacles are described by hyperquadratic surface functions and homogeneous transformation matrix of the obstacle coordinate system relative to the base coordinate system, and parameterized by obstacle properties; pseudo-distance is used to represent the distance from a point to an obstacle;

[0009] Step 3: Extract the desired pose of the trajectory points from the desired trajectory of the robotic arm's end effector in Step 1. Update the corresponding Jacobian matrix based on the current actual robotic arm configuration: calculate the coordinates of each hinge point and the origin of the coordinate system based on the homogeneous transformation matrix of the coordinate system of each module's upper and lower hinge points relative to the base coordinate system, calculate the parallel Jacobian matrix of a single module, and then construct the overall series Jacobian matrix of the whole mechanism through velocity recursion and vector product method.

[0010] Step 4: Using the shape and surface of the obstacle obtained in Step 2, construct a criterion function to evaluate the proximity of the obstacle to the robotic arm; using the structural parameters of the six-DOF parallel mechanism module obtained in Step 1, construct a criterion function to evaluate the approach stroke limit of the joints; calculate the current end-effector pose. With the expected pose of the trajectory point The difference is then used to decouple the motion of the robotic arm into the main motion that controls the end-effector posture and the self motion that projects the gradient of the criterion function onto the null space. The numerical solution and discretized integral of the overall mechanism velocity-level inverse kinematics are then performed to update the joint variables.

[0011] Step 5: Apply the overall mechanism forward kinematics to the joint variables updated in Step 4, and update the homogeneous transformation matrix of the upper and lower hinge point coordinate systems of each module in the base coordinate system and the end-effector pose, and determine the end-effector pose. Whether the calculation accuracy requirement is met, if the end-effector pose If the requirements are met, output the information of each joint and update the expected pose of the trajectory points in step three. Then return to step three to iterate and solve for the next trajectory point until all trajectory points have been traversed; if the requirements are not met, return directly to step three to continue iterating and calculating the current trajectory point.

[0012] Furthermore, the six-degree-of-freedom parallel mechanism module in step one consists of an upper platform, a lower platform, and six identical UPS motion chains. Each motion chain consists of a Hooke joint, a ball joint, and an electric cylinder. The six electric cylinders drive the upper platform to achieve the six-degree-of-freedom motion of the upper platform.

[0013] Define the lower hinge point coordinate system and the upper hinge point coordinate system of the i-th module as follows: and The origins of the lower hinge point coordinate system and the upper hinge point coordinate system are the centers of the circles at each hinge point, and are defined as follows: and The z-axis is perpendicular to the plane containing the hinge point circle, the x-axis is perpendicular to the line connecting the two hinge points, and the y-axis is determined by the right-hand rule.

[0014] If the total number of modules is set to n, then the robotic arm has 6n degrees of freedom and 6n actuators; the base coordinate system is set to the coordinate system of the lower hinge point of the first module. ;

[0015] Define the initial joint variables as column vectors , which corresponds to the length of 6n actuators in the joint space, where R is a real number field;

[0016] Define the relative pose of adjacent modules as the coordinate system of the lower hinge point of the (i+1)th module. Relative to the hinge point coordinate system of the i-th module The pose of adjacent modules is rigidly connected, and the relative pose of adjacent modules remains unchanged during the motion.

[0017] The desired trajectory of the robotic arm's end effector is defined as the trajectory of the end effector coordinate system relative to the base coordinate system. It consists of the desired poses of the endpoints of discrete trajectory points, and the desired pose of the endpoint of the i-th trajectory point is set as... The desired position is The desired posture is Represented using Euler angles;

[0018] The overall forward kinematics calculation includes parallel forward kinematics and series forward kinematics.

[0019] Furthermore, the shape of the regularity in step two is one of an ellipsoid, a cylinder, and a cube.

[0020] Furthermore, the method for updating the corresponding Jacobian matrix based on the current actual robotic arm configuration in step three includes the following steps:

[0021] Step 3: Calculate the Cartesian coordinates of all hinge points and the origin of the coordinate system in the base coordinate system;

[0022] Step 3.2. Based on the coordinates of the hinge points and the origin of the coordinate system of each module, establish the parallel Jacobian matrix of each module in the base coordinate system, and map the operation space of a single module to the joint space of the single module through the parallel inverse Jacobian matrix.

[0023] Step 3: Equivalent each module to a series of kinematic joints with six degrees of freedom, and construct the series Jacobian matrix of the overall system using velocity recursion and the vector product method. The expression is:

[0024]

[0025] in, , Let be the inverse of the parallel Jacobian matrix of the i-th module in the base coordinate system. , Let i be the velocity transfer matrix from module i to module n;

[0026] The expression is as follows:

[0027]

[0028] in, Let the origin of the coordinate system of the hinge point on the i-th module be... Pointing to the origin of the end coordinate system The vector, which is in the base coordinate system The following indicates that, For vector The constructed antisymmetric matrix, It is a 3x3 identity matrix. It is a zero matrix with three rows and three columns.

[0029] Furthermore, the specific implementation method of step four includes the following steps:

[0030] Step 41: Construct a criterion function to evaluate the proximity of obstacles to the robotic arm. A mapping from joint space to the criterion function is established, and then the gradient vector of the criterion function evaluating the proximity of the obstacle to the robotic arm in joint space is calculated. ; where, criterion function By converting the serial-parallel robotic arm into several serial links, the proximity of the obstacle to the robotic arm is evaluated based on the minimum pseudo distance from the converted links to the obstacle.

[0031] Step 42: Construct a criterion function to evaluate the approach of a joint to its travel limit. First, establish a mapping from joint space to the criterion function, and then calculate the gradient vector of the criterion function in joint space that evaluates the proximity of a joint to its travel limit. ; where, criterion function The degree to which a joint approaches its travel limit is evaluated by the distance from the current joint variable to the drive limit;

[0032] Step 4.3: Calculate the difference between the current end-effector pose and the desired pose of the trajectory point. Then, the motion of the robotic arm is decoupled into the main motion controlling the end effector's posture and the self motion projecting the gradient of the criterion function onto the null space. Numerical solutions and discretized integrations of the overall mechanism's velocity-level inverse kinematics are then performed to update the joint variables. The velocity-level inverse kinematics expression is as follows:

[0033]

[0034] in, The change in joint variables calculated for a single iteration; For the corresponding The primary motion, controlling the pose of the end effector. is the pseudo-inverse of the global Jacobian matrix, and E is the velocity transformation matrix; Let I be the self-motion term and I be the identity matrix. The amplification factor of the gradient vector of the criterion function for evaluating the proximity of an obstacle to the robotic arm in joint space. The amplification factor of the gradient vector in joint space for evaluating the criterion function of a joint approaching its travel limit;

[0035] When the Jacobian matrix has full row rank , It is an equation The minimum norm solution, pose change Transform from the Euler angular velocity level to the angular velocity level in the base coordinate system;

[0036] The self-motion of the ultra-redundant serial-parallel robotic arm is generated by the formula... By projecting the gradient of the criterion function onto the null space of the global Jacobian matrix, obstacle avoidance and travel limit avoidance are achieved without affecting the end-effector pose. Under the end-effector pose constraint, the self-motion in the null space adjusts the redundant degrees of freedom to generate a variety of robotic arm configurations.

[0037] The beneficial effects of this invention are:

[0038] The present invention discloses a method for obstacle avoidance and trajectory planning of a super-redundant serial-parallel robotic arm based on null space. This method introduces the gradient projection method based on null space into the super-redundant serial-parallel robotic arm, avoiding complex optimization algorithm calculations. It enables the robotic arm to avoid obstacles while ensuring accurate tracking of the end-effector trajectory, and also achieves the avoidance of joint over-limit during the movement.

[0039] The present invention discloses a method for obstacle avoidance and trajectory planning of a super-redundant serial-parallel robotic arm based on zero space. This method is simple to implement, easy to debug, and has low computational complexity. It only requires input of key parameters of the mechanism, obstacle attributes, desired trajectory, and a small number of coefficients to perform trajectory planning in joint space. Moreover, this method has strong scalability and is not limited by the number of parallel modules or their specific configuration. It fills the gap in the case of super-redundant serial-parallel robotic arms under the obstacle avoidance method system with fixed end-effector trajectory constraints. Attached Figure Description

[0040] Figure 1 This is a flowchart of an obstacle avoidance and trajectory planning method for a super-redundant serial-parallel robotic arm based on zero space, as described in this invention.

[0041] Figure 2 A schematic diagram of a six-degree-of-freedom parallel mechanism module for a super-redundant serial-parallel robotic arm;

[0042] Figure 3 This is a schematic diagram of a super-redundant serial-parallel robotic arm structure (when the number of modules is 4).

[0043] Figure 4 To simplify the pseudo-distance diagram from the link to the obstacle;

[0044] Figure 5 Schematic diagrams of various robotic arm configurations generated by zero-space self-motion under end-effector pose constraints;

[0045] Figure 6 This is a schematic diagram of the obstacle avoidance effect of a super-redundant serial-parallel robotic arm, where (a) is the right view, (b) is the front view, (c) is the axonometric view, and (d) is the top view.

[0046] Figure 7 shows the minimum pseudo-distance curves between the super-redundant serial-parallel robotic arm and the obstacle, where (a) is the graph corresponding to the self-motion term and (b) is the graph corresponding to the non-self-motion term.

[0047] Figure 8 A graph showing the change in the length of the 24 joints of a super-redundant serial-parallel robotic arm over time;

[0048] Figure 9 The graphs show the end pose error norm and iteration count for discrete-time iteration points, where (a) is the pose error curve for the iteration point and (b) is the iteration count curve for each step. Detailed Implementation

[0049] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only for explaining the invention and are not intended to limit the invention; that is, the described specific embodiments are merely a part of the embodiments of the invention, and not all of them. The components of the specific embodiments of the invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations, and the invention may also have other embodiments.

[0050] Therefore, the following detailed description of specific embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected specific embodiments of the invention. All other specific embodiments obtained by those skilled in the art based on these specific embodiments without inventive effort are within the scope of protection of this invention.

[0051] To further understand the invention's content, features, and effects, the following specific embodiments are provided, along with accompanying drawings. Figure 1 - Appendix Figure 9 Detailed explanation is as follows:

[0052] Example 1:

[0053] A method for obstacle avoidance and trajectory planning of a super-redundant serial-parallel robotic arm based on null space includes the following steps:

[0054] Step 1: Set the structural parameters of the six-DOF parallel mechanism module, the coordinate system of the upper and lower hinge points of the module, the number of modules, the base coordinate system, the initial joint variables, the relative poses of adjacent modules and the desired trajectory of the end effector of the robotic arm. Then, use global forward kinematics to calculate the homogeneous transformation matrix of the coordinate system of the upper and lower hinge points of each module in the base coordinate system and the end effector pose.

[0055] Furthermore, such as Figure 2 As shown, the six-degree-of-freedom parallel mechanism module in step one consists of an upper platform, a lower platform, and six identical UPS (Hooke joint-sliding pair-spherical pair) motion chains. Each motion chain consists of a Hooke joint, a spherical joint, and an electric cylinder. The six electric cylinders drive the upper platform to achieve the six-degree-of-freedom motion of the upper platform.

[0056] Define the lower hinge point coordinate system and the upper hinge point coordinate system of the i-th module as follows: and The origins of the lower hinge point coordinate system and the upper hinge point coordinate system are the centers of the circles at each hinge point, and are defined as follows: and The z-axis is perpendicular to the plane containing the hinge point circle, the x-axis is perpendicular to the line connecting the two hinge points, and the y-axis is determined by the right-hand rule.

[0057] If the total number of modules is set to n, then the robotic arm has 6n degrees of freedom and 6n actuators; the base coordinate system is set to the coordinate system of the lower hinge point of the first module. ;like Figure 3 The diagram shows a super-redundant serial-parallel robotic arm structure with 4 modules, consisting of four parallel mechanism modules connected in series, with 24 degrees of freedom and 24 actuators.

[0058] Define the initial joint variables as column vectors , which corresponds to the length of 6n actuators in the joint space, where R is a real number field;

[0059] Define the relative pose of adjacent modules as the coordinate system of the lower hinge point of the (i+1)th module. Relative to the hinge point coordinate system of the i-th module The pose of adjacent modules is rigidly connected, and the relative pose of adjacent modules remains unchanged during the motion.

[0060] The desired trajectory of the robotic arm's end effector is defined as the trajectory of the end effector coordinate system relative to the base coordinate system. It consists of the desired poses of the endpoints of discrete trajectory points, and the desired pose of the endpoint of the i-th trajectory point is set as... The desired position is The desired posture is Represented by ZYX Euler angles, where , , ;

[0061] The overall forward kinematics calculation includes parallel forward kinematics and series forward kinematics.

[0062] Furthermore, the specific steps for calculating the overall forward kinematics are as follows:

[0063] Step 11: Based on the forward kinematics of the parallel mechanism, transform the joint variables of the i-th module into a homogeneous transformation matrix. The forward kinematics of the parallel mechanism is solved through numerical iteration;

[0064] Steps 1 and 2: Convert the relative poses of adjacent modules into homogeneous transformation matrices. ;

[0065] Step 13: Transform the homogeneous transformation matrix between the coordinate systems to the base coordinate system using the forward kinematics of the cascaded mechanism. For the coordinate system of the lower hinge point of the i-th module and the coordinate system of the upper hinge point It can be calculated using the following formula:

[0066]

[0067]

[0068] Step 14: Perform a homogeneous transformation of the final coordinate system using the matrix. Transform into pose .

[0069] Step 2: Identify obstacles in the work environment and enclose them with regular shapes; the shape and surface of the obstacles are described by hyperquadratic surface functions and homogeneous transformation matrix of the obstacle coordinate system relative to the base coordinate system, and parameterized by obstacle properties; pseudo-distance is used to represent the distance from a point to an obstacle;

[0070] Furthermore, the shape of the regularity in step two is one of an ellipsoid, a cylinder, and a cube.

[0071] Furthermore, the hyperquadratic surface function is expressed in the obstacle coordinate system as:

[0072]

[0073] Where (m,n,p) represents the shape of the surface, (1,1,1) represents the ellipsoidal surface, (1,1,4) represents the cylindrical surface, and (4,4,4) represents the cubic surface; (h1,h2,h3) represents the size of the surface in each direction; and (x0,y0,z0) represents the coordinates of the center of the surface in the obstacle coordinate system.

[0074] Furthermore, obstacle attributes include: obstacle type, homogeneous transformation matrix of obstacle coordinate system relative to base coordinate system, obstacle shape parameters (m,n,p), obstacle geometric dimension parameters (h1,h2,h3), and the position of obstacle center in its own coordinate system (x0,y0,z0).

[0075] Furthermore, the pseudo-distance is the value of the hyperquadratic surface function, when When the point is located within the surface of the obstacle, When, the point is located on the curved surface of the obstacle, when At that time, the point is located outside the curved surface of the obstacle.

[0076] Step 3: Extract the desired pose of the trajectory points from the desired trajectory of the robotic arm's end effector in Step 1. Update the corresponding Jacobian matrix based on the current actual robotic arm configuration: calculate the coordinates of each hinge point and the origin of the coordinate system based on the homogeneous transformation matrix of the coordinate system of each module's upper and lower hinge points relative to the base coordinate system, calculate the parallel Jacobian matrix of a single module, and then construct the overall series Jacobian matrix of the whole mechanism through velocity recursion and vector product method.

[0077] Furthermore, the method for updating the corresponding Jacobian matrix based on the current actual robotic arm configuration in step three includes the following steps:

[0078] Step 3: Calculate the Cartesian coordinates of all hinge points and the origin of the coordinate system in the base coordinate system;

[0079] Step 3.2. Based on the coordinates of the hinge points and the origin of the coordinate system of each module, establish the parallel Jacobian matrix of each module in the base coordinate system, and map the operation space of a single module to the joint space of the single module through the parallel inverse Jacobian matrix.

[0080] Step 3: Equivalent each module to a series of kinematic joints with six degrees of freedom, and construct the series Jacobian matrix of the overall system using velocity recursion and the vector product method. The expression is:

[0081]

[0082] in, , Let be the inverse of the parallel Jacobian matrix of the i-th module in the base coordinate system. , is the velocity transfer matrix from the i-th module to the n-th module;

[0083] The expression is as follows:

[0084]

[0085] in, Let the origin of the coordinate system of the hinge point on the i-th module be... Pointing to the origin of the end coordinate system The vector, which is in the base coordinate system The following indicates that, For vector The constructed antisymmetric matrix, It is a 3x3 identity matrix. It is a zero matrix with three rows and three columns.

[0086] Step 4: Using the shape and surface of the obstacle obtained in Step 2, construct a criterion function to evaluate the proximity of the obstacle to the robotic arm; using the structural parameters of the six-DOF parallel mechanism module obtained in Step 1, construct a criterion function to evaluate the approach stroke limit of the joints; calculate the current end-effector pose. With the expected pose of the trajectory point The difference is then used to decouple the motion of the robotic arm into the main motion that controls the end-effector posture and the self motion that projects the gradient of the criterion function onto the null space. The numerical solution and discretized integral of the overall mechanism velocity-level inverse kinematics are then performed to update the joint variables.

[0087] Furthermore, the specific implementation method of step four includes the following steps:

[0088] Step 41: Construct a criterion function to evaluate the proximity of obstacles to the robotic arm. A mapping from joint space to the criterion function is established, and then the gradient vector of the criterion function evaluating the proximity of the obstacle to the robotic arm in joint space is calculated. ; where, criterion function By converting the serial-parallel robotic arm into several serial links, the proximity of the obstacle to the robotic arm is evaluated based on the minimum pseudo distance from the converted links to the obstacle.

[0089] Furthermore, the specific steps of step four-one are as follows:

[0090] Step 4.11: Simplify each parallel module into a single link. The starting point of the simplified link of the i-th module is the origin of the lower hinge point coordinate system. The endpoint is the origin of the lower hinge point coordinate system. Then, the starting point coordinates of each module are transformed to the obstacle coordinate system;

[0091] Step 4.12: Calculate the pseudo-distance from each simplified link to the obstacle. ;like Figure 4 This demonstrates the pseudo-distances from n simplified links to obstacles, where the point corresponding to each link pseudo-distance is the point on the link with the minimum value of the hyperquadratic surface function.

[0092] Furthermore, the specific steps for determining the pseudo-distance of each simplified link in step four-one-two are as follows:

[0093] The coordinates of a point on the i-th link in the obstacle coordinate system are calculated using the following formula:

[0094]

[0095] in, ;remember for The coordinates of the closest point on the robotic arm to the obstacle. Through parameters calculate, The corresponding point with the minimum pseudo-distance.

[0096] After obtaining the obstacle attributes, the following judgments and calculations are performed:

[0097] For an ellipsoidal surface, i.e., (m,n,p)=(1,1,1): the surface equation in the obstacle coordinate system is expressed as follows:

[0098]

[0099] in:

[0100]

[0101]

[0102]

[0103]

[0104] The parameter corresponding to the point on the link closest to the obstacle surface is:

[0105]

[0106] in, .

[0107] when hour, The value is 0, and the minimum distance point is ;when hour, The value is 1, and the minimum distance point is .

[0108] Will corresponding By substituting the surface equation into the obstacle coordinate system, the pseudo-distance value corresponding to the current module can be calculated. .

[0109] For cylindrical and cubic surfaces, i.e., (m,n,p)=(1,1,4) and (m,n,p)=(4,4,4): because the power of their functions is higher than 2, they cannot be represented by a matrix expression containing quadratic forms, and therefore the minimum pseudo-distance point cannot be found analytically using the parameters corresponding to the point on the link closest to the obstacle surface. Therefore, a method is adopted... A method for discretizing several points and finding the minimum value. The coordinates of the five corresponding points are The pseudo-distance value corresponding to the current module is then:

[0110] ;

[0111] Step 413: Define the criterion function for the proximity of the obstacle to the robotic arm as follows:

[0112]

[0113] in, , is the minimum pseudo distance from the n simplified links to the obstacle; The pseudo-distance is calculated based on a simplified linkage to determine the obstacle avoidance threshold. The actual parallel module is roughly a cylinder, and a threshold needs to be set to prevent the module surface from colliding with obstacles.

[0114] Step 414: Based on the global positive kinematics in Step 1, a mapping from joint space to the Cartesian space of all modules can be established. Based on Step 412, a mapping from Cartesian space to pseudo-distance can be established. Then, based on the above and Step 413, a mapping from joint space to criterion function can be established.

[0115] Step 415: Calculate the gradient vector in joint space of the criterion function for evaluating the proximity of the obstacle to the robotic arm using the finite difference method. The formula is as follows:

[0116]

[0117]

[0118] in, Represents the element in the i-th row of the gradient. It is a very small positive real number. It is a 6n-dimensional unit column vector with the element in the i-th row being 1.

[0119] Step 42: Construct a criterion function to evaluate the approach of a joint to its travel limit. First, establish a mapping from joint space to the criterion function, and then calculate the gradient vector of the criterion function in joint space that evaluates the proximity of a joint to its travel limit. ; where, criterion function The degree to which a joint approaches its travel limit is evaluated by the distance from the current joint variable to the drive limit;

[0120] Furthermore, the criterion function used in step four-two to evaluate the approach of the joint to its travel limit is defined as follows:

[0121]

[0122] in, The number of joints, The definition is as follows:

[0123]

[0124]

[0125] in, Let be the variable value of the i-th joint. This represents the distance from the i-th joint variable to the upper and lower limits of the joint; , These are the upper and lower limits of the joint variable, respectively. for The safety threshold is used to establish a mapping from joint space to criterion functions.

[0126] The gradient vector of the criterion function for evaluating the proximity of a joint to its travel limit in joint space is calculated using an analytical expression, as follows:

[0127]

[0128] ;

[0129] Step 4.3: Calculate the difference between the current end-effector pose and the desired pose of the trajectory point. Then, the motion of the robotic arm is decoupled into the main motion controlling the end effector's posture and the self motion projecting the gradient of the criterion function onto the null space. Numerical solutions and discretized integrations of the overall mechanism's velocity-level inverse kinematics are then performed to update the joint variables. The velocity-level inverse kinematics expression is as follows:

[0130]

[0131] in, The change in joint variables calculated for a single iteration; For the corresponding The primary motion, controlling the pose of the end effector. is the pseudo-inverse of the global Jacobian matrix, and E is the velocity transformation matrix; Let I be the self-motion term and I be the identity matrix. The amplification factor of the gradient vector of the criterion function for evaluating the proximity of an obstacle to the robotic arm in joint space. The amplification factor of the gradient vector in joint space for evaluating the criterion function of a joint approaching its travel limit;

[0132] When the Jacobian matrix has full row rank , It is an equation The minimum norm solution, pose change Transform from the Euler angular velocity level to the angular velocity level in the base coordinate system;

[0133] The self-motion of the ultra-redundant serial-parallel robotic arm is generated by the formula... By projecting the gradient of the criterion function onto the null space of the global Jacobian matrix, obstacle avoidance and travel limit avoidance are achieved without affecting the end-effector pose. Under the end-effector pose constraint, the self-motion in the null space adjusts the redundant degrees of freedom to generate a variety of robotic arm configurations.

[0134] Figure 5 This demonstrates various robotic arm configurations resulting from zero-space self-motion adjusting redundant degrees of freedom under end-effector pose constraints; , Defined using real-time correction:

[0135]

[0136]

[0137] in, Describes the 2-norm of a matrix. , It is a user-defined coefficient used to proportionally change... , The size of the variable. This real-time correction method ensures that the norm of the self-motion term is in a certain proportion to the norm of the main motion term, preventing excessive gradients from causing joint over-limits or sudden velocity changes.

[0138] The definition is as follows:

[0139]

[0140]

[0141] in, This is the transformation matrix from Euler angular velocity in the base coordinate system to angular velocity in the base coordinate system. These are the ZYX Euler angle coordinates in the current base coordinate system.

[0142] Step 5: Apply the overall mechanism forward kinematics to the joint variables updated in Step 4, and update the homogeneous transformation matrix of the upper and lower hinge point coordinate systems of each module in the base coordinate system and the end-effector pose, and determine the end-effector pose. Whether the calculation accuracy requirement is met, if the end-effector pose If the requirements are met, output the information of each joint and update the expected pose of the trajectory points in step three. Then return to step three to iterate and solve for the next trajectory point until all trajectory points have been traversed; if the requirements are not met, return directly to step three to continue iterating and calculating the current trajectory point.

[0143] Furthermore, the calculation accuracy requirements in step five include two accuracy requirements: position accuracy requirements and attitude accuracy requirements, with the specific conditions as follows:

[0144]

[0145] in, for The first three lines represent the positional error at the end; for The last three lines represent the attitude error at the end point; and These are the requirements for position accuracy and attitude accuracy, respectively.

[0146] This invention also proposes a zero-space-based super-redundant serial-parallel robotic arm obstacle avoidance and trajectory planning system, comprising:

[0147] Parameter initialization module: Defines the robotic arm's structural parameters, initial joint variables, coordinate system, and desired end effector trajectory;

[0148] Criterion function definition module: Defines obstacle avoidance criterion functions and joint over-limit avoidance criterion functions through obstacle attributes, joint upper and lower limits, and correlation coefficients;

[0149] Overall mechanism forward kinematics calculation module: Calculates the homogeneous transformation matrix of each module and the pose of the end-effector coordinate system based on the current joint variables.

[0150] Overall mechanism velocity-level inverse kinematics calculation module: calculates the current overall Jacobian matrix, and then performs the calculation of the main motion terms and self-motion terms at the velocity level. The self-motion terms include obstacle avoidance terms and joint over-limit avoidance terms.

[0151] End-point accuracy detection module: Determines whether the end-point accuracy of the current iteration point meets the standard, and proceeds to the next iteration based on the judgment result.

[0152] Example 2:

[0153] This embodiment is based on the method described in Embodiment 1. The difference is that this embodiment provides specific values ​​for some parameters to further illustrate the implementation process and technical effects of the method of the present invention.

[0154] The effectiveness of the method of the present invention was verified under common motion and obstacle conditions:

[0155] Module structural parameters are set as follows: number of modules n = 4, radius of both upper and lower hinge points = 0.15m, and relative pose of adjacent modules is the same. The upper and lower limits of the joint are respectively , The initial joint variables are taken as those when all joints are in the middle of their stroke. .

[0156] The desired trajectory of the robotic arm's end effector is defined by the following formula, causing it to move along a straight line in Cartesian space, with its attitude motion being a rotation about the x-axis. Divide the time trajectory defined by the following formula into 100 trajectory points.

[0157]

[0158] The properties of the obstacles are shown in Table 1:

[0159] Table 1:

[0160]

[0161] Criterion function Threshold in It is 25, the coefficient -1; Criterion function Threshold in The coefficient is 0.08. It is -0.5.

[0162] Position accuracy requirements and attitude accuracy requirements Set all to .

[0163] Based on the given parameters and trajectory, the method of this invention can be used to solve the problem, resulting in... Figure 6 The diagram illustrates the obstacle avoidance effect of the hyper-redundant serial-parallel manipulator. It shows that when the manipulator approaches an obstacle, it avoids it based on its own motion without affecting the end-effector pose. The self-motion term in the overall mechanism's velocity-level inverse kinematics projects the gradient of the criterion function onto the null space of the overall Jacobian matrix, enabling the hyper-redundant serial-parallel manipulator to achieve obstacle avoidance and joint overrun avoidance under end-effector trajectory constraints. When the method does not include a self-motion term (without utilizing the null space of the overall Jacobian matrix), the hyper-redundant serial-parallel manipulator cannot achieve obstacle avoidance and joint overrun avoidance under end-effector trajectory constraints. Figure 7 shows the minimum pseudo-distance curves between the hyper-redundant serial-parallel manipulator and the obstacle with and without a self-motion term. It can be seen that under the current planning requirements, when the algorithm has a self-motion term, the pseudo-distance is always greater than the allowed minimum value; when there is no self-motion term, the pseudo-distance will be lower than the allowed minimum value for a certain period, indicating a collision between the manipulator and the obstacle. The allowed minimum pseudo-distance is set based on the radius of the module hinge point and the size of the obstacle.

[0164] Figure 8 The diagram shows the change in the length of the 24 joints of the super-redundant serial-parallel robotic arm over time. It can be seen that the length of all joints is limited to the range of [0.3m, 0.5m], which means that the joints will not exceed their stroke. Figure 9 The curves showing the end-position pose error norm and iteration number at discrete-time iteration points are presented. It can be seen that both the position and pose error norms are less than 1×10. -6 The number of iterations for each trajectory point is less than or equal to 3.

[0165] In summary, the method of the present invention enables a super-redundant serial-parallel robotic arm to avoid obstacles and reasonably allocate the drive length of the actuators to avoid drive limits during operation in complex environments without affecting the end-effector trajectory.

[0166] It should be noted that relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0167] Although this application has been described above with reference to specific embodiments, various modifications can be made and components can be replaced with equivalents without departing from the scope of this application. In particular, as long as there is no structural conflict, the features in the specific embodiments disclosed in this application can be combined with each other in any way. The lack of an exhaustive description of these combinations in this specification is merely for the sake of brevity and resource conservation. Therefore, this application is not limited to the specific embodiments disclosed herein, but includes all technical solutions falling within the scope of the claims.

Claims

1. A method for obstacle avoidance and trajectory planning of a hyper-redundant series-parallel manipulator based on null space, characterized in that, Includes the following steps: Step 1: Set the structural parameters of the six-DOF parallel mechanism module, the coordinate system of the upper and lower hinge points of the module, the number of modules, the base coordinate system, the initial joint variables, the relative poses of adjacent modules and the desired trajectory of the end effector of the robotic arm. Then, use global forward kinematics to calculate the homogeneous transformation matrix of the coordinate system of the upper and lower hinge points of each module in the base coordinate system and the end effector pose. Step 2: Identify obstacles in the work environment and enclose them with regular shapes; the shape and surface of the obstacles are described by hyperquadratic surface functions and homogeneous transformation matrix of the obstacle coordinate system relative to the base coordinate system, and parameterized by obstacle properties; pseudo-distance is used to represent the distance from a point to an obstacle; Step three, extract the trajectory point desired pose from the end of the arm desired trajectory in step one as ; Update the corresponding Jacobian matrix based on the current actual robotic arm configuration: calculate the coordinates of each hinge point and the origin of the coordinate system based on the homogeneous transformation matrix of the coordinate system of each module's upper and lower hinge points relative to the base coordinate system, calculate the parallel Jacobian matrix of a single module, and then construct the overall series Jacobian matrix of the whole mechanism through velocity recursion and vector product method. Step 4: Using the shape and surface of the obstacle obtained in Step 2, construct a criterion function to evaluate the degree of proximity between the obstacle and the robotic arm; using the structural parameters of the six-degree-of-freedom parallel mechanism module obtained in Step 1, construct a criterion function to evaluate the approach stroke limit of the joint. Computing current end-effector pose Difference from trajectory point desired pose The motion of the robot arm is then decoupled into a primary motion that controls the end-effector pose and a secondary motion that projects the criterion function gradient to the null space, and a numerical solution of the overall mechanism velocity-level inverse kinematics and a discretized integration is performed to update the joint variables; Step five, the joint variables updated in step four are solved by using the whole mechanism forward kinematics and the homogeneous transformation matrix of each module upper and lower hinge point coordinate system in the base coordinate system and the end position are updated, and the end position is determined whether it reaches the calculation accuracy requirement. If the end position meets the requirement, the joint information is output and the trajectory point expected position in step three is updated , and then step three is returned to iteratively solve the next trajectory point until all trajectory points are traversed; if the requirement is not met, step three is directly returned to continue the iterative calculation for the current trajectory point.

2. The zero-space based obstacle avoidance and trajectory planning method for an over-actuated series-parallel manipulator according to claim 1, wherein, The six-degree-of-freedom parallel mechanism module in step one consists of an upper platform, a lower platform, and six identical UPS motion chains. Each motion chain consists of a Hooke joint, a ball joint, and an electric cylinder. The six electric cylinders drive the upper platform to achieve the six-degree-of-freedom motion of the upper platform. The lower hinge point coordinate system and the upper hinge point coordinate system of the ith module are respectively defined as and The origins of the lower hinge point coordinate system and the upper hinge point coordinate system are the centers of the hinge point circles, and are respectively defined as and The z-axis is perpendicular to the plane in which the hinge point circle is located, the x-axis is perpendicular to the line connecting the two hinge points, and the y-axis is determined by the right-hand rule. The total number of the modules is n, and the mechanical arm has 6n degrees of freedom and 6n actuators; the base coordinate system is the lower hinge point coordinate system of the first module ​ Let the initial joint variables be defined as a column vector R is the field of real numbers, corresponding to the lengths of the 6n actuators in the joint space. Define the relative pose of adjacent modules as the coordinate system of the lower hinge point of the (i+1)th module. Relative to the hinge point coordinate system of the i-th module The pose of adjacent modules is rigidly connected, and the relative pose of adjacent modules remains unchanged during the motion. The desired trajectory of the robotic arm's end effector is defined as the trajectory of the end effector coordinate system relative to the base coordinate system. It consists of the desired poses of the endpoints of discrete trajectory points, and the desired pose of the endpoint of the i-th trajectory point is set as... The desired position is The desired posture is Represented using Euler angles; The overall forward kinematics calculation includes parallel forward kinematics and series forward kinematics.

3. The obstacle avoidance and trajectory planning method for a super-redundant serial-parallel robotic arm based on null space as described in claim 2, characterized in that, The shape of the regular shape in step two is one of an ellipsoid, a cylinder, and a cube.

4. The obstacle avoidance and trajectory planning method for a super-redundant serial-parallel robotic arm based on null space as described in claim 3, characterized in that, Step 3, which updates the corresponding Jacobian matrix based on the current actual robotic arm configuration, includes the following steps: Step 3: Calculate the Cartesian coordinates of all hinge points and the origin of the coordinate system in the base coordinate system; Step 3.

2. Based on the coordinates of the hinge points and the origin of the coordinate system of each module, establish the parallel Jacobian matrix of each module in the base coordinate system, and map the operation space of a single module to the joint space of the single module through the parallel inverse Jacobian matrix. Step 3: Equivalent each module to a series of kinematic joints with six degrees of freedom, and construct the series Jacobian matrix of the overall system using velocity recursion and the vector product method. The expression is: ； in, , Let be the inverse of the parallel Jacobian matrix of the i-th module in the base coordinate system. , Let i be the velocity transfer matrix from module i to module n; The expression is as follows: ； in, Let the origin of the coordinate system of the hinge point on the i-th module be... Pointing to the origin of the end coordinate system The vector, which is in the base coordinate system The following indicates that, For vector The constructed antisymmetric matrix, It is a 3x3 identity matrix. It is a zero matrix with three rows and three columns.

5. The obstacle avoidance and trajectory planning method for a super-redundant serial-parallel robotic arm based on null space as described in claim 4, characterized in that, The specific implementation method of step four includes the following steps: Step 41: Construct a criterion function to evaluate the proximity of obstacles to the robotic arm. A mapping from joint space to the criterion function is established, and then the gradient vector of the criterion function evaluating the proximity of the obstacle to the robotic arm in joint space is calculated. ; where, criterion function By converting the serial-parallel robotic arm into several serial links, the proximity of the obstacle to the robotic arm is evaluated based on the minimum pseudo distance from the converted links to the obstacle. Step 42: Construct a criterion function to evaluate the approach of a joint to its travel limit. First, establish a mapping from joint space to the criterion function, and then calculate the gradient vector of the criterion function in joint space that evaluates the proximity of a joint to its travel limit. ; where, criterion function The degree to which a joint approaches its travel limit is evaluated by the distance from the current joint variable to the drive limit; Step 4.3: Calculate the difference between the current end-effector pose and the desired pose of the trajectory point. Then, the motion of the robotic arm is decoupled into the main motion controlling the end effector's posture and the self motion projecting the gradient of the criterion function onto the null space. Numerical solutions and discretized integrations of the overall mechanism's velocity-level inverse kinematics are then performed to update the joint variables. The velocity-level inverse kinematics expression is as follows: ； in, The change in joint variables calculated for a single iteration; For the corresponding The primary motion, controlling the pose of the end effector. is the pseudo-inverse of the global Jacobian matrix, and E is the velocity transformation matrix; Let I be the self-motion term and I be the identity matrix. The amplification factor of the gradient vector of the criterion function for evaluating the proximity of an obstacle to the robotic arm in joint space. The amplification factor of the gradient vector in joint space for evaluating the criterion function of a joint approaching its travel limit; When the Jacobian matrix has full row rank , It is an equation The minimum norm solution, pose change Transform from the Euler angular velocity level to the angular velocity level in the base coordinate system; The self-motion of the ultra-redundant serial-parallel robotic arm is generated by the formula... By projecting the gradient of the criterion function onto the null space of the global Jacobian matrix, obstacle avoidance and travel limit avoidance are achieved without affecting the end-effector pose. Under the end-effector pose constraint, the self-motion in the null space adjusts the redundant degrees of freedom to generate a variety of robotic arm configurations.

Images