Mechanical arm path planning method and system

Abstract

The invention relates to the technical field of mechanical arm control, and discloses a mechanical arm path planning method and system.The method comprises the steps that in a simulation environment, a reward function is generated based on the current pose and the target pose of the tail end of a mechanical arm, a deep reinforcement learning network is trained based on the reward function, and a global planning path is obtained; constructing a dynamic motion primitive system by taking the global planning path as teaching; obstacle information is obtained, and an obstacle potential field model is constructed under the condition that an obstacle affects the working space of the tail end of the mechanical arm; based on the obstacle potential field model, a rejection item in the dynamic motion primitive system is constructed, and an obstacle avoidance path is generated based on the rejection item and is used for guiding the tail end of the mechanical arm to move away from the obstacle until the tail end of the mechanical arm reaches the target pose, so that the technical problem of simultaneously realizing real-time obstacle avoidance and global optimum in mechanical arm path planning is solved.

Description

Technical Field

[0001] The present application relates to the field of robotic arm control technology, and in particular to a robotic arm path planning method and system. Background Art

[0002] At present, the path planning methods of robotic arms are mainly divided into global planning and local planning. Among them, global planning refers to the knowledge of obstacle information and target position in the environment. Before the robotic arm starts to move, a global planning path from the starting position to the target position is planned. Global planning usually takes the shortest path or the lowest energy consumption as the global optimal goal, but global planning only considers the environmental information before the movement, and cannot avoid obstacles that appear in real time during the movement. Local planning refers to the known target position. During the movement of the robotic arm, obstacle information, including geometric features and position features of the obstacles, is obtained in real time to plan a local planning path that avoids obstacles, and then moves to the target position based on this. The local planning path is not optimal from the perspective of the global movement process. In the path planning of the robotic arm, how to achieve both global optimization of path planning and real-time avoidance of obstacles that appear during the movement has not yet been reported in related technologies. Summary of the invention

[0003] The present application provides a robot arm path planning method and system, which can achieve both real-time obstacle avoidance and global optimization of path planning.

[0004] In order to achieve the above objectives, the main technical solutions adopted in this application include:

[0005] In a first aspect, an embodiment of the present application provides a robot arm path planning method, the method comprising: in a simulation environment, generating a reward function based on the current position and target position of the end of the robot arm, training a deep reinforcement learning network based on the reward function, and obtaining a global planning path; using the global planning path as a teaching method, constructing a dynamic motion primitive system; obtaining obstacle information and constructing an obstacle potential field model; based on the obstacle potential field model, constructing a repulsion term in the dynamic motion primitive system, and generating an obstacle avoidance path based on the repulsion term, which is used to guide the end of the robot arm to move away from the obstacle until it reaches the target position.

[0006] Among them, obstacle information is obtained, and when the obstacle affects the working space at the end of the robot arm, an obstacle potential field model is constructed.

[0007] An embodiment of the present application provides a robot arm path planning method, including generating a reward function based on the current position and target position of the robot arm end in a simulation environment, training a deep reinforcement learning network based on the reward function to obtain a global planning path; constructing a dynamic motion primitive system using the global planning path as a teaching; obtaining obstacle information and constructing an obstacle potential field model; constructing a repulsion term in the dynamic motion primitive system based on the obstacle potential field model, and generating an obstacle avoidance path based on the repulsion term to guide the robot arm end to move away from the obstacle until it reaches the target position, thereby achieving the technical effect of both real-time obstacle avoidance and global optimization of path planning in the path planning of the robot arm end.

[0008] Optionally, a reward function is generated based on the current posture and target posture of the end of the robotic arm, and the deep reinforcement learning network is trained based on the reward function, specifically including: determining whether the current posture of the end of the robotic arm meets the training end condition, if so, generating a sparse reward and ending the training; if not, generating a single-step reward based on the current posture, the posture of the previous step and the target posture; wherein the single-step reward includes a position reward and a posture reward.

[0009] Optionally, the training end condition includes: a position error between a current position and a target position is less than a specified distance, and a posture error between a current posture and a target posture is less than a specified angle.

[0010] Optionally, the attitude error includes a roll attitude angle error, a pitch attitude angle error and a yaw attitude angle error.

[0011] Optionally, the training end condition includes that the error between the current posture and the target posture satisfies a constraint condition, and the constraint condition can be expressed as:

[0012]

[0013] In the above formula, dis(c,g) represents the distance between the current position and the target position; abs(φ g -φ c ) represents the absolute value of the angle error between the rolling attitude angle in the current attitude and the target attitude; abs(θ g -θ c ) represents the absolute value of the angle error between the pitch attitude angle in the current attitude and the target attitude; abs(ψ g -ψ c ) represents the absolute value of the angular error between the yaw attitude angle in the current attitude and the target attitude.

[0014] Among them, if the error between the current posture and the target posture meets the constraint condition, a first sparse reward is generated, and the first sparse reward is a positive reward and can be set to 20.

[0015] Optionally, the training end condition also includes: the joint angle exceeds an angle threshold; and / or the number of training steps exceeds a step threshold; and / or the end of the robotic arm collides with an obstacle.

[0016] If the joint angle exceeds the angle threshold, a second sparse reward is generated, which is a negative reward and can be set to -10. If the number of training steps exceeds the step threshold, a third sparse reward is generated, which can be set to 0. If the end of the robot arm collides with an obstacle, a fourth sparse reward is generated, which is a negative reward and can be set to -20.

[0017] Optionally, a single-step reward is generated based on the current posture, the posture of the previous step and the target posture, wherein the single-step reward includes a position reward and a posture reward, specifically including: judging whether a first position error between the current position and the target position is smaller than a second position error between the position of the previous step and the target position, if so, generating a positive position reward, if not, generating a negative position reward; judging whether a first posture error between the current posture and the target posture is smaller than a second posture error between the posture of the previous step and the target posture, if so, generating a positive posture reward, if not, generating a negative posture reward.

[0018] Optionally, the expression of the single-step reward is:

[0019] r i =r i _dis+r i _pos,

[0020] In the above formula, r i represents the single-step reward; r i _dis represents the position reward; r i _pos represents the posture reward;

[0021] in,

[0022] r i _dis=-A[dis(p i ,p g )-dis(p i-1 ,p g )]

[0023] r i _pos=-B[dis(R i ,R g )-dis(R i-1 ,R g )],

[0024] In the above formula, A and B are proportional coefficients; dis(p i ,pg ) represents the first position error between the current position and the target position; dis(p i-1 ,p g ) represents the second position error between the previous step position and the target position; dis(R i ,R g ) represents the first posture error between the current posture and the target posture; dis(R i-1 ,R g ) represents the second posture error between the previous step posture and the target posture.

[0025] Optionally, the training of the deep reinforcement learning network based on the reward function further includes: during the training process, updating network parameters based on the reward function with the goal of maximizing discounted future cumulative rewards.

[0026] In the second aspect, an embodiment of the present application provides a robot arm path planning system, the system comprising: a training module, which is used to generate a reward function based on the current position and target position of the end of the robot arm in a simulation environment, and train a deep reinforcement learning network based on the reward function to obtain a global planning path; a teaching module, which is used to construct a dynamic motion primitive system using the global planning path as a teaching; an acquisition module, which is used to obtain obstacle information and construct an obstacle potential field model when the obstacle affects the working space of the end of the robot arm; an obstacle avoidance module, which is used to construct a repulsion term in the dynamic motion primitive system based on the obstacle potential field model, and generate an obstacle avoidance path based on the repulsion term, so as to guide the end of the robot arm to move away from the obstacle until it reaches the target position.

[0027] In a third aspect, an embodiment of the present application provides a computer device, comprising: a memory and a processor, wherein the memory and the processor are communicatively connected to each other, computer instructions are stored in the memory, and the processor executes the above-mentioned robot arm path planning method by executing the computer instructions.

[0028] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, on which computer instructions are stored, and the computer instructions are used to enable a computer to execute the above-mentioned robot arm path planning method.

[0029] In a fifth aspect, an embodiment of the present application provides a computer program product, including computer instructions, which are used to enable a computer to execute the above-mentioned robot arm path planning method. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] In order to more clearly illustrate the specific implementation methods of the present application or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0031] Figure 1 A flowchart of a robot arm path planning method provided in an embodiment of the present application;

[0032] Figure 2 A design flow chart of the reward function provided in the embodiment of the present application;

[0033] Figure 3 A schematic diagram of the reward function provided in an embodiment of the present application changing with the number of training steps;

[0034] Figure 4 A schematic diagram of the change of the discounted reward function with the number of training steps provided in an embodiment of the present application;

[0035] Figure 5 A schematic diagram of the entropy loss function provided in an embodiment of the present application changing with the number of training steps;

[0036] Figure 6 A schematic diagram of the position change of the end of the robot arm in the path planning provided in the embodiment of the present application;

[0037] Figure 7 A schematic diagram of the posture change of the end of the robot arm in the path planning provided in the embodiment of the present application;

[0038] Figure 8 A schematic diagram of the position change of the end of the robot arm in the path planning provided in the embodiment of the present application;

[0039] Fig. 9 A schematic diagram of the posture change of the end of the robot arm in the path planning provided in the embodiment of the present application;

[0040] Fig.10 A three-dimensional schematic diagram of an obstacle avoidance path provided in an embodiment of the present application;

[0041] Fig.11 A two-dimensional schematic diagram of an obstacle avoidance path provided in an embodiment of the present application;

[0042] Fig.12 A schematic diagram of the error between the obstacle avoidance path provided in the embodiment of the present application and the original global planned path;

[0043] Fig.13 A robot arm path planning system provided in an embodiment of the present application;

[0044] Fig.14 A schematic diagram of the structure of a computer device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0045] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present application.

[0046] At present, the path planning methods of robotic arms are mainly divided into global planning and local planning. Among them, global planning refers to the obstacle information and target position in the known environment. Before the robotic arm starts to move, a global planning path from the starting position to the target position is planned. Global planning usually takes the shortest path or the lowest energy consumption as the global optimal goal, but global planning only considers the environmental information before the movement, and cannot avoid obstacles that appear in real time during the movement. Local planning refers to the known target position. During the movement of the robotic arm, obstacle information, including geometric features and position features of the obstacles, is obtained in real time to plan a local planning path to avoid obstacles, and then move to the target position based on this. The local planning path is not optimal from the perspective of the global movement process. In the path planning of the robotic arm, how to achieve the global optimality of path planning and avoid obstacles that appear during the movement in real time has not been reported in the relevant technology. In addition, in the relevant technology, in the posture planning method of the end of the robotic arm based on the reinforcement learning strategy, the reward function is generally set sparsely, and the training time required is relatively long.

[0047] The embodiment of the present application provides a robot arm path planning method, which obtains a global planning path by training a deep reinforcement learning network, constructs a dynamic motion primitive system with the global planning path as a teaching example, and constructs an obstacle potential field model based on the acquired obstacle information to construct a repulsion term in the dynamic motion primitive system, thereby generating an obstacle avoidance path, achieving the technical effect of both real-time obstacle avoidance and global optimal path planning.

[0048] An embodiment of the present application provides an embodiment of a robot arm path planning method. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0049] Figure 1 is a flow chart of a robot arm path planning method provided in an embodiment of the present application, such as Figure 1As shown, the process includes the following steps:

[0050] Step S1: In a simulation environment, a reward function is generated based on the current posture and target posture of the end of the robotic arm, and a deep reinforcement learning network is trained based on the reward function to obtain a global planning path.

[0051] Among them, the deep reinforcement learning network adopts the proximal policy optimization algorithm (PPO), which has small computational complexity and high exploration degree, and is suitable for path exploration of the robotic arm.

[0052] In the simulation environment, the robot acts as an intelligent agent. The current position and target position of the end of the robot are used as the input of the deep reinforcement learning network, and the reward function is used as the output of the deep reinforcement learning network. The reward function is used to guide the next action of the end of the robot. If the current position is closer to the target position than the previous position, a positive reward value is given. If the current position is farther away from the target position than the previous position, a negative reward value is given.

[0053] After sufficient training, the end of the robot arm can learn the global optimal path from the initial posture to the target posture, that is, the global planning path. Among them, the posture includes position and posture. The optimal means that the global path is the shortest or the energy consumption is the lowest.

[0054] Step S3, using the global planning path as a teaching, construct a dynamic motion primitive system.

[0055] Among them, the global planning path is used as the target trajectory for learning the dynamic motion primitive system, that is, the global planning path is used as a teaching, and the dynamic motion primitive method is used to learn the path to achieve the motion mode that best fits the global planning path.

[0056] Specifically, the expression of the global planning path can be:

[0057]

[0058] In the above formula, p g is the target position; p is the spatial position of the end effector of the robot arm; is the speed of the end effector of the robot during the movement; is the acceleration of the end effector of the robot arm during the movement; α is the spring viscosity coefficient, the unit is N·s / m; β is the friction damping coefficient, the unit is N / m; f(t) is a nonlinear forcing function that can be used to change the spatial motion trajectory of the end effector of the robot arm.

[0059] in, Three variables representing the spatial position of the end effector of the robot arm. The components of the motion trajectory of the end effector of the robot arm on the three coordinate axes can be expressed as:

[0060]

[0061] Among them, f(t) can be expressed as:

[0062]

[0063] In the above formula, ψ i is a fixed basis function; w i is the weight, which is an adjustable parameter; N represents ψ i There are N exponential basis functions.

[0064] The above motion trajectory is a linear time-varying dynamic system. Such a system does not allow direct coupling with other dynamic systems, nor does it allow coordination of multiple degrees of freedom in the dynamic system.

[0065] Therefore, we introduce first-order linear dynamics to replace time and define the canonical system:

[0066]

[0067] In the above formula, α s is a constant, starting from an arbitrarily chosen initial state, such as s0 = 1, to the final state, s converges monotonically to 0. Therefore, s can be considered as a phase variable, indicating the start of motion, and s close to zero means that the target p has basically been reached. d .

[0068] In the frequency domain, the expression of the forcing function is:

[0069]

[0070] In the above formula, p0 is the coordinate of the initial position of the end effector of the robot arm, corresponding to the position at time t = 0 in the time domain, p0 = p(t = 0); σ i and c i are constants that determine the width and center of the basis function. Through the above process, the time domain system is transformed into the frequency domain system, eliminating the influence of time and achieving system self-consistency.

[0071] The trajectory parameters in the global planning path in the time domain, including the velocity information and acceleration information of the end effector of the robot arm, are added to the frequency domain expression parameters of the spatial motion trajectory of the end effector of the robot arm to construct the frequency domain expression of the target trajectory. Among them, the frequency domain expression of the spatial motion trajectory of the end effector of the robot arm is:

[0072]

[0073] Specifically, let the path point of the robot arm joint space be q i , the path point in Cartesian space is p i , the two are one-to-one corresponding based on the kinematics of the robot arm, and we can get:

[0074]

[0075] In the above formula, N is the total number of discrete path points.

[0076] Similarly, the corresponding trajectory acceleration can be obtained:

[0077]

[0078] The trajectory parameters of the robot end effector motion obtained above are added to the frequency domain expression of the robot end effector spatial trajectory to obtain the target trajectory. The frequency domain expression of the target trajectory can be:

[0079]

[0080] In the above formula, p g is the target position; p is the spatial position of the end effector of the robot arm in the target trajectory; is the speed of the end effector of the robot arm during the movement in the target trajectory; is the acceleration of the end effector of the robot arm during the movement in the target trajectory; α is the spring viscosity coefficient, the unit is N·s / m; β is the friction damping coefficient, the unit is N / m; f target is a nonlinear forcing function.

[0081] In order to make the actual motion trajectory of the end of the robot arm close to the target trajectory, it is necessary to make f as close to f as possible. target By introducing the local weighted regression rule, the parameter w in f is adjusted i , so that the local weighted quadratic error J i Minimum.

[0082]

[0083] The solution can be obtained:

[0084]

[0085] in,

[0086]

[0087] In the above formula, s represents the phase variable; p d represents the target; Γ is a fixed basis function ψ i The matrix formed.

[0088] Step S5, obtaining obstacle information and constructing an obstacle potential field model.

[0089] Among them, the method of obtaining obstacle information can be obtained through detection by sensors such as cameras, laser radars or ultrasonic waves loaded in the robotic arm. In the process of the end of the robotic arm moving according to the global planning path, the obstacle information in the environment is detected in real time by sensors. In the case where the obstacle affects the working space of the end of the robotic arm, an obstacle potential field model is constructed. After obtaining the obstacle information, it is necessary to determine whether the obstacle is within the working space of the end of the robotic arm. For example, if the distance between the end effector of the robotic arm and the obstacle is less than the preset safety distance, it means that the obstacle will affect the planned global planning path. The end of the robotic arm may collide with the obstacle while moving according to the global planning path. In this case, it is necessary to construct an obstacle potential field model to achieve real-time obstacle avoidance.

[0090] The obstacle potential field model can be constructed by constructing an artificial obstacle potential field model in a simulation environment based on the acquired obstacle information, including geometric features and position information, to simulate the spatial position relationship between the robot arm and the obstacle. If the obstacle is moving, the acquired obstacle information also includes information such as the speed, acceleration, and motion trajectory of the obstacle.

[0091] In some embodiments, solid obstacles in the environment are modeled based on potential fields, and potential field modeling should meet the following conditions: (a) the potential must surround the entire object to avoid collisions during movement; (b) the potential field of the obstacle should not be too large to avoid reducing the available robotic arm workspace. Therefore, a method for volume obstacle avoidance is implemented using a super-quadratic potential function, in which an equipotential function with zero energy level on the obstacle surface is defined:

[0092]

[0093] In the above formula, m and n are natural numbers, f i (p), i = 1, 2, 3 are real-valued functions. By adjusting the parameters m, n and the functions f1, f2, f3, obstacles of any shape can be modeled.

[0094] The above equipotential function satisfies the following conditions: the boundary of the obstacle is a set of equipotential zero energy levels; when the distance from the obstacle increases, the value of C(p) increases.

[0095] The present application embodiment takes a classic ellipsoid (n=1) as an example. First, consider a cube with a unit side length of l=1, whose center is at the origin and whose radius is

[0096]

[0097] By rescaling the three spherical axes, we can get the smallest enclosed ellipse. Assume that the sides of the parallelepiped are l p1 , l p2 and l p3 , by rescaling we get:

[0098]

[0099] Therefore, C(p) can be defined as:

[0100]

[0101] Secondly, define the dynamic potential function for the volume obstacle. The dynamic potential function needs to satisfy the following three properties: the magnitude of the potential decreases with the distance between the system and the obstacle; the magnitude of the potential increases with the speed of the system The potential increases with the direction of velocity and is zero when the system is not moving. and the direction towards the obstacle increases, and if the system is moving away from the obstacle, the potential should vanish.

[0102] Therefore, the dynamic potential function expression can be:

[0103]

[0104] In the above formula, λ,β∈R + is a constant term, the function C(p) is any equipotential line that satisfies conditions (a) and (b), and the angle θ is the velocity of the system. The angle between the system position p and the direction closer to the obstacle. The gradient of the equipotential function C(p) is always perpendicular to the obstacle surface. Therefore, at least for convex obstacles, the angle θ can be calculated using the following formula.

[0105]

[0106] The above method is difficult to define for a non-convex obstacle because it will lead to non-differentiable equipotentials, so the angle θ cannot be defined. For non-convex obstacles, some workarounds can be used: First, if neither the starting position nor the target position is in the "hole" of the obstacle, that is, they are not in the convex hull of the obstacle, then the convex hull itself can be used as an obstacle. Second, it is possible to consider relaxing the concept of gradient to allow subdifferentiation. Third, non-convex obstacles can be divided into multiple convex components, each of which will produce its own potential.

[0107] Step S7, based on the obstacle potential field model, construct a repulsion term in the dynamic motion primitive system, and generate an obstacle avoidance path based on the repulsion term to guide the end of the robot arm to move away from the obstacle until it reaches the target posture.

[0108] In the framework of the dynamic motion primitive method, obstacle avoidance planning can be achieved through a variety of methods. In the embodiment of the present application, obstacle avoidance is achieved by adding a repulsion term in the dynamic motion primitive system. The repulsion term can push the motion trajectory of the end of the robot arm away from obstacles. In the dynamic motion primitive system, the global planning path is used as a teaching, and the repulsion term is added. As a control item, the end of the robot arm moves along the obstacle avoidance path to achieve obstacle avoidance. At the same time, the obstacle avoidance path is as close as possible to the originally planned global planning path to achieve the global optimal planning path. The expression of the obstacle avoidance path is:

[0109]

[0110] According to the above obstacle potential field model, the repulsion term in the dynamic motion primitive system is constructed, wherein the expression of the repulsion term can be:

[0111]

[0112] An embodiment of the present application provides a robot arm path planning method, including generating a reward function based on the current position and target position of the end of the robot arm in a simulation environment, training a deep reinforcement learning network based on the reward function to obtain a global planning path; constructing a dynamic motion primitive system using the global planning path as a teaching; obtaining obstacle information and constructing an obstacle potential field model; based on the obstacle potential field model, constructing a repulsion term in the dynamic motion primitive system, and generating an obstacle avoidance path based on the repulsion term to guide the end of the robot arm to move away from the obstacle until it reaches the target position, thereby achieving the technical effect of real-time obstacle avoidance in path planning and global optimization of the path.

[0113] In some embodiments, Figure 2 It is a design flow chart of the reward function provided in an embodiment of the present application. Based on the current posture and target posture of the end of the robotic arm, a reward function is generated, and the deep reinforcement learning network is trained based on the reward function. Specifically, it includes: judging whether the current posture of the end of the robotic arm meets the training end condition. If so, a sparse reward is generated and the training is ended; if not, a single-step reward is generated according to the current posture, the posture of the previous step and the target posture; wherein the single-step reward includes a position reward and a posture reward.

[0114] The reward function is the sum of the sparse reward and the single-step reward. In addition to limiting the joint angle, training steps, collisions, etc., the sparse reward mainly focuses on whether the target posture is reached after one training.

[0115] If the current position of the end of the robot arm meets the training end conditions, a sparse reward is generated and the training ends, which includes any one or more of the following four situations:

[0116] 1) If the error between the current position and the target position meets the constraint conditions, a first sparse reward is generated. The constraint conditions are: the position error between the current position and the target position is less than the specified distance, and the posture error between the current posture and the target posture is less than the specified angle.

[0117] The constraint condition expression is:

[0118]

[0119] In the above formula, dis(c,g) represents the distance between the current position and the target position; abs(φ g -φ c ) represents the absolute value of the angle error between the rolling attitude angle in the current attitude and the target attitude; abs(θ g -θ c ) represents the absolute value of the angle error between the pitch attitude angle in the current attitude and the target attitude; abs(ψ g -ψ c ) represents the absolute value of the angular error between the yaw attitude angle in the current attitude and the target attitude.

[0120] Among them, the first sparse reward is a positive reward and can be set to 20.

[0121] 2) If the joint angle exceeds the angle threshold, a second sparse reward is generated.

[0122] For example, the angle threshold can be set to Π or -Π. When the joint angle is greater than Π or less than -Π, it is considered that the joint angle exceeds the angle threshold. The second sparse reward is a negative reward, which can be set to -10.

[0123] 3) If the number of training steps exceeds the step threshold, a third sparse reward is generated.

[0124] For example, the step count threshold may be set to 100 steps, and the third sparse reward may be set to 0.

[0125] 4) If the end of the robot arm collides with an obstacle, a fourth sparse reward is generated.

[0126] For example, if the distance between the end effector of the robot arm and the obstacle is detected to be less than the preset safety distance of 0.1m, it is considered that the end effector of the robot arm collides with the obstacle. The fourth sparse reward is a negative reward and can be set to -20.

[0127] In order to improve the training speed of the deep reinforcement learning network, the reward strategy sets a single-step reward in addition to the sparse reward. The single-step reward focuses on the reward after each step of movement during the training process. The single-step reward includes position reward and posture reward to constrain the position and posture of the end of the robot during the movement. The single-step reward is measured by the error between the current posture of the end of the robot and the target posture.

[0128] Generate a single-step reward based on the current posture, the previous posture, and the target posture, including:

[0129] Determine whether the first position error between the current position and the target position is less than the second position error between the position of the previous step and the target position. If so, the generated position reward is positive; if not, the generated position reward is negative.

[0130] Determine whether the first posture error between the current posture and the target posture is less than the second posture error between the posture of the previous step and the target posture. If so, the generated posture reward is positive, otherwise, the generated posture reward is negative.

[0131] Specifically, the expression of single-step reward is:

[0132] r i =r i _dis+r i _pos

[0133] In the above formula, r i represents the single-step reward; r i _dis represents the position reward, r i _pos represents the posture reward.

[0134] Specifically, the expressions of position reward and posture reward are as follows:

[0135] r i _dis=-A[dis(p i ,p g )-dis(p i-1 ,p g )]

[0136] r i _pos=-B[dis(R i ,R g )-dis(R i-1 ,R g )]

[0137] In the above formula, r i _dis represents the position reward, r i _pos represents the posture reward; A and B are proportional coefficients; dis(p i ,pg ) represents the first position error between the current position and the target position; dis(p i-1 ,p g ) represents the second position error between the previous step position and the target position; dis(R i ,R g ) represents the first posture error between the current posture and the target posture; dis(R i-1 ,R g ) represents the second posture error between the previous step posture and the target posture.

[0138] Among them, the position error can be expressed by Euclidean distance: dis(p i ,p g )=||p i -p g ||.

[0139] In the embodiment of the present application, the attitude error is represented by three attitude angle errors, including the roll attitude angle error, the pitch attitude angle error and the yaw attitude angle error. If the three attitude angle errors are directly introduced into the reward function, the training effect is poor. Therefore, it is considered to introduce the Lie group and Lie algebra theory in mathematics to measure the change of attitude. The Lie algebra so(3) is defined in the tangent space of the Lie group SO(3) and describes the local derivative of the Lie group. That is, SO(3) is a 3×3 rotation matrix, and so(3) is a vector, and the relationship between the two is:

[0140] log:SO(3)→so(3)

[0141] R 3×3 →φ=(u x ,u y ,u z )

[0142] In the above formula, the arrow represents the conversion relationship from the variable in the Lie group to the Lie algebra; R 3×3 is a 3×3 rotation matrix.

[0143] Taking the attitude angle error as the group parameter of the SO(3) group, we further calculate the Euclidean distance in the tangent space of the unit element || log(R i )-logR( j )|| is used to represent the distance between two SO(3) elements, that is, the distance between Lie group elements is defined by calculating the modulus of the Lie algebra. According to the forward kinematics of the manipulator, the initial posture pos0 of the end of the manipulator and the target posture pos g for:

[0144] pos0=R0p0

[0145] POS g =Rg p0

[0146] In the above formula, R0 represents the rotation matrix of the initial posture; R g represents the rotation matrix of the target posture, p0 represents the initial posture in the world coordinate system; p g Represents the target posture in the world coordinate system;

[0147] The problem of finding the posture error between the initial posture and the target posture is transformed into the problem of finding the distance between two SO(3) elements, thereby defining the posture error d(R0,R g )for:

[0148] d(R o ,R g )=||Log(R o )-Log(R g )||

[0149] In the embodiment of the present application, the deep reinforcement learning network adopts the proximal policy optimization algorithm (PPO), which has a small amount of calculation and a high degree of exploration, and is suitable for the path exploration of the robot arm. The learning method of the PPO algorithm is to start from the state value function and the action-value function The strategy π is generated from the value function θ (a|s).

[0150] π θ (s,a)=P[a|s,θ]≈π(a|s)

[0151] In the above formula, is the weight parameter vector of the policy function. Policy function π θ It is a probability density function. When taking the action generated by the strategy, sampling is performed according to the probability distribution. The parameter θ determines the form of the probability distribution of the strategy function. Then the probability of the agent taking action a at time step t and state s is obtained.

[0152] π θ (a|s)=P{a t =a|s t =s,θ t =θ}

[0153] in, is the evaluation function.

[0154] In order to improve the optimization efficiency, on this basis, the proximal strategy optimization algorithm uses the importance sampling technology to evaluate the target strategy using the data generated by the behavior strategy. The importance sampling technology comes from solving the expectation:

[0155] E x～p[f(x)]=∫f(x)p(x)dx

[0156] When the distribution p(x) of the random variable x is very complex, it is difficult to use analytical methods to generate samples for approximating the expectation, so a probability distribution q(x) with a simple probability distribution and easy sample generation is selected. Then the original expectation can be changed to:

[0157]

[0158] In the above formula, is the importance weight. This is how importance sampling is substituted into the policy gradient algorithm.

[0159] By importance sampling, π θ becomes π θ' , so (s t ,a t ) is the same as π θ' The data sampled after the environment interaction. θ' and π θ It is a different model, so we need to add a correction term. This correction term uses the importance sampling technique to change (s t ,a t ) with π θ The probability of sampling is removed (s t ,a t ) with π θ' The probability of sampling.

[0160]

[0161] in, Therefore eliminated.

[0162] In order to ensure that the demonstration model is not too different from the real model, an additional constraint is added during training. This constraint is π θ Follow π θ' The KL divergence (Kullback-Leibler Divergence) of the output action, which measures π θ and π θ' where β is the constraint weight.

[0163]

[0164] In some embodiments, during the training of the deep reinforcement learning network, the output reward function and loss function are used to update the network parameters with the goal of maximizing the discounted future cumulative reward.

[0165] In some embodiments, the above-mentioned proximal policy optimization algorithm (PPO) is simulated and verified. The simulation verification is carried out on the OpenAI Gym platform, and the path planning algorithm is built based on the proximal policy optimization algorithm framework of Stable-baselines, and the programming language used is Python.

[0166] In the simulation verification test, the robot arm is designed to be in an upright state as the initial state, at which time the joint angles are q = [0 0 00 0 0] T , the corresponding position and posture of the end effector of the robot arm can be expressed as:

[0167]

[0168] In the above formula, x, y, and z represent the coordinate positions of the end effector of the robotic arm, and φ, θ, and ψ represent the roll attitude angle error, pitch attitude angle error, and yaw attitude angle error in the attitude, respectively.

[0169] The designed target pose can be expressed as:

[0170]

[0171] First, the deep reinforcement learning network needs to be trained. The effect of algorithm training can be observed through the Tensorboard platform. Tensorboard reads the status of network training in the algorithm and records the changes in relevant parameters during training in order to adjust and optimize the neural network. In the proximal policy optimization algorithm, the changes in the reward function and the loss function can reflect whether the algorithm converges and whether the number of training times is sufficient. If the change trend of the reward function and the loss function is incorrect, it is necessary to consider whether the reward mechanism is set unreasonably, whether the various hyperparameters in the algorithm and the neural network are set inappropriately for the research model, and whether the number of training times is incorrect.

[0172] Figure 3 A schematic diagram of the reward function provided in an embodiment of the present application changing with the number of training steps. Figure 4 A schematic diagram of the discounted reward function provided in an embodiment of the present application changing with the number of training steps. Figure 5 Schematic diagram of the entropy loss function provided in the embodiment of the present application changing with the number of training steps. It should be added that Figure 3-Figure 5 In the figure, the light-colored curve is the original function change curve, and the dark-colored curve is the result after smoothing for the convenience of observing the change.

[0173] first, Figure 3The reward function changes during each training process. The reward function refers to the reward obtained by the robot arm in the process of searching for the target posture from the initial posture. According to the setting of the reward function, the end of each training process may be due to the target posture being reached, or due to the joint angle exceeding the limit, exceeding the training step threshold, or colliding with an obstacle during movement. Figure 4 The discounted reward function is shown. The discounted reward function is a reward function that takes into account the impact of the current state on the future. In the process of training a deep reinforcement learning network, the training goal is to maximize the optimal strategy of discounted future cumulative rewards. Figure 3 and Figure 4 It can be seen that the reward function tends to be flat and reaches a maximum value at the end of training, which verifies that the training of the deep reinforcement learning network algorithm proposed in the embodiment of the present application is convergent and effective.

[0174] Figure 5 It shows the entropy loss function during the training process. Through the operating environment, the robot arm has accumulated a lot of experience during the training process, recorded the experience with better results, and used the strategies adopted in the experience with better results as labels. Using cross entropy as the loss function, the strategy results output by the network can be optimized in the direction closer to the better strategy. Therefore, the smaller the cross entropy with the label strategy, the closer it is to the label strategy. In other words, it can be understood as learning from past successful experiences. Therefore, the decrease in the entropy loss function means that the network is gradually converging to the best performance. Therefore, Figure 5 It can be seen that the entropy loss function gradually decreases to 0, indicating that the algorithm has learned the optimal strategy through training.

[0175] Figure 6 A schematic diagram of the change in the position coordinates of the end of the robot arm in the path planning provided in an embodiment of the present application. Figure 7 A schematic diagram of the posture changes of the end of the robotic arm during path planning provided in an embodiment of the present application. Figure 6 and Figure 7 These are all test results verified by simulation under obstacle-free conditions. By training the deep reinforcement learning network, the global planning path with the least number of steps is obtained. According to the test results, the terminal position of the actual motion path of the end effector of the robot arm can be expressed as:

[0176]

[0177] The posture error between it and the target posture can be expressed as:

[0178] e dis =dis(a,g)=0.034(m)

[0179] e pos = abs(φ g -φ a)+abs(θ g -θ a )+abs(ψ g -ψ a )=0.009(rad)

[0180] In the above formula, e dis Indicates the position error between the end position of the actual motion and the target position, e pos It represents the attitude error between the end attitude of the actual motion and the target attitude.

[0181] The above test results are based on the case where the operating space is free space, but in the actual environment, the operating space of the robot arm will have many obstacles or non-collision points. Therefore, static obstacles are added to the simulation environment, assuming that the starting and target postures of the end of the robot arm remain unchanged, and the safety distance between the end effector of the robot arm and the static obstacle is set to 0.1m. Figure 8 A schematic diagram of the change in position of the end of the robot arm during path planning provided in an embodiment of the present application. Fig. 9 Schematic diagram of the change of the posture of the end of the robot arm in the path planning provided by the embodiment of the present application. If it is determined that the obstacle does not affect the working space of the end of the robot arm, that is, the distance between the end effector of the robot arm and the static obstacle is greater than or equal to 0.1m, the planned path is as follows Figure 8 and Fig. 9 As shown in the figure, and the planned path under the above obstacle-free conditions ( Figure 6 and Figure 7 ) is similar, and the planning process will not be repeated here. Figure 6 and Figure 8 It can be seen that the path planned in the presence of obstacles is different from the path planned in the free space without obstacles, which verifies the effectiveness and stability of the path planning algorithm proposed in the embodiment of the present application.

[0182] In order to verify the effectiveness of the dynamic motion primitive method and obstacle avoidance path planning, the embodiment of the present application defines a cylindrical obstacle in the simulation space, and the obstacle will affect the working space of the end of the robot arm. The cylindrical obstacle is a cylinder with a bottom radius of 0.2m and a height of 1m. Its specific parameters are:

[0183]

[0184] The function expression of the target trajectory can be:

[0185]

[0186] The volume obstacle avoidance method based on dynamic motion primitives can achieve local obstacle avoidance. Fig.10A three-dimensional schematic diagram of an obstacle avoidance path provided in an embodiment of the present application. Fig.11 A two-dimensional plan view of an obstacle avoidance path provided in an embodiment of the present application. Fig.10 and Fig.11 The obstacle avoidance effect of the path based on the improved dynamic motion primitive is demonstrated. Compared with the original global planning path that will pass through obstacles, the re-planned obstacle avoidance path can achieve the function of local obstacle avoidance and can track the original global planning path well in other path parts.

[0187] See also Fig.12 , Fig.12 A schematic diagram of the error between the obstacle avoidance path provided in the embodiment of the present application and the original global planning path. Due to the presence of local obstacles, the re-planned path and the original path have a certain error. When the obstacle does not affect the working space of the end of the robotic arm, the error of path learning is basically 0, indicating that its actual path can follow the planned global planning path. When the obstacle affects the working space of the end of the robotic arm, the actual path begins to deviate from the original global planning path to achieve local obstacle avoidance. Simulation proves that the re-planning method based on the improved dynamic motion primitive can achieve both tracking of the global planning path and a certain local obstacle avoidance capability to deal with unexpected situations that occur in actual movement.

[0188] See also Fig.13 , Fig.13 The robot arm path planning system provided in the embodiment of the present application includes: a training module, which is used to generate a reward function based on the current position and target position of the end of the robot arm in a simulation environment, and train the deep reinforcement learning network based on the reward function to obtain a global planning path; a teaching module, which is used to construct a dynamic motion primitive system with the global planning path as teaching; an acquisition module, which is used to obtain obstacle information and construct an obstacle potential field model; an obstacle avoidance module, which is used to construct a repulsion term in the dynamic motion primitive system based on the obstacle potential field model, and generate an obstacle avoidance path based on the repulsion term, so as to guide the end of the robot arm to move away from the obstacle until it reaches the target position.

[0189] The further functional description of each of the above modules and units is the same as that of the above corresponding embodiments and will not be repeated here.

[0190] The robotic arm path planning system in the embodiment of the present application is presented in the form of a functional unit, where the unit refers to an ASIC (Application Specific Integrated Circuit) circuit, a processor and memory that executes one or more software or fixed programs, and / or other devices that can provide the above-mentioned functions.

[0191] See also Fig.14 , Fig.14 The schematic diagram of the structure of the computer device provided for the embodiment of the present application, the computer device includes: one or more processors 10, a memory 20, and an interface for connecting each component, including a high-speed interface and a low-speed interface. Each component uses different buses to communicate with each other, and can be installed on a common mainboard or installed in other ways as needed. The processor can process the instructions executed in the computer device, including instructions stored in or on the memory to display the graphical information of the GUI on an external input / output device (such as a display device coupled to the interface). In some optional embodiments, if necessary, multiple processors and / or multiple buses can be used together with multiple memories and multiple memories. Similarly, multiple computer devices can be connected, and each device provides some necessary operations (for example, as a server array, a group of blade servers, or a multi-processor system). Fig.14 A processor 10 is taken as an example.

[0192] The processor 10 may be a central processing unit, a network processor or a combination thereof. The processor 10 may further include a hardware chip. The hardware chip may be a dedicated integrated circuit, a programmable logic device or a combination thereof. The programmable logic device may be a complex programmable logic device, a field programmable gate array, a general purpose array logic or any combination thereof.

[0193] The memory 20 stores instructions executable by at least one processor 10, so that the at least one processor 10 executes the method shown in the above embodiment.

[0194] The memory 20 may include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application required for at least one function; the data storage area may store data created according to the use of the computer device, etc. In addition, the memory 20 may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some optional embodiments, the memory 20 may optionally include a memory remotely arranged relative to the processor 10, and these remote memories may be connected to the computer device via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0195] The memory 20 may include a volatile memory, such as a random access memory; the memory may also include a non-volatile memory, such as a flash memory, a hard disk or a solid state drive; the memory 20 may also include a combination of the above types of memory.

[0196] The computer device further comprises a communication interface 30 for the computer device to communicate with other devices or a communication network.

[0197] The embodiment of the present application also provides a computer-readable storage medium. The above method according to the embodiment of the present application can be implemented in hardware, firmware, or can be implemented as a computer code that can be recorded in a storage medium, or can be implemented as a computer code that is originally stored in a remote storage medium or a non-temporary machine-readable storage medium and will be stored in a local storage medium through a network download, so that the method described herein can be stored in such software processing on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only storage memory, a random access memory, a flash memory, a hard disk or a solid-state hard disk, etc.; further, the storage medium can also include a combination of the above types of memories. It can be understood that a computer, a processor, a microprocessor controller, or programmable hardware includes a storage component that can store or receive software or computer code. When the software or computer code is accessed and executed by a computer, a processor, or hardware, the method shown in the above embodiment is implemented.

[0198] The embodiment of the present application provides a computer program product, which includes computer instructions, which are stored in a computer-readable storage medium. A processor of a computer device reads the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, so that the computer device executes the method of any embodiment of the present application.

[0199] Although the embodiments of the present application are described in conjunction with the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present application, and such modifications and variations are all within the scope defined by the appended claims.

[0200] The devices and modules described in the above embodiments may be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, the computer may be, for example, a personal computer, a laptop computer, a cellular phone, a camera phone, a smart phone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or a combination of any of these devices.

[0201] For the convenience of description, the above device is described in various units according to their functions. Of course, when implementing the present application, the functions of each unit can be implemented in the same or multiple software and / or hardware.

[0202] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, devices, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0203] The present application is described with reference to the flowcharts and / or block diagrams of the methods, apparatuses, and computer program products according to the embodiments of the present application. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of the processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0204] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0205] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0206] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0207] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the device embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

[0208] The above is only an embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the scope of the claims of the present application.

[0209] Although the embodiments of the present application have been described in conjunction with the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present application, and such modifications and variations are all within the scope defined by the appended claims.

Claims

1. A robot arm path planning method, characterized in that: The method comprises: In the simulation environment, a reward function is generated based on the current and target positions of the end of the robot arm. The deep reinforcement learning network is trained based on the reward function to obtain the global planning path. Using the global planning path as a teaching example, a dynamic motion primitive system is constructed; Obtain obstacle information and construct an obstacle potential field model; Based on the obstacle potential field model, the repulsion term in the dynamic motion primitive system is constructed, and an obstacle avoidance path is generated based on the repulsion term to guide the end of the robotic arm to move away from the obstacle until it reaches the target position.

2. The method according to claim 1, characterized in that The reward function is generated based on the current posture and target posture of the end of the robotic arm, specifically including: Determine whether the current posture of the end of the robotic arm meets the training end conditions. If so, generate a sparse reward; if not, generate a single-step reward based on the current posture, the posture of the previous step, and the target posture; wherein the single-step reward includes a position reward and a posture reward.

3. The method according to claim 2, characterized in that The training end condition includes: the error between the current posture and the target posture meets the constraint condition, and the constraint condition is that the position error between the current position and the target position is less than a specified distance, and the posture error between the current posture and the target posture is less than a specified angle.

4. The method according to claim 3, characterized in that The attitude error includes a roll attitude angle error, a pitch attitude angle error and a yaw attitude angle error.

5. The method according to claim 4, characterized in that The constraints can be expressed as: In the above formula, dis(c,g) represents the distance between the current position and the target position; abs(φ g -φ c ) represents the absolute value of the roll attitude angle error between the current attitude and the target attitude; abs(θ g -θ c ) represents the absolute value of the pitch attitude angle error between the current attitude and the target attitude; abs(ψ g -ψ c ) represents the absolute value of the yaw attitude angle error between the current attitude and the target attitude.

6. The method according to claim 2, characterized in that The training end condition also includes: The joint angle exceeds the angle threshold; and / or the training step number exceeds the step threshold; and / or the end of the robotic arm collides with an obstacle.

7. The method according to claim 2, characterized in that The single-step reward is generated according to the current posture, the posture of the previous step and the target posture, wherein the single-step reward includes a position reward and a posture reward, specifically including: Determine whether the first position error between the current position and the target position is less than the second position error between the position of the previous step and the target position. If so, the generated position reward is positive; if not, the generated position reward is negative. Determine whether the first posture error between the current posture and the target posture is less than the second posture error between the posture of the previous step and the target posture. If so, the generated posture reward is positive, otherwise, the generated posture reward is negative.

8. The method according to claim 7, characterized in that The expression of the single-step reward is: r i =r i _dis+r i _pos, In the above formula, r i represents the single-step reward; r i _dis represents the position reward; r i _pos represents the posture reward; in, r i _dis=-A[dis(p i ,p g )-dis(p i-1 ,p g )] r i _pos=-B[dis(R i ,R g )-dis(R i-1 ,R g )], In the above formula, A and B are proportional coefficients; dis(p i ,p g ) represents the first position error between the current position and the target position; dis(p i-1 ,p g ) represents the second position error between the previous step position and the target position; dis(R i ,R g ) represents the first posture error between the current posture and the target posture; dis(R i-1 ,R g ) represents the second posture error between the previous step posture and the target posture.

9. The method according to claim 1, characterized in that: The training of the deep reinforcement learning network based on the reward function also includes: During training, the network parameters are updated based on the reward function with the goal of maximizing the discounted future cumulative reward.

10. A robot arm path planning system, characterized in that: The system comprises: The training module is used to generate a reward function based on the current posture and target posture of the end of the robot arm in a simulation environment, and train the deep reinforcement learning network based on the reward function to obtain a global planning path; The teaching module is used to build a dynamic motion primitive system based on the global planning path; The acquisition module is used to obtain obstacle information and build an obstacle potential field model; The obstacle avoidance module is used to construct the repulsion term in the dynamic motion primitive system based on the obstacle potential field model, and generate an obstacle avoidance path based on the repulsion term to guide the end of the robot arm to move away from the obstacle until it reaches the target position.

Images