A trajectory control method and system of a mechanical arm, an electronic device, and a storage medium

Abstract

The present application relates to the technical field of mechanical motion control, and discloses a trajectory control method and system for a mechanical arm, an electronic device and a storage medium, wherein the trajectory constraint of the mechanical arm is determined according to the motion space boundary limitation of the mechanical arm; the motion trajectory of the mechanical arm is determined through the spherical parameter equation of the mechanical arm; under the trajectory constraint of the mechanical arm, the motion trajectory of the mechanical arm is executed, and the error between the end posture of the mechanical arm and the preset expected posture is minimized, so that the motion trajectory of the mechanical arm can be accurately controlled to a large extent.

Description

Technical Field

[0001] This invention relates to the field of mechanical motion control technology, and in particular to a trajectory control method, system, electronic device and storage medium for a robotic arm. Background Technology

[0002] Currently, robotic arms are commonly used for grasping and other tasks in complex environments such as substations. Compared to traditional robotic arms, modern robotic arms have more degrees of freedom and have become a research hotspot in recent years due to their high flexibility, adaptability, obstacle avoidance, obstacle avoidance, and joint limit avoidance.

[0003] However, in confined spaces, it is difficult for robotic arms to move along an ideal trajectory, which can lead to accidents such as damage to the robotic arm or objects in the space. Therefore, it is crucial to achieve high-precision and low-internal-force collaborative tasks for robotic arms. However, current robotic arms are difficult to control their movement trajectory accurately to a large extent. Summary of the Invention

[0004] In view of this, the present invention provides a trajectory control method, system, electronic device and storage medium for a robotic arm, which solves the technical problem that current robotic arms are difficult to accurately control their motion trajectory to a large extent.

[0005] The first aspect of this invention provides a trajectory control method for a robotic arm, comprising:

[0006] The trajectory constraints of the robotic arm are determined based on the boundary limitations of its motion space.

[0007] The motion trajectory of the robotic arm is determined by the spherical parametric equation of the robotic arm.

[0008] Under the trajectory constraints of the robotic arm, the motion trajectory of the robotic arm is executed, and the error between the end-effector posture of the robotic arm and the preset desired posture is minimized.

[0009] Preferably, the trajectory constraints of the robotic arm include obstacle avoidance constraints, joint limit constraints, dynamic constraints, and motion space boundary constraints.

[0010] Preferably, the step of determining the motion trajectory of the robotic arm through the spherical parametric equation of the robotic arm includes:

[0011] The spherical parameter equation of the robotic arm is determined based on its rotation angle. The spherical parameter equation is as follows:

[0012]

[0013]

[0014]

[0015] In the formula, x, y, and z are the x-axis, y-axis, and z-axis coordinates of the robotic arm's end effector on the sphere, respectively, and r is the distance of the robotic arm from the center of the obstacle. The end-effector polar angle of the robotic arm. This is the end effector azimuth angle of the robotic arm. ;

[0016] Based on the spherical parametric equation, the coordinate set of the robotic arm's end effector on the spherical surface for the next time step is updated according to the angular velocity of the robotic arm. The updated coordinate set of the robotic arm's end effector on the spherical surface for the next time step is:

[0017]

[0018]

[0019]

[0020] In the formula, Indicates the current moment. Indicates time interval, Indicates the next time step. , , Let x, y, and z represent the x-axis, y-axis, and z-axis coordinates of the robotic arm's end effector on the sphere at the next time step. , Let represent the polar angle and azimuth angle of the robotic arm's end effector at the next time step, respectively.

[0021]

[0022]

[0023] In the formula, , These are the polar angle and azimuth angle of the robotic arm's end effector at the current time t, respectively. , These are the angular velocities of the robotic arm's end effector in the latitudinal and longitudinal directions, respectively. , These are the angular velocity disturbance terms of the robotic arm's end effector in the latitudinal direction and the angular velocity disturbance term in the longitudinal direction, respectively.

[0024]

[0025]

[0026] In the formula, , Let represent the angular velocities of the robotic arm's end effector in the latitude and longitude directions, respectively, with a mean of 0 and a variance of σ. 2 The normal distribution;

[0027] The coordinates of the end effector at each time step are determined based on the updated coordinate set of the end effector on the sphere at each time step, and the motion trajectory of the end effector is formed.

[0028] Preferably, the method further includes the step of determining the optimal posture of the end effector of the robotic arm, with the optimization objective being to minimize the error between the end effector posture of the robotic arm and the preset desired posture;

[0029] The step of determining the optimal posture of the robotic arm's end effector, with the optimization objective of minimizing the error between the end effector posture and the preset desired posture, includes:

[0030] Construct rotation matrices corresponding to the initial posture and the preset desired posture of the end effector of the robotic arm, respectively;

[0031] The transformation matrix between the joint angle vector at the end of the robotic arm and the rotation matrix is ​​determined using the DH parameter method.

[0032] Based on the joint angle vector of the end of the robotic arm and the transformation matrix, determine the rotation matrix after joint angle vector transformation corresponding to the initial posture and the preset desired posture of the end of the robotic arm, respectively.

[0033] The attitude error function is determined based on the initial posture of the end effector of the robotic arm and the preset desired posture. The attitude error function is as follows:

[0034]

[0035] In the formula, The independent variable is the joint angle vector. The attitude error function, Here is a function to measure pose difference. , These are the rotation matrices of the initial posture of the end effector of the robotic arm after joint angle vector transformation, and the rotation matrices of the preset desired posture of the end effector of the robotic arm after joint angle vector transformation, respectively.

[0036] Based on the attitude error function and the motion cost function, an optimization problem is constructed to minimize the attitude error. The optimization problem is as follows:

[0037]

[0038] In the formula, T represents the total time. Let be the motion cost function. For trade-offs;

[0039] The optimization problem is solved to determine the joint angle vectors of the end effector of the robotic arm at each moment, thereby determining the optimal posture of the end effector.

[0040] Preferably, after the step of executing the motion trajectory of the robotic arm under the trajectory constraint of the robotic arm and minimizing the error between the end-effector posture of the robotic arm and the preset desired posture, the method further includes:

[0041] The robot arm collects image data during its movement and transmits the collected image data to the front end for display.

[0042] Preferably, the robotic arm has multiple degrees of freedom joints.

[0043] Secondly, the present invention also provides a trajectory control system for a robotic arm, comprising:

[0044] The constraint determination module is used to determine the trajectory constraints of the robotic arm based on the boundary limitations of the robotic arm's motion space.

[0045] The trajectory planning module is used to determine the motion trajectory of the robotic arm through the spherical parametric equations of the robotic arm;

[0046] The motion execution module is used to execute the motion trajectory of the robotic arm under the trajectory constraints of the robotic arm, and to minimize the error between the end-effector posture of the robotic arm and the preset desired posture.

[0047] Thirdly, the present invention also provides an electronic device, the electronic device including a memory and a processor, the memory storing a computer program, the computer program being executed by the processor causing the processor to perform the steps of the trajectory control method for the robotic arm as described in the first aspect.

[0048] Fourthly, the present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed, implements the steps of the trajectory control method for the robotic arm as described in the first aspect.

[0049] Fifthly, the present invention also provides a computer program product comprising a computer program stored on a non-transitory computer-readable storage medium, the computer program comprising program instructions, wherein, when the program instructions are executed by a computer, the computer performs the steps of the trajectory control method for the robotic arm as described in the first aspect.

[0050] As can be seen from the above technical solutions, the present invention has the following advantages:

[0051] This invention determines the trajectory constraints of the robotic arm by limiting its motion space boundary, and determines its motion trajectory by using the spherical parametric equation of the robotic arm. This ensures that the robotic arm's motion trajectory moves within a spherical space and satisfies the trajectory constraints of the robotic arm's motion space boundary, thereby preventing the robotic arm from colliding with other objects during movement. At the same time, by minimizing the error between the robotic arm's end-effector posture and the preset desired posture, the accuracy of the robotic arm's end-effector movement is improved, allowing for more accurate control of the robotic arm's motion trajectory. Attached Figure Description

[0052] Figure 1 This is a schematic diagram illustrating the application environment of the robotic arm used in an embodiment of the present invention.

[0053] Figure 2 A flowchart illustrating a trajectory control method for a robotic arm provided in an embodiment of the present invention;

[0054] Figure 3 This is a schematic diagram of the trajectory control system of a robotic arm provided in an embodiment of the present invention;

[0055] Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of the present invention. Detailed Implementation

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

[0057] The trajectory control method for the robotic arm provided in this application embodiment can be applied to, for example... Figure 1 The robotic arm shown is used in an application environment. The controller 101 controls the movement of the robotic arm 102. The controller 101 determines the trajectory constraints of the robotic arm 102 based on the boundary limitations of its motion space; it determines the motion trajectory of the robotic arm 102 through the spherical parametric equation; under the trajectory constraints, it executes the motion trajectory of the robotic arm 102, minimizing the error between the end effector posture of the robotic arm 102 and the preset desired posture.

[0058] The robotic arm 102 has multiple degrees of freedom joints.

[0059] like Figure 2 As shown, this application embodiment provides a trajectory control method for a robotic arm, which is applied to... Figure 1The following explanation uses controller 101 as an example, including steps S1 to S3.

[0060] Step S1: Determine the trajectory constraints of the robotic arm based on the motion space boundary limitations of the robotic arm.

[0061] The motion space of a robotic arm refers to the set of all positions and postures that the end effector of the robotic arm can reach.

[0062] Motion space boundary constraints refer to the constraints imposed on the trajectory of the robotic arm due to its design and the motion space it occupies, such as a narrow space, so that the trajectory constraints are met when the robotic arm performs motion actions according to the trajectory.

[0063] The trajectory constraints of the robotic arm include obstacle avoidance constraints, joint limit constraints, dynamic constraints, and motion space boundary constraints.

[0064] Obstacle avoidance constraints are designed to ensure that the robotic arm does not collide with obstacles while executing its trajectory, and require consideration of objects and obstacles in the motion space.

[0065] Specifically, based on the coordinates of the obstacle in the radar coordinate system and the distance from the single-line lidar as fed back by the single-line lidar, it is determined whether the front end of the robotic arm is about to collide with the obstacle. When the distance r between the radar on the robotic arm and the center of the obstacle is greater than the assumed constraint distance d, the robotic arm operates normally. In the rotation support, signals can be directly input to the servo motor of the robotic arm to control its rotation direction and rotation angle. During the rotation of the front end of the robotic arm, the rotation range that does not collide with the obstacle is set as... 1. Set the real-time rotation angle of the robotic arm to... ,when Approaching 1 o'clock, that is If the condition is met, an alarm will be triggered, the robotic arm will stop moving towards the obstacle, and will move laterally by d+ relative to the target object. The distance is 1, which is the radius of the distance between the radar on the robotic arm and the center of the obstacle. That is, the obstacle avoidance constraint is:

[0066] r>d+ In the formula, r and d+ All values ​​are vector values.

[0067] Joint limit constraints: Considering the range of motion and limitations of the robotic arm joints, it is necessary to ensure that the joint movements obtained from trajectory planning do not exceed their feasible range.

[0068] Joint limit constraints primarily determine the angle or position range of a joint. For angle constraints on a joint, the following formula can be used for quantification:

[0069] θ2_min ≤ θ2 ≤ θ2_max

[0070] Where θ2 represents the angle of the joint, θ2_min represents the minimum angle limit of the joint, and θ2_max represents the maximum angle limit of the joint.

[0071] The positional constraints of joints can be quantified using the following formula:

[0072] p_min ≤ p ≤ p_max

[0073] Where p represents the position of the joint, p_min represents the minimum position limit of the joint, and p_max represents the maximum position limit of the joint.

[0074] Dynamic constraints: Considering the dynamic characteristics of the robotic arm, it is necessary to ensure that the motion obtained from trajectory planning conforms to the dynamic model of the robotic arm.

[0075] The dynamics of a robotic arm are described using Newton-Euler equations or Lagrangian dynamics. The equations governing the dynamic constraints can be expressed as follows:

[0076]

[0077] In the formula, This is the inertia matrix of the robotic arm, which is related to the load and motion parameters of the robotic arm. It is the acceleration vector of the joint angle. It is the dynamic damping matrix of the robotic arm, which is related to the dynamic damping force. τ is the gravity vector of the robotic arm. τ is the external force vector acting on the robotic arm.

[0078] Therefore, the equations of dynamic constraints describe the motion state of the robotic arm under given acceleration, damping force, gravity and external forces, as well as the dynamic constraints.

[0079] Motion space boundary constraints: In this embodiment of the invention, the motion space is considered to be mostly an approximate rectangular space. To avoid collisions with the boundary of the rectangular space, a certain degree of redundancy is required. Therefore, this embodiment approximates the motion space boundary as a spherical space. The boundary constraints for the spherical space include:

[0080] 1) Maximum stretch distance constraint

[0081] The maximum extension distance of a robotic arm's end effector within a spherical space is limited by the link length. If the end effector of the robotic arm can only move within the sphere, then it cannot exceed the maximum half-radius (R) of that sphere. This can be expressed as:

[0082] r≤R

[0083] 2) Minimum retraction distance constraint

[0084] The robotic arm also has a minimum retraction distance r. min This is determined by the length of the first link of the robotic arm. This means the end effector of the robotic arm cannot retract to a length greater than r. min Smaller distance:

[0085] r≥r min

[0086] 3) Angle constraints

[0087] The joint angles of the robotic arm are also limited. For motion within a spherical space, if the position of the robotic arm's end effector is expressed in spherical coordinates (r, θ, ... (r,θ, Let θ be used to represent θ and θ'. These represent the polar angle and azimuth angle, respectively, and these angles are also subject to the physical limitations of the mechanical joints:

[0088] θ min ≤θ≤θ max

[0089] min ≤ ≤ max

[0090] In the formula, θ min θ max These are the minimum polar angle and the maximum polar angle, respectively. min , max These are the minimum azimuth and the maximum azimuth, respectively.

[0091] 4) Operating area constraints

[0092] In some situations, although a robotic arm can theoretically reach any position within the entire spherical space, its working area may be limited by the requirements of the operating environment. For example, if the robotic arm can only operate within a specific cylinder, then its position must satisfy the following conditions:

[0093]

[0094] In the formula, , These are the x and y coordinates of the center of the cylinder, respectively. c Let be the radius of the cylinder. If a height constraint on the cylinder is also required, then a height constraint must also be added.

[0095] Step S2: Determine the motion trajectory of the robotic arm using the spherical parametric equation of the robotic arm.

[0096] The rotation of the robotic arm involves its lifting and rotation; therefore, the end point of the robotic arm can be considered to move on a hemisphere. Let the total length of the extended robotic arm be L, which is the radius of the real-time sphere. Introduce θ to represent the polar angle. Let represent the azimuth angle and ω represent the rotational angular velocity. This connects the rotation angle of the telescopic arm to spherical coordinates: the rotation angle of the telescopic arm is related to the polar angle θ. Therefore, the rotation angle of the robotic arm can be expressed as:

[0097]

[0098] In the formula, t represents time.

[0099] Specifically, in this embodiment of the invention, the process of determining the motion trajectory of the robotic arm through the spherical parametric equation of the robotic arm includes steps S201 to S203. Wherein,

[0100] Step S201: Determine the spherical parametric equation of the robotic arm based on its rotation angle. The spherical parametric equation is as follows:

[0101]

[0102]

[0103]

[0104] In the formula, x, y, and z are the x-axis, y-axis, and z-axis coordinates of the robotic arm's end effector on the sphere, respectively, and r is the distance of the robotic arm from the center of the obstacle. The end-effector polar angle of the robotic arm. Let be the end effector azimuth angle of the robotic arm. To ensure that the end effector of the robotic arm moves on the upper surface of the sphere, .

[0105] Step S202: Based on the spherical parametric equation, update the coordinate set of the robot arm's end effector on the spherical surface for the next time step motion according to the robot arm's angular velocity. The updated coordinate set of the robot arm's end effector on the spherical surface for the next time step motion is:

[0106]

[0107]

[0108]

[0109] In the formula, Indicates the current moment. Indicates time interval, Indicates the next time step. , , Let x, y, and z represent the x-axis, y-axis, and z-axis coordinates of the robotic arm's end effector on the sphere at the next time step. , Let represent the polar angle and azimuth angle of the robotic arm's end effector at the next time step, respectively.

[0110]

[0111]

[0112] In the formula, , These are the polar angle and azimuth angle of the robotic arm's end effector at the current time t, respectively. , These are the angular velocities of the robotic arm's end effector in the latitudinal and longitudinal directions, respectively. , These are the angular velocity disturbance terms of the robotic arm's end effector in the latitudinal direction and the angular velocity disturbance term in the longitudinal direction, respectively.

[0113]

[0114]

[0115] In the formula, , Let represent the angular velocities of the robotic arm's end effector in the latitude and longitude directions, respectively, with a mean of 0 and a variance of σ. 2 It follows a normal distribution.

[0116] It should be noted that, due to the presence of certain disturbances during the movement of the robotic arm, these disturbances can simulate uncertainties in the real world, such as sensor errors, the effects of external forces, or other non-ideal factors. These disturbances can be random or change according to some pattern.

[0117] Therefore, embodiments of the present invention introduce angular velocity interference terms in the latitude and longitude directions of the robotic arm's end effector. , It can be a random variable, or a value generated according to a certain model (such as a Gaussian distribution). In the embodiments of the present invention, , All follow a mean of 0 and a variance of σ. 2 It follows a normal distribution.

[0118] Step S203: Determine the coordinates of the end effector at each time step based on the updated coordinate set of the end effector on the sphere at each time step, and construct the motion trajectory of the end effector.

[0119] It is understandable that the motion trajectory of the end effector of the robotic arm is obtained by combining the coordinate sets corresponding to each time step. In order to make the motion trajectory of the end effector of the robotic arm more precise, the time step interval can be smaller.

[0120] Step S3: Under the trajectory constraints of the robotic arm, execute the motion trajectory of the robotic arm and minimize the error between the end-effector posture of the robotic arm and the preset desired posture.

[0121] Among them, when performing motion according to the movement trajectory of the robotic arm, the error between the end-effector posture of the robotic arm and the preset desired posture should be minimized in order to meet the desired posture of the end-effector of the robotic arm.

[0122] Therefore, embodiments of the present invention further include a step of determining the optimal posture of the end effector of the robotic arm with the optimization objective of minimizing the error between the end effector posture of the robotic arm and the preset desired posture.

[0123] In some embodiments, the step of determining the optimal posture of the robotic arm's end effector, with the optimization objective of minimizing the error between the end effector posture and a preset desired posture, includes steps S301 to S306. Wherein,

[0124] Step S301: Construct the rotation matrices corresponding to the initial pose and the preset desired pose of the end effector of the robotic arm.

[0125] The rotation matrices corresponding to the initial pose and the preset desired pose are both represented using 3×3 rotation matrices. The rotation matrix R is represented as follows:

[0126] R

[0127] in, This represents an element in the rotation matrix R.

[0128] Step S302: Use the DH parameter method to determine the transformation matrix between the joint angle vector and the rotation matrix at the end of the robotic arm.

[0129] Among them, the DH (Denavit-Hartenberg) parameter method is used to describe the relationship between links of a robotic arm. It defines the position and orientation of links and joints through a set of standardized parameters, which facilitates the calculation of forward kinematics (calculating the position and orientation of the end effector of the robotic arm) and inverse kinematics (solving joint variables given the position and orientation of the end effector of the robotic arm).

[0130] The DH parameter method provides four DH parameters: link length, link offset, joint angle, and torsion angle. This embodiment of the invention only considers the end effector posture of the robotic arm; therefore, the transformation matrix between the joint angle vector and the rotation matrix of the robotic arm's end effector is determined using the DH parameter method.

[0131] Step S303: Determine the rotation matrices after joint angle vector transformation for the initial posture and the preset desired posture of the robotic arm end based on the joint angle vectors and transformation matrix of the robotic arm end.

[0132] The rotation matrix after joint angle vector transformation can be expressed as:

[0133]

[0134] In the formula, This is the rotation matrix after joint angle vector transformation. Let T be the transformation matrix, and T be the matrix transpose.

[0135] The transformation matrix can be used to associate the positions and orientations of the links of the robotic arm, and the position and orientation of the end effector of the entire robotic arm relative to the base can be obtained by matrix multiplication. In other words, the correlation and quantification relationship between the joint angle vector of the end effector and the rotation matrix is ​​determined.

[0136] Step S304: Determine the attitude error function based on the initial attitude of the robotic arm's end effector and the preset desired attitude. The attitude error function is as follows:

[0137]

[0138] In the formula, The independent variable is the joint angle vector. The attitude error function, Here is a function to measure pose difference. , These are the rotation matrices of the initial posture of the robotic arm's end effector after joint angle vector transformation, and the rotation matrices of the preset desired posture of the robotic arm's end effector after joint angle vector transformation, respectively.

[0139] Specifically, the attitude error function is used to measure the scalar value of the difference between the initial attitude of the robotic arm's end effector and the preset desired attitude, and this difference is minimized. The attitude difference measurement function f can be the root mean square error function.

[0140] Step S305: Construct an optimization problem that minimizes the attitude error based on the attitude error function and the motion cost function. The optimization problem is as follows:

[0141]

[0142] In the formula, T represents the total time. Let be the motion cost function. This is a trade-off.

[0143] Among them, the motion cost function Based on task requirements, by defining an appropriate motion cost function, the robotic arm can be guided to select the optimal path or sequence of actions to meet specific task objectives.

[0144] The elements that constitute the motion cost function include path length, smoothness, collision avoidance constraints, acceleration / velocity constraints, energy consumption, time, and task-specific objectives (such as accuracy requirements, access order of specific points, etc.). The motion cost function is determined by weighting multiple elements.

[0145] Step S306: Solve the optimization problem to determine the joint angle vector of the end effector of the robotic arm at each moment, thereby determining the optimal posture of the end effector of the robotic arm.

[0146] Among them, a mathematical solver can be used to optimize the solution of the optimization problem until the iteration conditions are met, such as when the maximum number of iterations or the difference between two iterations is less than the allowable value, the iteration stops and the joint angle vectors of the end effector of the robotic arm at each time are output. The optimal posture of the end effector of the robotic arm is determined by using the joint angle vectors.

[0147] In some embodiments, in order to monitor the movement of the robotic arm, the method further includes: acquiring image data of the robotic arm during its movement and transmitting the acquired image data to the front end for display.

[0148] In the specific implementation process, four camera vision sensors are installed on the robotic arm. Two of the cameras are mainly used together with the single-line lidar to identify the distance between the front gripper of the robotic arm and the obstacle, ensuring that the robotic arm does not collide with the obstacle during the movement. The remaining two vision camera sensors are located on both sides of the robotic arm to observe the posture and shaking of the porcelain bottle during the movement and provide real-time feedback to the control terminal.

[0149] After the camera collects data, it uses low-latency wireless transmission to send the collected images to the control terminal, which are displayed in real time on the remote control screen and fed back to the controller. The controller can then adjust the remote control speed and mode based on the real-time situation in the images.

[0150] It should be noted that this invention determines the trajectory constraint of the robotic arm by limiting the motion space boundary of the robotic arm, and determines the motion trajectory of the robotic arm by using the spherical parametric equation of the robotic arm. This ensures that the motion trajectory of the robotic arm moves within the spherical space and satisfies the trajectory constraint of the motion space boundary of the robotic arm, thereby avoiding collisions between the robotic arm and other objects during movement. At the same time, by minimizing the error between the end-effector posture of the robotic arm and the preset desired posture, the accuracy of the end-effector movement of the robotic arm is improved, and the motion trajectory of the robotic arm can be controlled more accurately to a large extent.

[0151] Based on the same inventive concept, this application also provides a trajectory control system for a robotic arm that implements the trajectory control method of the robotic arm described above.

[0152] The solution provided by the trajectory control system of the robotic arm is similar to the solution described in the above method. Therefore, the specific limitations of one or more embodiments of the trajectory control system of the robotic arm provided below can be found in the limitations of the trajectory control method of the robotic arm above, and will not be repeated here.

[0153] like Figure 3 As shown, this embodiment of the invention also provides a trajectory control system for a robotic arm, comprising:

[0154] The constraint determination module 100 is used to determine the trajectory constraints of the robotic arm based on the motion space boundary constraints of the robotic arm;

[0155] In some embodiments, the trajectory constraints of the robotic arm include obstacle avoidance constraints, joint limit constraints, dynamic constraints, and motion space boundary constraints.

[0156] The trajectory planning module 200 is used to determine the motion trajectory of the robotic arm through the spherical parametric equation of the robotic arm;

[0157] The motion execution module 300 is used to execute the motion trajectory of the robotic arm under the trajectory constraints of the robotic arm, and to minimize the error between the end-effector posture of the robotic arm and the preset desired posture.

[0158] In some embodiments, the trajectory planning module 200 is specifically used to determine the spherical parametric equation of the robotic arm based on the rotation angle of the robotic arm. The spherical parametric equation is as follows:

[0159]

[0160]

[0161]

[0162] In the formula, x, y, and z are the x-axis, y-axis, and z-axis coordinates of the robotic arm's end effector on the sphere, respectively, and r is the distance of the robotic arm from the center of the obstacle. The end-effector polar angle of the robotic arm. This is the end effector azimuth angle of the robotic arm. ;

[0163] Based on the spherical parametric equations, the coordinate set of the robotic arm's end effector on the sphere for the next time step is updated according to the angular velocity of the robotic arm. The updated coordinate set of the robotic arm's end effector on the sphere for the next time step is as follows:

[0164]

[0165]

[0166]

[0167] In the formula, Indicates the current moment. Indicates time interval, Indicates the next time step. , , Let x, y, and z represent the x-axis, y-axis, and z-axis coordinates of the robotic arm's end effector on the sphere at the next time step. , Let represent the polar angle and azimuth angle of the robotic arm's end effector at the next time step, respectively.

[0168]

[0169]

[0170] In the formula, , These are the polar angle and azimuth angle of the robotic arm's end effector at the current time t, respectively. , These are the angular velocities of the robotic arm's end effector in the latitudinal and longitudinal directions, respectively. , These are the angular velocity disturbance terms of the robotic arm's end effector in the latitudinal direction and the angular velocity disturbance term in the longitudinal direction, respectively.

[0171]

[0172]

[0173] In the formula, , Let represent the angular velocities of the robotic arm's end effector in the latitude and longitude directions, respectively, with a mean of 0 and a variance of σ. 2 The normal distribution;

[0174] The coordinates of the robotic arm's end effector at each time step are determined based on the updated coordinate set of the end effector's motion on the sphere at each time step, thus forming the motion trajectory of the robotic arm's end effector.

[0175] In some embodiments, the system further includes:

[0176] The attitude optimization module is used to determine the optimal attitude of the robotic arm's end effector, with the goal of minimizing the error between the end effector's attitude and the preset desired attitude. This includes:

[0177] Construct rotation matrices corresponding to the initial pose and the preset desired pose of the robotic arm's end effector, respectively;

[0178] The transformation matrix between the joint angle vector and the rotation matrix at the end of the robotic arm is determined using the DH parameter method.

[0179] Based on the joint angle vectors and transformation matrix of the end effector of the robotic arm, determine the rotation matrices after joint angle vector transformation corresponding to the initial posture and the preset desired posture of the end effector of the robotic arm, respectively.

[0180] The attitude error function is determined based on the initial posture of the robotic arm's end effector and the preset desired posture. The attitude error function is as follows:

[0181]

[0182] In the formula, The independent variable is the joint angle vector. The attitude error function, Here is a function to measure pose difference. , These are the rotation matrices of the initial posture of the robotic arm's end effector after joint angle vector transformation, and the rotation matrices of the preset desired posture of the robotic arm's end effector after joint angle vector transformation, respectively.

[0183] Based on the attitude error function and the motion cost function, an optimization problem is constructed to minimize the attitude error. The optimization problem is as follows:

[0184]

[0185] In the formula, T represents the total time. Let be the motion cost function. For trade-offs;

[0186] The optimization problem is solved to determine the joint angle vectors of the robotic arm's end effector at each moment, thereby determining the optimal posture of the robotic arm's end effector.

[0187] In some embodiments, the system further includes:

[0188] The image acquisition module is used to acquire image data of the robotic arm during its movement and transmit the acquired image data to the front end for display.

[0189] In some embodiments, the robotic arm has multiple degrees of freedom joints.

[0190] like Figure 4 As shown, this embodiment of the invention also provides an electronic device. The electronic device 10 includes a memory 20 and a processor 30. The memory 20 stores a computer program. When the computer program is executed by the processor 30, the processor 30 performs the steps of the trajectory control method for the robotic arm as described in any of the above embodiments.

[0191] This invention also provides a computer-readable storage medium storing a computer program thereon, which, when executed, implements the steps of the trajectory control method for the robotic arm as described in any of the above embodiments.

[0192] This invention also provides a computer program product, which includes a computer program stored on a non-transitory computer-readable storage medium. The computer program includes program instructions, wherein when the program instructions are executed by a computer, the computer performs the steps of the trajectory control method for the robotic arm as described in any of the above embodiments.

[0193] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, electronic devices, computer storage media, and computer program products described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0194] It should be noted that the terms "comprising" and "having" and any variations thereof in the specification, claims and accompanying drawings of this invention are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are explicitly listed, but may include other steps or units that are not explicitly listed or that are inherent to such processes, methods, products or devices.

[0195] In the several embodiments provided by this invention, it will be understood that each block in the flowchart or block diagram may represent a module, segment, or portion of code, which contains one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks may occur in a different order than those shown in the figures. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved.

[0196] In the embodiments provided by this invention, it should be understood that the disclosed systems, electronic devices, computer storage media, computer program products, and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative; for instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the mutual coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection of devices or units may be electrical, mechanical, or other forms.

[0197] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0198] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0199] If the integrated unit is implemented as a software functional unit and 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 invention, in essence, or the part 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 several instructions for executing all or part of the steps of the methods of the various embodiments of this invention through a computer device (which may be a personal computer, server, or network device, etc.). The aforementioned storage medium includes: USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, optical disks, and other media capable of storing program code.

[0200] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A trajectory control method for a robotic arm, characterized in that, include: The trajectory constraints of the robotic arm are determined based on the boundary limitations of its motion space. The motion trajectory of the robotic arm is determined by the spherical parametric equation of the robotic arm. Under the trajectory constraints of the robotic arm, the motion trajectory of the robotic arm is executed, and the error between the end-effector posture of the robotic arm and the preset desired posture is minimized; Also includes: The step of determining the optimal posture of the end effector of the robotic arm with the optimization objective of minimizing the error between the end effector posture and the preset desired posture; The step of determining the optimal posture of the robotic arm's end effector, with the optimization objective of minimizing the error between the end effector posture and the preset desired posture, includes: Construct rotation matrices corresponding to the initial posture and the preset desired posture of the end effector of the robotic arm, respectively; The transformation matrix between the joint angle vector at the end of the robotic arm and the rotation matrix is ​​determined using the DH parameter method. Based on the joint angle vector of the end effector of the robotic arm and the transformation matrix, determine the rotation matrix after joint angle vector transformation corresponding to the initial posture and the preset desired posture of the end effector of the robotic arm, respectively. The attitude error function is determined based on the initial posture of the end effector of the robotic arm and the preset desired posture. The attitude error function is as follows: In the formula, The independent variable is the joint angle vector. The attitude error function, Here is a function to measure pose difference. , These are the rotation matrices of the initial posture of the end effector of the robotic arm after joint angle vector transformation, and the rotation matrices of the preset desired posture of the end effector of the robotic arm after joint angle vector transformation, respectively. Based on the attitude error function and the motion cost function, an optimization problem is constructed to minimize the attitude error. The optimization problem is as follows: In the formula, T represents the total time. Let the motion cost function be... For trade-offs; The optimization problem is solved to determine the joint angle vectors of the end effector of the robotic arm at each moment, thereby determining the optimal posture of the end effector.

2. The trajectory control method for a robotic arm according to claim 1, characterized in that, The trajectory constraints of the robotic arm include obstacle avoidance constraints, joint limit constraints, dynamic constraints, and motion space boundary constraints.

3. The trajectory control method for a robotic arm according to claim 1, characterized in that, The step of determining the motion trajectory of the robotic arm using the spherical parametric equation of the robotic arm includes: The spherical parameter equation of the robotic arm is determined based on its rotation angle. The spherical parameter equation is as follows: In the formula, x, y, and z are the x-axis, y-axis, and z-axis coordinates of the robotic arm's end effector on the sphere, respectively, and r is the distance of the robotic arm from the center of the obstacle. The end-effector polar angle of the robotic arm. This is the end effector azimuth angle of the robotic arm. ; Based on the spherical parametric equation, the coordinate set of the robotic arm's end effector on the spherical surface for the next time step is updated according to the angular velocity of the robotic arm. The updated coordinate set of the robotic arm's end effector on the spherical surface for the next time step is as follows: In the formula, Indicates the current moment. Indicates time interval, Indicates the next time step. , , Let x, y, and z represent the x-axis, y-axis, and z-axis coordinates of the robotic arm's end effector on the sphere at the next time step. , Let represent the polar angle and azimuth angle of the robotic arm's end effector at the next time step, respectively. In the formula, , These are the polar angle and azimuth angle of the robotic arm's end effector at the current time t, respectively. , These are the angular velocities of the robotic arm's end effector in the latitudinal and longitudinal directions, respectively. , These are the angular velocity disturbance terms of the robotic arm's end effector in the latitudinal direction and the angular velocity disturbance term in the longitudinal direction, respectively. In the formula, , Let represent the angular velocities of the robotic arm's end effector in the latitude and longitude directions, respectively, with a mean of 0 and a variance of σ. 2 The normal distribution; The coordinates of the end effector at each time step are determined based on the updated coordinate set of the end effector on the sphere at each time step, and the motion trajectory of the end effector is formed.

4. The trajectory control method for a robotic arm according to claim 1, characterized in that, After the step of executing the motion trajectory of the robotic arm under the trajectory constraint of the robotic arm and minimizing the error between the end-effector posture and the preset desired posture, the method further includes: The robot arm collects image data during its movement and transmits the collected image data to the front end for display.

5. The trajectory control method for a robotic arm according to claim 1, characterized in that, The robotic arm has multiple degrees of freedom joints.

6. A trajectory control system for a robotic arm, characterized in that, include: The constraint determination module is used to determine the trajectory constraints of the robotic arm based on the boundary limitations of the robotic arm's motion space. The trajectory planning module is used to determine the motion trajectory of the robotic arm through the spherical parametric equations of the robotic arm; The motion execution module is used to execute the motion trajectory of the robotic arm under the trajectory constraints of the robotic arm, and to minimize the error between the end-effector posture of the robotic arm and the preset desired posture. Also used for: The step of determining the optimal posture of the end effector of the robotic arm with the optimization objective of minimizing the error between the end effector posture and the preset desired posture; The step of determining the optimal posture of the robotic arm's end effector, with the optimization objective of minimizing the error between the end effector posture and the preset desired posture, includes: Construct rotation matrices corresponding to the initial posture and the preset desired posture of the end effector of the robotic arm, respectively; The transformation matrix between the joint angle vector at the end of the robotic arm and the rotation matrix is ​​determined using the DH parameter method. Based on the joint angle vector of the end effector of the robotic arm and the transformation matrix, determine the rotation matrix after joint angle vector transformation corresponding to the initial posture and the preset desired posture of the end effector of the robotic arm, respectively. The attitude error function is determined based on the initial posture of the end effector of the robotic arm and the preset desired posture. The attitude error function is as follows: In the formula, The independent variable is the joint angle vector. The attitude error function, Here is a function to measure pose difference. , These are the rotation matrices of the initial posture of the end effector of the robotic arm after joint angle vector transformation, and the rotation matrices of the preset desired posture of the end effector of the robotic arm after joint angle vector transformation, respectively. Based on the attitude error function and the motion cost function, an optimization problem is constructed to minimize the attitude error. The optimization problem is as follows: In the formula, T represents the total time. Let the motion cost function be... For trade-offs; The optimization problem is solved to determine the joint angle vectors of the end effector of the robotic arm at each moment, thereby determining the optimal posture of the end effector.

7. An electronic device, characterized in that, The electronic device includes a memory and a processor. The memory stores a computer program, which, when executed by the processor, causes the processor to perform the steps of the trajectory control method for the robotic arm as described in any one of claims 1 to 5.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed, it implements the steps of the trajectory control method for the robotic arm as described in any one of claims 1 to 5.

9. A computer program product, characterized in that, The computer program product includes a computer program stored on a non-transitory computer-readable storage medium, the computer program including program instructions, wherein when the program instructions are executed by a computer, the computer performs the steps of the trajectory control method for the robotic arm as described in any one of claims 1 to 5.

Images