CN111844007B - Obstacle avoidance path planning method and device for mechanical arm of pollination robot - Google Patents
Obstacle avoidance path planning method and device for mechanical arm of pollination robot Download PDFInfo
- Publication number
- CN111844007B CN111844007B CN202010491344.2A CN202010491344A CN111844007B CN 111844007 B CN111844007 B CN 111844007B CN 202010491344 A CN202010491344 A CN 202010491344A CN 111844007 B CN111844007 B CN 111844007B
- Authority
- CN
- China
- Prior art keywords
- path
- node
- pose
- pollination
- point
- 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
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/08—Programme-controlled manipulators characterised by modular constructions
-
- A—HUMAN NECESSITIES
- A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
- A01H—NEW PLANTS OR NON-TRANSGENIC PROCESSES FOR OBTAINING THEM; PLANT REPRODUCTION BY TISSUE CULTURE TECHNIQUES
- A01H1/00—Processes for modifying genotypes ; Plants characterised by associated natural traits
- A01H1/02—Methods or apparatus for hybridisation; Artificial pollination ; Fertility
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J11/00—Manipulators not otherwise provided for
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
- B25J9/1697—Vision controlled systems
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Life Sciences & Earth Sciences (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Genetics & Genomics (AREA)
- Botany (AREA)
- Developmental Biology & Embryology (AREA)
- Environmental Sciences (AREA)
- Manipulator (AREA)
Abstract
The invention provides a method and a device for planning an obstacle avoidance path of a mechanical arm of a pollination robot, wherein the method comprises the following steps: acquiring a depth image through a depth camera, and acquiring the pose of an obstacle and the pose of a pollination target point of the mechanical arm according to the depth image; acquiring an initial path through a three-dimensional RRT algorithm with linear regression; optimizing a three-dimensional RRT algorithm through an A-algorithm to obtain an optimal path; and performing track smoothing on the optimal path through an artificial potential field method to obtain a final path, and controlling the mechanical arm according to the final path. According to the method, the three-dimensional RRT algorithm with linear regression is adopted, the purpose of RRT three-dimensional space exploration is further enhanced, the exploration direction is easier to develop towards an open place, the problem of sinking into local minima can be well avoided by the artificial potential field method combined with the three-dimensional RRT algorithm based on A-algorithm optimization, the track is smooth, the method is very suitable for the movement of a mechanical arm between branches, and the reliability of robot pollination is improved.
Description
Technical Field
The invention relates to the technical field of agricultural machinery, in particular to a robot arm obstacle avoidance path planning method and a robot arm obstacle avoidance path planning device of a pollination robot.
Background
Most fruit trees all need cross pollination to normally fruit, and under the condition that pollinating trees lack and weather is bad, natural pollination of the fruit trees can be adversely affected, and artificial pollination is needed at the moment. For example, pollination of a dragon fruit is difficult, natural pollination of a dragon fruit flower is difficult, artificial pollination is needed, and the dragon fruit is a typical nocturnal flowering plant, flowers generally begin to wilt gradually from evening to early morning until complete withering after sunlight irradiation. This is time consuming and laborious for artificial pollination and also easily damages the flowers.
In the related art, although there is a technology of automatically performing pollination by a pollination robot, a path planning is generally performed on a pollination mechanical arm by a simple RRT algorithm, but a three-dimensional RRT algorithm is easy to fall into a local minimum and other problems, multiple paths may be generated, the taken paths may be far from an optimal path, and the general quality of the paths is not good, for example, the paths may contain edges and corners and are not smooth enough, so that the pollination reliability of the pollination robot is caused.
Disclosure of Invention
The invention provides a method for planning obstacle avoidance paths of a mechanical arm of a pollination robot, which adopts a three-dimensional RRT (Rapidly Exploring Random Tree) algorithm with linear regression to quickly expand a random tree, so that the exploring purpose of the three-dimensional space of the RRT is further enhanced, the exploring direction is easier to develop towards an open place, the problem of sinking into local minimum values can be well avoided by using an artificial potential field method combined with the three-dimensional RRT algorithm optimized based on an A-algorithm, the track is smooth, the method is very suitable for the movement of the mechanical arm between branches, and the stability of the robot is improved.
The technical scheme adopted by the invention is as follows:
the embodiment of the first aspect of the invention provides a robot arm obstacle avoidance path planning method of a pollination robot, which comprises the following steps: acquiring a depth image through a depth camera, and acquiring the pose of an obstacle and the pose of a pollination target point of the mechanical arm according to the depth image; acquiring an initial path through a three-dimensional RRT algorithm with linear regression according to the obstacle pose and the pollination target point position; optimizing the three-dimensional RRT algorithm through an A-algorithm to obtain an optimal path; performing track smoothing on the optimal path through an artificial potential field method to obtain a final path; controlling the mechanical arm according to the final path
According to one embodiment of the invention, a depth image is acquired by a depth camera, and an obstacle pose and a pollination target point pose of the mechanical arm are acquired according to the depth image, comprising: identifying a top center point and a bottom coordinate point of the pistil according to the depth image; judging whether to pollinate the stamens or not according to the top center point and the bottom coordinate point based on the numerical value of the Y axis in the world coordinate system; identifying the position and the pose of the pistil and the position and the pose of the obstacle according to the depth image, and representing the position and the pose of the pistil by using a quaternion position and pose; and reversely pushing the pollination target point position of the mechanical arm according to the quaternion position.
According to one embodiment of the invention, according to the obstacle pose and the pollination target point pose, an initial path is obtained through a three-dimensional RRT algorithm with linear regression, which comprises the following steps: with the position X of the center point of the tail end of the mechanical arm init Taking a preset length D as a radius as a circle center, and selecting 10 random points in the three-dimensional space as newly added nodes X rand Expanding; respectively judging the newly added nodes X rand With pollination target point X goal Is a distance of (2); if the node X is newly added rand With the pollination target point X goal The distance of the new node X is smaller than a preset value rand Stopping expansion; if the node X is newly added rand With the pollination target point X goal If the distance of the new node X is greater than or equal to the preset value, further judging the new node X rand Whether the distance between the random node X and any other node X on the random tree is greater than that of the newly added node X rand And its parent node X par Is a distance of (2); if yes, the new node X is added rand The point is set as a path planning point X new The method comprises the steps of carrying out a first treatment on the surface of the Planning a point X according to the path new An initial path is acquired.
According to one embodiment of the inventionFor example, optimizing the three-dimensional RRT algorithm by an a-algorithm to obtain an optimal path includes: estimating each newly added node X using an valuation function of an A-algorithm rand To pollination target point x goal Is determined by the path estimation value of (a); and obtaining an optimal path according to the path estimation value.
According to one embodiment of the present invention, performing trajectory smoothing on the optimal path by using an artificial potential field method to obtain a final path, including: planning point X with the path new Is a gravitational field, and takes branches as a repulsive field; acquiring attractive force generated by the gravitational field and repulsive force generated by the repulsive force field; acquiring resultant force according to the attractive force and the repulsive force; planning a point X for the path according to the resultant force new Fitting is performed to obtain a final path.
According to one embodiment of the invention, controlling the mechanical arm according to the final path comprises: calculating radian required to rotate by each joint of the mechanical arm through inverse kinematics inverse solution; and controlling the mechanical arm according to the radian.
An embodiment of a second aspect of the present invention provides a robot arm obstacle avoidance path planning device for a pollination robot, including: the first acquisition module is used for acquiring a depth image through a depth camera and acquiring an obstacle pose and a pollination target point pose of the mechanical arm according to the depth image; the second acquisition module is used for acquiring an initial path through a three-dimensional RRT algorithm with linear regression according to the obstacle pose and the pollination target point pose; the optimization module is used for optimizing the three-dimensional RRT algorithm through an A-algorithm so as to obtain an optimal path; the smoothing module is used for carrying out track smoothing on the optimal path through an artificial potential field method so as to obtain a final path; and the control module is used for controlling the mechanical arm according to the final path.
According to one embodiment of the present invention, the first obtaining module is specifically configured to: identifying a top center point and a bottom coordinate point of the pistil according to the depth image; judging whether to pollinate the stamens or not according to the top center point and the bottom coordinate point based on the numerical value of the Y axis in the world coordinate system; identifying the position and the pose of the pistil and the position and the pose of the obstacle according to the depth image, and representing the position and the pose of the pistil by using a quaternion position and pose; and reversely pushing the pollination target point position of the mechanical arm according to the quaternion position.
According to an embodiment of the present invention, the second obtaining module is specifically configured to: with the position X of the center point of the tail end of the mechanical arm init Taking a preset length D as a radius as a circle center, and selecting 10 random points in the three-dimensional space as newly added nodes X rand Expanding; respectively judging the newly added nodes X rand With pollination target point X goal Is a distance of (2); if the node X is newly added rand With the pollination target point X goal The distance of the new node X is smaller than a preset value rand Stopping expansion; if the node X is newly added rand With the pollination target point X goal If the distance of the new node X is greater than or equal to the preset value, further judging the new node X rand Whether the distance between the random node X and any other node X on the random tree is greater than that of the newly added node X rand And its parent node X par Is a distance of (2); if yes, the new node X is added rand The point is set as a path planning point X new The method comprises the steps of carrying out a first treatment on the surface of the Planning a point X according to the path new An initial path is acquired.
According to one embodiment of the invention, the optimization module is specifically configured to: estimating each newly added node X using an valuation function of an A-algorithm rand To pollination target point x goal Is determined by the path estimation value of (a); and obtaining an optimal path according to the path estimation value.
The invention has the beneficial effects that:
the invention adopts the three-dimensional RRT algorithm with linear regression, so that the generation of newly added nodes of the three-dimensional RRT algorithm is more easily derived to the open space, and the exploring purpose of the RRT algorithm in the branch space is further enhanced; adopting the idea of shortest path optimization, adding an A algorithm into an RRT algorithm to construct a random tree, so that the RRT expanded random tree is developed towards the optimal path, and the randomness of the three-dimensional RRT algorithm in three-dimensional space searching is solved; aiming at the problems that the track generated by the three-dimensional improved RRT algorithm is not smooth, the track planning of a mechanical arm is not facilitated, and the like, the artificial potential field method is adopted to fit the track between the child node and the father node of the three-dimensional improved RRT algorithm, so that the technical problem that the generated track is not smooth is solved, the phenomenon that the artificial potential field method falls into a dead zone is effectively avoided, and the reliability of robot pollination is improved.
Drawings
FIG. 1 is a flow chart of a method for planning obstacle avoidance path of a robotic arm of a pollination robot, in accordance with one embodiment of the invention;
FIG. 2 is a schematic diagram of three-dimensional modeling of a depth camera according to an embodiment of the present invention;
FIG. 3 is a flow chart of a method for planning obstacle avoidance path of a robotic arm of a pollination robot, in accordance with another embodiment of the invention;
FIG. 4a is a schematic diagram of an initial path obtained using a basic RRT algorithm;
FIG. 4b is a schematic diagram of the initial path obtained by the three-dimensional RRT algorithm with linear regression;
FIG. 5 is a schematic diagram of an artificial potential field method according to one embodiment of the invention;
FIG. 6 is a schematic diagram of the robot arm obstacle avoidance principle under the ROS platform;
fig. 7 is a block schematic diagram of a robot arm obstacle avoidance path planning apparatus for a pollination robot in accordance with one embodiment of the invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
Fig. 1 is a flowchart of a method for planning obstacle avoidance paths of a robotic arm of a pollination robot, in accordance with one embodiment of the invention. As shown in fig. 1, the method comprises the steps of:
s1, acquiring a depth image through a depth camera, and acquiring the pose of the obstacle and the pose of the pollination target point of the mechanical arm according to the depth image.
The pose is a generic term of the position of the object reference point and the pose of the object, and is generally represented by a quaternion. The depth camera may be mounted at the upper end of the pollination robot arm.
S2, acquiring an initial path through a three-dimensional RRT algorithm with linear regression according to the pose of the obstacle and the pollination target point position.
Specifically, a new node X of a random tree in the RRT algorithm rand The selection of (2) employs a linear regression concept in statistics.
And S3, optimizing a three-dimensional RRT algorithm through an A-algorithm to obtain an optimal path.
The a-Star algorithm is a direct search method in a static road network that is most effective in solving the shortest path.
S4, performing track smoothing on the optimal path through an artificial potential field method to obtain a final path.
The artificial potential field path planning is a virtual force method proposed by Khatib (eussama Khatib, real-Time obstacle Avoidance for Manipulators and Mobile robots. Proc of The 1994 IEEE.). The basic idea is to design the movement of the robot in the surrounding environment into an abstract artificial gravitational field, the target point generates 'attraction' to the mobile robot, the obstacle generates 'repulsion' to the mobile robot, and finally the movement of the mobile robot is controlled by solving the resultant force. The path planned by the potential field method is generally smooth and safe.
And S5, controlling the mechanical arm according to the final path.
Specifically, when the pollination robot pollinates, a depth image can be acquired by a depth camera, then the obstacle pose and the pollination target point pose of the mechanical arm are acquired according to the depth image, and a node X is newly added in an RRT algorithm according to the obstacle pose and the pollination target point pose of the mechanical arm rand Adopts the linear regression idea in statistics to explore the purpose of RRT three-dimensional spaceFurther enhancing, optimizing the problem that the three-dimensional RRT algorithm is trapped into a local minimum value due to the large width of branches and leaves in pollination, and enabling the exploration direction to develop towards a wide place more easily. And then optimizing the three-dimensional RRT algorithm by adopting an A-algorithm, solving the randomness of the three-dimensional RRT algorithm when three-dimensional space search is carried out, adopting the idea of shortest path optimization, adding a heuristic valuation function A into the RRT algorithm to construct a random tree, and leading the RRT expanded random tree to develop towards an optimal path. And (3) performing track smoothing on the path obtained by the three-dimensional RRT algorithm which is improved by the regression function and optimized by the A-type algorithm by using an artificial potential field method to obtain a final path, and controlling the mechanical arm according to the final path. Therefore, the path planned by the method is suitable for the mechanical arm to move between branches, and is beneficial to improving the reliability of robot pollination.
According to one embodiment of the invention, the obtaining a depth image by a depth camera, and obtaining the pose of an obstacle and the pose of a pollination target point of a mechanical arm according to the depth image, may comprise: identifying a top center point and a bottom coordinate point of the pistil according to the depth image; judging whether to pollinate the current stamens or not according to the numerical values of the top central point and the bottom coordinate point based on the Y axis under the world coordinate system; if so, recognizing the position and the pose of the pistil and the position and the pose of the obstacle according to the depth image, and representing the position and the pose of the pistil by using a quaternion position and pose; and reversely pushing the pollination target point position of the mechanical arm according to the quaternion pose.
Specifically, as shown in fig. 2, L represents a mechanical arm, X, Y, Z represents three direction axes in a world coordinate system, three-dimensional modeling is performed through a depth camera, hand-eye calibration is performed, and internal parameters and external parameters of the depth camera are determined. The coordinate points of the top center point B of the pistil and the bottom A of the pistil are identified according to the depth image, the numerical values (namely Ya and Yb) of the two points based on the Y axis in the world coordinate system are judged, then whether the current pistil is pollinated according to the Ya and Yb, if Ya < Yb, the current pistil is pollinated as a pollination target point, if Ya > Yb, the current identification object is not the pistil, and the target point is abandoned.
Then the depth camera is used for identifying the pose of the current stamen and the obstacle, and the three pose are identifiedThe dimensional pose is represented by a quaternion. Wherein, the quaternion of the stamen pose is q (x, y, z, w), and the direction vector of the equivalent rotation axis is:
the equivalent rotation angle is θ, where:
and has x 2 +y 2 +z 2 +ω 2 =1. And finally, reversely pushing the position and the posture of the quaternion of the center point of the top of the stamen to obtain the position and the posture of the pollination target point of the mechanical arm.
According to one embodiment of the present invention, as shown in fig. 3, according to the pose of the obstacle and the pose of the pollination target point, the initial path is obtained by a three-dimensional RRT algorithm with linear regression, which may include:
s201, at the starting point X init Taking a preset length D as a radius as a circle center, and selecting 10 random points in the three-dimensional space as newly added nodes X rand And expanding, wherein the starting point is the position of the center point of the tail end of the mechanical arm.
The center point of the tail end of the mechanical arm is positioned at the X position init Is the starting point.
S202, respectively judging newly added nodes X rand With pollination target point X goal Is a distance of (3). I.e. determine the newly added node X rand With pollination target point X goal Whether the distance of (2) is less than a preset value.
S203, if node X is newly added rand With pollination target point X goal If the distance of (2) is smaller than the preset value, newly increasing node X rand Stopping the expansion.
S204, if node X is newly added rand With pollination target point X goal If the distance of the node X is greater than or equal to the preset value, further judging the newly added node X rand Whether the distance between the random tree and any other node X on the random tree is greater than that between the random tree and the newly added node X rand And its parent node X par Is a distance of (3).
S205, if yes, adding the current new node X rand The point is set as a path planning point X new ；
S206, planning point X according to the path new An initial path is acquired.
Specifically, first at a starting point X init (the position of the center point of the tail end of the mechanical arm) is used as a circle center, D is used as a radius, and 10 random points are selected in the three-dimensional space to be used as new sections X rand The points are expanded. Then respectively judging 10 newly added nodes X rand With pollination target point X goal Is a distance of (3). Newly added node X rand With pollination target point X goal If the distance of (2) is smaller than the preset value, ending the three-dimensional RRT algorithm, and newly adding a node X rand Stopping the expansion. If node X is newly added rand With pollination target point X goal If the distance of the node X is greater than or equal to the preset value, traversing the random tree to further judge the newly added node X rand Whether the distance between the random tree and any other node X on the random tree is greater than that between the random tree and the newly added node X rand And its parent node X par If node X is newly added rand Distance from any other node X on the random tree is greater than that of the newly added node X rand And its parent node X par I.e. the distance of (2) satisfies: x ε T { Dis (X) par ,X rand )<Dis(X,X rand ) New node X satisfying the above condition rand Set as a route planning point X new 。
A comparison of the initial path obtained using the three-dimensional RRT algorithm with linear regression and the initial path obtained using the basic RRT algorithm is shown in fig. 4a-4b, wherein fig. 4a represents the initial path schematic obtained using the basic RRT algorithm and fig. 4b represents the initial path schematic obtained using the three-dimensional RRT algorithm with linear regression. As is evident from the figure, the initial path obtained by adopting the three-dimensional RRT algorithm with linear regression is easier to develop towards the open place, and the exploratory aim is further enhanced.
According to an embodiment of the present invention, as shown in fig. 3, the optimization of the three-dimensional RRT algorithm by the a-x algorithm to obtain the optimal path may include:
s301, estimating each newly added node X by using a valuation function of an A-algorithm rand To pollination target point x goal Is used for the path estimation value of (a).
S302, obtaining an optimal path according to the path estimation value.
Specifically, each node of the random tree is defined with a valuation function: f (X) =g (X) +h (X);
wherein g (X) =road (X, X rand )，h(X)＝Dis(X rand ，X goal ) I.e. g (X) is a random node X rand The path cost required for reaching the node X in the tree is h (X) is a heuristic function, and is a newly added node X rand To pollination target point x goal Is the Euclidean distance from node X to random node X rand To target node X goal Is used for the path estimation value of (a).
Generating a qualified path planning point X by using the regression function constraint new Calculate the direction vector X par -X new And reference vector X goal -X init The included angles are stored in an array arr, the values are ordered from big to small, and the top 5 points are selected to be used as new father nodes X par By X par For the expansion with the circle center D as the radius, turning to step S202, when a point meeting the requirement is found, the node stops expanding.
Two arrays are established in the algorithm a, one being an Open array and one being a Close array. Open arrays represent the points to be detected, while Close represents the points that have been detected. Adding a starting point Xinit into an Open array, searching a point with the lowest f (x) value in the array, and taking the point as a current point; adding the selected point in the Open array into the Close array; the found father node X par Surrounding newly added node X new Adding as in Close array, ignoring if node is repeated, and calculating the values of f (x), g (x), h (x); judgment of X new Whether the node belongs to the Open array or not, if not, the node X is selected new Adding an Open array; judging whether the path including the node is better or not by calculating g (X), if the cost of the node is lower than that of other nodes, updating the node into node X par The method comprises the steps of carrying out a first treatment on the surface of the When pollinating the target point x goal If the Openarray is added, a path is successfully searched, otherwise, if the Openarray is empty, path planning fails。
According to one embodiment of the present invention, as shown in fig. 3, performing trajectory smoothing on the optimal path by using an artificial potential field method to obtain a final path may include:
s401, planning point X by path new The branch is taken as a repulsive force field to obtain the attractive force generated by the gravitational field and the repulsive force generated by the repulsive force field.
S402, acquiring resultant force according to the attractive force and the repulsive force.
S403, planning a point X for the path according to the resultant force new Fitting is performed to obtain a final path.
Specifically, as shown in fig. 5, gol 1 and gol 2 represent path planning points, and the optimal path generated by the three-dimensional RRT algorithm modified by the regression function and optimized by the a-algorithm generated in step S3 is defined as the parent node X of each segment par As the starting point, the new node X of each segment is used new Is gravitational field, uses branch as repulsive field, and its gravitational field is produced by newly added node, and is marked as U at (X)，
Where k is the proportionality coefficient of the gravitational field, X represents the current point, X new Represents a path planning point, i.e. a target point, p (x, x) new ) Is X point to target point X new Is a distance of (3).
Attraction force F generated at X point at (x) In order to achieve this, the first and second,
the repulsive force field is jointly generated by all obstacles shot by a depth camera and is marked as U Re (x),
Wherein the method comprises the steps of
Represents the repulsive force field generated by the ith obstacle at the point X,
wherein: η is the proportionality coefficient of the repulsive field, p 0 Indicating the extent of influence of the obstacle,
is the distance from the X node to the ith obstacle. Repulsive force F generated at point X Re (C) The method comprises the following steps: f (F) Re (X)
The resultant force generated by the potential field at point X is denoted as F Total
F Total (X)＝F at (X)+F Re (X)
Wherein F is Total The artificial potential field method combined with the three-dimensional RRT algorithm based on the A-algorithm optimization can well avoid the problem of sinking into local minima, is smooth in track, is very suitable for the mechanical arm to move between branches, and is beneficial to improving the stability of the robot.
According to one embodiment of the present invention, as shown in fig. 3, controlling the mechanical arm according to the final path may include:
s501, calculating radian required to rotate each joint of the mechanical arm through inverse kinematics inverse solution.
S502, controlling the mechanical arm according to the radian.
Specifically, after the final path is obtained, the radian of the motion required by each mechanical arm joint can be reversely pushed out through an inverse kinematics plug-in, and the information is transmitted to a mechanical arm controller so as to realize obstacle avoidance in the pollination process.
It can be understood that the robot arm obstacle avoidance path planning method of the pollination robot provided by the invention can be implemented on an ROS (Robot Operating System, robot operation platform) platform, wherein the ROS platform comprises a motion planner client and a motion planner server, specifically, as shown in fig. 6, the working process is as follows:
and the communication mechanism of the task message is carried out between the client and the server through the communication between the server and the external mechanical arm controller through an actionlib function package in the ROS. In the motion planning period, the client side needs to receive three-dimensional coordinate information of the stamens and the obstacles identified by the depth camera, calculate by a regression linear optimization three-dimensional RRT algorithm to obtain a plurality of possible paths, and transmit the paths to the motion planner server side. And the track motion of the pollination robot, which is fed back by the motion planner server, is also needed to be received at the motion planner client, wherein the track motion comprises the following steps: and the joint angle, the joint speed and the joint acceleration of the joint space are timely adjusted according to the track motion of the pollination robot, and the joint angle, the joint speed and the joint acceleration are sent to the mechanical arm controller by the motion planner server.
And performing A-algorithm on the determined tracks at a server side of the motion planner to perform optimal path selection, and performing curve fitting between the child nodes and the father nodes determined by the three-dimensional RRT algorithm by using an artificial potential field method, so that the motion track of the mechanical arm can more meet the requirements between branches. The motion planner server also needs to accept the existing radians of each joint transmitted back from the robotic arm and feed back to the motion planner client.
And calculating the track to be moved of each joint of the mechanical arm by using an IK inverse kinematics algorithm carried by the ROS to transmit the track planned by the server side of the motion planner to the mechanical arm controller, so as to finish obstacle avoidance.
In summary, according to the method for planning the obstacle avoidance path of the mechanical arm of the pollination robot, provided by the embodiment of the invention, the three-dimensional RRT algorithm with linear regression is adopted, so that the generation of new nodes of the three-dimensional RRT algorithm is more easily derived to an open space, and the exploring purpose of the RRT algorithm in the branch space is further enhanced; adopting the idea of shortest path optimization, adding an A algorithm into an RRT algorithm to construct a random tree, so that the RRT expanded random tree is developed towards the optimal path, and the randomness of the three-dimensional RRT algorithm in three-dimensional space searching is solved; aiming at the problems that the track generated by the three-dimensional improved RRT algorithm is not smooth, the track planning of a mechanical arm is not facilitated, and the like, the artificial potential field method is adopted to fit the track between the child node and the father node of the three-dimensional improved RRT algorithm, so that the technical problem that the generated track is not smooth is solved, the phenomenon that the artificial potential field method falls into a dead zone is effectively avoided, and the reliability of robot pollination is improved.
Corresponding to the above-mentioned robot arm obstacle avoidance path planning method of the pollination robot, the invention also provides a robot arm obstacle avoidance path planning device of the pollination robot. Since the device embodiments of the present invention correspond to the method embodiments, details not disclosed in the device embodiments may refer to the method embodiments described above, and details are not described in detail in the present invention.
Fig. 7 is a block schematic diagram of a robot arm obstacle avoidance path planning apparatus for a pollination robot in accordance with one embodiment of the invention. As shown in fig. 7, the apparatus includes: a first acquisition module 1, a second acquisition module 2, an optimization module 3, a smoothing module 4 and a control module 5.
The first acquisition module 1 is used for acquiring a depth image through a depth camera and acquiring the pose of an obstacle and the pose of a pollination target point of the mechanical arm according to the depth image; the second acquisition module 2 is used for acquiring an initial path through a three-dimensional RRT algorithm with linear regression according to the pose of the obstacle and the pose of the pollination target point; the optimization module 3 is used for optimizing the three-dimensional RRT algorithm through an A-algorithm so as to obtain an optimal path; the smoothing module 4 is used for carrying out track smoothing on the optimal path through an artificial potential field method so as to obtain a final path; the control module 5 is used for controlling the mechanical arm according to the final path.
According to one embodiment of the invention, the first acquisition module 1 is specifically configured to: identifying a top center point and a bottom coordinate point of the pistil according to the depth image; judging whether to pollinate the current stamens or not according to the numerical values of the top central point and the bottom coordinate point based on the Y axis under the world coordinate system; if so, recognizing the position and the pose of the pistil and the position and the pose of the obstacle according to the depth image, and representing the position and the pose of the pistil by using a quaternion position and pose; and reversely pushing the pollination target point position of the mechanical arm according to the quaternion pose.
According to one embodiment of the invention, the second acquisition module 2 is specifically configured to: at the starting point X init Taking a preset length D as a radius as a circle center, and selecting 10 random points in the three-dimensional space as newly added nodes X rand Expanding, wherein the starting point is the position of the center point of the tail end of the mechanical arm; respectively judging newly added nodes X rand With pollination target point X goal Is a distance of (2); if node X is newly added rand With pollination target point X goal If the distance of (2) is smaller than the preset value, newly increasing node X rand Stopping expansion; if node X is newly added rand With the pollination target point X goal If the distance of the new node X is greater than or equal to the preset value, further judging the new node X rand Whether the distance between the random tree and any other node X on the random tree is greater than that between the random tree and the newly added node X rand And its parent node X par Is a distance of (2); if yes, the new node X is added rand The point is set as a path planning point X new The method comprises the steps of carrying out a first treatment on the surface of the According to the route planning point X new An initial path is acquired.
According to one embodiment of the invention, the optimization module 3 is specifically configured to: estimating each newly added node X using an valuation function of an A-algorithm rand To pollination target point x goal Is determined by the path estimation value of (a); and obtaining an optimal path according to the path estimation value.
According to one embodiment of the invention, the smoothing module 4 is specifically configured to: planning point X by path new The method comprises the steps of taking branches as repulsive force fields as gravitational fields, and obtaining attractive force generated by the gravitational fields and repulsive force generated by the repulsive force fields; acquiring resultant force according to the attractive force and the repulsive force; planning point X according to resultant force new Fitting is performed to obtain a final path.
According to one embodiment of the invention, the control module 5 is specifically configured to: calculating radian required to rotate by each joint of the mechanical arm through inverse kinematics inverse solution; and controlling the mechanical arm according to the radian.
According to the robot arm obstacle avoidance path planning device for the pollination robot, provided by the embodiment of the invention, the three-dimensional RRT algorithm with linear regression is adopted, so that the generation of newly added nodes of the three-dimensional RRT algorithm is more easily derived to an open space, and the exploring purpose of the RRT algorithm in a branch space is further enhanced; adopting the idea of shortest path optimization, adding an A algorithm into an RRT algorithm to construct a random tree, so that the RRT expanded random tree is developed towards the optimal path, and the randomness of the three-dimensional RRT algorithm in three-dimensional space searching is solved; aiming at the problems that the track generated by the three-dimensional improved RRT algorithm is not smooth, the track planning of a mechanical arm is not facilitated, and the like, the artificial potential field method is adopted to fit the track between the child node and the father node of the three-dimensional improved RRT algorithm, so that the technical problem that the generated track is not smooth is solved, the phenomenon that the artificial potential field method falls into a dead zone is effectively avoided, and the reliability of robot pollination is improved.
In the description of the present invention, the terms "first," "second," and the like are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defining "a first" or "a second" may explicitly or implicitly include one or more such feature. The meaning of "a plurality of" is two or more, unless specifically defined otherwise.
Any process or method descriptions in flow charts or otherwise described herein may be understood as representing modules, segments, or portions of code which include one or more executable instructions for implementing specific logical functions or steps of the process, and additional implementations are included within the scope of the preferred embodiment of the present invention in which functions may be executed out of order from that shown or discussed, including substantially concurrently or in reverse order from that shown or discussed, depending on the functionality involved, as would be understood by those reasonably skilled in the art of the embodiments of the present invention.
Logic and/or steps represented in the flowcharts or otherwise described herein, e.g., a ordered listing of executable instructions for implementing logical functions, can be embodied in any computer-readable medium for use by or in connection with an instruction execution system, apparatus, or device, such as a computer-based system, processor-containing system, or other system that can fetch the instructions from the instruction execution system, apparatus, or device and execute the instructions. For the purposes of this description, a "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of the computer-readable medium would include the following: an electrical connection (electronic device) having one or more wires, a portable computer diskette (magnetic device), a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). In addition, the computer readable medium may even be paper or other suitable medium on which the program is printed, as the program may be electronically captured, via, for instance, optical scanning of the paper or other medium, then compiled, interpreted or otherwise processed in a suitable manner, if necessary, and then stored in a computer memory.
It is to be understood that portions of the present invention may be implemented in hardware, software, firmware, or a combination thereof. In the above-described embodiments, the various steps or methods may be implemented in software or firmware stored in a memory and executed by a suitable instruction execution system. As with the other embodiments, if implemented in hardware, may be implemented using any one or combination of the following techniques, as is well known in the art: discrete logic circuits having logic gates for implementing logic functions on data signals, application specific integrated circuits having suitable combinational logic gates, programmable Gate Arrays (PGAs), field Programmable Gate Arrays (FPGAs), and the like.
Those of ordinary skill in the art will appreciate that all or a portion of the steps carried out in the method of the above-described embodiments may be implemented by a program to instruct related hardware, where the program may be stored in a computer readable storage medium, and where the program, when executed, includes one or a combination of the steps of the method embodiments.
In addition, each functional unit in the embodiments of the present invention may be integrated in one processing module, or each unit may exist alone physically, or two or more units may be integrated in one module. The integrated modules may be implemented in hardware or in software functional modules. The integrated modules may also be stored in a computer readable storage medium if implemented in the form of software functional modules and sold or used as a stand-alone product.
The above-mentioned storage medium may be a read-only memory, a magnetic disk or an optical disk, or the like. While embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and not to be construed as limiting the invention, and that variations, modifications, alternatives and variations may be made to the above embodiments by one of ordinary skill in the art within the scope of the invention. Although embodiments of the present invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made therein without departing from the principles and spirit of the invention, the scope of which is defined in the appended claims and their equivalents.
Claims (6)
1. The obstacle avoidance path planning method for the mechanical arm of the pollination robot is characterized by comprising the following steps of:
acquiring a depth image through a depth camera, and acquiring the pose of an obstacle and the pose of a pollination target point of the mechanical arm according to the depth image;
acquiring an initial path through a three-dimensional RRT algorithm with linear regression according to the obstacle pose and the pollination target point position;
optimizing the three-dimensional RRT algorithm through an A-algorithm to obtain an optimal path;
performing track smoothing on the optimal path through an artificial potential field method to obtain a final path;
controlling the mechanical arm according to the final path;
according to the obstacle pose and the pollination target point pose, an initial path is obtained through a three-dimensional RRT algorithm with linear regression, and the method comprises the following steps:
at the starting point X init Taking a preset length D as a radius as a circle center, and selecting 10 random points in the three-dimensional space as newly added nodes X rand Expanding, wherein the starting point is the position of the center point of the tail end of the mechanical arm;
respectively judging the newly added nodes X rand With pollination target point X goal Is a distance of (2);
if the node X is newly added rand With the pollination target point X goal The distance of the newly added node X is smaller than a preset value rand Stopping expansion;
if the node X is newly added rand With the pollination target point X goal If the distance of the new node X is greater than or equal to the preset value, further judging the new node X rand Whether the distance between the random node X and any other node X on the random tree is greater than that of the newly added node X rand And its parent node X par Is a distance of (2);
if yes, the node X is newly added currently rand Set as a route planning point X new ；
Planning a point X according to the path new Acquiring an initial path;
optimizing the three-dimensional RRT algorithm by an a-algorithm to obtain an optimal path, including:
estimating each newly added node X using an valuation function of an A-algorithm rand To pollination target point x goal Is determined by the path estimation value of (a);
and obtaining an optimal path according to the path estimation value.
2. The obstacle avoidance path planning method of a robotic arm of a pollination robot according to claim 1, wherein acquiring a depth image by a depth camera, acquiring an obstacle pose and a pollination target point pose of the robotic arm according to the depth image, comprises:
identifying a top center point and a bottom coordinate point of the pistil according to the depth image;
judging whether to pollinate the current stamens or not according to the top central point and the bottom coordinate point based on the numerical value of the Y axis in the world coordinate system;
if so, identifying the stamen pose and the pose of the obstacle according to the depth image, and representing the stamen pose by using a quaternion pose;
and reversely pushing the pollination target point position of the mechanical arm according to the quaternion position.
3. The method for planning the obstacle avoidance path of the manipulator of the pollination robot according to claim 2, wherein the track smoothing of the optimal path is performed by a manual potential field method to obtain a final path, comprising:
planning point X with the path new The method comprises the steps of taking branches as repulsive force fields as gravitational fields, and obtaining attractive force generated by the gravitational fields and repulsive force generated by the repulsive force fields;
acquiring resultant force according to the attractive force and the repulsive force;
planning a point X for the path according to the resultant force new Fitting is performed to obtain a final path.
4. The method for planning an obstacle avoidance path of a robotic arm for a pollination as defined in claim 1, wherein controlling the robotic arm according to the final path comprises:
calculating radian required to rotate by each joint of the mechanical arm through inverse kinematics inverse solution;
and controlling the mechanical arm according to the radian.
5. Obstacle avoidance path planning device for mechanical arm of pollination robot, which is characterized by comprising:
the first acquisition module is used for acquiring a depth image through a depth camera and acquiring an obstacle pose and a pollination target point pose of the mechanical arm according to the depth image;
the second acquisition module is used for acquiring an initial path through a three-dimensional RRT algorithm with linear regression according to the obstacle pose and the pollination target point pose;
the optimization module is used for optimizing the three-dimensional RRT algorithm through an A-algorithm so as to obtain an optimal path;
the smoothing module is used for carrying out track smoothing on the optimal path through an artificial potential field method so as to obtain a final path;
the control module is used for controlling the mechanical arm according to the final path; the second obtaining module is specifically configured to:
at the starting point X init Taking a preset length D as a radius as a circle center, and selecting 10 random points in the three-dimensional space as newly added nodes X rand Expanding, wherein the starting point is the position of the center point of the tail end of the mechanical arm;
respectively judging the newly added nodes X rand With pollination target point X goal Is a distance of (2);
if the node X is newly added rand With the pollination target point X goal The distance of the newly added node X is smaller than a preset value rand Stopping expansion;
if the node X is newly added rand With the pollination target point X goal If the distance of the new node X is greater than or equal to the preset value, further judging the new node X rand Whether the distance between the random node X and any other node X on the random tree is greater than that of the newly added node X rand And its parent node X par Is a distance of (2);
if yes, the new node X is added rand Set as a route planning point X new ；
Planning a point X according to the path new Acquiring an initial path;
the optimization module is specifically used for:
estimating each newly added node X using an valuation function of an A-algorithm rand To pollination target point x goal Is determined by the path estimation value of (a);
and obtaining an optimal path according to the path estimation value.
6. The obstacle avoidance path planning device of claim 5, wherein the first acquisition module is specifically configured to:
identifying a top center point and a bottom coordinate point of the pistil according to the depth image;
judging whether to pollinate the current stamens or not according to the top central point and the bottom coordinate point based on the numerical value of the Y axis in the world coordinate system;
if so, identifying the stamen pose and the pose of the obstacle according to the depth image, and representing the stamen pose by using a quaternion pose;
and reversely pushing the pollination target point position of the mechanical arm according to the quaternion position.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010491344.2A CN111844007B (en) | 2020-06-02 | 2020-06-02 | Obstacle avoidance path planning method and device for mechanical arm of pollination robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010491344.2A CN111844007B (en) | 2020-06-02 | 2020-06-02 | Obstacle avoidance path planning method and device for mechanical arm of pollination robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111844007A CN111844007A (en) | 2020-10-30 |
| CN111844007B true CN111844007B (en) | 2023-04-28 |
Family
ID=72985574
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010491344.2A Active CN111844007B (en) | 2020-06-02 | 2020-06-02 | Obstacle avoidance path planning method and device for mechanical arm of pollination robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111844007B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112379672B (en) * | 2020-11-24 | 2022-05-10 | 浙大宁波理工学院 | Intelligent unmanned ship path planning method based on improved artificial potential field |
| CN113359717B (en) * | 2021-05-26 | 2022-07-26 | 浙江工业大学 | Mobile robot navigation obstacle avoidance method based on deep reinforcement learning |
| CN113211447B (en) * | 2021-05-27 | 2023-10-27 | 山东大学 | Mechanical arm real-time perception planning method and system based on bidirectional RRT algorithm |
| CN113298473A (en) * | 2021-06-11 | 2021-08-24 | 四川铁公铁信息技术有限公司 | Garden cargo transportation route planning system and method |
| CN113902675A (en) * | 2021-09-06 | 2022-01-07 | 七海行(深圳)科技有限公司 | Automatic kiwi fruit pollination method and device, pollination equipment and storage medium |
| CN114077255B (en) * | 2021-11-22 | 2023-07-18 | 江苏理工学院 | Intelligent vehicle road-finding method based on elliptical model artificial potential field method |
| CN115956500A (en) * | 2022-12-13 | 2023-04-14 | 山东大学 | Distributed automatic pepper pollination device and method based on deep learning |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108896052A (en) * | 2018-09-20 | 2018-11-27 | 鲁东大学 | A kind of mobile robot smooth paths planing method under the environment based on DYNAMIC COMPLEX |
| CN110216674A (en) * | 2019-06-20 | 2019-09-10 | 北京科技大学 | A kind of redundant degree of freedom mechanical arm visual servo obstacle avoidance system |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106695802A (en) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | Improved RRT<*> obstacle avoidance motion planning method based on multi-degree-of-freedom mechanical arm |
| CN108274465A (en) * | 2018-01-10 | 2018-07-13 | 上海理工大学 | Based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method |
| CN108563243B (en) * | 2018-06-28 | 2020-11-06 | 西北工业大学 | Unmanned aerial vehicle track planning method based on improved RRT algorithm |
| CN110125943B (en) * | 2019-06-27 | 2021-06-01 | 易思维（杭州）科技有限公司 | Obstacle avoidance path planning method for multi-degree-of-freedom mechanical arm |
-
2020
- 2020-06-02 CN CN202010491344.2A patent/CN111844007B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108896052A (en) * | 2018-09-20 | 2018-11-27 | 鲁东大学 | A kind of mobile robot smooth paths planing method under the environment based on DYNAMIC COMPLEX |
| CN110216674A (en) * | 2019-06-20 | 2019-09-10 | 北京科技大学 | A kind of redundant degree of freedom mechanical arm visual servo obstacle avoidance system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111844007A (en) | 2020-10-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111844007B (en) | Obstacle avoidance path planning method and device for mechanical arm of pollination robot | |
| Gao et al. | Gradient-based online safe trajectory generation for quadrotor flight in complex environments | |
| Cao et al. | RRT-based path planning for an intelligent litchi-picking manipulator | |
| CN109521774B (en) | Spraying robot track optimization method based on reinforcement learning | |
| Montiel et al. | Path planning for mobile robots using bacterial potential field for avoiding static and dynamic obstacles | |
| CN113219998B (en) | Improved bidirectional-RRT-based vehicle path planning method | |
| Sadat et al. | Feature-rich path planning for robust navigation of MAVs with mono-SLAM | |
| Van Henten et al. | Collision-free motion planning for a cucumber picking robot | |
| CN105676642B (en) | A kind of six-DOF robot station layout and run duration cooperative optimization method | |
| Ye et al. | Collision-free motion planning for the litchi-picking robot | |
| CN108274465A (en) | Based on optimization A*Artificial Potential Field machinery arm, three-D obstacle-avoiding route planning method | |
| CN105437232A (en) | Method and device for controlling multi-joint moving robot to avoid obstacle | |
| JP2022542241A (en) | Systems and methods for augmenting visual output from robotic devices | |
| CN114147708B (en) | Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm | |
| Zhang et al. | Sim2real learning of obstacle avoidance for robotic manipulators in uncertain environments | |
| CN114077256A (en) | Overwater unmanned ship path planning method | |
| CN110370274A (en) | A kind of mechanical arm dynamic obstacle avoidance control method based on Fast marching tree | |
| Ye et al. | Path planning for mobile robots in unstructured orchard environments: An improved kinematically constrained bi-directional RRT approach | |
| Ji et al. | Mapless-planner: A robust and fast planning framework for aggressive autonomous flight without map fusion | |
| CN115416016A (en) | Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method | |
| Zhang et al. | Safe and efficient robot manipulation: Task-oriented environment modeling and object pose estimation | |
| Wu et al. | Real-time three-dimensional smooth path planning for unmanned aerial vehicles in completely unknown cluttered environments | |
| Dai et al. | Think, act, and ask: Open-world interactive personalized robot navigation | |
| CN113400303A (en) | Six-axis robot fruit and vegetable picking path planning method based on RRT (recursive least squares) algorithm | |
| CN117452932A (en) | Multi-robot distributed collaborative operation method and system |
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 |