CN117182932B - Method and device for planning obstacle avoidance action of mechanical arm and computer equipment - Google Patents
Method and device for planning obstacle avoidance action of mechanical arm and computer equipment Download PDFInfo
- Publication number
- CN117182932B CN117182932B CN202311473569.5A CN202311473569A CN117182932B CN 117182932 B CN117182932 B CN 117182932B CN 202311473569 A CN202311473569 A CN 202311473569A CN 117182932 B CN117182932 B CN 117182932B
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- joint
- action
- motion
- determining
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000009471 action Effects 0.000 title claims abstract description 87
- 238000000034 method Methods 0.000 title claims abstract description 55
- 230000006870 function Effects 0.000 claims abstract description 86
- 230000000977 initiatory effect Effects 0.000 claims abstract description 73
- 238000001514 detection method Methods 0.000 claims abstract description 39
- 238000009499 grossing Methods 0.000 claims abstract description 18
- 238000012937 correction Methods 0.000 claims abstract description 17
- 230000036544 posture Effects 0.000 claims description 42
- 230000007704 transition Effects 0.000 claims description 23
- 238000004590 computer program Methods 0.000 claims description 14
- 238000004422 calculation algorithm Methods 0.000 claims description 13
- 230000035515 penetration Effects 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000002194 synthesizing effect Effects 0.000 claims description 3
- 238000012986 modification Methods 0.000 claims description 2
- 230000004048 modification Effects 0.000 claims description 2
- 230000010391 action planning Effects 0.000 abstract description 4
- 230000007613 environmental effect Effects 0.000 description 11
- 230000008569 process Effects 0.000 description 6
- 230000008859 change Effects 0.000 description 4
- 238000006243 chemical reaction Methods 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- OKTJSMMVPCPJKN-UHFFFAOYSA-N Carbon Chemical compound [C] OKTJSMMVPCPJKN-UHFFFAOYSA-N 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 229910021389 graphene Inorganic materials 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Landscapes
- Manipulator (AREA)
Abstract
The application relates to a robot arm obstacle avoidance action planning method, a robot arm obstacle avoidance action planning device and computer equipment. Comprising the following steps: determining an initial action track of the mechanical arm according to a preset initial action gesture and a preset final action gesture; performing collision detection on the mechanical arm moving according to the initial action track to obtain a collision result; the collision result includes a collision function; determining a first gradient corresponding to the collision function, and determining a second gradient of the smoothing function corresponding to the initial action track; and carrying out track correction on the initial action track through the first gradient and the second gradient to obtain a target action track. By adopting the method, the collision between the mechanical arm and the obstacle in the environment can be effectively avoided.
Description
Technical Field
The application relates to the technical field of obstacle avoidance of mechanical arms, in particular to a planning method, a device and computer equipment for obstacle avoidance actions of mechanical arms.
Background
Along with the continuous expansion of the application field of the mechanical arm, the obstacle avoidance function of the mechanical arm in dynamic environments such as multi-machine cooperation, man-machine cooperation and the like becomes particularly important. The obstacle avoidance of the mechanical arm means that in a complex environment, a reasonable path which can enable the mechanical arm to bypass an obstacle without collision is obtained through advanced planning.
At present, the focus of the motion planning algorithm for the mechanical arm is on a specific task target, so that how to plan the motion of the mechanical arm, and therefore the mechanical arm is controlled to achieve the task target. The prior art ignores planning of the transition pose of the robotic arm between the starting pose and the target pose. In the process of moving the mechanical arm from the initial posture to the target posture, the mechanical arm may collide with obstacles in the environment, so that the achievement of the final target posture is affected, and even the mechanical arm body is irreversibly damaged.
Disclosure of Invention
Based on the above, the application aims to provide a method, a device and computer equipment for planning the obstacle avoidance action of a mechanical arm, which can accurately obtain a target action track, so as to solve the technical problem that the mechanical arm is likely to collide with an obstacle in the environment.
In a first aspect, the present application provides a method for planning obstacle avoidance actions of a mechanical arm. Comprising the following steps:
determining an initial action track of the mechanical arm according to a preset initial action gesture and a preset final action gesture;
performing collision detection on the mechanical arm moving according to the initial action track to obtain a collision result; the collision result comprises a collision function;
Determining a first gradient corresponding to the collision function, and determining a second gradient of a smoothing function corresponding to the initial action track;
and carrying out track correction on the initial action track through the first gradient and the second gradient to obtain a target action track.
In one embodiment, before the determining the initial motion trajectory of the mechanical arm according to the preset initial motion gesture and the final motion gesture, the method further includes: determining whether the mechanical arm collides with an obstacle in the environment when in the initial action gesture; if not, determining a final action gesture corresponding to the mechanical arm when the mechanical arm realizes the target task through a joint inverse solution algorithm; the method further comprises the following steps: if the mechanical arm is in the final action posture and is not in collision with the obstacle, determining an initial action track of the mechanical arm according to a preset initial action posture and a preset final action posture.
In one embodiment, the determining the initial motion trajectory of the mechanical arm according to the preset initial motion gesture and the final motion gesture includes: performing linear interpolation between the initial motion gesture and the final motion gesture to obtain an interpolation motion gesture of each joint; and synthesizing the interpolation action gesture of each joint to obtain the initial action track of the mechanical arm.
In one embodiment, the performing linear interpolation between the initial motion pose and the final motion pose to obtain an interpolation motion pose for each joint includes: determining a target variable quantity of a joint with the largest variation amplitude and a maximum step length for allowing the joint to move between the initial action gesture and the final action gesture of the mechanical arm; determining the number of action gestures to be interpolated according to the target variable quantity and the maximum step length; and determining a plurality of interpolation motion postures of each joint from the initial motion posture according to the number of the motion postures.
In one embodiment, collision detection is performed on the mechanical arm moving according to the initial motion track, so as to obtain a collision result, including: determining a plurality of action postures of each joint movement in the mechanical arm at different moments and a transition posture between any two action postures; acquiring a first geometric model of the obstacle and a second geometric model of each joint; and determining collision results among a plurality of action postures, transition postures and obstacles corresponding to each joint according to the first geometric model and the second geometric model.
In one embodiment, determining a collision result between the plurality of motion poses, the transition pose, and the obstacle corresponding to each joint according to the first geometric model and the second geometric model includes: determining a nearest distance between each joint and an obstacle when the joint and the obstacle do not collide, and determining a penetration distance between each joint and the obstacle when the joint and the obstacle collide according to the first geometric model and the second geometric model; constructing an objective function corresponding to the mechanical arm according to the nearest distance and the penetration distance; determining detection information obtained when the motion gesture of each joint is in collision detection with the obstacle, and obtaining joint speed when the transition gesture is obtained; and constructing a collision function corresponding to the mechanical arm according to the objective function, the detection information and the joint speed.
In one embodiment, determining a first gradient corresponding to the collision function includes: determining the function gradient of the objective function through a finite difference method, and determining a curvature vector of each joint when the central point moves along the initial action track; and determining a first gradient corresponding to the collision function according to the function gradient and the curvature vector.
In one embodiment, performing track correction on the initial motion track through the first gradient and the second gradient to obtain a target motion track, including: acquiring a preset first learning rate and a preset second learning rate; obtaining a first variable quantity of each joint in the mechanical arm according to the first learning rate and the first gradient, and obtaining a second variable quantity of each joint in the mechanical arm according to the second learning rate and the second gradient; preliminarily superposing the first variation of the joint and the corresponding joint value of the joint to obtain a corrected initial action track; and overlapping the second variation of the joint with the joint value of the joint on the corrected initial motion track again to obtain a target motion track.
In a second aspect, the application further provides a robot arm obstacle avoidance motion planning device. Comprising the following steps:
the initial motion track determining module is used for determining the initial motion track of the mechanical arm according to the preset initial motion gesture and the final motion gesture;
the collision result determining module is used for performing collision detection on the mechanical arm moving according to the initial action track to obtain a collision result; the collision result comprises a collision function;
The target action track determining module is used for determining a first gradient corresponding to the collision function and determining a second gradient of the smoothing function corresponding to the initial action track; and carrying out track correction on the initial action track through the first gradient and the second gradient to obtain a target action track.
In a third aspect, the present application also provides a computer device. The computer device comprises a memory storing a computer program and a processor which when executing the computer program performs the steps of:
determining an initial action track of the mechanical arm according to a preset initial action gesture and a preset final action gesture;
performing collision detection on the mechanical arm moving according to the initial action track to obtain a collision result; the collision result comprises a collision function;
determining a first gradient corresponding to the collision function, and determining a second gradient of a smoothing function corresponding to the initial action track;
and carrying out track correction on the initial action track through the first gradient and the second gradient to obtain a target action track.
According to the method, the device and the computer equipment for planning the obstacle avoidance action of the mechanical arm, the initial action track of the mechanical arm can be directly determined according to the preset initial action gesture and the final action gesture, the preliminary planning which is directly performed in the joint space is realized, and as the conversion from the path point in the Cartesian space to the joint space does not exist, the singular point during the conversion from the Cartesian path point to the joint space point does not appear. The collision detection is carried out on the mechanical arm moving according to the initial action track, so that a collision result comprising a collision function can be obtained, a first gradient corresponding to the collision function can be determined, a second gradient corresponding to the smooth function of the initial action track is determined, and the initial action track can be subjected to track correction through the first gradient and the second gradient, so that a target action track is obtained. The mechanical arm obstacle avoidance motion planning algorithm provided by the invention takes the smoothness of obstacle avoidance and mechanical arm motion as an optimization target, so that the motion track of the mechanical arm planned based on the first gradient can ensure to avoid the obstacle in the environment, and the track correction based on the second gradient can also enable the motion of the mechanical arm to be smoother and smoother, thereby improving the accuracy of mechanical arm obstacle avoidance motion planning.
Drawings
FIG. 1 is an application environment diagram of a robot obstacle avoidance motion planning method in one embodiment;
FIG. 2 is a flow chart of a method for planning obstacle avoidance actions of a robot in one embodiment;
FIG. 3 is a flowchart illustrating a method for planning obstacle avoidance actions of a robot in another embodiment;
fig. 4 is an internal structural diagram of a computer device in one embodiment.
Detailed Description
In order to make the objects, technical solutions and advantages of the present application more apparent, the present application will be further described in detail with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the present application.
The method for planning the obstacle avoidance action of the mechanical arm, which is provided by the embodiment of the application, can be applied to an application environment shown in fig. 1. Wherein the terminal 102 communicates with the server 104 via a network. Wherein the terminal 102 may be a robotic arm of different degrees of freedom. The server 104 is configured to determine an initial motion trajectory of the mechanical arm according to a preset initial motion gesture and a final motion gesture, and perform collision detection on the mechanical arm moving according to the initial motion trajectory, so as to obtain a collision result. The server 104 is further configured to determine a first gradient corresponding to the collision function, and determine a second gradient of the smoothing function corresponding to the initial motion trajectory; and carrying out track correction on the initial action track through the first gradient and the second gradient to obtain a target action track, so that the terminal 102 moves according to the target action track. The server 104 is used to control the movement of the mechanical arm, and may be implemented by a stand-alone server or a server cluster formed by a plurality of servers.
In one embodiment, as shown in fig. 2, a method for planning obstacle avoidance actions of a mechanical arm is provided, and the method is applied to a server in fig. 1, and includes the following steps:
step 202, determining an initial motion track of the mechanical arm according to a preset initial motion gesture and a preset final motion gesture.
Wherein the initial motion pose and the final motion pose may be represented as a set of joint angles; the initial action gesture is the gesture that the arm is in when initial position, and the final action gesture is the gesture that the arm is in when reaching final position after the operation of target task according to predetermineeing.
The server responds to the triggering operation of the user on the initial motion track of the mechanical arm, and performs linear interpolation between the initial motion gesture and the final motion gesture of the mechanical arm to obtain the initial motion track of the mechanical armAnd can be usedThe motion trajectory of the robot arm is shown. For example, use +.>Andrespectively representing the initial motion gesture and the final motion gesture of the mechanical arm, wherein n represents the number of controllable joints in the mechanical arm, +.>And->The angle of the ith controllable joint in the initial and final motion attitudes of the mechanical arm are respectively represented.
In one embodiment, before determining the initial motion trajectory of the mechanical arm according to the preset initial motion gesture and the final motion gesture, the method further includes: determining whether the mechanical arm collides with an obstacle in the environment when in an initial action gesture; if not, determining the final action gesture corresponding to the mechanical arm when the mechanical arm realizes the target task through a joint inverse solution algorithm.
The target task that the robotic arm needs to perform is typically described in terms of the position and/or rotational pose that the robotic arm end effector needs to reach.
Specifically, the server detects whether the mechanical arm collides with an obstacle in the environment when being in an initial action posture through a collision detection algorithm in advance. If a collision occurs, the algorithm program is aborted and reported in error. If no collision occurs with an obstacle in the environment, the information of the target task to be executed by the mechanical arm is converted into another set of joint angles to be executed by the mechanical arm by a joint inverse solution algorithm, namely the final action gesture of the mechanical arm.
In one embodiment, the method further comprises: if the mechanical arm does not collide with the obstacle in the final action posture, determining an initial action track of the mechanical arm according to the preset initial action posture and the final action posture.
The server detects whether the mechanical arm collides with an obstacle in the environment or not when the mechanical arm is in a final action posture through a collision detection algorithm in advance. If collision occurs, the algorithm program is stopped and reported to be wrong; if no collision occurs, continuing to execute the step of determining the initial action track of the mechanical arm.
And 204, performing collision detection on the mechanical arm moving according to the initial action track to obtain a collision result.
Wherein the collision result comprises a collision function; the geometry information of the robot arm and the first geometry model of the obstacle are pre-introduced into the proceeding server, the geometry information of the robot arm comprising the second geometry model of each joint.
In one embodiment, collision detection is performed on a mechanical arm moving according to an initial motion track to obtain a collision result, including: determining a plurality of motion postures of each joint motion in the mechanical arm at different moments and a transition posture between any two motion postures; acquiring a first geometric model of the obstacle and a second geometric model of each joint; and determining collision results among the plurality of action postures, the transition postures and the obstacles corresponding to each joint according to the first geometric model and the second geometric model.
Specifically, collision detection is performed on all the motion gestures involved in the whole initial motion track of the mechanical arm and the transition gesture between every two motion gestures and the environmental obstacle. In performing collision detection, both the first geometric model of the obstacle in the environment and the second geometric model of each joint of the mechanical arm need to be subjected to collision detection. When all the motion gestures related in the motion track of the mechanical arm and whether the transition gesture between every two motion gestures collide with the obstacle or not are detected, only in all the detection, the collision between the obstacle and the joint of each mechanical arm is not detected, and we say that the motion track of the mechanical arm does not collide with the obstacle. At this time, the subsequent calculation of the algorithm is stopped, and the mechanical arm returns to the process of continuing to move according to the initial movement track. If at least one joint in the action gesture of one mechanical arm collides with an obstacle, the movement track of the mechanical arm and the obstacle are also considered to collide.
In one embodiment, a user saves a first geometric model created in advance, describing an obstacle stationary in a robot arm workspace, as a model file in obj format, the first geometric model such as information of shape, size, etc., and inputs a path of the model file to a server. It is easy to understand that the model file describing the geometric information of the robot arm has been stored in advance in the algorithm program of the collision detection algorithm of the server.
Step 206, determining a first gradient corresponding to the collision function, and determining a second gradient of the smoothing function corresponding to the initial motion trajectory.
Specifically, collision functionIs calculated by taking the whole motion trail of the mechanical arm as an input variable of the mechanical arm, and the initial motion gesture of the mechanical arm is +.>To final action gesture->The collision detection results of each joint of the mechanical arm and the environmental obstacle in all transition action postures are included as the basis. Therefore, the server performs gradient solving on the collision function to obtain a corresponding first gradient of +.>。
Further, a smoothing functionIs defined as:
wherein, the motion trackIs rewritten to have +.>The one-dimensional vector of each element, m is the number of transition motion gestures in the motion track, n is the number of joints in the mechanical arm, and G is a finite difference matrix. Therefore, the server performs gradient solving on the smooth function to obtain a corresponding second gradient of +. >。
And step 208, performing track correction on the initial action track through the first gradient and the second gradient to obtain a target action track.
Specifically, the server calculates a first variation of all joints of the mechanical arm on an initial motion track according to a first gradient of a collision functionAnd preliminarily superposing the first variation of the joint with the corresponding joint value to obtain a corrected initial action track. The server defines the variation of all joints of the mechanical arm on the whole motion track according to the second gradient of the smoothing function>And overlapping the second variation of the joint with the joint value on the corrected initial motion track again to obtain a target motion track. It is easy to understand that the primary superposition and the secondary superposition can be performed simultaneously based on the joint values of the same joint, that is, the track correction is performed simultaneously on the basis of the initial action track.
In one embodiment, the method further comprises: and correcting the action track of the mechanical arm according to the joint limit of the mechanical arm. The server checks joint values of all action gestures in the initial action track, and if one joint value is within the joint limit of the joint value, the joint value is not modified; if one joint value exceeds its joint limit, it is modified to the joint limit closest to it. For example, if the joint value exceeds the limit and is closer to the lower limit of the joint limit, the joint value is set as the lower limit of the joint limit.
According to the mechanical arm obstacle avoidance motion planning method, the initial motion track of the mechanical arm can be directly determined according to the preset initial motion gesture and the final motion gesture, so that the preliminary planning in the joint space is directly realized, and the conversion from the path point in the Cartesian space to the joint space is not performed, so that the singular point during the conversion from the Cartesian path point to the joint space point does not occur. The collision detection is carried out on the mechanical arm moving according to the initial action track, so that a collision result comprising a collision function can be obtained, a first gradient corresponding to the collision function can be determined, a second gradient corresponding to the smooth function of the initial action track is determined, and the initial action track can be subjected to track correction through the first gradient and the second gradient, so that a target action track is obtained. The mechanical arm obstacle avoidance motion planning algorithm provided by the invention takes the smoothness of obstacle avoidance and mechanical arm motion as an optimization target, so that the motion track of the mechanical arm planned based on the first gradient can ensure to avoid the obstacle in the environment, and the track correction based on the second gradient can also enable the motion of the mechanical arm to be smoother and smoother, thereby improving the accuracy of mechanical arm obstacle avoidance motion planning.
In one embodiment, determining the initial motion trajectory of the mechanical arm according to the preset initial motion gesture and the final motion gesture includes: performing linear interpolation between the initial motion gesture and the final motion gesture to obtain an interpolation motion gesture of each joint; and synthesizing the interpolation motion gesture of each joint to obtain the initial motion track of the mechanical arm.
The mechanical arm comprises a plurality of controllable joints, and when the server starts linear interpolation from the initial motion gesture of the mechanical arm, a plurality of interpolation motion gestures corresponding to each joint are obtained, so that the initial motion gesture of the mechanical arm is formed.
Further, performing linear interpolation between the initial motion pose and the final motion pose, obtaining an interpolation motion pose for each joint includes: determining the target variable quantity of the joint with the largest variable amplitude and the maximum step length for allowing the joint to move between the initial action gesture and the final action gesture of the mechanical arm; determining the number of action gestures to be interpolated according to the target variable quantity and the maximum step length; and determining a plurality of interpolation motion postures of each joint from the initial motion posture according to the number of the motion postures.
Specifically, the server determines the initial motion gesture at the robotic arm And final action gesture +.>Between them, the target variation of the joint with the largest variation amplitude +.>Maximum step size of allowing articulation +.>. The server determines the initial action gesture +.A user can move in the mechanical arm according to the target variable quantity and the maximum step length>And final action gesture +.>The number of action gestures needing interpolation is +.>. The server starts the action gesture from the mechanical arm +.>Firstly, calculating all interpolation motion gestures, so as to form an initial motion track of the mechanical arm, wherein the calculation process is as follows:
the server determines the interpolation step length of each joint according to the number of the action gestures：
Finally, the server obtains the ith interpolation action gesture of each joint as follows:。
in one embodiment, the method further comprises: and the server adjusts the articulation points in the action track of the mechanical arm according to the interval distance between the articulation points. In the initial motion trajectory, the server examines the motion amplitudes of the individual joints between every two motion poses to find the largest joint motion amplitude among them. If the maximum articulation amplitude is greater than the maximum step size that allows articulation, a corresponding number of motion gestures are inserted between the two motion gestures according to the ratio of the maximum articulation amplitude to the maximum step size. If the maximum articulation amplitude is less than the minimum step size that allows articulation, then the two motion poses are deleted from the temporally relatively later motion poses.
In the embodiment, the linear interpolation is performed between the initial motion gesture and the final motion gesture to accurately obtain the interpolation motion gesture of each joint, and the motion track adjustment mode of the joint point in the motion track of the mechanical arm is adjusted according to the interval distance between the joint points, so that the initial motion track of the mechanical arm is constructed more accurately. Meanwhile, as the number of joints of the mechanical arm is not required, the scheme can be suitable for robots with any serial configuration of the number of joints.
In one embodiment, determining a collision result between the plurality of motion poses, the transition pose, and the obstacle corresponding to each joint according to the first geometric model and the second geometric model includes: determining the nearest distance between each joint and the obstacle when the joint does not collide with the obstacle and the penetration distance between each joint and the obstacle when the joint collides with the obstacle according to the first geometric model and the second geometric model; constructing an objective function corresponding to the mechanical arm according to the nearest distance and the penetration distance; determining detection information obtained when the motion gesture of each joint is in collision detection with an obstacle, and obtaining joint speed when the transition gesture is obtained; and constructing a collision function corresponding to the mechanical arm according to the objective function, the detection information and the joint speed.
Specifically, when the server detects the collision of the mechanical arm, the collected data includes whether the collision occurs, if the environmental obstacle does not collide with a certain joint of the mechanical arm, the closest distance cd between the joint and the obstacle, and a group of points closest to each other, such asRepresents the closest point on the arm, +.>Representing the nearest point on the environmental barrier; if the environmental obstacle collides with a certain joint of the mechanical arm, the penetration distance of the geometric model of the environmental obstacle and the mechanical arm is continuously collected>And the penetration direction->. Thus, the server can construct the objective function corresponding to the mechanical arm according to the nearest distance and the penetration distance +.>：
Wherein,indicating that the mechanical arm is in the action posture->Penetration distance of its joint model b when colliding with an obstacle in the environment, +.>Indicating the influence distance>Indicating that the mechanical arm is in the action posture->The joint model b thereof is not at the closest distance from the obstacle when it collides with the obstacle. Then, the server constructs a collision function corresponding to the mechanical arm as follows:
wherein B represents a set of second geometric models of all joints of the mechanical arm, B represents a second geometric model of one joint of the mechanical arm, and Info represents that the mechanical arm is in an action posture Detection information obtained from collision detection with all environmental obstacles, < ->The function represents the forward kinematic calculation function of the mechanical arm,/->The joint velocity representing the transition posture of model b, which can be represented by the velocity of the center point of the joint,/->Representing the corresponding objective function of the mechanical arm. In this embodiment, by performing collision detection on the motion gesture of each joint and the transition gesture between every two motion gestures, the collision results under different collision conditions can be accurately formed into the objective function corresponding to the mechanical arm, and further the collision function corresponding to the mechanical arm can be more accurately constructed according to the objective function, the detection information and the joint speed.
In one embodiment, determining a first gradient corresponding to the collision function includes: determining the function gradient of the objective function by a finite difference method, and determining the curvature vector of the center point of each joint when the center point moves along the initial action track; and determining a first gradient corresponding to the collision function according to the function gradient and the curvature vector.
Wherein the collision function corresponds to a first gradientThe two-dimensional matrix is a two-dimensional matrix with m rows and n columns, wherein m is the number of transitional motion gestures in the current motion track, and n is the joint number of the mechanical arm.
Specifically, the first gradient is calculated in row units. Its i-th row (i=1,., m) is calculated as follows:
wherein,transpose of jacobian representing the center point of the j-th joint of the manipulator, +.>Line speed representing the j-th joint center point of the arm,/->Represents a 3x3 identity matrix, +.>A direction vector representing the linear velocity of the j-th joint center point of the mechanical arm; />Represents +.>The gradient of the function can be calculated by using a finite difference method, namely, the server translates the geometrical model corresponding to the j-th joint of the mechanical arm by 1 unit distance in the positive direction and the negative direction of x, y and z respectively, and then performs collision detection on the translated joint geometrical model and the environmental obstacle respectively and calculates the distance between the translated joint geometrical model and the environmental obstacle respectivelyThe value of the function, finally the server uses the calculated 3 groups +.>The value of the function determines the function gradient of the objective function>Is a value of (2); />The curvature vector of the j joint center point of the mechanical arm along the initial motion track of the mechanical arm is shown, by considering the current position of the center point of this joint and its three positions in the fore-and-aft motion profile, +.>The value of (2) can be approximated as the curvature of the circumscribed circle of the triangle formed by this location.
In this embodiment, the first gradient corresponding to the determined collision function may be accurately and quickly obtained based on the function gradient of the determined objective function and the curvature vector when the center point of the joint moves along the initial motion trajectory.
In one embodiment, performing track correction on the initial motion track through the first gradient and the second gradient to obtain a target motion track, including: acquiring a preset first learning rate and a preset second learning rate; obtaining a first variable quantity of each joint in the mechanical arm according to the first learning rate and the first gradient, and obtaining a second variable quantity of each joint in the mechanical arm according to the second learning rate and the second gradient; preliminarily superposing the first variation of the joint with the corresponding joint value to obtain a corrected initial action track; and overlapping the second variation of the joint with the joint value on the corrected initial motion track again to obtain a target motion track.
Specifically, the server determines the first of all joints of the mechanical arm on the initial motion track according to the first gradient of the collision functionVariation ofThe method comprises the following steps:
wherein,representing a first learning rate,/- >I (i=1,., n) represents the first amount of change of all joints of the i-th joint on the initial motion trajectory. Then, the server will->The value of the i-th column (i=1,.. N) is added to the joint value corresponding to the motion trajectory, and a corrected initial motion trajectory is obtained.
Then, the server determines a second variation of all joints of the mechanical arm on the corrected initial motion track according to a second gradient of the smoothing functionOr the second variation of all joints on the initial motion track is as follows:
wherein,representing a second learning rate,/->I (i=1.,. N) represents the second amount of change of the i-th joint on the corrected initial motion trajectory or on the initial motion trajectory. Then, the server will->The i (i=1,) and n) is added to the corrected initial motion trajectory or the joint value corresponding to the initial motion trajectory to obtain a target motion trajectory, so that the mechanical arm moves according to the target motion trajectory.
In one embodiment, the server performs a gradient solution to the smoothing function to obtain a corresponding second gradient pass
The process is carried out in a manner such that,is a finite difference matrix defined as:
wherein,representing tensor product, ++ >，/>，/>The definition of the e vector is:
thus, the server calculates the gradient of the smoothing functionThe method comprises the following steps: />。
In this embodiment, after a first variation of each joint is determined by a first gradient of a collision function, an initial motion track may be initially corrected by the first variation; after the second variation of each joint is determined through the second gradient of the smoothing function, the corrected initial motion track can be primarily corrected through the second variation, and then the planned target motion track of the mechanical arm is further planned, namely the mechanical arm obstacle avoidance motion planning algorithm in the joint space based on the gradient descent method is provided, so that obstacles in the environment can be avoided, and the motion of the mechanical arm is smoother. Therefore, the accuracy of obstacle avoidance action planning of the mechanical arm is improved.
In one embodiment, as shown in fig. 3, fig. 3 is a flow chart of a method for planning obstacle avoidance actions of a robot in another embodiment. The server acquires a first geometric model of the obstacle imported by the user and determines whether the mechanical arm collides with the obstacle in the environment when in an initial action gesture. If collision occurs, the algorithm program is stopped and reported to be wrong; if no collision occurs with the obstacle in the environment, determining the corresponding final action gesture when the mechanical arm realizes the target task through a joint inverse solution algorithm. The server continues to judge whether the mechanical arm collides with the obstacle or not when in the final action gesture, and if so, the algorithm program is stopped and the error is reported; and if no collision occurs, executing the step of determining the initial action track of the mechanical arm. The server performs linear interpolation between the initial action gesture and the final action gesture of the mechanical arm to obtain an initial action track of the mechanical arm; and performing collision detection on the mechanical arm moving according to the initial action track to obtain a collision result, and collecting detection information obtained during collision detection. If collision occurs between the initial motion trail of the mechanical arm and the environmental obstacle, returning to the process that the mechanical arm continues to move according to the initial motion trail; if no collision occurs between the initial motion track of the mechanical arm and the environmental obstacle, the server performs track preliminary correction on the initial motion track according to a first gradient corresponding to the collision function. The server determines a second gradient of the smoothing function corresponding to the initial motion track, and corrects the initial motion track again according to the second gradient corresponding to the smoothing function to obtain the target motion track. The server corrects the action track of the mechanical arm according to the joint limit of the mechanical arm; and adjusting the joint points in the action track of the mechanical arm according to the interval distance between the joint points. After adjusting the joint point in the motion track of the mechanical arm, returning to the process of detecting the collision between the second geometric model of the joint point and the obstacle, and continuing.
In one embodiment, the server may also perform motion planning in a cartesian space, and then convert the transition points in the cartesian space into corresponding transition points in the joint space by solving the inverse joint solution.
It should be understood that, although the steps in the flowcharts related to the above embodiments are sequentially shown as indicated by arrows, these steps are not necessarily sequentially performed in the order indicated by the arrows. The steps are not strictly limited to the order of execution unless explicitly recited herein, and the steps may be executed in other orders. Moreover, at least some of the steps in the flowcharts described in the above embodiments may include a plurality of steps or a plurality of stages, which are not necessarily performed at the same time, but may be performed at different times, and the order of the steps or stages is not necessarily performed sequentially, but may be performed alternately or alternately with at least some of the other steps or stages.
Based on the same inventive concept, the embodiment of the application also provides a robot obstacle avoidance motion planning device for realizing the above related robot obstacle avoidance motion planning method. The implementation scheme of the solution to the problem provided by the device is similar to the implementation scheme described in the above method, so the specific limitation in the embodiments of the device for planning the obstacle avoidance action of one or more mechanical arms provided below can be referred to the limitation of the method for planning the obstacle avoidance action of the mechanical arm hereinabove, and will not be repeated here.
In one embodiment, a robot arm obstacle avoidance motion planning device is provided, including: the device comprises an initial action track determining module, a collision result determining module and a target action track determining module, wherein:
the initial motion track determining module is used for determining the initial motion track of the mechanical arm according to the preset initial motion gesture and the final motion gesture;
the collision result determining module is used for performing collision detection on the mechanical arm moving according to the initial action track to obtain a collision result; the collision result includes a collision function;
the target action track determining module is used for determining a first gradient corresponding to the collision function and determining a second gradient of the smoothing function corresponding to the initial action track; and carrying out track correction on the initial action track through the first gradient and the second gradient to obtain a target action track.
All or part of each module in the obstacle avoidance action planning of the mechanical arm can be realized by software, hardware and a combination thereof. The above modules may be embedded in hardware or may be independent of a processor in the computer device, or may be stored in software in a memory in the computer device, so that the processor may call and execute operations corresponding to the above modules.
In one embodiment, a computer device is provided, which may be a server, the internal structure of which may be as shown in fig. 4. The computer device includes a processor, a memory, an Input/Output interface (I/O) and a communication interface. The processor, the memory and the input/output interface are connected through a system bus, and the communication interface is connected to the system bus through the input/output interface. Wherein the processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, computer programs, and a database. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage media. The database of the computer device is used to store the motion trajectories. The input/output interface of the computer device is used to exchange information between the processor and the external device. The communication interface of the computer device is used for communicating with an external terminal through a network connection. The computer program is executed by the processor to realize a robot arm obstacle avoidance motion planning method.
Those skilled in the art will appreciate that the structures shown in FIG. 4 are block diagrams only and do not constitute a limitation of the computer device on which the present aspects apply, and that a particular computer device may include more or less components than those shown, or may combine some of the components, or have a different arrangement of components.
In an embodiment, there is also provided a computer device comprising a memory and a processor, the memory having stored therein a computer program, the processor implementing the steps of the method embodiments described above when the computer program is executed.
In one embodiment, a computer-readable storage medium is provided, storing a computer program which, when executed by a processor, implements the steps of the method embodiments described above.
In one embodiment, a computer program product or computer program is provided that includes computer instructions stored in a computer readable storage medium. The processor of the computer device reads the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, so that the computer device performs the steps in the above-described method embodiments.
Those skilled in the art will appreciate that implementing all or part of the above-described embodiment methods may be accomplished by way of a computer program that instructs associated hardware to perform the method, and that the computer program may be stored on a non-volatile computer readable storage medium, which when executed, may comprise the embodiment flows of the above-described methods. Any reference to memory, database, or other medium used in the various embodiments provided herein may include at least one of non-volatile and volatile memory. The nonvolatile Memory may include Read-Only Memory (ROM), magnetic tape, floppy disk, flash Memory, optical Memory, high density embedded nonvolatile Memory, resistive random access Memory (ReRAM), magnetic random access Memory (Magnetoresistive Random Access Memory, MRAM), ferroelectric Memory (Ferroelectric Random Access Memory, FRAM), phase change Memory (Phase Change Memory, PCM), graphene Memory, and the like. Volatile memory can include random access memory (Random Access Memory, RAM) or external cache memory, and the like. By way of illustration, and not limitation, RAM can be in the form of a variety of forms, such as static random access memory (Static Random Access Memory, SRAM) or dynamic random access memory (Dynamic Random Access Memory, DRAM), and the like. The databases referred to in the various embodiments provided herein may include at least one of relational databases and non-relational databases. The non-relational database may include, but is not limited to, a blockchain-based distributed database, and the like. The processors referred to in the embodiments provided herein may be general purpose processors, central processing units, graphics processors, digital signal processors, programmable logic units, quantum computing-based data processing logic units, etc., without being limited thereto.
The technical features of the above embodiments may be arbitrarily combined, and all possible combinations of the technical features in the above embodiments are not described for brevity of description, however, as long as there is no contradiction between the combinations of the technical features, they should be considered as the scope of the description.
The foregoing examples represent only a few embodiments of the present application, which are described in more detail and are not thereby to be construed as limiting the scope of the present application. It should be noted that it would be apparent to those skilled in the art that various modifications and improvements could be made without departing from the spirit of the present application, which would be within the scope of the present application. Accordingly, the scope of protection of the present application shall be subject to the appended claims.
Claims (10)
1. The method for planning the obstacle avoidance action of the mechanical arm is characterized by comprising the following steps:
determining an initial action track of the mechanical arm according to a preset initial action gesture and a preset final action gesture;
performing collision detection on the mechanical arm moving according to the initial action track to obtain a collision result; the collision result comprises a collision function;
determining a first gradient corresponding to the collision function, and determining a second gradient of a smoothing function corresponding to the initial action track;
Performing track correction on the initial action track through the first gradient and the second gradient to obtain a target action track;
specifically, the collision functionThe following are provided:
；
wherein,for the set of action trajectories, +.>Is an action gesture; b represents a set of second geometric models of all joints of the mechanical arm, B represents a second geometric model of one joint of the mechanical arm,/a>The function represents the forward kinematic calculation function of the mechanical arm, < >>Representing the joint velocity in the transition posture of model b,/->Representing an objective function corresponding to the mechanical arm;
objective functionExpressed as:
；
wherein,indicating that the mechanical arm is in the action posture->Penetration distance of its joint model b when colliding with an obstacle in the environment, +.>Indicating the influence distance>Indicating that the mechanical arm is in the action posture->When the joint model b of the vehicle does not collide with the obstacle, the vehicle is at the nearest distance from the obstacle;
smoothing functionIs defined as:
；
wherein, the motion trackIs rewritten to have +.>The one-dimensional vector of each element, m is the number of transition motion gestures in the motion track, n is the number of joints in the mechanical arm, and G is a finite difference matrix.
2. The method of claim 1, further comprising, prior to determining the initial motion profile of the robotic arm based on the pre-set initial motion profile and the final motion profile:
Determining whether the mechanical arm collides with an obstacle in the environment when in the initial action gesture;
if not, determining a final action gesture corresponding to the mechanical arm when the mechanical arm realizes the target task through a joint inverse solution algorithm;
the method further comprises the steps of:
if the mechanical arm is in the final action posture and is not in collision with the obstacle, determining an initial action track of the mechanical arm according to a preset initial action posture and a preset final action posture.
3. The method of claim 1, wherein the robotic arm comprises a plurality of controllable joints; the determining the initial motion track of the mechanical arm according to the preset initial motion gesture and the final motion gesture comprises the following steps:
performing linear interpolation between the initial motion gesture and the final motion gesture to obtain an interpolation motion gesture of each joint;
and synthesizing the interpolation action gesture of each joint to obtain the initial action track of the mechanical arm.
4. A method according to claim 3, wherein said linearly interpolating between said initial motion pose and said final motion pose to obtain an interpolated motion pose for each of said joints comprises:
Determining a target variable quantity of a joint with the largest variation amplitude and a maximum step length for allowing the joint to move between the initial action gesture and the final action gesture of the mechanical arm;
determining the number of action gestures to be interpolated according to the target variable quantity and the maximum step length;
and determining a plurality of interpolation motion postures of each joint from the initial motion posture according to the number of the motion postures.
5. The method according to claim 1, wherein the performing collision detection on the mechanical arm moving according to the initial motion trajectory to obtain a collision result includes:
determining a plurality of action postures of each joint movement in the mechanical arm at different moments and a transition posture between any two action postures;
acquiring a first geometric model of the obstacle and a second geometric model of each joint;
and determining collision results among a plurality of action postures, transition postures and obstacles corresponding to each joint according to the first geometric model and the second geometric model.
6. The method of claim 5, wherein determining a collision result between the plurality of motion poses, the transition pose, and the obstacle corresponding to each of the joints from the first geometric model and the second geometric model comprises:
Determining a nearest distance between each joint and an obstacle when the joint and the obstacle do not collide, and determining a penetration distance between each joint and the obstacle when the joint and the obstacle collide according to the first geometric model and the second geometric model;
constructing an objective function corresponding to the mechanical arm according to the nearest distance and the penetration distance;
determining detection information obtained when the motion gesture of each joint is in collision detection with the obstacle, and obtaining joint speed when the transition gesture is obtained;
and constructing a collision function corresponding to the mechanical arm according to the objective function, the detection information and the joint speed.
7. The method of claim 6, wherein the determining the first gradient corresponding to the collision function comprises:
determining the function gradient of the objective function through a finite difference method, and determining a curvature vector of each joint when the central point moves along the initial action track;
and determining a first gradient corresponding to the collision function according to the function gradient and the curvature vector.
8. The method of claim 1, wherein performing trajectory modification on the initial motion trajectory by the first gradient and the second gradient to obtain a target motion trajectory comprises:
acquiring a preset first learning rate and a preset second learning rate;
obtaining a first variable quantity of each joint in the mechanical arm according to the first learning rate and the first gradient, and obtaining a second variable quantity of each joint in the mechanical arm according to the second learning rate and the second gradient;
preliminarily superposing the first variation of the joint and the corresponding joint value of the joint to obtain a corrected initial action track;
and overlapping the second variation of the joint with the joint value of the joint on the corrected initial motion track again to obtain a target motion track.
9. A robot obstacle avoidance maneuver planning apparatus configured to perform the robot obstacle avoidance maneuver planning method of any of claims 1-8, the apparatus comprising:
the initial motion track determining module is used for determining the initial motion track of the mechanical arm according to the preset initial motion gesture and the final motion gesture;
The collision result determining module is used for performing collision detection on the mechanical arm moving according to the initial action track to obtain a collision result; the collision result comprises a collision function;
the target action track determining module is used for determining a first gradient corresponding to the collision function and determining a second gradient of the smoothing function corresponding to the initial action track; and carrying out track correction on the initial action track through the first gradient and the second gradient to obtain a target action track.
10. A computer device comprising a memory and a processor, the memory storing a computer program, characterized in that the processor implements the steps of the method of any one of claims 1 to 8 when the computer program is executed.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311473569.5A CN117182932B (en) | 2023-11-08 | 2023-11-08 | Method and device for planning obstacle avoidance action of mechanical arm and computer equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311473569.5A CN117182932B (en) | 2023-11-08 | 2023-11-08 | Method and device for planning obstacle avoidance action of mechanical arm and computer equipment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117182932A CN117182932A (en) | 2023-12-08 |
| CN117182932B true CN117182932B (en) | 2024-01-30 |
Family
ID=89002053
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311473569.5A Active CN117182932B (en) | 2023-11-08 | 2023-11-08 | Method and device for planning obstacle avoidance action of mechanical arm and computer equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117182932B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118219282B (en) * | 2024-05-24 | 2024-09-03 | 广州信邦智能装备股份有限公司 | Mechanical arm running track correction method, system and storage medium |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9981383B1 (en) * | 2016-08-02 | 2018-05-29 | X Development Llc | Real-time trajectory generation for actuators of a robot to reduce chance of collision with obstacle(s) |
| CN110640736A (en) * | 2019-10-23 | 2020-01-03 | 南京工业大学 | Mechanical arm motion planning method for intelligent manufacturing |
| CN113119109A (en) * | 2021-03-16 | 2021-07-16 | 上海交通大学 | Industrial robot path planning method and system based on pseudo-distance function |
| KR20210096842A (en) * | 2020-01-29 | 2021-08-06 | 한국과학기술원 | Method and Apparatus for Collision-Free Trajectory Optimization of Redundant Manipulator given an End-Effector Path |
| CN115157262A (en) * | 2022-07-29 | 2022-10-11 | 中山大学 | Obstacle avoidance and optimization control method and system for joint-limited redundant mechanical arm and robot |
| CN115302502A (en) * | 2022-07-08 | 2022-11-08 | 浙江大学 | Mechanical arm trajectory planning method combining grabbing network and grabbing pose selection and adjustment |
| CN115900716A (en) * | 2022-12-05 | 2023-04-04 | 哈尔滨工业大学（深圳） | Joint space planning method, device, equipment and medium based on gradient projection optimization |
-
2023
- 2023-11-08 CN CN202311473569.5A patent/CN117182932B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9981383B1 (en) * | 2016-08-02 | 2018-05-29 | X Development Llc | Real-time trajectory generation for actuators of a robot to reduce chance of collision with obstacle(s) |
| CN110640736A (en) * | 2019-10-23 | 2020-01-03 | 南京工业大学 | Mechanical arm motion planning method for intelligent manufacturing |
| KR20210096842A (en) * | 2020-01-29 | 2021-08-06 | 한국과학기술원 | Method and Apparatus for Collision-Free Trajectory Optimization of Redundant Manipulator given an End-Effector Path |
| CN113119109A (en) * | 2021-03-16 | 2021-07-16 | 上海交通大学 | Industrial robot path planning method and system based on pseudo-distance function |
| CN115302502A (en) * | 2022-07-08 | 2022-11-08 | 浙江大学 | Mechanical arm trajectory planning method combining grabbing network and grabbing pose selection and adjustment |
| CN115157262A (en) * | 2022-07-29 | 2022-10-11 | 中山大学 | Obstacle avoidance and optimization control method and system for joint-limited redundant mechanical arm and robot |
| CN115900716A (en) * | 2022-12-05 | 2023-04-04 | 哈尔滨工业大学（深圳） | Joint space planning method, device, equipment and medium based on gradient projection optimization |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117182932A (en) | 2023-12-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111538949B (en) | Redundant robot inverse kinematics solving method and device and redundant robot | |
| Lu et al. | Collision-free and smooth joint motion planning for six-axis industrial robots by redundancy optimization | |
| CN110682286B (en) | Real-time obstacle avoidance method for cooperative robot | |
| Xu et al. | Motion planning of manipulators for simultaneous obstacle avoidance and target tracking: An RNN approach with guaranteed performance | |
| CN111360827B (en) | Visual servo switching control method and system | |
| JP5261495B2 (en) | Real-time self-collision and obstacle avoidance using weight matrix | |
| CN112605996B (en) | Model-free collision avoidance control method for redundant mechanical arm | |
| CN106625666A (en) | Control method and device of redundant mechanical arm | |
| CN108638055B (en) | Autonomous obstacle avoidance planning method for seven-degree-of-freedom space manipulator | |
| JPH1097316A (en) | Movement plan and control method for system followed by many mobile objects | |
| CN117182932B (en) | Method and device for planning obstacle avoidance action of mechanical arm and computer equipment | |
| Wang et al. | A multi-target trajectory planning of a 6-dof free-floating space robot via reinforcement learning | |
| Mbakop et al. | Path planning and control of mobile soft manipulators with obstacle avoidance | |
| CN114932549A (en) | Motion planning method and device of spatial redundant mechanical arm | |
| Chen et al. | Optimizing the obstacle avoidance trajectory and positioning error of robotic manipulators using multigroup ant colony and quantum behaved particle swarm optimization algorithms | |
| CN113927603B (en) | Mechanical arm dragging control method and device, computer equipment and storage medium | |
| Ikeda et al. | On-line optimization of avoidance ability for redundant manipulator | |
| Yang et al. | Collision avoidance trajectory planning for a dual-robot system: using a modified APF method | |
| Sun et al. | Hybrid task constrained planner for robot manipulator in confined environment | |
| CN109895097B (en) | Sub-gradient method of motion model of welding robot | |
| CN109794939B (en) | Parallel beam planning method for welding robot motion | |
| Pu et al. | The Algorithm of Multiple Obstacle Avoidance Tasks for Dual-Arm Robots | |
| Zhao et al. | A cooperative obstacle-avoidance approach for two-manipulator based on A* algorithm | |
| CN117444962A (en) | Robot tail end control method and device and computer equipment | |
| Liu et al. | Task Planning of Manipulator Based on Dynamic Space Constraint and Torque Sensor |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |