CN110900611A - Novel mechanical arm target positioning and path planning method - Google Patents
Novel mechanical arm target positioning and path planning method Download PDFInfo
- Publication number
- CN110900611A CN110900611A CN201911282846.8A CN201911282846A CN110900611A CN 110900611 A CN110900611 A CN 110900611A CN 201911282846 A CN201911282846 A CN 201911282846A CN 110900611 A CN110900611 A CN 110900611A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- point
- target
- joint
- target object
- 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.)
- Pending
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/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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The invention relates to a novel mechanical arm target positioning and path planning method, which comprises the following steps in sequence: (1) positioning a target object, shooting a working scene of the mechanical arm by using a camera, marking characteristic pixel points in a picture according to a chromatic value H of the surface color of the target object, and finding a picture area where the target object is located, namely a target object area, by using a sliding window method; (2) calculating the pose of the target point; (3) modeling a mechanical arm and an obstacle; (4) and planning an obstacle avoidance path of the mechanical arm in the joint space. The invention uses the vision system to detect the pose of the target point in real time, thereby improving the universality of the planning algorithm; in addition, frequent inverse kinematics solutions are effectively avoided by planning in joint space, and meanwhile, the advantages of an artificial potential field method and an RRT algorithm are combined, so that the planning rapidity and the joint movement smoothness are improved.
Description
Technical Field
The invention relates to the field of visual point position and mechanical arm control, in particular to a novel mechanical arm target positioning and path planning method.
Background
With the rapid development of society, the cost of human resources is gradually increased, and industrial robots undertake more and more production tasks in industrial production. The rapid development of the related technologies around industrial robots is undoubtedly promoted by the inevitably higher production requirements of the industrial robots and the emphasis of the manufacturing industry on flexible production, agile manufacturing and intelligent manufacturing.
The mechanical arm has one step of common operation in the production process, namely, the mechanical arm moves from the current position to the target position rapidly and smoothly, and other obstacles cannot be collided in the process. Unlike a specific production process such as welding, the step has no strict requirement on the motion track of the tail end point of the mechanical arm, and obstacle avoidance and target point arrival are main constraints. The traditional method artificially plans a reasonable path by using an artificial teaching mode, and a mechanical arm can complete corresponding operation according to the planned path because a production line is accurately assembled, and the method has the following defects: once the target point is changed or the production task is changed, manual teaching needs to be carried out again, even the production line needs to be adjusted, and time and labor are wasted.
Disclosure of Invention
The invention aims to provide a novel mechanical arm target positioning and path planning method which collects target point information in real time through a visual sensor, improves the universality of a planning algorithm, avoids frequent inverse kinematics solution, and achieves the purposes of reducing the path search time and smoothing joint motion.
In order to achieve the purpose, the invention adopts the following technical scheme: a novel mechanical arm target positioning and path planning method comprises the following steps:
(1) positioning a target object: shooting a working scene of the mechanical arm by using a camera, marking characteristic pixel points in a picture according to a chromatic value H of the surface color of the target object, and finding a picture area where the target object is located, namely a target object area, by using a sliding window method;
(2) calculating the pose of the target point: extracting equation parameters of at least 3 intersected characteristic curves on the surface of the target object from the target object region by using a Canny edge detection and Hough transform algorithm; solving the pixel coordinate of the intersection point between every two curves according to a curve equation, converting the pixel coordinate into a user coordinate, and solving the pose of a target point of the mechanical arm;
(3) modeling the mechanical arm and the obstacle: establishing a mechanical arm kinematics model by using a D-H parameter method, and solving a forward and reverse kinematics equation; establishing a collision model by adopting an envelope method, abstracting a mechanical arm connecting rod and a barrier into a cylinder and a sphere respectively, and designing a collision detection algorithm;
(4) and (3) planning an obstacle avoidance path of the mechanical arm in a joint space: solving joint angles of start points and stop points, selecting a step length lambda to traverse adjacent joint angles of the mechanical arm, calculating coordinates of each joint by using a positive kinematic equation, and performing collision detection; calculating the resultant potential energy by adopting an improved artificial potential field method, and selecting a group of joint angles with the minimum resultant potential energy as a rotation basis of the mechanical arm; if the target point falls into the local minimum point or the oscillation point, a temporary virtual target point is found by adopting an RRT fast random tree algorithm, so that the target point jumps out of the local optimum, and then the target point is converted into a real target point for continuing planning.
The step (1) specifically comprises the following steps:
(1a) determining a value range of a chromatic value H of a characteristic color of the surface of a target object in advance, scanning each pixel point in a shot picture line by line, judging whether the chromatic value H is in the value range, if so, marking the pixel point as a characteristic pixel point to obtain a binary image, wherein all the characteristic pixel points are marked as white;
(1b) selecting a square window with the side length of a pixel points, wherein a is the length of the longest line segment which can be surrounded by the contour of the target object in the original image; placing the window on the binary image obtained in the step (1a), performing line-by-line translation by taking a/2 pixel points as step length, counting the number of characteristic pixel points contained in the window at different positions, and when the number of the characteristic pixel points is greater than 1/3 of the total number of the pixel points contained in the square window, determining that the window covers the target area; and then changing the moving step length of the window into one pixel point, performing trial translation in four directions of up, down, left and right, wherein the translation distance in each direction does not exceed a/2 pixels until the window can contain the most characteristic pixel points, recording the pixel coordinates (x, y) of the anchor point at the upper left of the window at the moment, and the area covered by the window at the moment is the target area.
The step (2) specifically comprises the following steps:
(2a) extracting the edge of the target object in the obtained target object region by using a Canny edge detection algorithm;
(2b) detecting a characteristic curve of the surface of the target object in the edge image by using a Hough transform algorithm, and calculating a curve equation of the characteristic curve;
(2c) simultaneous correlation curve equation, solving the pixel coordinates (uv) of the intersection point of at least two curves on the surface of the target object, and utilizing the formula
Converting the pixel coordinate of curve intersection point into user coordinate (X)WYWZW) Wherein the matrix
Is the 1 st, 2 nd and 4 th columns of a camera parameter matrix
For the 3 rd column of the camera parameter matrix, the parameters in the matrix are obtained by calibrating the camera by a Zhang Yongyou calibration method; zCFor the scale parameter, reflecting the distance of the object from the camera, ZWIs a curve intersection point vertical coordinate;
(2d) calculating a gravity center coordinate from the user coordinates of the intersection points of the two curves calculated in the step (2c) and the geometric position relation between the intersection points and the gravity center of the target object, and expressing the rotation angle of the whole target object relative to a user coordinate system by using an included angle corresponding to the direction cosine of a space straight line determined by the two intersection points; the horizontal and vertical coordinates of the mechanical arm target point are the horizontal and vertical coordinates of the gravity center of the target object, the vertical coordinates of the mechanical arm target point are the maximum height of the target object, and the rotation angles of the mechanical arm target point and the rotation angles of the target object are in one-to-one correspondence, so that the target point pose of the mechanical arm is obtained;
the step (3) specifically comprises the following steps:
(3a) performing kinematic modeling on the mechanical arm, firstly, solving D-H parameters according to structural parameters of the mechanical arm, and establishing a mechanical arm kinematic model; solving a positive kinematic equation, and transforming from the joint i-1 to the joint i through two rotation transformations and two translation transformations, wherein a transformation matrix is as follows:
in the formula, c θiRepresents cos θi,s θiDenotes sin θi(ii) a The transformation matrix of the base coordinate system to the hand coordinate system is:
obtaining a Cartesian space coordinate by a positive kinematics formula according to a known joint corner; the inverse kinematics is that the known Cartesian space coordinate is used for solving the joint rotation angle, an analytical method is used for solving 8 groups of solutions, and the optimal solution is selected according to the minimum principle of joint change;
(3b) carrying out collision detection on the mechanical arm and the barrier, firstly, selecting a cylinder enveloping method according to the morphological characteristics of the mechanical arm body, and enveloping a large arm, a joint of the large arm and the small arm, a small arm and a tail end of the mechanical arm by using 4 cylinders; then, enveloping the obstacle by using a sphere, adding the radius of a cylinder for enveloping the mechanical arm to the sphere, abstracting the mechanical arm connecting rod into a line segment, converting a collision model of the mechanical arm and the obstacle into a collision detection problem between the line segment and the sphere in the space, wherein a collision detection algorithm is used for judging whether the line segment and the sphere in the space are intersected or not.
The specific steps of the step (4) are as follows:
(4a) determining joint angles corresponding to the start point and the stop point according to the pose of the start point of the mechanical arm and the pose of the target point of the mechanical arm, namely the pose of the stop point, by an analytical method and a joint change minimum principle;
(4b) given the search step length lambda of the joint space, traverse the adjacent joint angle theta of the robot armi-λ、θi、θi+ λ, where θiIs the current joint angle; obtaining coordinates of each joint of the mechanical arm according to a positive kinematic equation, performing collision detection, discarding the coordinates if the coordinates collide with each other, and otherwise, calculating Euclidean distances from the obstacle to the wrist joint and the elbow joint and from the wrist joint to the elbow jointThe Euclidean distance of the target point;
(4c) substituting the Euclidean distance parameters obtained in the step (4b) into an equation for solving potential energy by an improved artificial potential field method, recording the minimum combined potential energy in traversal, calculating the corresponding tail end position, comparing the tail end position with a tail end position queue of previous multiple traversals, if the calculated tail end position appears in the tail end position queue, enabling the mechanical arm to reach a final target point or enabling the mechanical arm to be locally optimal, entering the step (4d), and if not, moving the mechanical arm to the position with the minimum combined potential energy; the local optimal state means that when the mechanical arm does not reach a target point, the corresponding combined potential energy falls into the minimum value of a local range, so that the mechanical arm falls into a vibration or stop state;
(4d) judging whether the final target point is reached, if so, ending the whole method; otherwise, judging whether the virtual target point is reached, if so, changing the target point into a real final target point, and continuing the operation of the step (4b), otherwise, performing local optimal processing on the mechanical arm when the mechanical arm falls into local optimal.
In the step (4c), the Euclidean distance parameter obtained in the step (4b) is substituted into an equation for solving potential energy by an improved artificial potential field method, and the method specifically comprises the following steps:
(4c1) according to the formula Uatt＝1/2Ka(P-Pg)2Calculating gravitational potential energy, wherein KaIs the gravitational coefficient, P is the end position of the arm, PgIs the position of a target point;
(4c2) the elbow and wrist joints of the mechanical arm are subjected to repulsive force, and the total repulsive force potential energy is the sum of the repulsive force potential energies of the two joints; when the mechanical arm moves to a target point, the repulsive force potential energy is automatically set to 0; calculating the average radius of the connecting rod and the maximum radius of the convex part of the connecting rod, and using the difference between the two as dsaf tailThe formula of the repulsion potential energy solved by the improved artificial potential field method is shown as follows:
in the formula of Ur tail pPotential energy of repulsion, KrIs the coefficient of repulsion, d is the distance from the obstacle to the mechanical arm, d0Extent of action of repulsive potential field, dsaf tailThe safety distance is the difference between the average radius of the connecting rod and the maximum radius of the protrusion of the connecting rod;
(4c3) substituting the Euclidean distance parameters obtained in the step (4b) into the equation for solving the gravitational potential energy in the step (4c1) and the repulsive potential energy in the artificial potential field method improved in the step (4c2) to obtain the gravitational potential energy UattAnd repulsive force potential energy Ur tail pThen according to the formula U ═ Uatt+Ur tail pAnd calculating resultant potential energy, and pushing the mechanical arm to move according to the direction in which the resultant potential energy is reduced fastest.
The specific steps of performing the local optimum processing in the step (4d) are as follows:
(4d1) setting the terminal coordinate of the local minimum value point or the oscillation point as the root node q of the RRT fast random treeinitSetting a search step length L and iteration times N;
(4d2) random point q is generated by applying random function according to position of obstacle and working space of mechanical armtargetAnd finding the distance q on the random tree at that timetargetNearest node qnearObtaining qnearqtargetDistance q on line segmentnearA point of step length L, set to qnewJudging the line segment qnearqnewIf the point is collided, discarding the point, otherwise adding the point to the node of the random tree;
(4d3) judging whether iteration is finished, if the iteration is not finished, continuing the step (4d2), otherwise, solving a node which is farthest away from the obstacle except the root node on the random tree, and setting the node as a virtual target point;
(4d4) and (4c) returning to the step (4c), finishing the planning to the virtual target point, enabling the mechanical arm to jump out of the local optimum, and then replacing the target point with a real target point.
According to the technical scheme, the invention has the advantages that: firstly, the eye-to-hand vision system is adopted, so that the target point information can be obtained in real time, and the universality is improved; secondly, path search is carried out in joint space, so that frequent inverse kinematics solution is avoided, the path search time is reduced, and the joint motion is smoother; thirdly, the artificial potential field method and the RRT algorithm are combined and improved, and respective defects are made up.
Drawings
FIG. 1 is a flow chart of the method of the present invention.
Detailed Description
As shown in fig. 1, a novel robot arm target positioning and path planning method includes the following steps:
(1) positioning a target object: shooting a working scene of the mechanical arm by using a camera, marking characteristic pixel points in a picture according to a chromatic value H of the surface color of the target object, and finding a picture area where the target object is located, namely a target object area, by using a sliding window method;
(2) calculating the pose of the target point: extracting equation parameters of at least 3 intersected characteristic curves on the surface of the target object from the target object region by using a Canny edge detection and Hough transform algorithm; solving the pixel coordinate of the intersection point between every two curves according to a curve equation, converting the pixel coordinate into a user coordinate, and solving the pose of a target point of the mechanical arm;
(3) modeling the mechanical arm and the obstacle: establishing a mechanical arm kinematics model by using a D-H parameter method, and solving a forward and reverse kinematics equation; establishing a collision model by adopting an envelope method, abstracting a mechanical arm connecting rod and a barrier into a cylinder and a sphere respectively, and designing a collision detection algorithm;
(4) and (3) planning an obstacle avoidance path of the mechanical arm in a joint space: solving joint angles of start points and stop points, selecting a step length lambda to traverse adjacent joint angles of the mechanical arm, calculating coordinates of each joint by using a positive kinematic equation, and performing collision detection; calculating the resultant potential energy by adopting an improved artificial potential field method, and selecting a group of joint angles with the minimum resultant potential energy as a rotation basis of the mechanical arm; if the target point falls into the local minimum point or the oscillation point, a temporary virtual target point is found by adopting an RRT fast random tree algorithm, so that the target point jumps out of the local optimum, and then the target point is converted into a real target point for continuing planning.
As shown in fig. 1, the step (1) specifically includes the following steps:
(1a) determining a value range of a chromatic value H of a characteristic color of the surface of a target object in advance, scanning each pixel point in a shot picture line by line, judging whether the chromatic value H is in the value range, if so, marking the pixel point as a characteristic pixel point to obtain a binary image, wherein all the characteristic pixel points are marked as white;
(1b) selecting a square window with the side length of a pixel points, wherein a is the length of the longest line segment which can be surrounded by the contour of the target object in the original image; placing the window on the binary image obtained in the step (1a), performing line-by-line translation by taking a/2 pixel points as step length, counting the number of characteristic pixel points contained in the window at different positions, and when the number of the characteristic pixel points is greater than 1/3 of the total number of the pixel points contained in the square window, determining that the window covers the target area; and then changing the moving step length of the window into one pixel point, performing trial translation in four directions of up, down, left and right, wherein the translation distance in each direction does not exceed a/2 pixels until the window can contain the most characteristic pixel points, recording the pixel coordinates (x, y) of the anchor point at the upper left of the window at the moment, and the area covered by the window at the moment is the target area.
As shown in fig. 1, the step (2) specifically includes the following steps:
(2a) extracting the edge of the target object in the obtained target object region by using a Canny edge detection algorithm;
(2b) detecting a characteristic curve of the surface of the target object in the edge image by using a Hough transform algorithm, and calculating a curve equation of the characteristic curve;
(2c) simultaneous correlation curve equation, solving the pixel coordinates (uv) of the intersection point of at least two curves on the surface of the target object, and utilizing the formula
Converting the pixel coordinate of curve intersection point into user coordinate (X)WYWZW) Wherein the matrix
Is the 1 st, 2 nd and 4 th columns of a camera parameter matrix
For the 3 rd column of the camera parameter matrix, the parameters in the matrix are obtained by calibrating the camera by a Zhang Yongyou calibration method; zCFor the scale parameter, reflecting the distance of the object from the camera, ZWIs a curve intersection point vertical coordinate;
(2d) calculating a gravity center coordinate from the user coordinates of the intersection points of the two curves calculated in the step (2c) and the geometric position relation between the intersection points and the gravity center of the target object, and expressing the rotation angle of the whole target object relative to a user coordinate system by using an included angle corresponding to the direction cosine of a space straight line determined by the two intersection points; the horizontal and vertical coordinates of the mechanical arm target point are the horizontal and vertical coordinates of the gravity center of the target object, the vertical coordinates of the mechanical arm target point are the maximum height of the target object, and the rotation angles of the mechanical arm target point and the rotation angles of the target object are in one-to-one correspondence, so that the target point pose of the mechanical arm is obtained;
as shown in fig. 1, the step (3) specifically includes the following steps:
(3a) performing kinematic modeling on the mechanical arm, firstly, solving D-H parameters according to structural parameters of the mechanical arm, and establishing a mechanical arm kinematic model; solving a positive kinematic equation, and transforming from the joint i-1 to the joint i through two rotation transformations and two translation transformations, wherein a transformation matrix is as follows:
in the formula, c θiRepresents cos θi,s θiDenotes sin θi(ii) a The transformation matrix of the base coordinate system to the hand coordinate system is:
obtaining a Cartesian space coordinate by a positive kinematics formula according to a known joint corner; the inverse kinematics is that the known Cartesian space coordinate is used for solving the joint rotation angle, 8 groups of solutions can be solved by using an analytical method, and the optimal solution is selected according to the minimum principle of joint change;
(3b) carrying out collision detection on the mechanical arm and the barrier, firstly, selecting a cylinder enveloping method according to the morphological characteristics of the mechanical arm body, and enveloping a large arm, a joint of the large arm and the small arm, a small arm and a tail end of the mechanical arm by using 4 cylinders; then, enveloping the obstacle by using a sphere, adding the radius of a cylinder for enveloping the mechanical arm to the sphere, abstracting the mechanical arm connecting rod into a line segment, converting a collision model of the mechanical arm and the obstacle into a collision detection problem between the line segment and the sphere in the space, wherein a collision detection algorithm is used for judging whether the line segment and the sphere in the space are intersected or not.
As shown in fig. 1, the specific steps of step (4) are as follows:
(4a) determining joint angles corresponding to the start point and the stop point according to the pose of the start point of the mechanical arm and the pose of the target point of the mechanical arm, namely the pose of the stop point, by an analytical method and a joint change minimum principle;
(4b) given the search step length lambda of the joint space, traverse the adjacent joint angle theta of the robot armi-λ、θi、θi+ λ, where θiIs the current joint angle; obtaining coordinates of each joint of the mechanical arm according to a positive kinematic equation, performing collision detection, if the coordinates are collided, discarding the coordinates, and if the coordinates are not collided, calculating Euclidean distances from the obstacle to the wrist joint and the elbow joint and calculating the Euclidean distances from the wrist joint to a target point;
(4c) substituting the Euclidean distance parameters obtained in the step (4b) into an equation for solving potential energy by an improved artificial potential field method, recording the minimum combined potential energy in traversal, calculating the corresponding tail end position, comparing the tail end position with a tail end position queue of previous multiple traversals, if the calculated tail end position appears in the tail end position queue, enabling the mechanical arm to reach a final target point or enabling the mechanical arm to be locally optimal, entering the step (4d), and if not, moving the mechanical arm to the position with the minimum combined potential energy; the local optimal state means that when the mechanical arm does not reach a target point, the corresponding combined potential energy falls into the minimum value of a local range, so that the mechanical arm falls into a vibration or stop state; the number of times 8 is used here, but other suitable numbers may be used.
(4d) Judging whether the final target point is reached, if so, ending the whole method; otherwise, judging whether the virtual target point is reached, if so, changing the target point into a real final target point, and continuing the operation of the step (4b), otherwise, performing local optimal processing on the mechanical arm when the mechanical arm falls into local optimal.
In the step (4c), the Euclidean distance parameter obtained in the step (4b) is substituted into an equation for solving potential energy by an improved artificial potential field method, and the method specifically comprises the following steps:
(4c1) according to the formula Uatt＝1/2Ka(P-Pg)2Calculating gravitational potential energy, wherein KaIs the gravitational coefficient, P is the end position of the arm, PgIs the position of a target point;
(4c2) the elbow and wrist joints of the mechanical arm are subjected to repulsive force, and the total repulsive force potential energy is the sum of the repulsive force potential energies of the two joints; when the mechanical arm moves to a target point, the repulsive force potential energy is automatically set to 0; calculating the average radius of the connecting rod and the maximum radius of the convex part of the connecting rod, and using the difference between the two as dsafeThe formula of the repulsion potential energy solved by the improved artificial potential field method is shown as follows:
in the formula of UrepPotential energy of repulsion, KrIs the coefficient of repulsion, d is the distance from the obstacle to the mechanical arm, d0Extent of action of repulsive potential field, dsafeThe safety distance is the difference between the average radius of the connecting rod and the maximum radius of the protrusion of the connecting rod;
(4c3) substituting the Euclidean distance parameters obtained in the step (4b) into the equation for solving the gravitational potential energy in the step (4c1) and the repulsive potential energy in the artificial potential field method improved in the step (4c2) to obtain the gravitational potential energy UattAnd repulsive force potential energy UrepThen according to the formula U ═ Uatt+UrepAnd calculating resultant potential energy, and pushing the mechanical arm to move according to the direction in which the resultant potential energy is reduced fastest.
The specific steps of performing the local optimum processing in the step (4d) are as follows:
(4d1) setting the terminal coordinate of the local minimum value point or the oscillation point as the root node q of the RRT fast random treeinitSetting a search step length L and iteration times N; n should not be so large that10；
(4d2) Random point q is generated by applying random function according to position of obstacle and working space of mechanical armtargetAnd finding the distance q on the random tree at that timetargetNearest node qnearObtaining qnearqtargetDistance q on line segmentnearA point of step length L, set to qnewJudging the line segment qnearqnewIf the point is collided, discarding the point, otherwise adding the point to the node of the random tree;
(4d3) judging whether iteration is finished, if the iteration is not finished, continuing the step (4d2), otherwise, solving a node which is farthest away from the obstacle except the root node on the random tree, and setting the node as a virtual target point;
(4d4) and (4c) returning to the step (4c), finishing the planning to the virtual target point, enabling the mechanical arm to jump out of the local optimum, and then replacing the target point with a real target point.
In conclusion, the invention uses the vision system to detect the pose of the target point in real time, thereby improving the universality of the planning algorithm; in addition, frequent inverse kinematics solutions are effectively avoided by planning in joint space, and meanwhile, the advantages of an artificial potential field method and an RRT algorithm are combined, so that the planning rapidity and the joint movement smoothness are improved.
Claims (7)
1. A novel mechanical arm target positioning and path planning method comprises the following steps:
(1) positioning a target object: shooting a working scene of the mechanical arm by using a camera, marking characteristic pixel points in a picture according to a chromatic value H of the surface color of the target object, and finding a picture area where the target object is located, namely a target object area, by using a sliding window method;
(2) calculating the pose of the target point: extracting equation parameters of at least 3 intersected characteristic curves on the surface of the target object from the target object region by using a Canny edge detection and Hough transform algorithm; solving the pixel coordinate of the intersection point between every two curves according to a curve equation, converting the pixel coordinate into a user coordinate, and solving the pose of a target point of the mechanical arm;
(3) modeling the mechanical arm and the obstacle: establishing a mechanical arm kinematics model by using a D-H parameter method, and solving a forward and reverse kinematics equation; establishing a collision model by adopting an envelope method, abstracting a mechanical arm connecting rod and a barrier into a cylinder and a sphere respectively, and designing a collision detection algorithm;
(4) and (3) planning an obstacle avoidance path of the mechanical arm in a joint space: solving joint angles of start points and stop points, selecting a step length lambda to traverse adjacent joint angles of the mechanical arm, calculating coordinates of each joint by using a positive kinematic equation, and performing collision detection; calculating the resultant potential energy by adopting an improved artificial potential field method, and selecting a group of joint angles with the minimum resultant potential energy as a rotation basis of the mechanical arm; if the target point falls into the local minimum point or the oscillation point, a temporary virtual target point is found by adopting an RRT fast random tree algorithm, so that the target point jumps out of the local optimum, and then the target point is converted into a real target point for continuing planning.
2. The novel mechanical arm target positioning and path planning method according to claim 1, characterized in that: the step (1) specifically comprises the following steps:
(1a) determining a value range of a chromatic value H of a characteristic color of the surface of a target object in advance, scanning each pixel point in a shot picture line by line, judging whether the chromatic value H is in the value range, if so, marking the pixel point as a characteristic pixel point to obtain a binary image, wherein all the characteristic pixel points are marked as white;
(1b) selecting a square window with the side length of a pixel points, wherein a is the length of the longest line segment which can be surrounded by the contour of the target object in the original image; placing the window on the binary image obtained in the step (1a), performing line-by-line translation by taking a/2 pixel points as step length, counting the number of characteristic pixel points contained in the window at different positions, and when the number of the characteristic pixel points is greater than 1/3 of the total number of the pixel points contained in the square window, determining that the window covers the target area; and then changing the moving step length of the window into one pixel point, performing trial translation in four directions of up, down, left and right, wherein the translation distance in each direction does not exceed a/2 pixels until the window can contain the most characteristic pixel points, recording the pixel coordinates (x, y) of the anchor point at the upper left of the window at the moment, and the area covered by the window at the moment is the target area.
3. The novel mechanical arm target positioning and path planning method according to claim 1, characterized in that: the step (2) specifically comprises the following steps:
(2a) extracting the edge of the target object in the obtained target object region by using a Canny edge detection algorithm;
(2b) detecting a characteristic curve of the surface of the target object in the edge image by using a Hough transform algorithm, and calculating a curve equation of the characteristic curve;
(2c) simultaneous correlation curve equations are used to solve the pixel coordinates (u v) of the intersection of at least two curves on the surface of the object using the formula
Converting the pixel coordinate of curve intersection point into user coordinate (X)WYWZW) Wherein the matrix
Is the 1 st, 2 nd and 4 th columns of a camera parameter matrix
For the 3 rd column of the camera parameter matrix, the parameters in the matrix are obtained by calibrating the camera by a Zhang Yongyou calibration method; zCFor the scale parameter, reflecting the distance of the object from the camera, ZWIs a curve intersection point vertical coordinate;
(2d) calculating a gravity center coordinate from the user coordinates of the intersection points of the two curves calculated in the step (2c) and the geometric position relation between the intersection points and the gravity center of the target object, and expressing the rotation angle of the whole target object relative to a user coordinate system by using an included angle corresponding to the direction cosine of a space straight line determined by the two intersection points; the horizontal and vertical coordinates of the mechanical arm target point are the horizontal and vertical coordinates of the gravity center of the target object, the vertical coordinates of the mechanical arm target point are the maximum height of the target object, and the rotation angles of the mechanical arm target point and the rotation angles of the target object are in one-to-one correspondence, so that the target point pose of the mechanical arm is obtained;
(2d) and (3) calculating barycentric coordinates of the target: determining an equation of a space straight line by the two characteristic point user coordinates obtained in the step (2c), and expressing the rotation angle of the whole target object relative to a user coordinate system by using an included angle corresponding to the direction cosine of the space straight line; the horizontal and vertical coordinates of the mechanical arm target point are the horizontal and vertical coordinates of the gravity center of the target object, the vertical coordinates of the mechanical arm target point are the maximum height of the target object, and the rotation angles of the mechanical arm target point and the rotation angles of the target object are in one-to-one correspondence, so that the target point pose of the mechanical arm is obtained.
4. The novel robot arm target positioning and path planning method of claim 1, wherein: the step (3) specifically comprises the following steps:
(3a) performing kinematic modeling on the mechanical arm, firstly, solving D-H parameters according to structural parameters of the mechanical arm, and establishing a mechanical arm kinematic model; solving a positive kinematic equation, and transforming from the joint i-1 to the joint i through two rotation transformations and two translation transformations, wherein a transformation matrix is as follows:
in the formula, c θiRepresents cos θi，sθiDenotes sin θi(ii) a The transformation matrix of the base coordinate system to the hand coordinate system is:
obtaining a Cartesian space coordinate by a positive kinematics formula according to a known joint corner; the inverse kinematics is that the known Cartesian space coordinate is used for solving the joint rotation angle, an analytical method is used for solving 8 groups of solutions, and the optimal solution is selected according to the minimum principle of joint change;
(3b) carrying out collision detection on the mechanical arm and the barrier, firstly, selecting a cylinder enveloping method according to the morphological characteristics of the mechanical arm body, and enveloping a large arm, a joint of the large arm and the small arm, a small arm and a tail end of the mechanical arm by using 4 cylinders; then, enveloping the obstacle by using a sphere, adding the radius of a cylinder for enveloping the mechanical arm to the sphere, abstracting the mechanical arm connecting rod into a line segment, converting a collision model of the mechanical arm and the obstacle into a collision detection problem between the line segment and the sphere in the space, wherein a collision detection algorithm is used for judging whether the line segment and the sphere in the space are intersected or not.
5. The novel robot arm target positioning and path planning method of claim 1, wherein: the specific steps of the step (4) are as follows:
(4a) determining joint angles corresponding to the start point and the stop point according to the pose of the start point of the mechanical arm and the pose of the target point of the mechanical arm, namely the pose of the stop point, by an analytical method and a joint change minimum principle;
(4b) given the search step length lambda of the joint space, traverse the adjacent joint angle theta of the robot armi-λ、θi、θi+ λ, where θiIs the current joint angle; obtaining coordinates of each joint of the mechanical arm according to a positive kinematic equation, performing collision detection, if the coordinates are collided, discarding the coordinates, and if the coordinates are not collided, calculating Euclidean distances from the obstacle to the wrist joint and the elbow joint and calculating the Euclidean distances from the wrist joint to a target point;
(4c) substituting the Euclidean distance parameters obtained in the step (4b) into an equation for solving potential energy by an improved artificial potential field method, recording the minimum combined potential energy in traversal, calculating the corresponding tail end position, comparing the tail end position with a tail end position queue of previous multiple traversals, if the calculated tail end position appears in the tail end position queue, enabling the mechanical arm to reach a final target point or enabling the mechanical arm to be locally optimal, entering the step (4d), and if not, moving the mechanical arm to the position with the minimum combined potential energy; the local optimal state means that when the mechanical arm does not reach a target point, the corresponding combined potential energy falls into the minimum value of a local range, so that the mechanical arm falls into a vibration or stop state;
(4d) judging whether the final target point is reached, if so, ending the whole method; otherwise, judging whether the virtual target point is reached, if so, changing the target point into a real final target point, and continuing the operation of the step (4b), otherwise, performing local optimal processing on the mechanical arm when the mechanical arm falls into local optimal.
6. The novel robot arm target positioning and path planning method of claim 5, wherein: in the step (4c), the Euclidean distance parameter obtained in the step (4b) is substituted into an equation for solving potential energy by an improved artificial potential field method, and the method specifically comprises the following steps:
(4c1) according to the formula Uatt＝1/2Ka(P-Pg)2Calculating gravitational potential energy, wherein KaIs the gravitational coefficient, P is the end position of the arm, PgIs the position of a target point;
(4c2) the elbow and wrist joints of the mechanical arm are subjected to repulsive force, and the total repulsive force potential energy is the sum of the repulsive force potential energies of the two joints; when the mechanical arm moves to a target point, the repulsive force potential energy is automatically set to 0; calculating the average radius of the connecting rod and the maximum radius of the convex part of the connecting rod, and using the difference between the two as dsafeThe formula of the repulsion potential energy solved by the improved artificial potential field method is shown as follows:
in the formula of UrepPotential energy of repulsion, KrIs the coefficient of repulsion, d is the distance from the obstacle to the mechanical arm, d0Extent of action of repulsive potential field, dsafeThe safety distance is the difference between the average radius of the connecting rod and the maximum radius of the protrusion of the connecting rod;
(4c3) substituting the Euclidean distance parameters obtained in the step (4b) into the equation for solving the gravitational potential energy in the step (4c1) and the repulsive potential energy in the artificial potential field method improved in the step (4c2) to obtain the gravitational potential energy UattAnd repulsive force potential energy UrepThen according to the formula U ═ Uatt+UrepAnd calculating resultant potential energy, and pushing the mechanical arm to move according to the direction in which the resultant potential energy is reduced fastest.
7. The novel robot arm target positioning and path planning method of claim 5, wherein: the specific steps of performing the local optimum processing in the step (4d) are as follows:
(4d1) setting the terminal coordinate of the local minimum value point or the oscillation point as the root node q of the RRT fast random treeinitSetting a search step length L and iteration times N;
(4d2) random point q is generated by applying random function according to position of obstacle and working space of mechanical armtargetAnd finding the distance q on the random tree at that timetargetNearest node qnearObtaining qnearqtargetDistance q on line segmentnearA point of step length L, set to qnewJudging the line segment qnearqnewIf the point is collided, discarding the point, otherwise adding the point to the node of the random tree;
(4d3) judging whether iteration is finished, if the iteration is not finished, continuing the step (4d2), otherwise, solving a node which is farthest away from the obstacle except the root node on the random tree, and setting the node as a virtual target point;
(4d4) and (4c) returning to the step (4c), finishing the planning to the virtual target point, enabling the mechanical arm to jump out of the local optimum, and then replacing the target point with a real target point.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911282846.8A CN110900611A (en) | 2019-12-13 | 2019-12-13 | Novel mechanical arm target positioning and path planning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911282846.8A CN110900611A (en) | 2019-12-13 | 2019-12-13 | Novel mechanical arm target positioning and path planning method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110900611A true CN110900611A (en) | 2020-03-24 |
Family
ID=69825462
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911282846.8A Pending CN110900611A (en) | 2019-12-13 | 2019-12-13 | Novel mechanical arm target positioning and path planning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110900611A (en) |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111421540A (en) * | 2020-04-01 | 2020-07-17 | 唐山航宏电子科技有限公司 | Mechanical arm motion control method |
| CN111857142A (en) * | 2020-07-17 | 2020-10-30 | 广州大学 | Path planning obstacle avoidance auxiliary method based on reinforcement learning |
| CN111880522A (en) * | 2020-06-01 | 2020-11-03 | 东莞理工学院 | Novel autonomous assembly robot path planning autonomous navigation system and method |
| CN112743574A (en) * | 2020-12-28 | 2021-05-04 | 深圳市优必选科技股份有限公司 | Optimization method, device and equipment for mechanical arm design |
| CN112809682A (en) * | 2021-01-27 | 2021-05-18 | 佛山科学技术学院 | Mechanical arm obstacle avoidance path planning method and system and storage medium |
| CN112894817A (en) * | 2021-01-27 | 2021-06-04 | 杭州电子科技大学 | Mechanical arm motion planning method in task space |
| CN113232021A (en) * | 2021-05-19 | 2021-08-10 | 中国科学院自动化研究所苏州研究院 | Mechanical arm grabbing path collision detection method |
| CN114581441A (en) * | 2022-05-05 | 2022-06-03 | 深圳百里科技有限公司 | Part detection method, device, equipment and storage medium |
| CN116572248A (en) * | 2023-06-07 | 2023-08-11 | 哈尔滨工业大学 | Redundant mechanical arm collision avoidance and path planning method for joint space planning |
| CN116652956A (en) * | 2023-06-20 | 2023-08-29 | 上海微亿智造科技有限公司 | Photographing path self-adaptive planning method and device for appearance detection |
| CN116572248B (en) * | 2023-06-07 | 2024-07-16 | 哈尔滨工业大学 | Redundant mechanical arm collision avoidance and path planning method for joint space planning |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6477260B1 (en) * | 1998-11-02 | 2002-11-05 | Nissan Motor Co., Ltd. | Position measuring apparatus using a pair of electronic cameras |
| US20150302591A1 (en) * | 2014-04-16 | 2015-10-22 | Hyundai Motor Company | System for detecting obstacle using road surface model setting and method thereof |
| CN106485735A (en) * | 2015-09-01 | 2017-03-08 | 南京理工大学 | Human body target recognition and tracking method based on stereovision technique |
| CN106780507A (en) * | 2016-11-24 | 2017-05-31 | 西北工业大学 | A kind of sliding window fast target detection method based on super-pixel segmentation |
| CN107609451A (en) * | 2017-09-14 | 2018-01-19 | 斯坦德机器人（深圳）有限公司 | A kind of high-precision vision localization method and system based on Quick Response Code |
| CN108247637A (en) * | 2018-01-24 | 2018-07-06 | 中南大学 | A kind of industrial machine human arm vision anticollision control method |
| CN108596009A (en) * | 2017-12-29 | 2018-09-28 | 西安智加科技有限公司 | A kind of obstacle detection method and system for agricultural machinery automatic Pilot |
| CN109141364A (en) * | 2018-08-01 | 2019-01-04 | 北京进化者机器人科技有限公司 | Obstacle detection method, system and robot |
| CN109520507A (en) * | 2018-12-05 | 2019-03-26 | 智灵飞（北京）科技有限公司 | A kind of unmanned plane real-time route planing method based on improvement RRT |
| CN109986564A (en) * | 2019-05-20 | 2019-07-09 | 上海应用技术大学 | Industrial machinery arm paths planning method |
| CN110228069A (en) * | 2019-07-17 | 2019-09-13 | 东北大学 | A kind of online avoidance motion planning method of mechanical arm |
-
2019
- 2019-12-13 CN CN201911282846.8A patent/CN110900611A/en active Pending
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6477260B1 (en) * | 1998-11-02 | 2002-11-05 | Nissan Motor Co., Ltd. | Position measuring apparatus using a pair of electronic cameras |
| US20150302591A1 (en) * | 2014-04-16 | 2015-10-22 | Hyundai Motor Company | System for detecting obstacle using road surface model setting and method thereof |
| CN106485735A (en) * | 2015-09-01 | 2017-03-08 | 南京理工大学 | Human body target recognition and tracking method based on stereovision technique |
| CN106780507A (en) * | 2016-11-24 | 2017-05-31 | 西北工业大学 | A kind of sliding window fast target detection method based on super-pixel segmentation |
| CN107609451A (en) * | 2017-09-14 | 2018-01-19 | 斯坦德机器人（深圳）有限公司 | A kind of high-precision vision localization method and system based on Quick Response Code |
| CN108596009A (en) * | 2017-12-29 | 2018-09-28 | 西安智加科技有限公司 | A kind of obstacle detection method and system for agricultural machinery automatic Pilot |
| CN108247637A (en) * | 2018-01-24 | 2018-07-06 | 中南大学 | A kind of industrial machine human arm vision anticollision control method |
| CN109141364A (en) * | 2018-08-01 | 2019-01-04 | 北京进化者机器人科技有限公司 | Obstacle detection method, system and robot |
| CN109520507A (en) * | 2018-12-05 | 2019-03-26 | 智灵飞（北京）科技有限公司 | A kind of unmanned plane real-time route planing method based on improvement RRT |
| CN109986564A (en) * | 2019-05-20 | 2019-07-09 | 上海应用技术大学 | Industrial machinery arm paths planning method |
| CN110228069A (en) * | 2019-07-17 | 2019-09-13 | 东北大学 | A kind of online avoidance motion planning method of mechanical arm |
Non-Patent Citations (8)
| Title |
|---|
| 乔慧芬等: "移动机器人的实时路径规划研究与仿真", 《计算机仿真》 * |
| 何兆楚等: "RRT与人工势场法结合的机械臂避障规划", 《工业工程》 * |
| 唐彪等: "改进人工势场法的机械臂避障路径规划研究", 《无线互联科技》 * |
| 姬伟等: "基于改进人工势场的苹果采摘机器人机械手避障方法", 《农业机械学报》 * |
| 王俊龙等: "改进人工势场法的机械臂避障路径规划", 《计算机工程与应用》 * |
| 祝敬等: "基于改进人工势场法的机械臂避障路径规划", 《计算机测量与控制》 * |
| 罗胜华等: "一种基于改进人工势场法的移动机器人路径规划", 《微计算机信息》 * |
| 艾青林等: "基于双重配准的机器人双目视觉三维拼接方法研究", 《机电工程》 * |
Cited By (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111421540A (en) * | 2020-04-01 | 2020-07-17 | 唐山航宏电子科技有限公司 | Mechanical arm motion control method |
| CN111880522A (en) * | 2020-06-01 | 2020-11-03 | 东莞理工学院 | Novel autonomous assembly robot path planning autonomous navigation system and method |
| CN111857142A (en) * | 2020-07-17 | 2020-10-30 | 广州大学 | Path planning obstacle avoidance auxiliary method based on reinforcement learning |
| CN111857142B (en) * | 2020-07-17 | 2022-08-02 | 广州大学 | Path planning obstacle avoidance auxiliary method based on reinforcement learning |
| CN112743574B (en) * | 2020-12-28 | 2022-07-19 | 深圳市优必选科技股份有限公司 | Optimization method, device and equipment for mechanical arm design |
| CN112743574A (en) * | 2020-12-28 | 2021-05-04 | 深圳市优必选科技股份有限公司 | Optimization method, device and equipment for mechanical arm design |
| CN112894817A (en) * | 2021-01-27 | 2021-06-04 | 杭州电子科技大学 | Mechanical arm motion planning method in task space |
| CN112894817B (en) * | 2021-01-27 | 2022-04-29 | 杭州电子科技大学 | Mechanical arm motion planning method in task space |
| CN112809682A (en) * | 2021-01-27 | 2021-05-18 | 佛山科学技术学院 | Mechanical arm obstacle avoidance path planning method and system and storage medium |
| CN113232021A (en) * | 2021-05-19 | 2021-08-10 | 中国科学院自动化研究所苏州研究院 | Mechanical arm grabbing path collision detection method |
| CN114581441A (en) * | 2022-05-05 | 2022-06-03 | 深圳百里科技有限公司 | Part detection method, device, equipment and storage medium |
| CN116572248A (en) * | 2023-06-07 | 2023-08-11 | 哈尔滨工业大学 | Redundant mechanical arm collision avoidance and path planning method for joint space planning |
| CN116572248B (en) * | 2023-06-07 | 2024-07-16 | 哈尔滨工业大学 | Redundant mechanical arm collision avoidance and path planning method for joint space planning |
| CN116652956A (en) * | 2023-06-20 | 2023-08-29 | 上海微亿智造科技有限公司 | Photographing path self-adaptive planning method and device for appearance detection |
| CN116652956B (en) * | 2023-06-20 | 2024-03-22 | 上海微亿智造科技有限公司 | Photographing path self-adaptive planning method and device for appearance detection |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110900611A (en) | Novel mechanical arm target positioning and path planning method | |
| CN113276106B (en) | Climbing robot space positioning method and space positioning system | |
| CN103170973B (en) | Man-machine cooperation device and method based on Kinect video camera | |
| CN107471218B (en) | Binocular vision-based hand-eye coordination method for double-arm robot | |
| CN109976347B (en) | Visual servo path planning method based on rapid expansion random tree and potential field method | |
| JP3208953B2 (en) | Three-dimensional position and posture recognition method based on vision and three-dimensional position and posture recognition device based on vision | |
| CN111645074A (en) | Robot grabbing and positioning method | |
| CN109940626B (en) | Control method of eyebrow drawing robot system based on robot vision | |
| CN108415434B (en) | Robot scheduling method | |
| CN111462154A (en) | Target positioning method and device based on depth vision sensor and automatic grabbing robot | |
| CN108818530A (en) | Stacking piston motion planing method at random is grabbed based on the mechanical arm for improving RRT algorithm | |
| CN111300384B (en) | Registration system and method for robot augmented reality teaching based on identification card movement | |
| CN112775959A (en) | Method and system for determining grabbing pose of manipulator and storage medium | |
| CN113246143A (en) | Mechanical arm dynamic obstacle avoidance trajectory planning method and device | |
| CN114299039B (en) | Robot and collision detection device and method thereof | |
| CN2645862Y (en) | Mobile mechanical arm system | |
| CN110928311A (en) | Indoor mobile robot navigation method based on linear features under panoramic camera | |
| CN117840995A (en) | Automatic wall building method and system based on two-stage visual servo | |
| CN112288801A (en) | Four-in-one self-adaptive tracking shooting method and device applied to inspection robot | |
| KR102452315B1 (en) | Apparatus and method of robot control through vision recognition using deep learning and marker | |
| Walck et al. | Automatic observation for 3d reconstruction of unknown objects using visual servoing | |
| Motai et al. | SmartView: hand-eye robotic calibration for active viewpoint generation and object grasping | |
| CN108211276A (en) | A kind of automatically picking up balls robot system and control method | |
| JPH06258028A (en) | Method and system for visually recognizing three dimensional position and attitude | |
| CN111598945A (en) | Three-dimensional positioning method for automobile engine crankshaft bush cover |
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 | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20200324 |
|
| WD01 | Invention patent application deemed withdrawn after publication |