Mechanical arm control method and system and storage medium

Abstract

The invention relates to a mechanical arm control method. The method comprises the steps that a mechanical arm trajectory function is established according to the current position, the target position and the current speed of each motor of a mechanical arm; a straight line from the current position p0 of the tail end of the mechanical arm to the target position pt of the tail end of the mechanical arm is used as a target track, and parameters of a track function of the mechanical arm are optimized until optimization conditions are met; and the motor is controlled according to the motor position of the next time step of the mechanical arm trajectory function after parameter optimization. The invention further discloses a system for implementing the method and a storage medium. Parameters in the trajectory function of the mechanical arm are optimized, the current position, the target position and the current speed of the motor are associated, the trajectory can be smooth and close to a straight line as much as possible when the mechanical arm is controlled to move, and the trajectory can be dynamically adjusted in the movement process.

Description

Technical Field

[0001] The present invention relates to the field of robotic arm motion control, and more specifically, to a robotic arm control method, system, and storage medium. Background Art

[0002] The control of a robotic arm requires the cooperation of multiple motors. What the end user directly cares about is the position of the tool at the end of the robotic arm, while what the upper computer of the robotic arm can directly control are only the various motors on the robotic arm. Connecting these two is the robotic arm kinematics. It is divided into forward kinematics and inverse kinematics. The former can convert the positions of the current motors into the position and posture of the end tool, and the latter can convert the position and posture of the end tool into the motor positions.

[0003] A link robotic arm refers to a mechanical structure formed by connecting two or more rods through joints. This structure usually includes multiple links and joints, and each link is connected together through joints to form a kinematic chain. The number of links of a multi-link robotic arm can be arbitrary, such as five links, six links, etc., and the specific number depends on the design requirements. Each joint of this type of robotic arm usually has a rotating motor, and the position and posture of the end of the robotic arm tool depend on the positions of the motors at each joint. For this type of robotic arm, if the motors are rotated at a constant speed to reach the target position, the end of the robotic arm tool will move in a certain curve. Therefore, in order to ensure that the end of the robotic arm tool moves in a straight line translation, a suitable variable speed control strategy must be available.

[0004] Smoothing trajectories in Cartesian space or joint space to ensure straight-line motion of the end and meet constraints. Common methods include: cubic spline, quintic spline, or B-spline. The disadvantage of this type of method is that the planning is static and cannot dynamically handle new situations that occur during the motion of the robotic arm (such as motor motion errors and the user's desire to modify the destination midway). The reason is that, for example, modifying the destination midway may cause the acceleration of the motor motion to be too large or exceed the motion limit, resulting in the robotic arm being unable to execute or the motion being unstable, and it can only stop and re-plan, which makes this method difficult to meet the needs of some users for real-time manipulation. Summary of the Invention

[0005] In order to overcome the problem in the above-mentioned prior art that it is difficult for a robotic arm to simultaneously meet smooth trajectories and dynamic adjustment, the present invention provides a robotic arm control method, system, and storage medium, realizing the goal that when controlling the motion of the robotic arm, the trajectory can meet smoothness and be as close to a straight line as possible, and can dynamically adjust the trajectory during the motion process.

[0006] To solve the above technical problems, the technical solution adopted by the present invention is: a robotic arm control method, including:

[0007] Establish a manipulator trajectory function based on the current position, target position, and current speed of each motor of the manipulator;

[0008] Using the straight line from the current position p0 of the manipulator end to the target position p of the manipulator end t as the target trajectory, optimize the parameters of the manipulator trajectory function until the optimization conditions are met;

[0009] Control the motor according to the motor position at the next time step of the manipulator trajectory function after parameter optimization.

[0010] In the above technical solution, the current position, target position, and current speed of each motor are considered in the manipulator trajectory function. Therefore, when optimizing the parameters of the manipulator trajectory function with the target trajectory, even if the initial speed is not 0, the movement trajectory of the manipulator can be made as close as possible to the target trajectory. At the same time, since the parameter optimization considers the motor position and current speed (i.e., the initial speed), it avoids the situation of excessive motor acceleration or the motor executing motion beyond the limit, and enables the manipulator to stably execute actions even when changing the destination during the movement process.

[0011] The optimization conditions can be diverse. It can be a fixed number of times for parameter optimization, that is, after the set number of iterations of parameter optimization, it is considered that the optimization is completed; it can also be that the parameters need to meet a certain preset value after optimization, or the optimized movement trajectory needs to meet a certain preset value.

[0012] When the manipulator is not equipped with an execution tool, the manipulator end refers to the end of the moving arm itself. When the manipulator is equipped with an execution tool, the manipulator end refers to the end of the execution tool.

[0013] Preferably, the optimization conditions include making the deviation degree between the movement trajectory corresponding to the manipulator trajectory function and the target trajectory less than a preset threshold. The deviation degree can be expressed in various forms, and its purpose is to reflect the similarity between the movement trajectory corresponding to the manipulator trajectory function and the target trajectory. For example, the maximum distance or average distance between the movement trajectory and the target trajectory can be used to represent the deviation degree, or the maximum curvature or average curvature of the movement trajectory can be used to represent the deviation degree. The fitting degree between the movement trajectory and the target trajectory can also be used.

[0014] Preferably, the process of establishing the manipulator trajectory function includes: determining each motor function of the motor, determining the parameters to be optimized in the motor function; and constituting the manipulator trajectory function according to each motor function.

[0015] Under the variable x of the manipulator trajectory function, obtain the deviation degree between the manipulator trajectory function and the target trajectory.

[0016] After obtaining the current position, target position, and current speed of the motor, the trajectory function θ of the motor can be determined. i (t) = at 3 + bt 2 + ct + d. This function represents the position of the i-th joint motor over time t, and the quantities of the parameters a, b, c, and d in the motor trajectory function need to be determined and optimized. Considering the convenience of optimizing the parameters, let g i (x) = ax 3 + bx 2 + cx + d replace the above trajectory function, where x represents the ratio of the current time to the final time. When x = 1, it means the time has reached the final moment.

[0017] In the g i (x) function, the motor position at time 0 is at the initial position, which is the current position θ0 of the motor. Thus, d = θ0 can be obtained. Secondly, to ensure that the instantaneous speed at time 0 is the current speed of the motor, i.e., g i ′(0) = v0, from which c = v0 can be deduced. To ensure that the position reaches the target position when x = 1, b = θ t - θ0 - v0 - a can be obtained. The values of the parameters a, b, c, and d are obtained, and after adjustment, only the parameter a needs to be optimized. The manipulator trajectory function is That is, by optimizing the parameter a in the motor trajectory function of each motor, the parameter optimization of the manipulator trajectory function can be completed.

[0018] Among them, is the vector function of each motor position, is the vector composed of the parameters a to be optimized for each motor, including a1, a2,....a n , where n is the number of motors; F is the forward kinematics function of the manipulator. It should be noted that during optimization, it is that is optimized, that is, the parameter a in the motor trajectory function corresponding to all motors is optimized simultaneously.

[0019] Preferably, the iterative optimization of the parameters of the manipulator trajectory function is achieved by the gradient descent method.

[0020] Preferably, the process of optimizing the parameters of the manipulator trajectory function is as follows: Use the current parameters to uniformly sample the trajectory points at various moments or multiple moments of the manipulator trajectory function, calculate the deviation degree between the trajectory points and the target trajectory. If it is greater than the preset threshold, continue to optimize the current parameters; if it is less than the preset threshold, construct the manipulator trajectory function with the optimized parameters. Among them, the current parameters refer to the parameters before optimization. And after the parameters are optimized, in the next optimization, the current parameters are the parameters obtained from the most recent optimization.

[0021] Preferably, the deviation degree is the distance between the trajectory point and the target trajectory; specifically, the optimization condition is that the distance between the trajectory point with the maximum distance from the target trajectory or multiple trajectory points and the target trajectory is less than a preset threshold, or the average value of the distances between multiple trajectory points in the robotic arm trajectory function and the target trajectory is less than a preset threshold.

[0022] Preferably, the deviation degree is the curvature of the trajectory point; specifically, the optimization condition is that the maximum value of the curvatures corresponding to multiple uniformly sampled trajectory points in the robotic arm trajectory function is less than a preset threshold, or the average value of the curvatures corresponding to multiple trajectory points is less than a preset threshold.

[0023] A robotic arm control system for implementing the above-mentioned robotic arm control method, comprising a robotic arm and a controller module, wherein the controller module is used to be electrically connected to the robotic arm, obtain data of the motors of the robotic arm, and control the movement of the robotic arm; the controller module includes a processor for performing operations.

[0024] A storage medium for storing a computer program; wherein the computer program, when executed by a processor, implements the above-mentioned robotic arm control method.

[0025] Compared with the prior art, the beneficial effects of the present invention are as follows: optimizing the parameters in the robotic arm trajectory function and associating the current position, target position, and current speed of the motor, so as to achieve that the trajectory can meet the requirements of smoothness and be as close to a straight line as possible when controlling the movement of the robotic arm, and can dynamically adjust the target of the trajectory during the movement process. Description of the Drawings

[0026] Figure 1 is a flowchart of a robotic arm control method of the present invention; Detailed Embodiments

[0027] The drawings are only for illustrative purposes and cannot be construed as a limitation of this patent; for better illustrating this embodiment, some components in the drawings will be omitted, enlarged, or reduced, and do not represent the dimensions of the actual product; for those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted. The positional relationships described in the drawings are only for illustrative purposes and cannot be construed as a limitation of this patent.

[0028] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components; in the description of the present invention, it should be understood that if there are terms such as "upper", "lower", "left", "right", "long", "short", etc. indicating the orientation or positional relationship, they are based on the orientation or positional relationship shown in the accompanying drawings. This is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation. Therefore, the terms describing the positional relationship in the accompanying drawings are only for illustrative purposes and should not be construed as a limitation of this patent. For those of ordinary skill in the art, the specific meanings of the above terms can be understood according to specific circumstances.

[0029] The technical solution of the present invention will be further specifically described below through specific embodiments in conjunction with the accompanying drawings:

[0030] Embodiment 1

[0031] As Figure 1 shown, Embodiment 1 of a robotic arm control method includes

[0032] Establish a robotic arm trajectory function based on the current positions, target positions, and current speeds of the motors of the robotic arm;

[0033] Taking the straight line from the current position p0 of the end of the robotic arm to the target position p t of the end of the robotic arm as the target trajectory, optimize the parameters of the robotic arm trajectory function until the optimization condition is met;

[0034] Control the motors according to the motor positions at the next time step of the robotic arm trajectory function after parameter optimization.

[0035] In this embodiment, the optimization condition can be a fixed number of times for parameter optimization, that is, after the set number of times of iterative optimization of the parameters, it is regarded as the completion of optimization; it can also be required that the parameters meet a certain preset value after optimization, or the optimized motion trajectory meets a certain preset value.

[0036] In this embodiment, the optimization condition includes making the deviation degree between the motion trajectory corresponding to the robotic arm trajectory function and the target trajectory less than a preset threshold. The deviation degree can be expressed in various forms, and its purpose is to be able to reflect the similarity between the motion trajectory corresponding to the robotic arm trajectory function and the target trajectory. For example, the maximum distance or average distance between the motion trajectory and the target trajectory can be used to represent the deviation degree, or the maximum curvature or average curvature of the motion trajectory can be used to represent the deviation degree. The fitting degree between the motion trajectory and the target trajectory can also be used.

[0037] Working principle or process of the present invention: The current position, target position, and current speed of each motor are considered in the robotic arm trajectory function. Therefore, when optimizing the parameters of the robotic arm trajectory function along the target trajectory, even if the initial speed is not zero, the movement trajectory of the robotic arm can be made as close as possible to the target trajectory. At the same time, since the parameter optimization takes into account the motor position and current speed (i.e., the initial speed), situations where the motor acceleration is too large or the motor executes a movement beyond the limit are avoided, enabling the robotic arm to stably execute actions even when the destination changes during the movement process.

[0038] Beneficial effects of this embodiment: Optimize the parameters in the robotic arm trajectory function and associate the current position, target position, and current speed of the motor, achieving the goal that the trajectory can be smooth and as close to a straight line as possible when controlling the movement of the robotic arm, and being able to dynamically adjust the trajectory during the movement process.

[0039] Embodiment 2

[0040] For an n-link robotic arm, assume its motor position vector is The forward kinematics function of the robotic arm is For the current initial motor position of the robotic arm at any moment And the end position of the robotic arm It is necessary to determine the motor target position vector at the current moment So that when controlled in this way at each moment, the final motor position of the robotic arm can reach Thereby enabling the end position of the robotic arm tool to reach And the overall trajectory is as close as possible to the straight line from p0 to p t The straight line.

[0041] An embodiment 2 of a robotic arm control method, including:

[0042] Step 1: Establish a trajectory function g i (x) = ax 3 + bx 2 + cx + d for the motors of the robotic arm according to the current position, target position, and current speed of each motor, and initialize the parameters a, b, c, d. Specifically, in the g i (x) function, the motor position at time 0 is at the initial position, that is, the current position θ0 of the motor, so d = θ0 can be obtained. Secondly, it is necessary to ensure that the instantaneous speed at time 0 is the current speed of the motor, that is, g i ′(0) = v0, from which c = v0 can be deduced. By ensuring that the position can reach the target position when x = 1, b = θ t - θ0 - v0 - a can be obtained, and the values of the parameters a, b, c, d are obtained.

[0043] Based on the motor trajectory functions of each motor, the manipulator trajectory function is obtained as is a vector function of the positions of each motor, is a vector composed of the parameters a to be optimized for each motor, including a1, a2,....a n , where n is the number of motors; F is the forward kinematics function of the manipulator. It should be noted that during optimization, is optimized, that is, the parameters a in the motor trajectory functions corresponding to all motors are optimized simultaneously.

[0044] The manipulator trajectory function is a combination of the motor trajectory functions of each motor. Therefore, by optimizing the parameter a in the motor trajectory function, the parameters of the manipulator trajectory function can be optimized.

[0045] Step 2: Taking the straight line from the current position p0 of the manipulator end to the target position p t of the manipulator end as the target trajectory, optimize the parameters of the manipulator trajectory function. The specific process is as follows:

[0046] S2.1: Use the current parameters to uniformly sample the trajectory points at each moment or multiple moments of the manipulator trajectory function. The distance from the trajectory point to the straight line where the target trajectory is located at the corresponding independent variable x at a certain moment is obtained as: After sampling step by step according to the specified step size Δx, the trajectory point x with the maximum distance from the target trajectory is obtained m ;

[0047] S2.2: Determine whether the corresponding distance exceeds the preset threshold. If it exceeds, use the gradient descent method to continue optimizing the parameter . Specifically, calculate the descent gradient m of the maximum trajectory point x with respect to the parameter vector . Then, the current parameter vector value can be subtracted by the result of multiplying the gradient vector by the learning rate. If it is less than the preset threshold, the optimization of the parameter is completed.

[0048] Step 3: Control the motors according to the motor positions at the next time step of the manipulator trajectory function after parameter optimization. In this embodiment, the time step is a specified value, and the motor positions at the next time step are calculated through a time step Δt After that, command the motors to move with this position as the target, so as to finally complete the motor control under the current time slice.

[0049] The working principle and related technical effects of this embodiment: The manipulator trajectory function considers the current position, target position, and current speed of each motor, with g i(x) is the motor trajectory function, and there is only one parameter a to be optimized. When optimizing the parameter a based on the target trajectory, with the current speed as the basis, even if the initial speed is not 0, the motion trajectory of the robotic arm can be made as close as possible to the target trajectory. At the same time, since the parameter optimization takes into account the motor position and the current speed (i.e., the initial speed), it can avoid the situation where the motor acceleration is too large or the motor executes a motion beyond the limit, enabling the robotic arm to execute actions stably even when the destination is changed during the motion process. At the same time, the optimization of the parameter a uses the distance between the maximum trajectory point and the target trajectory point in the robotic arm trajectory function to express the deviation degree between the motion trajectory and the target trajectory. Since only the distance needs to be calculated, the calculation efficiency is high and the code maintainability is good.

[0050] This embodiment also provides another implementation manner. In step two, it is judged whether the average value of the distances between multiple trajectory points in the robotic arm trajectory function and the target trajectory is less than a preset threshold. If so, the optimization of the parameter a is completed; if not, the parameter a is continuously iteratively optimized.

[0051] Embodiment 3

[0052] Embodiment 3 of a robotic arm control method, based on Embodiment 1 or Embodiment 2, is different from Embodiment 1 or Embodiment 2 in that the deviation degree is expressed by the curvature corresponding to the trajectory point. In step two, the specific process is as follows:

[0053] S2.1: Use the current parameters to uniformly sample the trajectory points at each moment or multiple moments of the robotic arm trajectory function to obtain the curvature of the trajectory point at the corresponding independent variable x at a certain moment After gradually sampling according to the specified step size Δx, the trajectory point x with the largest curvature is obtained m ;

[0054] S2.2: Judge whether the corresponding curvature exceeds the preset threshold. If it exceeds, the gradient descent method is used to continue optimizing the parameter , and if it is less than the preset threshold, the optimization of the parameter is completed.

[0055] Curvature is a quantity in mathematics that describes the degree of bending of a curve at a certain point. Using curvature is most in line with the problem to be solved by this invention patent from a mathematical definition. And in mathematics, the average curvature of a curve can also be calculated by means of integration, etc., so that the bending degree of the entire trajectory can be described by this quantity. Compared with the method of Embodiment 2, it is not necessary to sample and find the maximum value every time for optimization, but directly optimize. However, the disadvantage of using the curvature method is that the calculation process is very complex. It is necessary to use tools such as matlab to deduce the final calculation formula of this method, which is up to tens of thousands of characters. This method is not conducive to software code maintenance and has low calculation efficiency.

[0056] The remaining features and working principles of this embodiment are the same as those of Embodiment 2.

[0057] In addition, this embodiment also provides another implementation manner. It is determined that the average curvature corresponding to multiple trajectory points is less than a preset threshold. If so, the optimization of parameter a is completed; if not, the iteration optimization of parameter a continues.

[0058] Embodiment 4

[0059] An embodiment of a robotic arm control system for implementing the robotic arm control method of any of the above embodiments, including a robotic arm and a controller module. The controller module is used to be electrically connected to the robotic arm, obtain data of the motors of the robotic arm, and control the movement of the robotic arm; the controller module includes a processor for performing operations.

[0060] Embodiment 5

[0061] An embodiment of a storage medium for storing a computer program; wherein when the computer program is executed by a processor, the robotic arm control method of any of the above embodiments is implemented.

[0062] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, rather than limitations on the implementation manners of the present invention. For those of ordinary skill in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to enumerate all the implementation manners here. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention shall be included within the protection scope of the claims of the present invention.

Claims

1. A robot arm control method, characterized in that: include: Establishing a robot arm trajectory function according to the current position, target position and current speed of each motor of the robot arm; From the current position p0 of the end of the robot arm to the target position p of the end of the robot arm t The straight line is the target trajectory, and the parameters of the robot trajectory function are optimized until the optimization conditions are met; The motor is controlled according to the motor position of the next time step of the robot arm trajectory function after parameter optimization.

2. The robot arm control method according to claim 1, characterized in that: The optimization condition includes ensuring that the degree of deviation between the motion trajectory corresponding to the robot arm trajectory function and the target trajectory is less than a preset threshold.

3. The robot arm control method according to claim 2, characterized in that: The process of establishing the robot arm trajectory function includes: determining each motor function of the motor, determining the parameters to be optimized in the motor function; constructing the robot arm trajectory function according to each motor function; Under the robot trajectory function variable x, the deviation degree between the robot trajectory function and the target trajectory is obtained.

4. The robot arm control method according to claim 3, characterized in that: The motor function is specifically: g i (x)=ax 3 +bx 2 +cx+d d=θ0 c=v0 b=θ t -θ0-v0-a Among them, θ0 is the current position of the motor; θ t is the target position of the motor; v0 is the current speed; x represents the ratio of the current time to the final time The parameter to be optimized in the function is a; The robot arm trajectory function is in, is the vector function of each motor position, is the vector composed of the parameters a of each motor to be optimized; F is the forward kinematics function of the robot arm.

5. The robot arm control method according to claim 2, characterized in that: The iterative optimization of the parameters of the robot trajectory function is achieved through the gradient descent method.

6. The robot arm control method according to any one of claims 2 to 5, characterized in that: The process of optimizing the parameters of the robot arm trajectory function is to use the current parameters to uniformly sample the trajectory points of the robot arm trajectory function at each moment or multiple moments, calculate the degree of deviation between the trajectory points and the target trajectory, and continue to optimize the current parameters if it is greater than a preset threshold; If it is less than the preset threshold, the robot trajectory function is constructed with the optimized parameters.

7. The robot arm control method according to claim 6, characterized in that: The degree of deviation is: the distance between the trajectory point and the target trajectory; the optimization condition is specifically: the distance between the trajectory point with the largest distance to the target trajectory or the distances between multiple trajectory points and the target trajectory are all less than a preset threshold or the average value of the distances between multiple trajectory points and the target trajectory in the robot arm trajectory function is less than a preset threshold.

8. The robot arm control method according to claim 6, characterized in that: The degree of deviation is: the curvature of the trajectory point; the optimization condition is specifically: the maximum value of the curvature corresponding to multiple trajectory points uniformly sampled by the robot arm trajectory function is less than a preset threshold, or the average value of the curvature corresponding to multiple trajectory points is less than a preset threshold.

9. A robotic arm control system, characterized in that: A robotic arm control method for implementing any one of claims 1-9, comprising a robotic arm and a controller module, wherein the controller module is used to be electrically connected to the robotic arm, obtain data of the motor of the robotic arm and control the movement of the robotic arm; the controller module includes a processor for performing operations.

10. A storage medium, characterized in that: Used to store computer programs; wherein when the computer programs are executed by a processor, the robot arm control method as described in any one of claims 1-8 is implemented.

Images