CN108858162B - Position determination method and device for four-axis mechanical arm - Google Patents
Position determination method and device for four-axis mechanical arm Download PDFInfo
- Publication number
- CN108858162B CN108858162B CN201810691166.0A CN201810691166A CN108858162B CN 108858162 B CN108858162 B CN 108858162B CN 201810691166 A CN201810691166 A CN 201810691166A CN 108858162 B CN108858162 B CN 108858162B
- Authority
- CN
- China
- Prior art keywords
- obstacle
- reference line
- geometric model
- line segment
- projection
- 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/06—Programme-controlled manipulators characterised by multi-articulated arms
-
- 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/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
- B25J9/04—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type by rotating at least one arm, excluding the head movement itself, e.g. cylindrical coordinate type or polar coordinate type
- B25J9/041—Cylindrical coordinate type
- B25J9/042—Cylindrical coordinate type comprising an articulated arm
-
- 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/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
- B25J9/04—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type by rotating at least one arm, excluding the head movement itself, e.g. cylindrical coordinate type or polar coordinate type
- B25J9/041—Cylindrical coordinate type
- B25J9/042—Cylindrical coordinate type comprising an articulated arm
- B25J9/044—Cylindrical coordinate type comprising an articulated arm with forearm providing vertical linear movement
-
- 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/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
-
- 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/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- 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/1612—Programme controls characterised by the hand, wrist, grip control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Evolutionary Computation (AREA)
- Artificial Intelligence (AREA)
- Physics & Mathematics (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Manipulator (AREA)
Abstract
The invention provides a position determining method and device of a four-axis mechanical arm. The position determining method of the four-axis mechanical arm comprises the following steps: acquiring joint values of joints on the four-axis mechanical arm; obtaining position information of the plane geometric model of the hand grab according to the joint value, the positive kinematic model of the four-axis mechanical arm and the plane geometric model of the hand grab on the four-axis mechanical arm; obtaining the distance and the direction of the hand grab relative to the obstacle according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle and the position information of the plane geometric model of the obstacle; wherein the direction indicates a direction of the hand grip away from the obstacle. The position determining method of the four-axis mechanical arm provided by the invention improves the accuracy and richness of the position relation between the hand grab and the barrier.
Description
Technical Field
The invention relates to the technical field of automatic control, in particular to a position determining method and device for a four-axis mechanical arm.
Background
A Selective Compliance Assembly Robot Arm (SCARA) is a special type of industrial Robot of the cylindrical coordinate type. The four-axis SCARA robot arm is provided with 3 rotary joints, the axes of the rotary joints are parallel to each other, and the four-axis SCARA robot arm can be positioned and oriented in a plane. The four-axis SCARA robot also has a prismatic joint that allows movement of the end piece (e.g., a hand grip) perpendicular to the plane.
In some cases, it is necessary to obtain a position relationship between the mechanical arm and an obstacle in the environment, so as to determine whether the mechanical arm collides with the obstacle at a certain pose, and perform path planning or motion planning of the mechanical arm. Currently, only the distance between the robot arm and the obstacle can be obtained, and the distance is positive. That is, only the distance between the robot arm and the obstacle when the two do not collide can be obtained.
However, the above-described positional relationship between the robot arm and the obstacle is too simple, and the efficiency of path planning or motion planning is reduced.
Disclosure of Invention
The invention provides a position determining method and device of a four-axis mechanical arm, which improve the accuracy and richness of the position relation between a hand grab and an obstacle.
In a first aspect, the present invention provides a position determination method for a four-axis robot arm, including:
acquiring joint values of joints on the four-axis mechanical arm;
obtaining position information of the plane geometric model of the hand grab according to the joint value, the positive kinematic model of the four-axis mechanical arm and the plane geometric model of the hand grab on the four-axis mechanical arm;
obtaining the distance and the direction of the hand grab relative to the obstacle according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle and the position information of the plane geometric model of the obstacle; wherein the direction indicates a direction of the hand grip away from the obstacle.
In a second aspect, the present invention provides a position determination apparatus for a four-axis robot arm, comprising:
the acquisition module is used for acquiring joint values of joints on the four-axis mechanical arm;
the processing module is used for acquiring position information of the plane geometric model of the hand grab according to the joint value, the positive kinematic model of the four-axis mechanical arm and the plane geometric model of the hand grab on the four-axis mechanical arm;
obtaining the distance and the direction of the hand grab relative to the obstacle according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle and the position information of the plane geometric model of the obstacle; wherein the direction indicates a direction of the hand grip away from the obstacle.
In a third aspect, the present invention provides a position determining apparatus for a four-axis robotic arm, comprising a processor, a memory for storing instructions, and a transceiver for communicating with other devices, the processor being configured to execute the instructions stored in the memory, such that the position determining apparatus for the four-axis robotic arm performs the method of the first aspect.
In a fourth aspect, an embodiment of the present application provides a storage medium, including: a readable storage medium and a computer program for implementing the method of any one of the first aspect.
The invention provides a position determining method and device of a four-axis mechanical arm, which are used for modeling a hand grab and an obstacle into a plane geometric model. The distance and the direction of the hand grab relative to the obstacle can be obtained through joint values of joints on the four-axis mechanical arm, a positive kinematic model of the four-axis mechanical arm, a plane geometric model of the hand grab and a plane geometric model of the obstacle. The accuracy and the richness of the position relation between the hand grab and the barrier are improved, and an accurate basis is provided for the path planning or the motion planning of the follow-up four-axis mechanical arm.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the description of the embodiments or the prior art will be briefly introduced below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to these drawings without creative efforts.
Fig. 1 is a flowchart of a position determination method of a four-axis robot arm according to an embodiment of the present invention;
FIG. 2 is a schematic structural diagram of a four-axis robot arm according to the present invention;
fig. 3A is a schematic diagram of a relative position relationship between the hand grip and the obstacle according to the second embodiment of the present invention;
fig. 3B is another schematic diagram of the relative position relationship between the hand grip and the obstacle according to the second embodiment of the present invention;
fig. 4A is a schematic diagram of a relative position relationship between the hand grip and the obstacle according to the third embodiment of the present invention;
fig. 4B is another schematic diagram of the relative position relationship between the hand grip and the obstacle according to the third embodiment of the present invention;
fig. 5A is a schematic diagram of a relative position relationship between the hand grip and the obstacle according to the fourth embodiment of the present invention;
fig. 5B is another schematic diagram of the relative position relationship between the hand grip and the obstacle according to the fourth embodiment of the present invention;
fig. 6 is a schematic structural diagram of a position determination device of a four-axis robot arm according to an embodiment of the present invention;
fig. 7 is a schematic structural diagram of a position determination device of a four-axis robot arm according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The terms "first," "second," "third," "fourth," and the like in the description and in the claims, as well as in the drawings, if any, are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used is interchangeable under appropriate circumstances such that the embodiments of the application described herein are, for example, capable of operation in sequences other than those illustrated or otherwise described herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed, but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.
Fig. 1 is a flowchart of a position determination method of a four-axis robot arm according to an embodiment of the present invention. In the position determination method of the four-axis robot arm provided by this embodiment, the execution main body may be a position determination device of the four-axis robot arm. As shown in fig. 1, the method for determining the position of a four-axis robot arm provided in this embodiment may include:
s101, obtaining joint values of joints on the four-axis mechanical arm.
In particular, joints are an important concept in robotic manipulators. A joint refers to a device that connects two components. The connection is not a fixed connection but a limited relative movement can occur. Alternatively, the movement may comprise both rotational and translational movement. Joint values of a joint may be used to describe the relative positions of the joints. For example, for a rotary joint, the joint value for the joint may include the angle through which the rotary joint rotates.
In this step, joint values of all joints of the four-axis robot arm can be acquired. Or acquiring joint values of partial joints on the four-axis mechanical arm according to the requirement of subsequent data processing.
This is illustrated by way of example below.
Exemplarily, fig. 1 is a schematic structural view of a four-axis robot arm according to the present invention. As shown in fig. 1, the four-axis robot arm may include three rotational joints and one moving joint 14. The three rotary joints may be referred to in order as a first rotary joint 11, a second rotary joint 12 and a third rotary joint 13. The three rotary joints are rotatable about a rotation axis in a horizontal plane. The prismatic joint 14 is movable in the vertical direction. The distal end of the prismatic joint 14 may be provided with a hand grip (not shown).
In one implementation, acquiring joint values of joints on the four-axis mechanical arm may include:
obtain θ ═ θ1,θ2,θ3,θ4). Wherein, theta1Is the angle of rotation of the first rotary shaft 111, theta2Is the angle of rotation of the second axis of rotation 121, theta4Is the angle through which the third rotation axis 131 rotates, theta3Is the vertical distance moved by the prismatic joint 14.
In another implementation manner, acquiring joint values of joints on the four-axis mechanical arm may include:
obtain θ ═ θ1,θ2,θ4)。
S102, obtaining position information of the plane geometric model of the hand grab according to the joint value, the positive kinematic model of the four-axis mechanical arm and the plane geometric model of the hand grab on the four-axis mechanical arm.
Specifically, the robot kinematics includes forward kinematics and reverse kinematics. The positive kinematics model is a model that can obtain the position and posture of the robot end by giving the variables of each joint of the robot.
It should be noted that, in this embodiment, the positive kinematics model of the four-axis robot arm is not limited, and may be different according to the structure of the four-axis robot arm.
The plane geometric model of the hand grasp refers to a corresponding plane geometric shape after the hand grasp is mapped to a horizontal plane. Alternatively, the planar geometric model may include a circle and a polygon. The polygons may include triangles, rectangles, and regular or non-regular polygons having a number of sides greater than 4.
Optionally, in S102, obtaining position information of the planar geometric model of the hand grab according to the joint value, the positive kinematics model of the four-axis mechanical arm, and the planar geometric model of the hand grab on the four-axis mechanical arm, may include:
and acquiring the pose information of the hand grab according to the joint value and the positive kinematics model. The pose information may include coordinates of a center point of the gripper in a plane coordinate system of the robot arm and an angle of the gripper rotating around the center point. The original point of the plane coordinate system of the mechanical arm is the intersection point of the axis of the rotating shaft of the first rotating joint on the four-axis mechanical arm and the horizontal plane.
And obtaining the position information of the plane geometric model of the hand grab according to the pose information of the hand grab and the plane geometric model of the hand grab.
This is illustrated below in connection with fig. 1.
As shown in fig. 1, the origin of the robot arm plane coordinate system is the intersection of the rotation axis z of the first rotary joint 11 and the horizontal plane. A hand grip (not shown) is provided at the distal end of the prismatic joint 14, and the point of connection of the hand grip to the prismatic joint 14 may be the center point of the hand grip. Let θ be (θ)1,θ2,θ4). The pose information of the hand grab can be obtained according to the joint value and a positive kinematic model of the four-axis mechanical arm by the following formula
x＝a1cos(θ1)+a2cos(θ1+θ2)
y＝a1sin(θ1)+a2sin(θ1+θ2)
Wherein, a1The length between the rotation axis of the first rotary joint 11 and the rotation axis of the second rotary joint 12 may be referred to as the joint length of the first axis. a is2The length between the rotation axis of the second rotary joint 12 and the rotation axis of the third rotary joint 13 may be referred to as the joint length of the second axis. (x, y) is the coordinate of the central point of the hand grab in the plane coordinate system of the mechanical arm,
for the angle the hand grips are turned around the central point.
And then, obtaining the position information of the plane geometric model of the hand grab according to the pose information of the hand grab and the plane geometric model of the hand grab.
Optionally, in an implementation manner, the planar geometric model of the hand grip is a circle, and the position information of the planar geometric model of the hand grip may include: the coordinate of the central point of the hand grab in the plane coordinate system of the mechanical arm and the radius of the circle.
In this implementation, the center point of the hand grip is the center of the circle. The coordinate of the central point of the hand grab in the plane coordinate system of the mechanical arm is (x, y). Angle of rotation of the gripper around the centre point
The distance and the direction of the hand grab relative to the obstacle are not influenced.
Optionally, in another implementation manner, the grasping plane geometric model is a polygon, and in S102, obtaining the position information of the grasping plane geometric model according to the pose information of the grasping plane and the grasping plane geometric model may include:
and obtaining the coordinates of each vertex of the polygon in the plane coordinate system of the mechanical arm according to the pose information of the hand grab, the coordinates of each vertex of the polygon in the plane coordinate system of the hand grab and the positive kinematic model. Wherein, the origin of the plane coordinate system of the hand grab is the central point of the hand grab.
This is illustrated by way of example below.
The coordinates of the vertices of the polygon in the plane coordinate system of the grab reflect the positions of the vertices of the polygon relative to the center point of the grab. Angle of rotation of the gripper around the centre point
Will affect the distance and direction of the grab relative to the obstacle. The coordinates of the vertex of the polygon in the grasping plane coordinate system are assumed to be
The coordinates of the vertexes of the polygon in the plane coordinate system of the mechanical arm can be obtained according to the pose information of the hand grab, the positive kinematic model and the coordinates of the vertexes of the polygon in the plane coordinate system of the hand grab by the following formula
S103, obtaining the distance and the direction of the hand grab relative to the obstacle according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle and the position information of the plane geometric model of the obstacle.
Specifically, the plane geometric model of the obstacle refers to a corresponding plane geometric shape after the obstacle is mapped to a horizontal plane. The description of the plane geometric model of the hand grasping can be referred to, the principle is similar, and the description is omitted here.
The distance between the hand grab and the obstacle can reflect the relative position relationship between the hand grab and the obstacle and the collision degree. The distance between the hand grab and the obstacle can be larger than 0 and can also be smaller than 0. When the distance is greater than 0, no collision occurs between the hand grab and the obstacle. In this case, the larger the distance, the farther the hand grip is from the obstacle. When the distance is less than 0, it indicates that a collision has occurred between the hand grip and the obstacle. In this case, the larger the absolute value of the distance, the more serious the collision between the hand grip and the obstacle.
Wherein the direction of the handgrip relative to the obstacle indicates the direction of the handgrip away from the obstacle or, as understood, the direction of the distance between the handgrip and the obstacle increasing.
Therefore, according to the position determining method of the four-axis mechanical arm provided by the embodiment, the height information of the hand grab and the obstacle in the vertical direction is not considered, and the hand grab and the obstacle are modeled into a plane geometric model. The distance and the direction of the hand grab relative to the obstacle can be obtained through joint values of joints on the four-axis mechanical arm, a positive kinematic model of the four-axis mechanical arm, a plane geometric model of the hand grab and a plane geometric model of the obstacle. The accuracy and the richness of the position relation between the hand grab and the barrier are improved, the modeling complexity of the hand grab and the barrier is simplified, the calculated amount is reduced, the calculation efficiency is improved, and an accurate basis is provided for the path planning or the motion planning of the follow-up four-axis mechanical arm.
Optionally, the position determining method for the four-axis mechanical arm provided in this embodiment may further include:
and obtaining the gradient direction of the joint relative to the barrier according to the joint value, the direction of the hand grab relative to the barrier and the inverse kinematics model of the four-axis mechanical arm.
Specifically, after the direction of the hand grab relative to the obstacle is obtained, in order to perform path planning and motion planning on the four-axis mechanical arm so that the mechanical arm is as far away from the obstacle as possible, the direction of the hand grab relative to the obstacle needs to be converted into the gradient direction of the joint relative to the obstacle. Furthermore, the joint of four-axis arm moves along the gradient direction, can realize holding in hand and keep away from the barrier, increases the effect of holding in hand and the distance between the barrier.
This is illustrated below in connection with fig. 1.
For the four-axis robot arm structure shown in fig. 1, the joint value θ ═ can be calculated by the following formula according to the joint value θ ═ (θ)1,θ2) Obtaining the gradient direction (theta) of the joint relative to the barrier by using the inverse kinematics model of the four-axis mechanical arm and the direction phi of the hand grab relative to the barrier1,θ2)。
θ1＝-(a1sin(θ1)+a2sin(θ1+θ2))cos(φ)+(a1cos(θ1)+a2cos(θ1+θ2))sin(φ)
θ2＝-a2sin(θ1+θ2)cos(φ)+a2cos(θ1+θ2)sin(φ)
The inverse kinematics model is a model in which the position and posture of the robot end are known and all joint variables of the robot can be obtained.
It should be noted that, in this embodiment, the inverse kinematics model of the four-axis mechanical arm is not limited, and may be different according to the structure of the four-axis mechanical arm.
The embodiment provides a position determining method of a four-axis mechanical arm, which comprises the following steps: and acquiring joint values of joints on the four-axis mechanical arm. And obtaining the position information of the plane geometric model of the hand grab according to the joint value, the positive kinematics model of the four-axis mechanical arm and the plane geometric model of the hand grab on the four-axis mechanical arm. And obtaining the distance and the direction of the hand grab relative to the obstacle according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle and the position information of the plane geometric model of the obstacle. The position determining method of the four-axis mechanical arm provided by the embodiment improves the accuracy of the position relationship between the hand grab and the obstacle, improves the calculation efficiency, and provides an accurate basis for the path planning or the motion planning of the subsequent four-axis mechanical arm.
The embodiment of the invention also provides a position determination method of the four-axis mechanical arm. The present embodiment specifically provides a specific implementation manner of S103 when the plane geometric models of the hand grasp and the obstacle are both circular on the basis of the first embodiment.
Specifically, in this embodiment, the plane geometric model of the hand grip is a first circle, and the plane geometric model of the obstacle is a second circle. In S103, obtaining the distance and the direction of the hand grip relative to the obstacle according to the plane geometric model of the hand grip, the position information of the plane geometric model of the hand grip, the plane geometric model of the obstacle, and the position information of the plane geometric model of the obstacle may include:
by passing
Obtaining the distance of the hand grasp relative to the obstacle by
The orientation of the grip relative to the obstacle is obtained.
Wherein (x, y) is the coordinate of the center of the first circle, r is the radius of the first circle, and (x)1,y1) Is the coordinate of the center of the second circle, r1The radius of the second circle.
Specifically, the geometric model of the plane grasped by the hand is a circle (referred to as a first circle). The position information of the plane geometric model of the hand grip comprises: the coordinates of the center of the circle (x, y) and the radius r. The planar geometric model of the obstacle is a circle (referred to as a second circle). The position information of the planar geometric model of the obstacle includes: coordinates of the center of a circle (x)1,y1) And radius r1. The distance between the hand grab and the obstacle is the distance between the center point of the hand grab and the center point of the obstacle minus the sum of the radius of the hand grab and the radius of the obstacle. The direction of the hand grab relative to the obstacle is the direction of the center point of the hand grab relative to the center point of the obstacle.
The following is a description by specific examples.
In an example, fig. 3A is a schematic diagram of a relative position relationship between the hand grip and the obstacle according to the second embodiment of the present invention. As shown in fig. 3A, d > 0, at which time no collision occurs between the grab and the obstacle.
In another example, fig. 3B is another schematic diagram of a relative position relationship between the handgrip and the obstacle according to the second embodiment of the present invention. As shown in FIG. 3B, d < 0. At this time, a collision has occurred between the grip and the obstacle. The hand needs to be held in the direction of the obstacle phi by at least the distance | d |.
The embodiment provides a position determining method of a four-axis mechanical arm. Wherein, the plane geometric models of the hand grab and the barrier are both circular. The position determining method of the four-axis mechanical arm provided by the embodiment improves the accuracy of the position relation between the hand grab and the obstacle, and improves the calculation efficiency.
The third embodiment of the invention also provides a position determination method of the four-axis mechanical arm. In the present embodiment, the planar geometric model of the hand grip is one of a circle and a polygon, and the planar geometric model of the obstacle is the other of a circle and a polygon. That is, the planar geometric model of the hand grip is circular, and the planar geometric model of the obstacle is polygonal. Alternatively, the planar geometric model of the obstacle is circular and the planar geometric model of the hand grip is polygonal. The present embodiment specifically provides a specific implementation manner of S103 on the basis of the first embodiment.
Specifically, in this embodiment, in S103, obtaining the distance and the direction of the hand grab relative to the obstacle according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle, and the position information of the plane geometric model of the obstacle may include:
and projecting the circle center of the circle to the datum line to obtain a reference point, and projecting the polygon to the datum line to obtain a first reference line segment. Wherein the reference line is a perpendicular bisector of one side of the polygon.
And respectively expanding R towards the two ends of the first reference line segment to obtain a second reference line segment. Wherein R is the radius of the circle.
And determining the minimum value of the distance between the reference point and the two end points of the second reference line segment as a projection distance, and determining the direction of the reference point relative to the end point which is closer to the reference point in the two end points as a projection direction, wherein the projection direction indicates the direction in which the hand grab is far away from the obstacle.
And repeatedly executing the steps to obtain N projection distances and N projection directions for each datum line in the N datum lines of the polygon. Wherein N is an integer greater than 1.
And determining the minimum value of the N projection distances as the distance between the hand grab and the obstacle, and determining the projection direction corresponding to the minimum value of the N projection distances as the direction between the hand grab and the obstacle.
Optionally, determining a minimum value of distances between the reference point and two end points of the second reference line segment as the projection distance may include:
if the reference point is located on the second reference line segment, determining the minimum value of the distances between the reference point and two end points of the second reference line segment as the projection distance, wherein the projection distance is smaller than 0.
If the reference point is not on the second reference line segment, determining the minimum value of the distances between the reference point and two end points of the second reference line segment as the projection distance, wherein the projection distance is greater than 0.
The following description will be made in detail by taking an example in which the plane geometric model of the hand grip is a circle and the plane geometric model of the obstacle is a regular pentagon. Assuming that the center of the circle is O, the coordinates in the plane coordinate system of the robot arm are (x, y), and the radius is R. Five vertices of the regular pentagon are represented as A, B, C, D and E, and the coordinates of each vertex in the arm plane coordinate system are represented as (x)1,y1),…,(x5,y5). The pentagon has 5 reference lines.
In an example, fig. 4A is a schematic diagram of a relative position relationship between a hand grip and an obstacle according to a third embodiment of the present invention. As shown in fig. 4A, obtaining the distance and the direction of the hand grip with respect to the obstacle according to the plane geometric model of the hand grip, the position information of the plane geometric model of the hand grip, the plane geometric model of the obstacle, and the position information of the plane geometric model of the obstacle may include:
and step 31, determining 1 reference line in the 5 reference lines corresponding to the regular pentagon, specifically, the perpendicular bisector L of the line segment AB.
And 32, projecting the circle center O to the datum line L to obtain a reference point L. Projecting the regular pentagon to the datum line L to obtain a first reference line segment (L)min,lmax)。
Specifically, a point obtained by projecting the circle center O of the circle to the reference line L may be represented as L, and the formula is as follows:
wherein (x)1,y1) And (x)2,y2) The coordinates of vertex a and vertex B, respectively.
Vertex of regular pentagon (x)i,yi) The point projected onto the reference line L can be represented as Li，i＝1,…,5。
The formula is as follows:
will liThe minimum value in (d) is determined as lminIs prepared byiThe maximum value of (1) is determined asmax. The first reference line segment obtained by projecting the regular pentagon to the reference line L can be a plurality of pairs (L)min,lmax) And (4) showing.
Step 34, obtaining a reference point l and a second reference line segment (l)min-R,lmax+ R) between the two end points, respectively: lmin-R-l and lmax+R-l。
For this projection, the projection distance is determined as d ═ min (l)min-R-l,lmax+ R-l), marked d1. End point closer to reference point lIs 1min-R. Since the reference point l is not on the second reference line segment, d1>0. At this time, no collision occurs between the hand grip and the obstacle. Furthermore, the reference point is referenced to the endpoint lminThe direction of R is determined as the projection direction, marked phi1。
Step 35, repeating the steps 31-34 for each reference line in the 5 reference lines of the regular pentagon, and obtaining 5 projection distances (d)1,...,d5) And 5 projection directions (phi)1...,φ5)。
Step 36, determining the minimum value of the 5 projection distances as the distance d between the hand grab and the obstacle is min (d1,...,d5) And determining the projection direction corresponding to the distance d as the direction phi of the hand grab relative to the obstacle.
In another example, fig. 4B is another schematic diagram of a relative position relationship between the hand grip and the obstacle according to the third embodiment of the present invention. As shown in fig. 4B, the distance and the direction of the hand grab relative to the obstacle are obtained according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle, and the position information of the plane geometric model of the obstacle, which can be referred to in the description of steps 31 to 36, and the principles are similar and will not be described herein again. In contrast, in step 34, with respect to the projection of the reference line L, d is determined since the reference point L is located on the second reference line segment1<0. At this time, a collision has occurred between the grip and the obstacle.
The embodiment provides a position determining method of a four-axis mechanical arm. Wherein, the plane geometric model of the hand grasp is one of a circle and a polygon, and the plane geometric model of the obstacle is the other one of a circle and a polygon. The position determining method of the four-axis mechanical arm provided by the embodiment improves the accuracy of the position relation between the hand grab and the obstacle, and improves the calculation efficiency.
The fourth embodiment of the invention also provides a position determination method of the four-axis mechanical arm. On the basis of the first embodiment, the present embodiment specifically provides a specific implementation manner of S103 when the geometric models of the plane of the hand grasp and the obstacle are both polygons.
Specifically, in this embodiment, the step of obtaining the distance and the direction of the hand grab relative to the obstacle according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle, and the position information of the plane geometric model of the obstacle in S103 may include:
and projecting the first polygon to the reference line to obtain a third reference line segment, and projecting the second polygon to the reference line to obtain a fourth reference line segment. Wherein the reference line is a perpendicular bisector of one side of the first polygon or a perpendicular bisector of one side of the second polygon.
And determining the minimum value of the distance between the two end points of the third reference line segment and any two end points of the fourth reference line segment as a projection distance, and determining a projection direction according to the two end points corresponding to the projection distance, wherein the projection direction indicates the direction of the hand grab far away from the obstacle.
And repeatedly executing the steps to obtain P + Q projection distances and P + Q projection directions for each datum line in the P datum lines of the first polygon and the Q datum lines of the second polygon. Wherein P, Q are each integers greater than 1.
And determining the minimum value of the P + Q projection distances as the distance of the hand grab relative to the obstacle, and determining the projection direction corresponding to the minimum value of the P + Q projection distances as the direction of the hand grab relative to the obstacle.
Optionally, determining a minimum value of distances between two end points of the third reference line segment and any two end points of the fourth reference line segment as the projection distance, which may include:
and if the third reference line segment is overlapped with the fourth reference line segment, determining the minimum value of the distances between the two end points of the third reference line segment and any two end points of the fourth reference line segment as the projection distance, wherein the projection distance is less than 0.
And if the third reference line segment is not overlapped with the fourth reference line segment, determining the minimum value of the distances between the two end points of the third reference line segment and any two end points of the fourth reference line segment as the projection distance, wherein the projection distance is greater than 0.
The following is a description by specific examples.
Assume that the geometric model of the hand grasp is a rectangle with the four vertices denoted as A ', B', C 'and D'. The rectangle has 2 datum lines. The planar geometric model of the obstacle is a regular pentagon with five vertices represented as A, B, C, D and E. The pentagon has 5 reference lines.
In an example, fig. 5A is a schematic diagram of a relative position relationship between the hand grip and the obstacle according to the fourth embodiment of the present invention. As shown in fig. 5A, obtaining the distance and the direction of the hand grip with respect to the obstacle according to the plane geometric model of the hand grip, the position information of the plane geometric model of the hand grip, the plane geometric model of the obstacle, and the position information of the plane geometric model of the obstacle may include:
and step 41, determining 1 reference line in the 5 reference lines corresponding to the regular pentagon, specifically, the perpendicular bisector L of the line segment AB.
Step 42, projecting the rectangle to the reference line L to obtain a third reference line segment (L'min,l'max) Projecting the regular pentagon to the datum line L to obtain a fourth reference line segment (L)min,lmax)。
See the description of step 32 in the third embodiment, the principle is similar, and the description is omitted here.
Step 43, obtaining the distance between any two endpoints of the third reference line segment and any two endpoints of the fourth reference line segment, which are respectively: lmin-l'min、lmin-l'max、lmax-l'minAnd lmax-l'max。
For the projection, determining the minimum value of the distance between any two end points of the third reference line segment and the fourth reference line segment as the projection distance, and marking as d1. Two end points nearest to each other are respectively lminAnd l'max. L 'to'maxRelative to the endpoint lminOfThe direction is determined as the projection direction and is marked as phi1。
In this example, the third reference line segment does not overlap the fourth reference line segment, d1>0. At this time, no collision occurs between the hand grip and the obstacle.
Step 44, repeating the above steps 41 to 43 for 2 reference lines of a rectangle and 5 reference lines of a regular pentagon, and obtaining 7 projection distances (d)1,...,d7) And 7 projection directions (phi)1...,φ7)。
And step 45, determining the minimum value in the 7 projection distances as the distance d between the hand grab and the obstacle, and determining the projection direction corresponding to the minimum value in the 7 projection distances as the direction phi between the hand grab and the obstacle.
In another example, fig. 5B is another schematic diagram of the relative position relationship between the hand grip and the obstacle according to the fourth embodiment of the present invention. As shown in fig. 5B, the distance and the direction of the hand grab relative to the obstacle are obtained according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle, and the position information of the plane geometric model of the obstacle, which can be referred to in the description of steps 41 to 45, and the principles are similar and will not be described herein again. In contrast, in step 43, with respect to the projection of the reference line L, d is determined because the third reference line segment overlaps the fourth reference line segment1<0. At this time, a collision has occurred between the grip and the obstacle.
The embodiment provides a position determining method of a four-axis mechanical arm. Wherein, the plane geometric models of the hand grab and the barrier are polygons. The position determining method of the four-axis mechanical arm provided by the embodiment improves the accuracy of the position relation between the hand grab and the obstacle, and improves the calculation efficiency.
Fig. 6 is a schematic structural diagram of a position determination device of a four-axis robot arm according to an embodiment of the present invention. The position determining apparatus of the four-axis robot arm provided in this embodiment is used to execute the position determining method of the four-axis robot arm provided in the embodiment shown in fig. 1 to 5B. As shown in fig. 6, the position determining apparatus for a four-axis robot arm provided in this embodiment may include:
and the acquisition module 21 is used for acquiring joint values of joints on the four-axis mechanical arm.
And the processing module 22 is used for obtaining the position information of the plane geometric model of the hand grab according to the joint value, the positive kinematic model of the four-axis mechanical arm and the plane geometric model of the hand grab on the four-axis mechanical arm.
And obtaining the distance and the direction of the hand grab relative to the obstacle according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle and the position information of the plane geometric model of the obstacle. Wherein the direction indicates a direction of the hand grip away from the obstacle.
Optionally, the processing module 22 is further configured to:
and obtaining the gradient direction of the joint relative to the barrier according to the joint value, the direction of the hand grab relative to the barrier and the inverse kinematics model of the four-axis mechanical arm.
Optionally, the processing module 22 is specifically configured to:
and acquiring pose information of the hand grab according to the joint value and the positive kinematic model, wherein the pose information comprises coordinates of a central point of the hand grab in a plane coordinate system of the mechanical arm and an angle of the hand grab rotating around the central point. The original point of the plane coordinate system of the mechanical arm is the intersection point of the axis of the rotating shaft of the first rotating joint on the four-axis mechanical arm and the horizontal plane.
And obtaining the position information of the plane geometric model of the hand grab according to the pose information of the hand grab and the plane geometric model of the hand grab.
Optionally, the planar geometric model of the hand grip is circular, and the position information of the planar geometric model of the hand grip includes: the coordinate of the central point of the hand grab in the plane coordinate system of the mechanical arm and the radius of the circle.
Optionally, the geometric model of the hand is a polygon, and the processing module 22 is specifically configured to:
and obtaining the coordinates of each vertex of the polygon in the plane coordinate system of the mechanical arm according to the pose information, the coordinates of each vertex of the polygon in the plane coordinate system of the hand grasping plane and the positive kinematics model. Wherein, the origin of the plane coordinate system of the hand grab is the central point of the hand grab.
Optionally, the plane geometric model of the hand grip is a first circle, the plane geometric model of the obstacle is a second circle, and the processing module 22 is specifically configured to:
by passing
Obtaining the distance of the hand grasp relative to the obstacle by
The orientation of the grip relative to the obstacle is obtained.
Wherein (x, y) is the coordinate of the center of the first circle, r is the radius of the first circle, and (x)1,y1) And r1 is the radius of the second circle.
Optionally, the planar geometric model of the hand grip is one of a circle and a polygon, the planar geometric model of the obstacle is the other of a circle and a polygon, and the processing module 22 is specifically configured to:
and projecting the circle center of the circle to the datum line to obtain a reference point, and projecting the polygon to the datum line to obtain a first reference line segment. Wherein the reference line is a perpendicular bisector of one side of the polygon.
And respectively expanding R towards the two ends of the first reference line segment to obtain a second reference line segment. Wherein R is the radius of the circle.
And determining the minimum value of the distance between the reference point and the two end points of the second reference line segment as a projection distance, and determining the direction of the reference point relative to the end point which is closer to the reference point in the two end points as a projection direction, wherein the projection direction indicates the direction in which the hand grab is far away from the obstacle.
And repeatedly executing the steps to obtain N projection distances and N projection directions for each datum line in the N datum lines of the polygon. Wherein N is an integer greater than 1.
And determining the minimum value of the N projection distances as the distance between the hand grab and the obstacle, and determining the projection direction corresponding to the minimum value of the N projection distances as the direction between the hand grab and the obstacle.
Optionally, the processing module 22 is specifically configured to:
if the reference point is located on the second reference line segment, determining the minimum value of the distances between the reference point and two end points of the second reference line segment as the projection distance, wherein the projection distance is smaller than 0.
If the reference point is not on the second reference line segment, determining the minimum value of the distances between the reference point and two end points of the second reference line segment as the projection distance, wherein the projection distance is greater than 0.
Optionally, the geometric model of the hand is a first polygon, the geometric model of the obstacle is a second polygon, and the processing module 22 is specifically configured to:
and projecting the first polygon to the reference line to obtain a third reference line segment, and projecting the second polygon to the reference line to obtain a fourth reference line segment. Wherein the reference line is a perpendicular bisector of one side of the first polygon or a perpendicular bisector of one side of the second polygon.
And determining the minimum value of the distance between the two end points of the third reference line segment and any two end points of the fourth reference line segment as a projection distance, and determining a projection direction according to the two end points corresponding to the projection distance, wherein the projection direction indicates the direction of the hand grab far away from the obstacle.
And repeatedly executing the steps to obtain P + Q projection distances and P + Q projection directions for each datum line in the P datum lines of the first polygon and the Q datum lines of the second polygon. Wherein P, Q are each integers greater than 1.
And determining the minimum value of the P + Q projection distances as the distance of the hand grab relative to the obstacle, and determining the projection direction corresponding to the minimum value of the P + Q projection distances as the direction of the hand grab relative to the obstacle.
Optionally, the processing module 22 is specifically configured to:
and if the third reference line segment is overlapped with the fourth reference line segment, determining the minimum value of the distances between the two end points of the third reference line segment and any two end points of the fourth reference line segment as the projection distance, wherein the projection distance is less than 0.
And if the third reference line segment is not overlapped with the fourth reference line segment, determining the minimum value of the distances between the two end points of the third reference line segment and any two end points of the fourth reference line segment as the projection distance, wherein the projection distance is greater than 0.
The position determining apparatus for a four-axis mechanical arm according to this embodiment is used to execute the position determining method for a four-axis mechanical arm according to the embodiment shown in fig. 1-5B, and its technical principle and technical effect are similar, and are not described herein again.
Fig. 7 is a schematic structural diagram of a position determination device of a four-axis robot arm according to an embodiment of the present invention. As shown in fig. 7, the position determining apparatus of the four-axis robot arm includes a processor 31, a memory 32, and a transceiver 33, where the memory 32 is used to store instructions, the transceiver 33 is used to communicate with other apparatuses, and the processor 31 is used to execute the instructions stored in the memory 32, so that the position determining apparatus of the four-axis robot arm performs the position determining method of the four-axis robot arm provided in the embodiment shown in fig. 1 to 5B, and the specific implementation manner and the technical effect are similar, and are not described again here.
Those of ordinary skill in the art will understand that: all or a portion of the steps of implementing the above-described method embodiments may be performed by hardware associated with program instructions. The program may be stored in a computer-readable storage medium. When executed, the program performs steps comprising the method embodiments described above; and the aforementioned storage medium includes: various media that can store program codes, such as ROM, RAM, magnetic or optical disks.
Finally, it should be noted that: the above embodiments are only used for illustrating the technical solutions of the embodiments of the present invention, and are not limited thereto; although embodiments of the present invention have been described in detail with reference to the foregoing embodiments, those skilled in the art will understand that: the technical solutions described in the foregoing embodiments may still be modified, or some or all of the technical features may be equivalently replaced; and the modifications or the substitutions do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention.
Claims (14)
1. A position determination method of a four-axis mechanical arm is characterized by comprising the following steps:
acquiring joint values of joints on the four-axis mechanical arm;
obtaining position information of the plane geometric model of the hand grab according to the joint value, the positive kinematic model of the four-axis mechanical arm and the plane geometric model of the hand grab on the four-axis mechanical arm;
obtaining the distance and the direction of the hand grab relative to the obstacle according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle and the position information of the plane geometric model of the obstacle; wherein the direction indicates a direction of the hand grip away from the obstacle;
the obtaining of the distance and the direction of the hand grasp relative to the obstacle according to the plane geometric model of the hand grasp, the position information of the plane geometric model of the hand grasp, the plane geometric model of the obstacle, and the position information of the plane geometric model of the obstacle includes:
projecting the circle center of the circle to a datum line to obtain a reference point, and projecting the polygon to the datum line to obtain a first reference line segment; wherein the datum line is a perpendicular bisector of one side of the polygon;
expanding R towards two ends of the first reference line segment respectively to obtain a second reference line segment; wherein R is the radius of the circle;
determining a minimum value of distances between the reference point and two end points of the second reference line segment as a projection distance, and determining a direction of the reference point relative to an end point of the two end points which is closer to the reference point as a projection direction, wherein the projection direction indicates a direction in which the hand grab is far away from the obstacle;
for each datum line in the N datum lines of the polygon, repeatedly executing the steps to obtain N projection distances and N projection directions; wherein N is an integer greater than 1;
determining the minimum value of the N projection distances as the distance of the hand grab relative to the obstacle, and determining the projection direction corresponding to the minimum value of the N projection distances as the direction of the hand grab relative to the obstacle; alternatively, the first and second electrodes may be,
the method for obtaining the distance and the direction of the hand grab relative to the obstacle according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle and the position information of the plane geometric model of the obstacle comprises the following steps:
projecting the first polygon to a reference line to obtain a third reference line segment, and projecting the second polygon to the reference line to obtain a fourth reference line segment; wherein the reference line is a perpendicular bisector of one side of the first polygon or a perpendicular bisector of one side of the second polygon;
determining the minimum value of the distance between two end points of the third reference line segment and any two end points of the fourth reference line segment as a projection distance, and determining a projection direction according to the two end points corresponding to the projection distance, wherein the projection direction indicates the direction in which the gripper is far away from the obstacle;
repeatedly executing the steps to obtain P + Q projection distances and P + Q projection directions for each datum line in the P datum lines of the first polygon and the Q datum lines of the second polygon; wherein P, Q are integers greater than 1;
and determining the minimum value of the P + Q projection distances as the distance of the hand grab relative to the obstacle, and determining the projection direction corresponding to the minimum value of the P + Q projection distances as the direction of the hand grab relative to the obstacle.
2. The method of claim 1, further comprising:
and obtaining the gradient direction of the joint relative to the obstacle according to the joint value, the direction of the hand grab relative to the obstacle and an inverse kinematics model of the four-axis mechanical arm.
3. The method of claim 1, wherein obtaining position information for the planar geometric model of the hand grasp from the joint values, the positive kinematic model of the four-axis robotic arm, and the planar geometric model of the hand grasp on the four-axis robotic arm comprises:
obtaining pose information of the hand grab according to the joint values and the positive kinematic model, wherein the pose information comprises coordinates of a central point of the hand grab in a plane coordinate system of a mechanical arm and an angle of the hand grab rotating around the central point; the origin of the plane coordinate system of the mechanical arm is the intersection point of the axis of the rotating shaft of the first rotating joint on the four-axis mechanical arm and the horizontal plane;
and obtaining the position information of the plane geometric model of the hand grab according to the pose information of the hand grab and the plane geometric model of the hand grab.
4. The method of claim 3, wherein the planar geometric model of the hand grip is a circle, and the position information of the planar geometric model of the hand grip comprises: the coordinates of the center point of the hand grip in the robot arm plane coordinate system and the radius of the circle.
5. The method according to claim 3, wherein the planar geometric model of the hand grasp is a polygon, and the obtaining the position information of the planar geometric model of the hand grasp according to the pose information of the hand grasp and the planar geometric model of the hand grasp comprises:
obtaining the coordinates of each vertex of the polygon in the mechanical arm plane coordinate system according to the pose information, the coordinates of each vertex of the polygon in the grabbing plane coordinate system and the positive kinematics model; and the origin of the plane coordinate system of the hand grab is the central point of the hand grab.
6. The method of any one of claims 1-5, wherein determining the minimum of the distances between the reference point and the two end points of the second reference line segment as the projection distance comprises:
if the reference point is located on the second reference line segment, determining the minimum value of the distance between the reference point and two end points of the second reference line segment as a projection distance, wherein the projection distance is smaller than 0;
if the reference point is not on the second reference line segment, determining the minimum value of the distance between the reference point and two end points of the second reference line segment as a projection distance, wherein the projection distance is greater than 0.
7. The method of any one of claims 1-5, wherein determining the minimum of the distances between the two endpoints of the third reference line segment and any two endpoints of the fourth reference line segment as the projection distance comprises:
if the third reference line segment is overlapped with the fourth reference line segment, determining the minimum value of the distances between two end points of the third reference line segment and any two end points of the fourth reference line segment as a projection distance, wherein the projection distance is smaller than 0;
if the third reference line segment is not overlapped with the fourth reference line segment, determining the minimum value of the distances between two end points of the third reference line segment and any two end points of the fourth reference line segment as a projection distance, wherein the projection distance is greater than 0.
8. A position determination device of four-axis arm, characterized by includes:
the acquisition module is used for acquiring joint values of joints on the four-axis mechanical arm;
the processing module is used for acquiring position information of the plane geometric model of the hand grab according to the joint value, the positive kinematic model of the four-axis mechanical arm and the plane geometric model of the hand grab on the four-axis mechanical arm;
obtaining the distance and the direction of the hand grab relative to the obstacle according to the plane geometric model of the hand grab, the position information of the plane geometric model of the hand grab, the plane geometric model of the obstacle and the position information of the plane geometric model of the obstacle; wherein the direction indicates a direction of the hand grip away from the obstacle;
the plane geometric model of the hand grip is one of a circle and a polygon, the plane geometric model of the obstacle is the other of the circle and the polygon, and the processing module is specifically configured to:
projecting the circle center of the circle to a datum line to obtain a reference point, and projecting the polygon to the datum line to obtain a first reference line segment; wherein the datum line is a perpendicular bisector of one side of the polygon;
expanding R towards two ends of the first reference line segment respectively to obtain a second reference line segment; wherein R is the radius of the circle;
determining a minimum value of distances between the reference point and two end points of the second reference line segment as a projection distance, and determining a direction of the reference point relative to an end point of the two end points which is closer to the reference point as a projection direction, wherein the projection direction indicates a direction in which the hand grab is far away from the obstacle;
for each datum line in the N datum lines of the polygon, repeatedly executing the steps to obtain N projection distances and N projection directions; wherein N is an integer greater than 1;
determining the minimum value of the N projection distances as the distance of the hand grab relative to the obstacle, and determining the projection direction corresponding to the minimum value of the N projection distances as the direction of the hand grab relative to the obstacle; alternatively, the first and second electrodes may be,
the plane geometric model of the hand grab is a first polygon, the plane geometric model of the obstacle is a second polygon, and the processing module is specifically configured to:
projecting the first polygon to a reference line to obtain a third reference line segment, and projecting the second polygon to the reference line to obtain a fourth reference line segment; wherein the reference line is a perpendicular bisector of one side of the first polygon or a perpendicular bisector of one side of the second polygon;
determining the minimum value of the distance between two end points of the third reference line segment and any two end points of the fourth reference line segment as a projection distance, and determining a projection direction according to the two end points corresponding to the projection distance, wherein the projection direction indicates the direction in which the gripper is far away from the obstacle;
repeatedly executing the steps to obtain P + Q projection distances and P + Q projection directions for each datum line in the P datum lines of the first polygon and the Q datum lines of the second polygon; wherein P, Q are integers greater than 1;
and determining the minimum value of the P + Q projection distances as the distance of the hand grab relative to the obstacle, and determining the projection direction corresponding to the minimum value of the P + Q projection distances as the direction of the hand grab relative to the obstacle.
9. The apparatus of claim 8, wherein the processing module is further configured to:
and obtaining the gradient direction of the joint relative to the obstacle according to the joint value, the direction of the hand grab relative to the obstacle and an inverse kinematics model of the four-axis mechanical arm.
10. The apparatus of claim 8, wherein the processing module is specifically configured to:
obtaining pose information of the hand grab according to the joint values and the positive kinematic model, wherein the pose information comprises coordinates of a central point of the hand grab in a plane coordinate system of a mechanical arm and an angle of the hand grab rotating around the central point; the origin of the plane coordinate system of the mechanical arm is the intersection point of the axis of the rotating shaft of the first rotating joint on the four-axis mechanical arm and the horizontal plane;
and obtaining the position information of the plane geometric model of the hand grab according to the pose information of the hand grab and the plane geometric model of the hand grab.
11. The apparatus of claim 10, wherein the planar geometric model of the hand grip is a circle, and the position information of the planar geometric model of the hand grip comprises: the coordinates of the center point of the hand grip in the robot arm plane coordinate system and the radius of the circle.
12. The apparatus of claim 10, wherein the geometric model of the hand grasp is a polygon, and the processing module is specifically configured to:
obtaining the coordinates of each vertex of the polygon in the mechanical arm plane coordinate system according to the pose information, the coordinates of each vertex of the polygon in the grabbing plane coordinate system and the positive kinematics model; and the origin of the plane coordinate system of the hand grab is the central point of the hand grab.
13. The apparatus according to any one of claims 8 to 12, wherein the processing module is specifically configured to:
if the reference point is located on the second reference line segment, determining the minimum value of the distance between the reference point and two end points of the second reference line segment as a projection distance, wherein the projection distance is smaller than 0;
if the reference point is not on the second reference line segment, determining the minimum value of the distance between the reference point and two end points of the second reference line segment as a projection distance, wherein the projection distance is greater than 0.
14. The apparatus according to any one of claims 8 to 12, wherein the processing module is specifically configured to:
if the third reference line segment is overlapped with the fourth reference line segment, determining the minimum value of the distances between two end points of the third reference line segment and any two end points of the fourth reference line segment as a projection distance, wherein the projection distance is smaller than 0;
if the third reference line segment is not overlapped with the fourth reference line segment, determining the minimum value of the distances between two end points of the third reference line segment and any two end points of the fourth reference line segment as a projection distance, wherein the projection distance is greater than 0.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810691166.0A CN108858162B (en) | 2018-06-28 | 2018-06-28 | Position determination method and device for four-axis mechanical arm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810691166.0A CN108858162B (en) | 2018-06-28 | 2018-06-28 | Position determination method and device for four-axis mechanical arm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108858162A CN108858162A (en) | 2018-11-23 |
| CN108858162B true CN108858162B (en) | 2020-10-30 |
Family
ID=64296691
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810691166.0A Active CN108858162B (en) | 2018-06-28 | 2018-06-28 | Position determination method and device for four-axis mechanical arm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108858162B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111958590B (en) * | 2020-07-20 | 2021-09-28 | 佛山科学技术学院 | Mechanical arm anti-collision method and system in complex three-dimensional environment |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5347459A (en) * | 1993-03-17 | 1994-09-13 | National Research Council Of Canada | Real time collision detection |
| KR20080075051A (en) * | 2007-02-10 | 2008-08-14 | 삼성전자주식회사 | Robot cleaner and control method thereof |
| CN104969134B (en) * | 2013-02-06 | 2017-06-09 | 三菱电机株式会社 | Interference checking device |
| CN104260108A (en) * | 2014-09-09 | 2015-01-07 | 上海浩淼自动化设备有限公司 | SCARA type mechanical arm |
| US10350756B2 (en) * | 2014-12-25 | 2019-07-16 | Kawasaki Jukogyo Kabushiki Kaisha | Method of automatically avoiding obstacle for arm robot and control device |
| CN107214701B (en) * | 2017-06-12 | 2019-10-18 | 南京理工大学 | A kind of livewire work mechanical arm automatic obstacle avoiding paths planning method based on movement primitive library |
-
2018
- 2018-06-28 CN CN201810691166.0A patent/CN108858162B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN108858162A (en) | 2018-11-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9827675B2 (en) | Collision avoidance method, control device, and program | |
| US11358282B2 (en) | System and method for constraint management of one or more robots | |
| JP5114019B2 (en) | Method for controlling the trajectory of an effector | |
| US8560122B2 (en) | Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same | |
| US9043030B2 (en) | Manipulator and path generation method thereof | |
| Cruciani et al. | Dexterous manipulation graphs | |
| JP5580850B2 (en) | Fast grip contact calculation for serial robots | |
| JP2012223845A (en) | Method and apparatus for predicting interference between target section of robot and peripheral object | |
| JP5250858B2 (en) | Multiple robot collision detection method and robot apparatus | |
| JPH09265313A (en) | Speed arithmetic device for robot and speed arithmetic method for robot | |
| JP2019084649A (en) | Interference determination method, interference determination system, and computer program | |
| US20220402127A9 (en) | Multi-angle end effector | |
| CN111216136A (en) | Multi-degree-of-freedom mechanical arm control system, method, storage medium and computer | |
| CN108858162B (en) | Position determination method and device for four-axis mechanical arm | |
| Cruciani et al. | Dual-arm in-hand manipulation and regrasping using dexterous manipulation graphs | |
| JP6515828B2 (en) | Interference avoidance method | |
| Zhang et al. | Kuka youBot arm shortest path planning based on geodesics | |
| Kansal et al. | Kinematic and dynamic analysis of a dexterous multi-fingered delta robot for object catching | |
| KR20220099535A (en) | Inverse kinematics solver for wrist offset robots | |
| JP2011245614A5 (en) | ||
| Borangiu et al. | Constraints-based motion planning for an automatic, flexible laser scanning robotized platform | |
| Dobashi et al. | Geometrical Solution of Finger Arrangement Problem onto Target Part toward Design of Chuck-type Hand with Three Parallel Stick Fingers | |
| WO2022162969A1 (en) | Robot system, movement path generation device and movement path generation method | |
| US20230311307A1 (en) | System, method, and program for generating three-dimension model | |
| Tittel | Analytical solution for the inverse kinematics problem of the franka emika panda seven-dof light-weight robot arm |
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 |