Track control method and system of mechanical arm, electronic equipment and storage medium

Abstract

The invention relates to the technical field of mechanical motion control, and discloses a trajectory control method and system of a mechanical arm, electronic equipment and a storage medium. The trajectory constraint of the mechanical arm is determined according to the motion space boundary limitation of the mechanical arm; the motion track 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, the error between the tail end posture of the mechanical arm and the preset expected posture is minimum, and the motion trajectory of the mechanical arm can be accurately controlled to a large extent.

Description

Technical Field

[0001] The present invention relates to the technical field of mechanical motion control, and in particular to a trajectory control method, system, electronic equipment and storage medium of a robotic arm. Background Art

[0002] At present, robotic arms are usually used to perform grasping and other tasks in complex working conditions in complex environments of substations. Compared with traditional robotic arms, today's robotic arms have more degrees of freedom. They have become a research hotspot in recent years because of their strong flexibility, strong adaptability, ability to avoid obstacles, strangeness, and joint limitations.

[0003] However, in a narrow space, it is difficult for the robotic arm to move along an ideal trajectory, which may lead to accidents such as damage to the robotic arm or objects in the space. Therefore, it is crucial to achieve collaborative tasks with high precision and low internal force for the robotic arm. However, current robotic arms find it difficult to accurately control their motion trajectory 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 it is difficult for the current robotic arm to accurately control the motion trajectory to a large extent.

[0005] A first aspect of the present invention provides a trajectory control method for a robotic arm, comprising:

[0006] Determine the trajectory constraints of the robot arm according to the boundary restrictions of the robot arm's motion space;

[0007] The motion trajectory of the robot arm is determined by the spherical parameter equation of the robot arm;

[0008] Under the trajectory constraint of the robotic arm, the motion trajectory of the robotic arm is executed, and the error between the end posture of the robotic arm and the preset expected 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 by using the spherical parameter equation of the robotic arm comprises:

[0011] The spherical parameter equation of the robotic arm is determined according to the rotation angle of the robotic arm, and the spherical parameter equation is:

[0012]

[0013]

[0014]

[0015] In the formula, x, y, z are the x-axis coordinates, y-axis coordinates, and z-axis coordinates of the end of the robot arm on the sphere, and r is the distance between the robot arm and the center of the obstacle. is the end polar angle of the robot arm, is the end azimuth of the robot arm, ;

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

[0017]

[0018]

[0019]

[0020] In the formula, Indicates the current moment, Indicates the time interval, represents the next time step, , , They represent the x-axis coordinate, y-axis coordinate, and z-axis coordinate of the end of the robot on the sphere at the next time step. , Respectively represent the polar angle and azimuth angle of the end of the robot arm in the next time step, where,

[0021]

[0022]

[0023] In the formula, , are the polar angle and azimuth angle of the end of the robot arm at the current time t, , are the angular velocities of the end of the robotic arm in the latitude and longitude directions, respectively. , are the angular velocity interference terms of the end of the robotic arm in the latitude direction and the longitude direction, respectively, where:

[0024]

[0025]

[0026] In the formula, , The angular velocity of the end of the robot arm in the latitude direction and the angular velocity in the longitude direction correspond to a mean of 0 and a variance of σ. 2 Normal distribution of

[0027] The coordinate point of the end of the robotic arm at each time step is determined according to the updated coordinate set of the end of the robotic arm moving on the spherical surface at each time step, and the motion trajectory of the end of the robotic arm is constructed.

[0028] Preferably, the method further comprises: a step of determining the optimal posture of the end of the robotic arm with minimization of the error between the end posture of the robotic arm and a preset desired posture as an optimization goal;

[0029] The step of determining the optimal posture of the end of the robotic arm with minimizing the error between the end posture of the robotic arm and the preset desired posture as the optimization goal comprises:

[0030] Constructing rotation matrices corresponding to the initial posture of the end of the robotic arm and the preset desired posture;

[0031] Determine the transformation matrix of the joint angle vector of the end of the robotic arm and the rotation matrix using the DH parameter method;

[0032] Determine, according to the joint angle vector of the end of the robotic arm and the transformation matrix, the rotation matrix corresponding to the initial posture of the end of the robotic arm and the preset expected posture after the joint angle vector transformation;

[0033] The posture error function is determined according to the initial posture of the end of the robotic arm and the preset expected posture, and the posture error function is:

[0034]

[0035] In the formula, The independent variable is the joint angle vector The attitude error function is is the measurement function of the posture difference, , The initial posture of the end of the robot arm is transformed by the rotation matrix of the joint angle vector, and the preset expected posture of the end of the robot arm is transformed by the rotation matrix of the joint angle vector;

[0036] An optimization problem of minimizing the posture error is constructed according to the posture error function and the motion cost function. The optimization problem is:

[0037]

[0038] In the formula, T is the total time, is the motion cost function, For trade-offs;

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

[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 posture of the robotic arm and the preset expected posture, the method further includes:

[0041] The image data of the robot arm during the movement is collected, and the collected image data is transmitted to the front end for display.

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

[0043] In a second aspect, the present invention further provides a trajectory control system for a robotic arm, comprising:

[0044] A constraint determination module, used to determine the trajectory constraints of the robot arm according to the boundary restrictions of the motion space of the robot arm;

[0045] A trajectory planning module is used to determine the motion trajectory of the robot arm through the spherical parameter equation of the robot arm;

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

[0047] In a third aspect, the present invention further provides an electronic device comprising a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of the trajectory control method of the robotic arm as described in the first aspect.

[0048] In a fourth aspect, the present invention further provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed, the steps of the trajectory control method of the robot arm as described in the first aspect are implemented.

[0049] In a fifth aspect, the present invention further provides a computer program product, comprising a computer program stored on a non-transitory computer-readable storage medium, wherein the computer program comprises program instructions, wherein when the program instructions are executed by a computer, the computer executes the steps of the robot arm trajectory control method as described in the first aspect.

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

[0051] The present invention determines the trajectory constraints of the robot arm through the boundary restrictions of the robot arm's motion space, and determines the motion trajectory of the robot arm through the spherical parameter equation of the robot arm, so that the motion trajectory of the robot arm moves in the spherical space and satisfies the trajectory constraints of the boundary restrictions of the robot arm's motion space, thereby avoiding the robot arm from colliding with other objects during movement. At the same time, by minimizing the error between the end posture of the robot arm and the preset expected posture, the end motion accuracy of the robot arm is improved, and the motion trajectory of the robot arm is accurately controlled to a greater extent. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 A schematic diagram of an application environment for a robotic arm provided by an embodiment of the present invention;

[0053] Figure 2 A flow chart of a trajectory control method of a robotic arm provided by an embodiment of the present invention;

[0054] Figure 3 A schematic structural diagram of a trajectory control system for a robotic arm provided by an embodiment of the present invention;

[0055] Figure 4 A schematic diagram of the structure of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0056] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0057] The trajectory control method of the robot arm provided in the embodiment of the present application can be applied to Figure 1 In the application environment of the robot arm shown in the figure, the controller 101 controls the movement of the robot arm 102, and the controller 101 determines the trajectory constraint of the robot arm 102 according to the boundary limit of the movement space of the robot arm 102; determines the movement trajectory of the robot arm 102 through the spherical parameter equation of the robot arm 102; under the trajectory constraint of the robot arm 102, the movement trajectory of the robot arm 102 is executed, and the error between the end posture of the robot arm 102 and the preset expected posture is minimized.

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

[0059] like Figure 2 As shown, the present application embodiment provides a trajectory control method for a robot arm, and the method is applied to Figure 1The controller 101 in the embodiment is taken as an example to illustrate, and includes steps S1 to S3.

[0060] Step S1, determining the trajectory constraints of the robot arm according to the boundary restrictions of the motion space of the robot arm.

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

[0062] The motion space boundary restriction refers to the constraint on the trajectory of the robot arm due to the robot body design and the motion space in which the robot arm is located, such as a narrow space, so that the trajectory constraint of the robot arm is met when the motion action is subsequently performed according to the motion trajectory.

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

[0064] Obstacle avoidance constraints ensure that the robot does not collide with obstacles when executing the trajectory, and it is necessary to consider objects and obstacles in the motion space.

[0065] Specifically, according to the coordinates of the obstacle in the radar coordinate system fed back by the single-line laser radar and the distance from the single-line laser radar, it is judged 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 considered constraint distance d, the robotic arm operates normally. In the slewing support, the servo motor of the robotic arm can be directly input with a signal to control its rotation direction and rotation angle. During the rotation of the front end robotic arm, the rotation range that does not collide with the robotic arm is set to 1. The real-time rotation angle of the robot arm is set to ,when Approaching 1 o'clock, that is When the robot moves toward the obstacle, an alarm is sounded and the robot arm stops moving toward the obstacle and moves horizontally relative to the target object. 1, in order to escape the distance radius from the radar on the robot arm to the center of the obstacle. That is, the obstacle avoidance constraint is:

[0066] r>d+ , where r and d+ 1 are all vector values.

[0067] Joint limit constraints: Considering the movable range and limitations of the robot arm joints, it is necessary to ensure that the joint motion obtained by trajectory planning does not exceed its feasible range.

[0068] The joint limit constraint mainly determines the angle or position range of the joint. The angle limit of the joint can be quantified using the following formula:

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

[0070] Among them, θ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 position limits of the joints can be quantified using the following formula:

[0072] p_min ≤ p ≤ p_max

[0073] Among them, 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 robot arm, it is necessary to ensure that the motion obtained by trajectory planning is consistent with the dynamic model of the robot arm.

[0075] The dynamic model of the robot arm is described by Newton-Euler equations or Lagrangian dynamics. The equation of dynamic constraints can be expressed as:

[0076]

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

[0078] It can be seen that the equations of dynamic constraints describe the motion state and dynamic constraints of the robot under given acceleration, damping force, gravity and external force.

[0079] Motion space boundary constraints: The embodiments of the present invention consider that the motion space is mostly approximately rectangular space. In order to avoid collision with the boundary of the rectangular space, there should be a certain redundancy. Therefore, the embodiment of the present application approximates the motion space boundary to a spherical space. The boundary constraints for the spherical space include:

[0080] 1) Maximum reach constraint

[0081] The maximum reach of the end effector of the robot arm in spherical space is limited by the length of the connecting rod. If the end of the robot arm can only move within the sphere, then it cannot exceed the maximum half R of this sphere. This can be expressed as:

[0082] r≤R

[0083] 2) Minimum retraction distance constraint

[0084] The robot arm also has a minimum retraction distance r min , which is determined by the length of the first link of the robot arm. This means that the end of the robot arm cannot be retracted to a greater distance than r min Smaller distance:

[0085] r≥r min

[0086] 3) Angle constraint

[0087] The joint angles of the robot arm are also limited. For motion in spherical space, if the position of the end of the robot arm is expressed in the spherical coordinate system (r, θ, )(r,θ, ) is represented by, where θ and Represent the polar angle and azimuth respectively, then 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 are the minimum polar angle and the maximum polar angle respectively, min , max are the minimum azimuth and maximum azimuth, respectively.

[0091] 4) Operation area constraints

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

[0093]

[0094] In the formula, , are the horizontal and vertical coordinates of the center of the cylinder, c is the radius of the cylinder. If the height limit of the cylinder is also considered, the height constraint condition must also be added.

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

[0096] The rotation of the robot arm involves the lifting and rotation of the robot arm, so the end point of the robot arm can be regarded as moving on a hemisphere. Assuming that the total length of the robot arm extended in real time is L, which is the real-time spherical radius. Introduce θ to represent the polar angle, represents the azimuth angle, and the angular velocity of rotation is w, thereby linking the rotation angle of the telescopic arm with the spherical coordinates: the rotation angle of the telescopic arm is related to the polar angle θ, so the rotation angle of the robotic arm can be expressed as:

[0097]

[0098] Where t is the time.

[0099] Specifically, the process of determining the motion trajectory of the robot arm by using the spherical parameter equation of the robot arm in the embodiment of the present invention includes steps S201 to S203.

[0100] Step S201: determine the spherical parameter equation of the robot arm according to the rotation angle of the robot arm. The spherical parameter equation is:

[0101]

[0102]

[0103]

[0104] In the formula, x, y, z are the x-axis coordinates, y-axis coordinates, and z-axis coordinates of the end of the robot arm on the sphere, and r is the distance between the robot arm and the center of the obstacle. is the end polar angle of the robot arm, is the end azimuth of the robot arm. In order to ensure that the end of the robot arm moves on the upper half of the sphere, .

[0105] Step S202: Based on the spherical parameter equation, the coordinate set of the end of the robotic arm moving on the spherical surface in the next time step is updated according to the angular velocity of the robotic arm. The updated coordinate set of the end of the robotic arm moving on the spherical surface in the next time step is:

[0106]

[0107]

[0108]

[0109] In the formula, Indicates the current moment, Indicates the time interval, represents the next time step, , , They represent the x-axis coordinate, y-axis coordinate, and z-axis coordinate of the end of the robot on the sphere at the next time step. , Respectively represent the polar angle and azimuth angle of the end of the robot arm in the next time step, where,

[0110]

[0111]

[0112] In the formula, , are the polar angle and azimuth angle of the end of the robot arm at the current time t, , are the angular velocities of the end of the robotic arm in the latitude and longitude directions, respectively. , are the angular velocity interference terms of the end of the robotic arm in the latitude direction and the longitude direction, respectively, where:

[0113]

[0114]

[0115] In the formula, , The angular velocity of the end of the robot arm in the latitude direction and the angular velocity in the longitude direction correspond to a mean of 0 and a variance of σ. 2 The normal distribution of .

[0116] It should be noted that there are certain interference items during the movement of the robot arm, which can simulate the uncertainty in the real world, such as sensor errors, external forces or other non-ideal factors. These interference items can be random or change according to certain rules.

[0117] Therefore, the embodiment of the present invention introduces the angular velocity interference term of the end of the robotic arm in the latitude direction and the angular velocity interference term in the longitude direction. , It can be a random variable, or a value generated according to a certain model (such as Gaussian distribution). In this embodiment of the present invention, , All have a mean of 0 and a variance of σ 2 The normal distribution of .

[0118] Step S203, determining the coordinate point of the end of the robotic arm at each time step according to the updated coordinate set of the end of the robotic arm moving on the spherical surface at each time step, and forming a motion trajectory of the end of the robotic arm.

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

[0120] Step S3: Under the trajectory constraint of the robot arm, execute the motion trajectory of the robot arm, and minimize the error between the end posture of the robot arm and the preset expected posture.

[0121] When the movement is performed according to the movement trajectory of the robot arm, the error between the end posture of the robot arm and the preset expected posture should be minimized to meet the expected posture of the end of the robot arm.

[0122] To this end, the embodiment of the present invention further includes a step of determining the optimal posture of the end of the robotic arm with minimizing the error between the end posture of the robotic arm and a preset desired posture as the optimization goal.

[0123] In some embodiments, the step of determining the optimal posture of the end of the robotic arm with minimizing the error between the end posture of the robotic arm and the preset desired posture as the optimization goal includes steps S301 to S306.

[0124] Step S301: construct rotation matrices corresponding to the initial posture of the end of the robot arm and the preset expected posture.

[0125] The rotation matrices corresponding to the initial posture and the preset desired posture are both represented by 3×3 rotation matrices. The rotation matrix R is represented by:

[0126] R

[0127] in, Represents the elements in the rotation matrix R.

[0128] Step S302: Determine the transformation matrix of the joint angle vector at the end of the robot arm and the rotation matrix using the DH parameter method.

[0129] Among them, the DH (Denavit-Hartenberg, rule-matrix) parameter method is used to describe the relationship between the robot arm links, and defines the position and direction of the links and joints through a set of standardized parameters, so as to facilitate the calculation of forward kinematics (calculating the position and posture of the end of the robot arm) and inverse kinematics (solving joint variables given the position and posture of the end of the robot arm).

[0130] The DH parameter method provides four DH parameters, namely, link length, link offset, joint angle and torsion angle. The embodiment of the present invention only considers the posture of the end of the robot arm, so the transformation matrix of the joint angle vector and the rotation matrix of the end of the robot arm is determined by the DH parameter method.

[0131] Step S303 , determining the rotation matrices corresponding to the initial posture of the end of the robotic arm and the preset expected posture after the transformation of the joint angle vector according to the joint angle vector of the end of the robotic arm and the transformation matrix.

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

[0133]

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

[0135] Among them, the position and posture of each link of the robotic arm can be associated through the transformation matrix, and the position and posture of the end of the entire robotic arm relative to the base can be obtained by matrix multiplication, that is, the quantitative relationship between the joint angle vector of the end of the robotic arm and the rotation matrix is ​​determined.

[0136] Step S304: determining a posture error function according to the initial posture of the end of the robot arm and the preset expected posture, the posture error function is:

[0137]

[0138] In the formula, The independent variable is the joint angle vector The attitude error function is is the measurement function of the posture difference, , They are respectively the rotation matrix of the initial posture of the end of the robotic arm after being transformed by the joint angle vector, and the rotation matrix of the preset expected posture of the end of the robotic arm after being transformed by the joint angle vector.

[0139] The posture error function is used to measure the scalar value of the difference between the initial posture of the end of the robot arm and the preset expected posture, and minimize it. The measurement function f of the posture difference can be a root mean square error function.

[0140] Step S305: construct an optimization problem for minimizing the posture error according to the posture error function and the motion cost function. The optimization problem is:

[0141]

[0142] In the formula, T is the total time, is the motion cost function, For trade-offs.

[0143] Among them, the motion cost function It can be defined according to the task requirements. By defining a suitable motion cost function, the robot arm can be guided to select the best path or action sequence to meet the specific task objectives.

[0144] The elements for constructing the motion cost function include path length, smoothness, collision avoidance constraints, acceleration / speed constraints, energy consumption, time, and task-specific goals (such as accuracy requirements, order of visiting specific points, etc.), and 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 of the robotic arm at each moment, thereby determining the optimal posture of the end of the robotic arm.

[0146] Among them, a mathematical solver can be used to optimize and solve the optimization problem until the iteration condition is reached, 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 vector of the end of the robot arm at each moment is output, and the joint angle vector is used to determine the optimal posture of the end of the robot arm.

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

[0148] During the specific implementation process, four camera vision sensors are installed on the robotic arm, two of which are mainly used to identify the distance between the front end gripper of the robotic arm and obstacles together with the single-line laser radar to ensure that the robotic arm does not collide with obstacles during 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 movement, and provide real-time feedback to the control end.

[0149] After the camera collects data, it uses low-latency wireless transmission to transmit the collected image to the control end, which is displayed in real time on the remote control screen and fed back to the controller. The controller changes the remote control speed and method according to the real-time situation in the image.

[0150] It should be noted that the present invention determines the trajectory constraints of the robot arm through the boundary restrictions of the robot arm's motion space, and determines the motion trajectory of the robot arm through the spherical parameter equation of the robot arm, so that the motion trajectory of the robot arm moves in the spherical space and satisfies the trajectory constraints of the boundary restrictions of the robot arm's motion space, thereby avoiding the robot arm from colliding with other objects during movement. At the same time, by minimizing the error between the end posture of the robot arm and the preset expected posture, the end motion accuracy of the robot arm is improved, and the motion trajectory of the robot arm is accurately controlled to a greater extent.

[0151] Based on the same inventive concept, an embodiment of the present application also provides a trajectory control system of a robotic arm for implementing the above-mentioned trajectory control method of the robotic arm.

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

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

[0154] A constraint determination module 100, for determining the trajectory constraints of the robot arm according to the motion space boundary restrictions of the robot 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 robot arm through the spherical parameter equation of the robot arm;

[0157] The motion execution module 300 is used to execute the motion trajectory of the robot arm under the trajectory constraint of the robot arm, and minimize the error between the end posture of the robot arm and the preset expected posture.

[0158] In some embodiments, the trajectory planning module 200 is specifically used to determine the spherical parameter equation of the robot arm according to the rotation angle of the robot arm, and the spherical parameter equation is:

[0159]

[0160]

[0161]

[0162] In the formula, x, y, z are the x-axis coordinates, y-axis coordinates, and z-axis coordinates of the end of the robot arm on the sphere, and r is the distance between the robot arm and the center of the obstacle. is the end polar angle of the robot arm, is the end azimuth of the robot arm, ;

[0163] Based on the spherical parametric equation, the coordinate set of the end of the robotic arm moving on the spherical surface in the next time step is updated according to the angular velocity of the robotic arm. The updated coordinate set of the end of the robotic arm moving on the spherical surface in the next time step is:

[0164]

[0165]

[0166]

[0167] In the formula, Indicates the current moment, Indicates the time interval, represents the next time step, , , They represent the x-axis coordinate, y-axis coordinate, and z-axis coordinate of the end of the robot on the sphere at the next time step. , Respectively represent the polar angle and azimuth angle of the end of the robot arm in the next time step, where,

[0168]

[0169]

[0170] In the formula, , are the polar angle and azimuth angle of the end of the robot arm at the current time t, , are the angular velocities of the end of the robotic arm in the latitude and longitude directions, respectively. , are the angular velocity interference terms of the end of the robotic arm in the latitude direction and the longitude direction, respectively, where:

[0171]

[0172]

[0173] In the formula, , The angular velocity of the end of the robot arm in the latitude direction and the angular velocity in the longitude direction correspond to a mean of 0 and a variance of σ. 2 Normal distribution of

[0174] The coordinate point of the end of the manipulator at each time step is determined according to the updated coordinate set of the end of the manipulator on the spherical surface at each time step, and the motion trajectory of the end of the manipulator is constructed.

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

[0176] The posture optimization module is used to determine the optimal posture of the end of the robot arm by minimizing the error between the end posture of the robot arm and the preset expected posture as the optimization goal, including:

[0177] Constructing the rotation matrix corresponding to the initial posture of the end of the robot arm and the preset desired posture;

[0178] Determine the transformation matrix of the joint angle vector and the rotation matrix of the end of the robot arm using the DH parameter method;

[0179] Determine the rotation matrix after the joint angle vector transformation corresponding to the initial posture of the end of the robotic arm and the preset expected posture according to the joint angle vector of the end of the robotic arm and the transformation matrix;

[0180] The posture error function is determined according to the initial posture of the end of the robot arm and the preset expected posture. The posture error function is:

[0181]

[0182] In the formula, The independent variable is the joint angle vector The attitude error function is is the measurement function of the posture difference, , They are respectively the rotation matrix of the initial posture of the end of the robot arm after being transformed by the joint angle vector, and the rotation matrix of the preset expected posture of the end of the robot arm after being transformed by the joint angle vector;

[0183] According to the posture error function and the motion cost function, the optimization problem of minimizing the posture error is constructed. The optimization problem is:

[0184]

[0185] In the formula, T is the total time, is the motion cost function, For trade-offs;

[0186] The optimization problem is solved to determine the joint angle vector of the end of the robot arm at each moment, thereby determining the optimal posture of the end of the robot arm.

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

[0188] The image acquisition module is used to collect image data of the robot arm during movement and transmit the collected 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, an embodiment of the present invention further provides an electronic device, the electronic device 10 includes a memory 20 and a processor 30, the memory 20 stores a computer program, and when the computer program is executed by the processor 30, the processor 30 executes the steps of the trajectory control method of the robot arm in any of the above embodiments.

[0191] An embodiment of the present invention further provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed, the steps of the trajectory control method of the robot arm in any of the above embodiments are implemented.

[0192] An embodiment of the present invention also provides a computer program product, which includes a computer program stored on a non-transitory computer-readable storage medium, and the computer program includes program instructions, wherein when the program instructions are executed by a computer, the computer executes the steps of the trajectory control method of the robotic arm in any of the above embodiments.

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

[0194] It should be noted that the terms "including" and "having" and any variations thereof in the specification and claims of the present invention and the above-mentioned drawings are intended to cover non-exclusive inclusions. For example, a process, method, system, product or apparatus comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or apparatus.

[0195] In several embodiments provided by the present invention, it is understood that each box in the flow chart or block diagram can represent a module, a program segment or a part of a code, and a part of a module, a program segment or a code includes one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from that marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved.

[0196] In the several embodiments provided by the present 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 only schematic. For example, the division of units is only a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

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

[0198] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0199] If the integrated unit is implemented in the form of 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 the present 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, and the computer software product is stored in a storage medium, including a number of instructions for executing all or part of the steps of the various embodiments of the method of the present invention through a computer device (which can be a personal computer, a server, or a network device, etc.). The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (full name in English: Read-Only Memory, English abbreviation: ROM), random access memory (full name in English: Random Access Memory, English abbreviation: RAM), disk or optical disk, etc. Various media that can store program codes.

[0200] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions 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: Determine the trajectory constraints of the robot arm according to the boundary restrictions of the robot arm's motion space; The motion trajectory of the robot arm is determined by the spherical parameter equation of the robot arm; Under the trajectory constraint of the robotic arm, the motion trajectory of the robotic arm is executed, and the error between the end posture of the robotic arm and the preset expected posture is minimized.

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

3. The trajectory control method of a robot arm according to claim 1, characterized in that: The step of determining the motion trajectory of the robotic arm by using the spherical parameter equation of the robotic arm comprises: The spherical parameter equation of the robotic arm is determined according to the rotation angle of the robotic arm, and the spherical parameter equation is: In the formula, x, y, z are the x-axis coordinates, y-axis coordinates, and z-axis coordinates of the end of the robot arm on the sphere, and r is the distance between the robot arm and the center of the obstacle. is the end polar angle of the robot arm, is the end azimuth of the robot arm, ; Based on the spherical parameter equation, the coordinate set of the next time step motion of the end of the robotic arm on the spherical surface is updated according to the angular velocity of the robotic arm. The updated coordinate set of the next time step motion of the end of the robotic arm on the spherical surface is: In the formula, Indicates the current moment, Indicates the time interval, represents the next time step, , , They represent the x-axis coordinate, y-axis coordinate, and z-axis coordinate of the end of the robot on the sphere at the next time step. , Respectively represent the polar angle and azimuth angle of the end of the robot arm in the next time step, where, In the formula, , are the polar angle and azimuth angle of the end of the robot arm at the current time t, , are the angular velocities of the end of the robotic arm in the latitude and longitude directions, respectively. , are the angular velocity interference terms of the end of the robotic arm in the latitude direction and the longitude direction, respectively, where: In the formula, , The angular velocity of the end of the robot arm in the latitude direction and the angular velocity in the longitude direction correspond to a mean of 0 and a variance of σ. 2 Normal distribution of The coordinate point of the end of the robotic arm at each time step is determined according to the updated coordinate set of the end of the robotic arm moving on the spherical surface at each time step, and the motion trajectory of the end of the robotic arm is constructed.

4. The trajectory control method of a robot arm according to claim 1, characterized in that: The method further comprises: determining the optimal posture of the end of the robotic arm with minimization of the error between the end posture of the robotic arm and a preset desired posture as an optimization goal; The step of determining the optimal posture of the end of the robotic arm with minimizing the error between the end posture of the robotic arm and the preset desired posture as the optimization goal comprises: Constructing rotation matrices corresponding to the initial posture of the end of the robotic arm and the preset desired posture; Determine the transformation matrix of the joint angle vector of the end of the robotic arm and the rotation matrix using the DH parameter method; Determine, according to the joint angle vector of the end of the robotic arm and the transformation matrix, the rotation matrix corresponding to the initial posture of the end of the robotic arm and the preset expected posture after the joint angle vector transformation; The posture error function is determined according to the initial posture of the end of the robotic arm and the preset expected posture, and the posture error function is: In the formula, The independent variable is the joint angle vector The attitude error function is is the measurement function of the posture difference, , The initial posture of the end of the robot arm is transformed by the rotation matrix of the joint angle vector, and the preset expected posture of the end of the robot arm is transformed by the rotation matrix of the joint angle vector; An optimization problem of minimizing the posture error is constructed according to the posture error function and the motion cost function. The optimization problem is: In the formula, T is the total time, is the motion cost function, For trade-offs; The optimization problem is solved to determine the joint angle vector of the end of the robotic arm at each moment, thereby determining the optimal posture of the end of the robotic arm.

5. The trajectory control method of a robot 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 posture of the robotic arm and the preset expected posture, the method further includes: The image data of the robot arm during the movement is collected, and the collected image data is transmitted to the front end for display.

6. The trajectory control method of a robot arm according to claim 1, characterized in that: The robot arm has multiple degrees of freedom joints.

7. A trajectory control system for a robotic arm, characterized in that: include: A constraint determination module, used to determine the trajectory constraints of the robot arm according to the boundary restrictions of the motion space of the robot arm; A trajectory planning module is used to determine the motion trajectory of the robot arm through the spherical parameter equation of the robot arm; The motion execution module is used to execute the motion trajectory of the robotic arm under the trajectory constraint of the robotic arm, and minimize the error between the end posture of the robotic arm and the preset expected posture.

8. An electronic device, characterized in that: The electronic device includes a memory and a processor, wherein a computer program is stored in the memory, and when the computer program is executed by the processor, the processor executes the steps of the trajectory control method of the robot arm as described in any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed, the steps of the trajectory control method of the robot arm as described in any one of claims 1 to 6 are implemented.

10. A computer program product, characterized in that The computer program product includes a computer program stored on a non-transitory computer-readable storage medium, and the computer program includes program instructions, wherein when the program instructions are executed by a computer, the computer is caused to perform the steps of the trajectory control method of the robotic arm as described in any one of claims 1 to 6.

Images