Obstacle avoidance method based on dynamic motion primitive and machine control system

Abstract

The embodiment of the invention provides an obstacle avoidance method based on a dynamic motion primitive and a machine control system. The obstacle avoidance method comprises the following steps: planning an original track based on the dynamic motion primitive; when an obstacle is detected, calculating a coupling term according to the end point information and the obstacle information; adding the coupling item into the original trajectory to calculate a new trajectory; and outputting the new trajectory to the robotic arm. A sign function is introduced into a coupling term, so that the robot arm can be prevented from linearly colliding with an obstacle; square calculation of a position included angle is introduced into a coupling item, and the situation that a robot arm collides with an obstacle can be avoided; and the coupling terms are divided by the number of the point clouds for normalization, so that the new trajectory can be closer to the route of the original trajectory, and the space loss is reduced.

Description

Technical Field

[0001] The present invention relates to a method for controlling a robotic arm according to Dynamic Movement Primitives (DMP) and a robotic control system, and particularly to a method for adding a coupling term in DMP to control the robotic arm to avoid obstacles and a robotic control system. Background Art

[0002] Dynamic Movement Primitives (DMP) are widely applied to the trajectory control of robots (such as robotic arms). DMP can be used for trajectory imitation. The user gives a reference trajectory in advance. For example, the user can use offline trajectory planning or demonstrate the reference trajectory manually. Then, the reference trajectory is modeled using DMP to control the robotic arm to move along the reference trajectory. DMP can construct a second-order dynamic system with multiple parameters, making the control of the trajectory highly non-linear and highly real-time. In addition, the original trajectory can be changed by changing the parameters in DMP. For example, a coupling term can be added in DMP to make the robotic arm avoid obstacles. However, the current coupling term still has defects. For example, after adding the coupling term, the robotic arm will deviate excessively from the original trajectory (such as the reference trajectory). In addition, in some cases, the robotic arm cannot successfully avoid obstacles.

[0003] Therefore, the current coupling term still needs to be improved to solve the above problems. Summary of the Invention

[0004] An embodiment of the present invention provides an obstacle avoidance method based on Dynamic Movement Primitives (DMP), including: planning an original trajectory according to the dynamic movement primitive, task parameters, spatial information, and endpoint information, where the endpoint information includes the position and velocity of the endpoint of the robotic arm; when an obstacle is detected, calculating a coupling term according to the endpoint information and the obstacle information, where the obstacle information includes the position and velocity of the obstacle; adding the coupling term to the original trajectory to calculate a new trajectory; and when the obstacle is detected, outputting the new trajectory to the robotic arm; where the coupling term is expressed as follows:

[0005]

[0006] where R is a rotation matrix, v relative = v - v 0 is the relative velocity between the endpoint and the obstacle, v is the velocity of the endpoint, v 0 is the velocity of the obstacle, sign() is the sign function, θ is the position angle between the position of the obstacle and the position of the endpoint, |d| = |y 0 - y| is the absolute value of the distance between the obstacle and the endpoint, y 0x is the position of the obstacle, y is the position of the endpoint, exp() is the exponential function with the natural constant e as the base, is the direction angle between the velocity of the endpoint and the velocity of the original trajectory, |Δy| = |y d - y| is the Euclidean distance between the position of the new trajectory and the position of the endpoint, γ 4 , μ 4 , η 4 , γ 2 , γ 2 , μ', ρ are constant parameters.

[0007] An embodiment of the present invention also proposes a machine control system for controlling the movement of a robotic arm, including a trajectory planning module, a machine vision module, an obstacle avoidance module, and an adder. The trajectory planning module is configured to plan an original trajectory according to endpoint information, obstacle information, task parameters, and spatial information. The machine vision module is connected to the trajectory planning module and is configured to calculate and transmit the endpoint information and the obstacle information to the trajectory planning module when an obstacle is detected. The obstacle avoidance module is connected to the machine vision module and the trajectory planning module and is configured to calculate a coupling term according to the endpoint information and the obstacle information. The adder is connected to the trajectory planning module and the obstacle avoidance module and is configured to add the coupling term to the original trajectory to calculate a new trajectory and output the new trajectory to the robotic arm.

[0008] The obstacle avoidance method and the machine control system of the present invention propose an improved coupling term, which has the following advantages: (1) The coupling term introduces a sign function, and when the position angle is zero, the output value of the sign function is specified as a non-zero real number, which can avoid the robotic arm from directly colliding with the obstacle; (2) The coupling term introduces the calculation of the square of the position angle. When the position angle is close to zero, the change amplitude of the coupling term is larger, which can avoid the situation of the robotic arm rubbing against the obstacle; and (3) The coupling term is normalized by dividing it by the number of point clouds, so that the new trajectory can be closer to the route of the original trajectory, achieving the effect of reducing spatial loss while successfully avoiding obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0009] Figure 1 is a block diagram of the machine control system according to an embodiment of the present invention;

[0010] Figure 2 is a flowchart of the obstacle avoidance method according to an embodiment of the present invention;

[0011] Figure 3A is a schematic diagram with a non-zero angle;

[0012] Figure 3B is a schematic diagram with a zero angle;

[0013] Figure 4 is the curve graph of the position angle versus the angular velocity in the embodiment of the present invention;

[0014] Figure 5 is the schematic diagram of the trajectory of the embodiment of the present invention in three-dimensional space.

[0015]

Symbol Explanation

[0016] 10: Machine control system

[0017] 11: Robot arm

[0018] 12: Trajectory planning module

[0019] 13: Machine vision module

[0020] 14: Obstacle avoidance module

[0021] 15: Trajectory generation module

[0022] 16: Adder

[0023] 20: Obstacle avoidance method

[0024] 40: Graph

[0025] S21, S22, S23, S24, S25: Steps

[0026] C: Coupling term

[0027] CMD: Reference trajectory

[0028] g: Target position

[0029] K: Elastic constant

[0030] N: Number of kernel functions

[0031] NUM: Number of point clouds

[0032] ω i : Weight

[0033] Differential of state variable

[0034] y: Current position of the end point

[0035] Acceleration of the end point

[0036] y 0 : Current position of the obstacle

[0037] y init : Initial position of the end point

[0038] y new : New position of the end point

[0039] v: Velocity of the end point

[0040] v 0 : Velocity of the obstacle

[0041] vd: Initial velocity of the end point

[0042] τ: Time scale factor

[0043] X, Y, Z: Axes Detailed implementation manners

[0044] To make the objectives, features, and advantages of the present invention more obvious and understandable, specific embodiments of the present invention are hereinafter given, and detailed descriptions are made in conjunction with the accompanying drawings as follows.

[0045] Figure 1 FIG. is a functional block diagram of a machine control system 10 according to an embodiment of the present invention. The machine control system 10 includes a robotic arm 11, a trajectory planning module 12, a machine vision module 13, an obstacle avoidance module 14, a trajectory generation module 15, and an adder 16. In some embodiments, the robotic arm 11 may be a six-axis articulated robot or any type of multi-axis articulated robot and is capable of moving in a plane or three-dimensional space.

[0046] The trajectory generation module 15 is configured to translate a task set by a user (such as carrying an item to a specified position) into spatial parameters, which mainly include the initial position y of the end point init , a reference trajectory CMD (including the initial position y of the end point init , the initial velocity v d and acceleration), and a target position g. The task parameters include the number N of kernel functions, the DMP internal constants D, K, and the weights in the forcing function.

[0047] The trajectory planning module 12 is connected to the machine vision module 13 and the trajectory generation module 15 and is configured to plan an original trajectory according to the task parameters, spatial information, and end point information. In some embodiments, in an offline state, the trajectory planning module 12 performs trajectory imitation learning according to a pre-given reference trajectory to construct the trajectory into a second-order dynamic system, so that the trajectory fitted by the trajectory planning module 12 has high nonlinearity and high immediacy. During the trajectory imitation learning process, a Bayesian optimization method can be used to obtain the number N of kernel functions, the weights in the forcing function, and the DMP internal constants D, K set artificially. The reference trajectory is, for example, an artificial demonstration trajectory, a non-uniform rational B-spline (NURBS), or an S-curve.

[0048] The machine vision module 13 is connected to the trajectory planning module 12 and the obstacle avoidance module 14, and is configured to capture images and record timestamps (Timestamps), and generate endpoint information and obstacle information to the trajectory planning module 12 and the obstacle avoidance module 14 based on these. The endpoint information includes the position y and speed v of the endpoint, and the obstacle information includes the position y 0 , speed v 0 and the number of point clouds NUM. Specifically, the machine vision module 13 is configured to capture multiple images of the endpoint (e.g., the fixture, drill bit, suction cup, etc. at the end) and at least one obstacle in the space. Then, the machine vision module 13 is configured to detect the objects (e.g., the endpoint and the obstacle) and their positions in the multiple images, convert the obstacle into the form of a point cloud (pointcloud) and calculate the corresponding number of point clouds NUM, and calculate the speed of the object based on the displacement and time difference of the object in different images. Finally, the machine vision module 13 transmits the position y and speed v of the endpoint, the position y 0 , speed v 0 and the number of point clouds NUM to the trajectory planning module 12 and the obstacle avoidance module 14. The machine vision module 13 is, for example, an industrial camera, which includes a microprocessor and an image capture unit. The image capture unit is, for example, a Charge Coupled Device (CCD) or a Complementary Metal-Oxide Semiconductor (CMOS) imager.

[0049] The obstacle avoidance module 14 is connected to the machine vision module 13 and the adder 16, and is configured to calculate the coupling term C according to the endpoint information and the obstacle information. The adder 16 is connected to the trajectory planning module 12, the machine vision module 13 and the robotic arm 11, and is configured to add the coupling term C to the original trajectory to calculate a new trajectory, and transmit the new trajectory to the robotic arm 11.

[0050] Briefly speaking, Figure 1 the machine control system 10 can calculate the coupling term C according to the obstacle information and add it to the original trajectory while the robotic arm 11 is performing tasks to calculate a new trajectory. In some embodiments, the machine control system 10 periodically (e.g., in milliseconds) calculates the speeds and positions of the robotic arm 11 and the obstacle to instantaneously correct the trajectory of the robotic arm 11. In this way, the robotic arm 11 can instantaneously perform tasks and avoid obstacles.

[0051] Figure 2 is a flowchart of the obstacle avoidance method 20 according to an embodiment of the present invention. The obstacle avoidance method 20 can be executed by the machine control system 10 and includes the following steps.

[0052] Step S21: Plan the original trajectory based on the dynamic motion primitives, task parameters, spatial information, and endpoint information.

[0053] Step S22: Determine whether an obstacle is detected. If so, proceed to Step S23; if not, proceed to Step S25.

[0054] Step S23: Calculate the coupling term based on the endpoint information and obstacle information.

[0055] Step S24: Add the coupling term to the original trajectory to calculate the new trajectory.

[0056] Step S25: Output the original trajectory or the new trajectory to the robotic arm. Return to Step S21.

[0057] In Step S21, the trajectory planning module 12 plans the original trajectory according to the task parameters, spatial information, endpoint information, and obstacle information. Specifically, the trajectory planning module 12 plans the original trajectory according to the following equations (1) and (2):

[0058]

[0059]

[0060] where τ is the time scale factor, is the acceleration of the endpoint, K is the elastic constant, g is the target position of the endpoint, y is the current position of the endpoint, D is the damping constant, v is the velocity of the endpoint, y init is the initial position of the endpoint, x is the state variable, is the differential of the state variable, a x is a constant, and f(x) is a forcing function composed of N basis functions. The forcing function f(x) can be defined as follows:

[0061]

[0062] where is the i-th basis function, ω i is the weight of the i-th basis function, and the basis function can be defined as:

[0063]

[0064] where h i and c i represent the center and width of the i-th (i ∈ [1, N]) basis function respectively, and h i and c i can be calculated according to the following formulas respectively:

[0065]

[0066] h i = (c i+1 - c i ) -2 , h N = h N-1

[0067] where λ is a predefined constant. The state variable x represents the transitional state in the expression of the trajectory planning module 12. By combining the state variable x with the formula in the trajectory planning module 12, the acceleration command of the end point can be obtained. Furthermore, the current position y of the end point can be obtained.

[0068] Therefore, in step S21, the trajectory planning module 12 plans the original trajectory according to equations (1) and (2), so that the robotic arm 11 moves from the current position y along the original trajectory to the target position g.

[0069] In step S22, the machine vision module 13 determines whether an obstacle is detected. Whether an obstacle is detected or not, the machine vision module 13 calculates the end point information based on the captured video and transmits it to the trajectory planning module 12. When an obstacle is detected, the machine vision module 13 calculates the obstacle information based on the captured video and transmits it to the obstacle avoidance module 14.

[0070] In step S23, the obstacle avoidance module 14 calculates the coupling term C according to the end point information and the obstacle information. Specifically, the obstacle avoidance module 14 calculates the coupling term according to the following equation (3):

[0071]

[0072] where R is the rotation matrix, v relative = v - v 0 is the relative velocity between the end point and the obstacle, sign() is the sign function, |d| = |y 0 - y| is the absolute value of the distance between the obstacle and the end point, exp() is the exponential function with the natural constant e as the base, is the direction angle between the velocity of the end point and the velocity of the original trajectory, |Δy| = |y d - y| is the Euclidean distance between the position of the new trajectory and the position of the original trajectory (or the end point), γ 4 , μ 4 , η 4 , γ 2 , γ 2 , μ', ρ are constant parameters.

[0073] Specifically, the rotation matrix R is used to describe the vector (y 0The operation of rotating 90 degrees around the center of the y-axis (-y)×v can be expressed as follows:

[0074]

[0075] where y 0 is the position of the obstacle, y is the position of the end point, v is the velocity of the end point, is the rotation angle of 90 degrees, and × indicates calculating the outer product of two vectors. 0 -y)×v) plane clockwise or counterclockwise. Adding a velocity component containing a 90-degree rotation to the original trajectory can change the velocity direction of the end point to avoid obstacles.

[0076] The angle between the velocity of the end point and the velocity of the original trajectory It can be expressed as the following formula (5):

[0077]

[0078] where v d is the velocity of the end point when there is no obstacle, y d is the position of the end point when it moves according to the original trajectory. In other words, v d is the original speed of the end point when the obstacle does not exist, for example, the speed calculated according to equation (1). d is the original position of the end point when the obstacle does not exist, for example, the position calculated according to formula (1). If an obstacle exists, the trajectory planning module 12 and the obstacle avoidance module 14 use the new position ynew generated by the new trajectory to calculate the position, speed and acceleration generated by the next obstacle avoidance trajectory.

[0079] Figure 3A This is a schematic diagram of the position angle θ not being zero. The current obstacle position y 0 and the position y of the end point (y 0 The position angle θ between the velocity v and the velocity v is defined as follows:

[0080]

[0081] Among them, y 0 is the position of the obstacle, y is the position of the end point, v is the speed of the end point, T represents the transpose operation, cos -1 Represents the arccosine function.

[0082] sign() represents the sign function, which is defined as follows:

[0083]

[0084] where p is a non-zero real number. In some embodiments, p is any real number approaching zero, such as 0.001, -0.001, or 0.00002.

[0085] Figure 3B is a schematic diagram where the position angle θ is zero. In the known art, the coupling term multiplies the position angle θ with other variables without introducing a sign function. Therefore, when the position angle θ is zero, the coupling term in the known art is also zero, making the new trajectory coincide with the original trajectory, which may cause the robotic arm to directly collide with an obstacle. To solve this problem, the present invention introduces a sign function sign(θ) into the coupling term C, and specifies that the output value of the sign function sign(θ) is a non-zero real number when the position angle θ is zero, so that the coupling term C is never zero, ensuring that the robotic arm 11 can avoid obstacles.

[0086] In some embodiments, the obstacle avoidance module 14 calculates the coupling term C according to the following formula (8):

[0087]

[0088] where formula (8) uses |θ| 2 to replace |θ| in formula (3), which can make the change amplitude of the coupling term C larger when the position angle θ is close to zero. Therefore, it can avoid the situation where although the coupling term C is not zero but not large enough when the position angle θ is close to zero, resulting in the robotic arm 11 rubbing against the obstacle.

[0089] In some embodiments, the obstacle avoidance module 14 calculates the coupling term C according to the following formula (9):

[0090]

[0091] where NUM is the number of point clouds of the obstacle. Dividing the coupling term C by the number of point clouds NUM can normalize the coupling term C, making the influence of obstacles of different sizes on the coupling term C consistent. After normalizing the coupling term C, the coupling term C is appropriately reduced, so that the thrust applied to the robotic arm 11 is reduced, allowing the robotic arm 11 to avoid obstacles along a route closer to the original trajectory. Therefore, normalizing the coupling term C can make the new trajectory closer to the original trajectory and reduce the loss of free space.

[0092] Figure 4 is the curve graph 40 of the position angle versus the angular velocity in an embodiment of the present invention. The horizontal axis of graph 40 is the position angle θ (unit: radian, rad), and the vertical axis is the angular velocity (unit: radian per second, rad / s). The known curve is represented by a dashed line and can be described by the following formulas (10) and (11):

[0093]

[0094]

[0095] As can be seen from Equation (12), when the angular velocity is at the position angle θ of zero, it is also zero, and the coupling term of the known technology is also zero, making the new trajectory coincide with the original trajectory, which will cause the robotic arm to linearly collide with the obstacle.

[0096] The curve of the present invention is represented by a solid line and can be described by the following Equation (12):

[0097]

[0098] As can be seen from Equations (12) and (7), since sign(θ) is used to replace θ, in the angular velocity of the present invention a non-zero position angle θ will be replaced by positive 1 or negative 1, and a position angle θ of zero will be replaced by a non-zero real number p, which makes the coupling term C never zero, ensuring that the robotic arm 11 can successfully avoid obstacles.

[0099] In step S25, the adder 16 adds the coupling term C to the original trajectory to calculate the new trajectory. Therefore, the new trajectory can be expressed as the following Equation (13):

[0100]

[0101] In some embodiments, the coupling term C is calculated according to one of Equations (3), (9), and (10). In a preferred embodiment, the parameter γ 4 in Equations (3), (9), and (10) is 8000, the parameter η 4 is 0.5, the parameter μ 4 is 10 / π, the parameter γ 2 is 5e-5, the parameter μ' is 0.11, and the parameter ρ is 0.5.

[0102] In some embodiments, when an obstacle exists, the new position y of the end point calculated from the new trajectory new can replace the position y of the end point given by the machine vision module 13 for the trajectory planning module 12 and the obstacle avoidance module 14 to calculate the next new trajectory (i.e., the obstacle avoidance trajectory). In practice, environmental factors (such as insufficient light source, inaccurate focusing, etc.) will cause the position y of the end point given by the machine vision module 13 to jitter, resulting in the generated obstacle avoidance trajectory jittering; therefore, using the new position y new to calculate the next obstacle avoidance trajectory can improve the problem of trajectory jitter caused by environmental factors.

[0103] Figure 5It is a schematic diagram of the trajectory of the embodiment of the present invention in three-dimensional space. In the three-dimensional space composed of the X, Y, and Z axes, the original trajectory is represented by a solid line, the new trajectory is represented by a long and short line, and the obstacle is represented by a point cloud. The number of point clouds NUM is the number of black dots included in the obstacle. Through the machine control system 10 and the obstacle avoidance method 20 of the present invention, the original trajectory can be corrected to a new trajectory, and then the robotic arm 11 can be controlled to move from the initial position y init to the target position g and can successfully avoid obstacles.

[0104] In some embodiments, Figure 2 the obstacle avoidance method 20 can be compiled into program code and stored in the memory built into the computer, which is configured to instruct the processing unit of the computer to execute steps S21 to S25. The processing unit can be a Central Processing Unit (CPU), a General Purpose Microprocessor, or a combination of related chips of a General Purpose Microprocessor and a Special Purpose Processor. The memory is configured to store any data required for the operation of the processing unit. The memory can be a non-volatile memory, such as Static Random Access Memory (SRAM), flash memory, Solid-State Drive (SSD), etc.

[0105] In some embodiments, Figure 1 the machine control system 10 can be implemented by an Application Specific Integrated Circuit (ASIC).

[0106] In summary, the obstacle avoidance method of the present invention proposes an improved coupling term, which has the following advantages: (1) The coupling term introduces a sign function, and when the position angle is zero, the output value of the sign function is specified as a non-zero real number, which can prevent the robotic arm from directly colliding with obstacles; (2) The coupling term introduces the calculation of the square of the position angle. When the position angle is close to zero, the change amplitude of the coupling term is larger, which can avoid the situation of the robotic arm rubbing against obstacles; and (3) The coupling term is normalized by dividing it by the number of point clouds, so that the new trajectory can be closer to the route of the original trajectory, achieving the effect of reducing space loss while successfully avoiding obstacles.

[0107] Although this case has been disclosed as above with embodiments, the above embodiments are not configured to limit the invention of this case. Any person familiar with this art can make various changes and modifications based on the above embodiments without departing from the spirit and scope of this case. Therefore, the protection scope of this case shall be subject to the scope defined by the appended claims.

Claims

1. A collision avoidance method based on dynamic motion primitives for controlling the movement of a robotic arm, characterized in that, it includes: planning an original trajectory according to dynamic motion primitives, task parameters, endpoint information and spatial information, where the endpoint information includes the position and velocity of the endpoint of the robotic arm; when an obstacle is detected, calculating a coupling term according to the endpoint information and the obstacle information, where the obstacle information includes the position and velocity of the obstacle; adding the coupling term to the original trajectory to calculate a new trajectory; and when the obstacle is detected, outputting the new trajectory to the robotic arm; wherein, the coupling term is expressed as follows: where, R is the rotation matrix, v relative = v - v 0 is the relative velocity between the end point and the obstacle, v is the velocity of the end point, v 0 is the velocity of the obstacle, sign() is the sign function, θ is the position angle between the position of the obstacle and the position of the end point, |d| = |y 0 - y| is the absolute value of the distance between the obstacle and the end point, y0 is the position of the obstacle, y is the position of the end point, exp() is the exponential function with the natural constant e as the base, Δθ is the direction angle between the velocity of the end point and the velocity of the original trajectory, |Δy| = |y d - y| is the Euclidean distance between the position of the new trajectory and the position of the end point, γ 4 , μ 4 , η 4 , γ 2 , γ 2 , μ', ρ are constant parameters.

2. The method according to claim 1, characterized in that, the coupling term is also expressed as follows:

3. The method according to claim 1, characterized in that, the coupling term is also expressed as follows: where NUM is the number of point clouds of the obstacle.

4. The method according to claim 2 or 3, characterized in that, the position angle is defined as follows: where y 0 is the position of the obstacle, y is the position of the end point, v is the velocity of the end point, T represents the transpose operation, and cos -1 represents the inverse cosine function.

5. The method according to claim 4, characterized in that, the sign function is defined as follows: where p is a non-zero real number.

6. The method according to claim 1, characterized in that, the direction angle is defined as follows: where v d is the initial velocity of the end point without the obstacle, and y d is the position of the end point when moving along the original trajectory.

7. The method according to claim 1, characterized in that, the spatial information includes the target position of the endpoint and the initial position of the endpoint, and the original trajectory is defined as follows: where τ is the time scale factor, is the acceleration of the end point, K is the elastic constant, g is the target position of the end point, y is the position of the end point, D is the damping constant, v is the velocity of the end point, y init is the initial position of the end point, x is the state variable, is the differential of the state variable, a x is a constant, and f(x) is a forcing function composed of N basis functions.

8. The method according to claim 7, characterized in that, the forcing function is defined as follows: wherein is the i-th basis function, and ω i is the weight of the i-th basis function. The basis function is defined as: where h i and c i represent the center and width of the i-th (i ∈ [1, N]) basis function respectively, and h i and c i are calculated according to the following formulas respectively: h i = (c i+1 - c i ) -2 , h N = h N-1 where λ is a predefined constant.

9. The method according to claim 1, characterized in that, it further includes: taking multiple images of the endpoint and the obstacle and recording multiple timestamps; detecting the positions of the endpoint and the obstacle in the multiple images; and calculating the velocity of the endpoint and the velocity of the obstacle according to the displacement and time difference of the endpoint and the obstacle in the multiple images.

10. The method according to claim 3, characterized in that, it further includes: taking multiple images of the endpoint and the obstacle and recording multiple timestamps; detecting the position of the endpoint and the position of the obstacle in the multiple images; calculating the velocity of the endpoint and the velocity of the obstacle according to the displacement and time difference of the endpoint and the obstacle in the multiple images; and converting the obstacle into a point cloud form and calculating the corresponding number of point clouds.

11. The method according to claim 9 or 10, characterized in that, it further includes: replacing the position of the endpoint with the new position of the endpoint calculated from the new trajectory for the next calculation of the new trajectory.

12. The method according to claim 1, characterized in that, The constant parameter γ 4 is 8000, the constant parameter η 4 is 0.5, the constant parameter μ 4 is 10 / π, the constant parameter γ 2 is 5e-5, the constant parameter μ' is 0.11, and the constant parameter ρ is 0.

5.

13. A machine control system for controlling the movement of a robotic arm, characterized in that, it includes: a trajectory planning module configured to plan an original trajectory according to endpoint information, obstacle information, task parameters and spatial information; a machine vision module connected to the trajectory planning module and configured to calculate and transmit the endpoint information and the obstacle information to the trajectory planning module when an obstacle is detected; An obstacle avoidance module, connected to the machine vision module and the trajectory planning module, configured to calculate a coupling term according to the end point information and the obstacle information; and An adder, connected to the trajectory planning module and the obstacle avoidance module, configured to add the coupling term to the original trajectory to calculate a new trajectory and output the new trajectory to the robotic arm; wherein, the coupling term is expressed as follows: where R is the rotation matrix, v relative = v - v 0 is the relative velocity between the end point and the obstacle, v is the velocity of the end point, v 0 is the velocity of the obstacle, sign() is the sign function, θ is the position angle between the position of the obstacle and the position of the end point, |d| = |y 0 - y| is the absolute value of the distance between the obstacle and the end point, y 0 is the position of the obstacle, y is the position of the end point, exp() is the exponential function with the natural constant e as the base, Δθ is the direction angle between the velocity of the end point and the velocity of the original trajectory, |Δy| = |y d - y| is the Euclidean distance between the position of the new trajectory and the position of the end point, γ 4 , μ 4 , η 4 , γ 2 , γ 2 , μ', ρ are constant parameters.

14. The system according to claim 13, wherein, the coupling term is also expressed as follows:

15. The system according to claim 13, wherein, the coupling term is also expressed as follows: where NUM is the number of point clouds of the obstacle.

16. The system according to claim 14 or 15, wherein, the position included angle is defined as follows: where y 0 is the position of the obstacle, y is the position of the end point, v is the velocity of the end point, T represents the transpose operation, and cos -1 represents the inverse cosine function.

17. The system according to claim 16, wherein, the sign function is defined as follows: where p is a non-zero real number.

18. The system according to claim 13, wherein, the direction included angle is defined as follows: where v d is the initial velocity of the end point when there is no such obstacle, and y d is the position of the end point when moving along the original trajectory.

19. The system according to claim 13, wherein, the spatial information includes the target position of the end point and the initial position of the end point, and the original trajectory is defined as follows: where τ is the time scale factor, is the acceleration of the end point, K is the elastic constant, g is the target position of the end point, y is the position of the end point, D is the damping constant, v is the velocity of the end point, y init is the initial position of the end point, x is the state variable, is the differential of the state variable, a x is a constant, and f(x) is a forcing function composed of N basis functions.

20. The system according to claim 19, wherein, the forcing function is defined as follows: where φ i (x) is the i-th basis function, ω i is the weight of the i-th basis function, and the basis function φ i (x) is defined as: where h i and c i represent the center and width of the i-th (i ∈ [1, N]) basis function respectively, N is the number of kernel functions, h i and c i are calculated according to the following formulas respectively: h i = (c i+1 - c i ) -2 , h N = h N-1 where λ is a predefined constant, and the task parameters include the number of kernel functions, the damping constant D, the elastic constant K, and the weight of the forcing function.

21. The system according to claim 19, wherein, further includes: A trajectory generation module, connected to the obstacle avoidance module, configured to generate spatial parameters, and the spatial parameters include the initial position of the end point, the initial velocity of the end point, the acceleration of the end point, and the target position.

22. The system according to claim 13, wherein, the machine vision module includes: An image capture unit, configured to capture multiple images of the end point and the obstacle and record multiple timestamps; and A microprocessor, connected to the image capture unit, configured to detect multiple positions of the end point and multiple positions of the obstacle in the multiple images; calculate the velocity of the end point and the velocity of the obstacle according to the displacement and time difference of the end point and the obstacle in the multiple images; and convert the obstacle into a point cloud form and calculate the corresponding number of point clouds.

23. The system according to claim 22, wherein, the obstacle avoidance module is configured to replace the position of the end point with the new position of the end point calculated from the new trajectory for the next calculation of the new trajectory.

24. The system according to claim 13, wherein, The constant parameter γ 4 is 8000, the constant parameter η 4 is 0.5, the constant parameter μ 4 is 10 / π, the constant parameter γ 2 is 5e-5, the constant parameter μ' is 0.11, and the constant parameter ρ is 0.5.

Images