CN114872053A - Planning method and device for effective working space of end tool and storage medium - Google Patents
Planning method and device for effective working space of end tool and storage medium Download PDFInfo
- Publication number
- CN114872053A CN114872053A CN202210706510.5A CN202210706510A CN114872053A CN 114872053 A CN114872053 A CN 114872053A CN 202210706510 A CN202210706510 A CN 202210706510A CN 114872053 A CN114872053 A CN 114872053A
- Authority
- CN
- China
- Prior art keywords
- coordinate
- coordinate axis
- axis
- maximum
- value
- 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.)
- Granted
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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Numerical Control (AREA)
Abstract
The application relates to a planning method, a device and a storage medium of an effective working space of a terminal tool, wherein the method comprises the following steps: setting limit values of a first coordinate axis in a base coordinate system as design variables of an effective working space, wherein the limit values comprise a maximum limit value and a minimum limit value; determining a non-linear equation for the effective workspace based on a constraint, wherein the constraint comprises a set of spatial vectors of extreme orientations of the end tool, and determining an objective function for the design variable from the non-linear equation; points are taken at equal intervals on a plane formed by a second coordinate axis and a third coordinate axis in the base coordinate system, and the maximum value and the minimum value on the first coordinate axis, which can be reached by the end tool corresponding to each point on the plane, are calculated through an optimization objective function; and determining the maximum effective working space according to the maximum value and the minimum value corresponding to all the points on the plane. The invention realizes the maximum effective working space planning when the tail end tool and the tail end of the mechanical arm are not coaxial.
Description
Technical Field
The application mainly relates to the field of industrial robots, in particular to a method and a device for planning an effective working space of a terminal tool and a storage medium.
Background
In a practical application scenario of the robot arm, there is often a certain constraint requirement on the orientation of the end tool, and taking a 6-axis surgical robot arm as an example, the orientation of the z axis of the end tool (such as a syringe needle, hereinafter referred to as a needle) is limited according to the requirements of the surgery, but the orientations of the x axis and the y axis are not limited, that is, the needle can rotate 360 ° around the z axis. The spatial extent to which the tool center point can be reached with the workpiece oriented is defined as the effective working space. Therefore, the effective working space of the needle head needs to be calculated when the mechanical arm structure is designed, and the effective space range which can be reached by the needle head is obtained while all required orientations of the needle head under the design working condition are met.
CN111844027A discloses a method and an apparatus for determining an optimal working space of a robot arm. The method comprises the following steps: establishing a kinematics equation of the target mechanical arm according to the characteristic parameter table of the target mechanical arm, and acquiring a Jacobian matrix of the target mechanical arm; acquiring a plurality of parameter groups to be determined according to the characteristic parameter table; respectively acquiring a terminal Cartesian space corresponding to each parameter group to be determined and a Jacobian matrix condition number of the terminal Cartesian space according to a kinematic equation, a Jacobian matrix and the parameter groups to be determined; and determining the optimal working space of the target mechanical arm according to the Jacobian matrix condition numbers of the end Cartesian space and the tail end Cartesian space. The method only considers the limitation of mechanical structure parameters of the mechanical arm, and does not consider the orientation constraint of the end tool, so that the method cannot be applied to a scene with the orientation requirement on the end tool.
Therefore, a method, an apparatus and a storage medium for planning an effective working space of an end tool are needed.
Content of application
The technical problem to be solved by the application is to provide a method, a device and a storage medium for planning an effective working space of a terminal tool, and solve the problem that the existing working space determining method cannot meet the scene that the terminal tool has orientation requirements.
In order to solve the above technical problem, the present application provides a method for planning an effective working space of an end tool, including: setting limit values of a first coordinate axis in a base coordinate system as design variables of an effective working space, wherein the limit values comprise a maximum limit value and a minimum limit value; determining a non-linear equation for the effective workspace based on a constraint, wherein the constraint comprises a set of spatial vectors of extreme orientations of an end tool, and determining an objective function for the design variable from the non-linear equation; points are taken at equal intervals on a plane formed by a second coordinate axis and a third coordinate axis in a base coordinate system, and the maximum value and the minimum value on the first coordinate axis which can be reached by the end tool corresponding to each point on the plane are calculated by optimizing the objective function; and determining the maximum effective working space according to the maximum value and the minimum value corresponding to all the points on the plane.
In an embodiment of the application, the constraints further comprise structural parameters of the robot arm and end-tool parameters.
In an embodiment of the present application, the non-linear equation is:
wherein, T tool As a coordinate transformation matrix from the base coordinate system to the tool coordinate system, F (-) is the kinematics positive solution function of the robot arm, q is the robot arm axis angle vector, tool is the end tool parameter, [ z ] 1 ，z 2 ，z 3 ]For projection of the tip orientation of the tip tool under a base coordinate system, [ x ] q ，y q ，z q ]The coordinate under the base coordinate system is determined according to the axial angle vector of the mechanical arm.
In an embodiment of the present application, the objective function is:
wherein min (x) is the minimum value of the first coordinate axis, max (x) is the maximum value of the first coordinate axis, f (·) is a function of the value of the first coordinate axis and the angular vector of the mechanical arm axis, q (·) a Is the optimal mechanical arm axial angle vector of the minimum value corresponding to the first coordinate axis, q b Is the optimal arm axis angle vector corresponding to the maximum value of the first coordinate axis.
In an embodiment of the present application, the step of calculating, by optimizing the objective function, a maximum value and a minimum value on a first coordinate axis that can be reached by the end tool corresponding to each point on the plane includes: optimizing the objective function by sequential quadratic programming to obtain a group of optimal mechanical arm axial angle vectors corresponding to each point according to the KKT condition of the objective function by using the following equality constraint and inequality constraint;
q min ＜q＜q max
wherein [ z ] 1 ，z 2 ，z 3 ]For the projection of the tip orientation of the tip tool under the base coordinate system, [ z ] n1 ，z n2 ，z n3 ]For the projection of the end of the nth set of constraints towards the base coordinate system, y q And z q The coordinate on the second coordinate axis and the coordinate on the third coordinate axis are determined according to the angular vector of the mechanical arm axis, y and z are the coordinate of the point on the plane on the second coordinate axis and the coordinate on the third coordinate axis, q min Vector q formed by the minimum reachable angle of the mechanical arm joint max The vector is formed by the maximum reachable angle of the mechanical arm joint, and q is the axial angle vector of the mechanical arm;
and taking one of the values of the group of first coordinate axes corresponding to the group of optimal mechanical arm axis angle vectors as the maximum value, and taking the other value as the minimum value.
In an embodiment of the present application, the step of taking points at equal intervals on a plane formed by the second coordinate axis and the third coordinate axis in the base coordinate system includes: determining second coordinates of each point in a first step increment from the origin on the second coordinate axis; and determining a third coordinate of each point by increasing the second step length from the origin on the third coordinate axis until the difference value between the maximum value and the minimum value of the first coordinate axis corresponding to the points determined by the second coordinate and the third coordinate is less than a first threshold value, and then continuously determining the third coordinate of each point by decreasing the second step length from the origin on the third coordinate axis.
In an embodiment of the present application, the method for planning the effective workspace of the end tool further includes: and determining a first working range required by the end tool in the second coordinate axis and a second working range required by the end tool in the third coordinate axis according to working condition requirements, and taking points at equal intervals on a plane formed by the second coordinate axis and the third coordinate axis in a base coordinate system according to the first working range and the second working range.
In an embodiment of the present application, the step of taking points at equal intervals according to the first working range and the second working range on a plane formed by a second coordinate axis and a third coordinate axis in a base coordinate system includes: determining a second coordinate of each point in a first step increment mode in the first working range from an origin to one half on the second coordinate axis, wherein the effective working space is symmetrical about the origin on the second coordinate axis; and determining the third coordinate of each point by taking the second working range as a step length from the origin on the third coordinate axis in an increasing manner until the difference value between the maximum value and the minimum value of the first coordinate axis corresponding to the point determined by the second coordinate and the third coordinate is less than a first threshold value, and then determining the third coordinate of each point by taking the second working range as a step length from the origin on the third coordinate axis in a decreasing manner.
In an embodiment of the present application, the step of determining the maximum effective working space according to the maximum value and the minimum value corresponding to all the points on the plane includes: taking the minimum value from the maximum values corresponding to all the points on the plane as the maximum limit value, and taking the maximum value from the minimum values corresponding to all the points on the plane as the minimum limit value; and determining the maximum effective working space according to the limit maximum value and the limit minimum value.
In an embodiment of the present application, the step of obtaining the maximum effective working space according to the limit maximum value and the limit minimum value includes: and generating a maximum limit curve surface corresponding to a plane formed by the second coordinate axis and the third coordinate axis, and generating a minimum limit curve surface corresponding to a plane formed by the second coordinate axis and the third coordinate axis to obtain a maximum effective working space.
In an embodiment of the present application, the method for planning the effective workspace of the end tool further includes: and judging whether the maximum effective working space is larger than the required working space, if not, adjusting the structural parameters of the mechanical arm and reacquiring the maximum effective working space.
In order to solve the above technical problem, the present application provides a planning apparatus for an effective working space of an end tool, including: a memory for storing instructions executable by the processor; a processor for executing the instructions to implement the method for planning an effective workspace of an end tool as described above.
In order to solve the above technical problem, the present application provides a computer readable medium storing computer program code, which when executed by a processor implements the method for planning an effective workspace of an end tool as described above.
The method for planning the effective working space of the end tool plans the maximum effective working space by setting the limit value of the first coordinate axis as the design variable of the effective working space and taking the space vector set of the end tool in the limit direction as the constraint condition, thereby realizing the planning of the maximum effective working space under the condition that the end tool and the tail end of the mechanical arm are not coaxial, and having wide application scenes; compared with the conventional Monte Carlo method for randomly taking points in a working space, the method has the advantages that after the orientation requirement of the end tool is given, the point on the plane only needs to take the coordinates of two coordinate axes in the base coordinate system, and the calculation amount is greatly reduced.
Drawings
In order to make the aforementioned objects, features and advantages of the present application more comprehensible, embodiments thereof are described in detail below with reference to the accompanying drawings, in which:
FIG. 1 is a schematic diagram of a robotic arm and end tool configuration according to one embodiment of the present invention;
FIG. 2 is a flow chart illustrating a method for planning an effective workspace of an end tool according to an embodiment of the invention;
fig. 3 is a system block diagram of an apparatus for planning an effective workspace of an end tool according to an embodiment of the present application.
Detailed Description
In order to make the aforementioned objects, features and advantages of the present application more comprehensible, embodiments accompanying the present application are described in detail below.
In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present application, but the present application may be practiced in other ways than those described herein and thus is not limited to the specific embodiments disclosed below.
Flow charts are used herein to illustrate operations performed by systems according to embodiments of the present application. It should be understood that the preceding or following operations are not necessarily performed in the exact order in which they are performed. Rather, various steps may be processed in reverse order or simultaneously. Meanwhile, other operations are added to or removed from these processes.
FIG. 1 is a schematic diagram of a robotic arm and end-tool configuration in accordance with an embodiment of the present invention. Figure 1 shows two different poses of the robotic arm. As shown in fig. 1, the robotic arm includes a base 101, a plurality of axes 102, a plurality of joint angles 103, a wrist joint 104, and a robotic arm tip 105. End tool 20 is mounted on robot arm end 105 by tool link 30. The end tool 20 includes a tool center point 201. In this embodiment, the end tool 20 is a needle. The base 101 of the robot arm is fixed with the center point of the base 101 as the origin and the central axis of the base passing through the origin and perpendicular to the surface of the base 101 as Z B Axis, to establish X parallel to the surface of the base 101 B Axis and Y B Shaft, wherein X B Axis and Y B The axes are mutually perpendicular, and a base coordinate system B is obtained. The tool center point 201 of the end tool 20 is set as the origin, and a straight line passing through the tool center point 201 and coinciding with the tool axis is set as Z T Axis, X T Axis and Y T The axis only needs to be guaranteed to be Z T The axes are perpendicular to each other two by two to obtain the tool coordinate system T. When the tool link 30 is attached to the end of the arm 105 at a fixed break angle, the end of the arm 105 is oriented at a fixed angle rather than collinear with the end tool 20. Thus, when the end tool 20 is not translated and is about Z T As the shaft rotates, the wrist joint 104 will rotate in space in one revolution along the dotted line, i.e., the robotic arm can assume an infinite number of poses for the same orientation of the end tool 20. There is a need for a method to determine the position of wrist joint 104 that meets the orientation requirements of end tool 20 while planning the maximum available working space as possible.
FIG. 2 is a flow chart of a method for planning an effective workspace of an end tool according to an embodiment of the invention. As shown in fig. 2, a method 200 for planning an effective workspace of an end tool comprises the steps of:
step S201: and setting limit values of the first coordinate axis in the base coordinate system as design variables of the effective working space, wherein the limit values comprise a limit maximum value and a limit minimum value.
In an embodiment of the invention, X of the base coordinate system is chosen B The axis being a first coordinate axis, Y of a base coordinate system B Axis and Z B The axes are the second and third coordinate axes, respectively. The coordinates of the tool center point of the end tool under the base coordinate system may be represented as (x, y, z). Given the y and z coordinates, it is desirable to plan the effective workspace of the end tool with the tool center point at X B A limit maximum value Xmax and a limit minimum value Xmin on the axis.
In some embodiments, Y may be selected B Axis or Z B The axis is a first coordinate axis, and the remaining two coordinate axes are a second coordinate axis and a third coordinate axis, respectively. Correspondingly, the limit value of the axis as the first coordinate axis is selected as the effective working spaceAnd (6) counting variables.
Step S202: the method includes determining a non-linear equation for an effective workspace based on a constraint, and determining an objective function for a design variable from the non-linear equation, wherein the constraint includes a set of spatial vectors for extreme orientations of the end tool.
In an embodiment of the present invention, the nonlinear equation of the effective workspace determined based on the constraint is:
wherein, T tool As a coordinate transformation matrix from the base coordinate system to the tool coordinate system, F (-) is the kinematics positive solution function of the robot arm, q is the robot arm axis angle vector, tool is the end tool parameter, [ z ] 1 ，z 2 ，z 3 ]For projection of the tip orientation of the tip tool under the base coordinate system, [ x ] q ，y q ，z q ]The coordinate under the base coordinate system is determined according to the axial angle vector of the mechanical arm.
In an embodiment of the invention, the constraints comprise a set of spatial vectors of extreme orientations of the end-tool. The set of extreme orientation spatial vectors of the end tool refers to the set of extreme orientation spatial vectors that the tool center point needs to reach at all positions within the workspace. The set of spatial vectors for the extreme orientations of the end-tool can be expressed as:
where A is the set of spatial vectors for the extreme orientations of the end tool, [ z ] 11 ，z 12 ，z 13 ]Represents the projection of the end orientation of the 1 st set of constraints under the base coordinate system, [ z ] n1 ，z n2 ，z n3 ]Representing the projection of the end of the nth set of constraints towards the base coordinate system.
In some embodiments, the constraints further include structural parameters of the robotic arm and end-tool parameters. The structural parameters of the mechanical arm include the mechanicalArm DH parameters (Denavit Hartenberg parameters) and the limits of the arm joint angles. The DH parameter is a mechanical arm mathematical model and a coordinate system determination system which use four parameters to express the position angle relationship between two pairs of joint connecting rods. The limit range of each joint angle of the mechanical arm can be expressed as:
wherein
And
the vector formed by the minimum reachable angle and the vector formed by the maximum reachable angle of the mechanical arm joint are respectively corresponding, and the number of elements in the vectors is consistent with that of the mechanical arm joints. End tool parameters include, but are not limited to, the tool coordinate system Z T The angle between the axis and the z-axis of the coordinate system at the end of the robot arm and the position of the tool center point relative to the end of the robot arm.
In step S202, for a non-linear equation, given a y-coordinate and a z-coordinate, a maximum x-value and a minimum x-value are obtained, which may be translated to solve the following objective function:
where min (x) is the minimum value of the first coordinate axis, max (x) is the maximum value of the first coordinate axis, f (-) is a sub-function of the kinematics positive solution function of the robot arm, and f (-) can also be expressed as x (q), which describes the relationship between x and q. q. q of a Is the optimal mechanical arm axial angle vector of the minimum value corresponding to the first coordinate axis, q b Is the optimal arm axis angle vector corresponding to the maximum value of the first coordinate axis.
In an embodiment of the present invention, taking a six-axis robot as an example, the DH parameters used are shown in table 1:
TABLE 1
Then according to the six-axis angle and the tool coordinate system Z T The angle theta between the axis and the z-axis of the coordinate system of the end of the robot arm and the position of the center point of the tool with respect to the end of the robot arm [ dx, dy, dz ]]The formula for the function x (q) can be found:
x(q)＝cos(q1)/10+(3*cos(q1)*cos(q2))/25+conj(dx)*(sin(q6)*(cos(q4)*sin(q1)+sin(q4)*(cos(q1)*sin(q2)*sin(q3+pi/2)-cos(q1)*cos(q2)*cos(q3+pi/2)))+cos(q6)*(cos(q5)*(sin(q1)*sin(q4)-cos(q4)*(cos(q1)*sin(q2)*sin(q3+pi/2)-cos(q1)*cos(q2)*cos(q3+pi/2)))-sin(q5)*(cos(q1)*cos(q2)*sin(q3+pi/2)+cos(q1)*cos(q3+pi/2)*sin(q2))))+(sin(q5)*(sin(q1)*sin(q4)-cos(q4)*(cos(q1)*sin(q2)*sin(q3+pi/2)-cos(q1)*cos(q2)*cos(q3+pi/2))))/10+conj(dz)*(sin(q5)*(sin(q1)*sin(q4)-cos(q4)*(cos(q1)*sin(q2)*sin(q3+pi/2)-cos(q1)*cos(q2)*cos(q3+pi/2)))+cos(q5)*(cos(q1)*cos(q2)*sin(q3+pi/2)+cos(q1)*cos(q3+pi/2)*sin(q2)))+(cos(q5)*(cos(q1)*cos(q2)*sin(q3+pi/2)+cos(q1)*cos(q3+pi/2)*sin(q2)))/10+(4*cos(q1)*cos(q2)*sin(q3+pi/2))/25+(4*cos(q1)*cos(q3+pi/2)*sin(q2))/25。
step S203: and (3) equally spacing points on a plane formed by a second coordinate axis and a third coordinate axis in the base coordinate system, and calculating the maximum value and the minimum value on the first coordinate axis, which can be reached by the end tool corresponding to each point on the plane, through an optimization objective function.
And taking points at equal intervals on a plane formed by the second coordinate axis and the third coordinate axis in the base coordinate system, wherein each point comprises a y coordinate and a z coordinate. For Y B Z B Each point on the plane needs to be respectively paired with min (x (q) a ) And min (1/x (q)) b ) These two functions are solved optimally.
In some embodiments, the step of calculating the maximum value and the minimum value on the first coordinate axis that can be reached by the end tool corresponding to each point on the plane through the optimization objective function comprises:
optimizing the objective function by Sequential Quadratic Programming (SQP) to obtain a set of optimal mechanical arm axial angle vectors q corresponding to each point according to KKT Conditions (KKT Conditions) of the objective function by using following equality constraints and inequality constraints a And q is b ；
q min ＜q a ＜q max
q min ＜q b ＜q max
Wherein [ z ] 1 ，z 2 ，z 3 ]For projection of the tip orientation of the tip tool under the base coordinate system, [ z ] n1 ，z n2 ，z n3 ]For projection of the end of the n-th set of constraints towards the base coordinate system, y q And z q The coordinate on the second coordinate axis and the coordinate on the third coordinate axis are determined according to the angular vector of the mechanical arm axis, y and z are the coordinate of a point on the plane on the second coordinate axis and the coordinate on the third coordinate axis, q min Vector q formed by the minimum reachable angle of the mechanical arm joint max Vector q formed by the maximum reachable angle of the mechanical arm joint a Is one of the optimal mechanical arm axial angle vectors, q b Is another optimum mechanical arm axis angle vector.
Obtaining the optimal mechanical arm axial angle vector q a Corresponding x (q) a ) The value of (a) is the minimum value of the first coordinate axis corresponding to the point, and the optimal mechanical arm axis angle vector q is taken b Corresponding 1/x (q) b ) Is the maximum value of the first coordinate axis corresponding to the point. Since the extreme orientation spatial vector set A of the end tool has n sets of limited end orientations, the matrix is transformed according to the coordinates of the robot arm to the end of the workpiece
It can be seen that when the spatial vector set of extreme orientations of the end tool has n sets of limited end orientations, it is necessary to substitute T for the end orientation of each set of limits in the spatial vector set a of extreme orientations of the end tool (i.e. each row vector in the matrix a) tool Z of (a) 1 ，z 2 ，z 3 And obtaining n groups of xmax and xmin, wherein each point corresponds to the maximum value and the minimum value on the n groups of first coordinate axes.
In some embodiments, the step of taking points at equal intervals on a plane formed by the second coordinate axis and the third coordinate axis in the base coordinate system comprises:
first, second coordinates of respective points are determined in increments of a first step from the origin on the second coordinate axis. With Y B Axis and Z B The axes are respectively a second coordinate axis and a third coordinate axis, the first step length is 0.01 as an example, and the y coordinate of each point is 0,0.01, 0.02. And then, determining a third coordinate of each point by increasing the second step length from the origin on a third coordinate axis, wherein the z coordinate of each point is 0,0.1,0.2 and. Until the difference value between the maximum value and the minimum value of the first coordinate axis corresponding to the point determined by the second coordinate and the third coordinate is smaller than the first threshold value, namely, the minimum value xmin of the x coordinate and the maximum value xmax of the x coordinate corresponding to the point determined by the y coordinate and the z coordinate are calculated through the optimization objective function. And if the difference value of xmax-xmin is smaller than the first threshold value, continuously determining the third coordinate of each point on the third coordinate axis by the second step length decreasing from the origin, namely the z coordinate of each point is 0, -0.1, -0.2 in sequence.
In some embodiments, the method for planning the effective workspace of the end tool further comprises: and determining a first working range required by the end tool in the second coordinate axis and a second working range required by the end tool in the third coordinate axis according to working condition requirements, and equally spacing points according to the first working range and the second working range on a plane formed by the second coordinate axis and the third coordinate axis in the base coordinate system. For example, a needle having a working range of 0.2m (in meters) in the second axis and 0.1m in the third axis is required.
In some embodiments, the step of taking points at equal intervals on a plane composed of the second coordinate axis and the third coordinate axis in the base coordinate system according to the first working range and the second working range comprises:
determining a second coordinate of each point in a first working range from an origin to one half on a second coordinate axis in an increasing mode by a first step length, wherein the effective working space is symmetrical about the origin on the second coordinate axis; and gradually increasing and determining the third coordinate of each point on the third coordinate axis by taking the second working range as a step length from the origin point until the difference value between the maximum value and the minimum value of the first coordinate axis corresponding to the point determined by the second coordinate and the third coordinate is less than a first threshold value, and continuously determining the third coordinate of each point on the third coordinate axis by gradually decreasing by taking the second working range as a step length from the origin point. Continue with Y B The axis being a second coordinate axis, Z B The axis is the third coordinate axis, for example, in the base coordinate system, the effective working space of the end tool is at Y due to the symmetry of the mechanical structure B The axis is symmetrical about the origin, so only half of the first working range is needed to be calculated, i.e. the working range of the y coordinate should satisfy 0-0.1 m in this embodiment. Taking the first step size of 0.01 as an example, the y coordinate of each point is 0,0.01,0.02, …, and 0.1 in sequence. In this embodiment, the working range of the Z coordinate should be no less than the second working range (i.e., 0.1m), while the effective working space is in Z B Can be translated in the axial direction, so it is necessary to be in Z B Searching a continuous interval with the length of 0.1m on the axis, and along the X in the interval B The movable range in the axial direction is larger than other sections. Extensive searching and calculation is therefore required. Firstly, the z coordinates of each point are 0,0.1,0.2, … and 0.5 in sequence, and when the difference value between the minimum value xmin of the x coordinate and the maximum value xmax of the x coordinate corresponding to the point determined by the y coordinate and the z coordinate is less than a first threshold value, the z coordinates of each point are continuously taken to be 0, -0.1, -0.2, … and-0.5 in sequence.
In some embodiments, structural features of the robotic arm may be incorporated when the end tool is in Z B When the range of motion of the axis in the negative direction is wider, the third coordinates of each point can be directly selected to be 0, -0.1, -0.2, …, -0.5 in sequence.
Step S204: and determining the maximum effective working space according to the maximum value on the first coordinate axis and the minimum value on the first coordinate axis corresponding to all the points on the plane. In this embodiment, the minimum value is taken from the maximum values on the first coordinate axes corresponding to all points on the plane as the maximum limit value, and the maximum value is taken from the minimum values on the first coordinate axes corresponding to all points on the plane as the minimum limit value; and determining the maximum effective working space according to the limit maximum value and the limit minimum value. According to step S203, each point on the plane corresponds to the maximum value and the minimum value on the n sets of first coordinate axes. All points on the plane correspond to the maximum value xmax of the m x n x coordinates, where m is the number of points on the plane. Selecting the minimum of m x n xmax as the final maximum limit value; similarly, all the points on the plane correspond to the minimum value xmin of the m × n x coordinates, and the maximum value in the m × n xmin is selected as the final limit minimum value.
In some embodiments, the step of deriving the maximum available workspace from the extreme maximum and minimum values comprises: and generating a maximum limit curve surface corresponding to a plane formed by the second coordinate axis and the third coordinate axis, and generating a minimum limit curve surface corresponding to a plane formed by the second coordinate axis and the third coordinate axis to obtain a maximum effective working space.
In some embodiments, the method for planning the effective working space of the end tool further includes determining whether the maximum effective working space is larger than the required working space, and if not, adjusting the structural parameters of the robot arm and reacquiring the maximum effective working space. The structural parameters of the mechanical arm can be adjusted by using a genetic algorithm, the volume of the maximum effective working space obtained by planning is used as a score function, the DH parameters which can be modified by the mechanical arm are used as individual parameters, iterative optimization is carried out, and the structural parameters of the mechanical arm corresponding to the maximum effective working space are obtained.
The method for planning the effective working space of the end tool plans the maximum effective working space by setting the limit value of the first coordinate axis as the design variable of the effective working space and taking the space vector set of the end tool in the limit direction as the constraint condition, thereby realizing the planning of the maximum effective working space under the condition that the end tool and the tail end of the mechanical arm are not coaxial, and having wide application scenes; compared with the conventional Monte Carlo method for randomly taking points in a working space, the method has the advantages that after the orientation requirement of the end tool is given, the point on the plane only needs to take the coordinates of two coordinate axes in the base coordinate system, and the calculation amount is greatly reduced.
The present application further includes an end tool active workspace planning apparatus comprising a memory and a processor. Wherein the memory is to store instructions executable by the processor; the processor is configured to execute the instructions to implement the foregoing method for planning an effective workspace of an end tool. Fig. 3 is a block diagram of a system of an apparatus for planning an effective workspace of an end tool according to an embodiment of the present application. Referring to fig. 3, the planning apparatus 300 for the effective work space of the end tool (hereinafter, referred to as planning apparatus 300) may include an internal communication bus 301, a processor 302, a Read Only Memory (ROM)303, a Random Access Memory (RAM)304, and a communication port 305. When implemented on a personal computer, the planning apparatus 300 may also include a hard disk 306. An internal communication bus 301 may enable data communication among the components of the planner 300. The processor 302 may make the determination and issue the prompt. In some embodiments, processor 302 may be comprised of one or more processors. A communication port 305 may enable data communication of the planning apparatus 300 with the outside. In some embodiments, the planning device 300 may send and receive information and data from a network through a communication port 305. The programming device 300 may also include various forms of program storage units and data storage units, such as a hard disk 303, Read Only Memory (ROM)303 and Random Access Memory (RAM)304, capable of storing various data files for computer processing and/or communication, as well as possible program instructions for execution by the processor 302. The processor executes these instructions to implement the main parts of the method. The results processed by the processor are communicated to the user device through the communication port and displayed on the user interface.
The text classification method described above may be implemented as a computer program, stored in the hard disk 306, and loaded into the processor 302 for execution, so as to implement the method for planning the effective work space of the end tool of the present application.
The present application also includes a computer readable medium having stored thereon computer program code which, when executed by a processor, implements the method of planning an effective workspace of an end tool as described above.
The method for planning the effective workspace of the end tool, when implemented as a computer program, may also be stored as an article of manufacture in a computer-readable storage medium. For example, computer-readable storage media can include but are not limited to magnetic storage devices (e.g., hard disk, floppy disk, magnetic strips), optical disks (e.g., Compact Disk (CD), Digital Versatile Disk (DVD)), smart cards, and flash memory devices (e.g., electrically Erasable Programmable Read Only Memory (EPROM), card, stick, key drive). In addition, various storage media described herein can represent one or more devices and/or other machine-readable media for storing information. The term "machine-readable medium" can include, without being limited to, wireless channels and various other media (and/or storage media) capable of storing, containing, and/or carrying code and/or instructions and/or data.
It should be understood that the above-described embodiments are illustrative only. The embodiments described herein may be implemented in hardware, software, firmware, middleware, microcode, or any combination thereof. For a hardware implementation, the processor may be implemented within one or more Application Specific Integrated Circuits (ASICs), Digital Signal Processors (DSPs), Digital Signal Processing Devices (DSPDs), Programmable Logic Devices (PLDs), Field Programmable Gate Arrays (FPGAs), processors, controllers, micro-controllers, microprocessors, and/or other electronic units designed to perform the functions described herein, or a combination thereof.
Aspects of the present application may be embodied entirely in hardware, entirely in software (including firmware, resident software, micro-code, etc.) or in a combination of hardware and software. The above hardware or software may be referred to as "data block," module, "" engine, "" unit, "" component, "or" system. The processor may be one or more Application Specific Integrated Circuits (ASICs), Digital Signal Processors (DSPs), digital signal processing devices (DAPDs), Programmable Logic Devices (PLDs), Field Programmable Gate Arrays (FPGAs), processors, controllers, microcontrollers, microprocessors, or a combination thereof. Furthermore, aspects of the present application may be represented as a computer product, including computer readable program code, embodied in one or more computer readable media. For example, computer-readable media can include but are not limited to magnetic storage devices (e.g., hard disk, floppy disk, magnetic strips … …), optical disks (e.g., Compact Disk (CD), Digital Versatile Disk (DVD) … …), smart cards, and flash memory devices (e.g., card, stick, key drive … …).
The computer readable medium may comprise a propagated data signal with the computer program code embodied therein, for example, on a baseband or as part of a carrier wave. The propagated signal may take any of a variety of forms, including electromagnetic, optical, and the like, or any suitable combination. The computer readable medium can be any computer readable medium that can communicate, propagate, or transport the program for use by or in connection with an instruction execution system, apparatus, or device. Program code on a computer readable medium may be propagated over any suitable medium, including radio, electrical cable, fiber optic cable, radio frequency signals, or the like, or any combination of the preceding.
As used in this application and the appended claims, the terms "a," "an," "the," and/or "the" are not intended to be inclusive in the singular, but rather are intended to be inclusive in the plural unless the context clearly dictates otherwise. In general, the terms "comprises" and "comprising" merely indicate that steps and elements are included which are explicitly identified, that the steps and elements do not form an exclusive list, and that a method or apparatus may include other steps or elements.
The relative arrangement of the components and steps, the numerical expressions, and numerical values set forth in these embodiments do not limit the scope of the present application unless specifically stated otherwise. Meanwhile, it should be understood that the sizes of the respective portions shown in the drawings are not drawn in an actual proportional relationship for the convenience of description. Techniques, methods, and apparatus known to those of ordinary skill in the relevant art may not be discussed in detail, but are intended to be part of the specification where appropriate. In all examples shown and discussed herein, any particular value should be construed as merely illustrative, and not limiting. Thus, other examples of the exemplary embodiments may have different values. It should be noted that: like reference numbers and letters refer to like items in the following figures, and thus, once an item is defined in one figure, further discussion thereof is not required in subsequent figures.
Further, although the terms used in the present application are selected from publicly known and used terms, some of the terms mentioned in the specification of the present application may be selected by the applicant at his or her discretion, the detailed meanings of which are described in relevant parts of the description herein. Further, it is required that the present application is understood not only by the actual terms used but also by the meaning of each term lying within.
Also, this application uses specific language to describe embodiments of the application. Reference throughout this specification to "one embodiment," "an embodiment," and/or "some embodiments" means that a particular feature, structure, or characteristic described in connection with at least one embodiment of the present application is included in at least one embodiment of the present application. Therefore, it is emphasized and should be appreciated that two or more references to "an embodiment" or "one embodiment" or "an alternative embodiment" in various places throughout this specification are not necessarily all referring to the same embodiment. Furthermore, some features, structures, or characteristics of one or more embodiments of the present application may be combined as appropriate.
Having thus described the basic concept, it will be apparent to those skilled in the art that the foregoing disclosure is by way of example only, and is not intended to limit the present application. Various modifications, improvements and adaptations to the present application may occur to those skilled in the art, although not explicitly described herein. Such modifications, improvements and adaptations are proposed in the present application and thus fall within the spirit and scope of the exemplary embodiments of the present application.
Claims (13)
1. A method for planning an effective workspace of an end tool, comprising:
setting limit values of a first coordinate axis in a base coordinate system as design variables of an effective working space, wherein the limit values comprise a maximum limit value and a minimum limit value;
determining a non-linear equation for the effective workspace based on a constraint, wherein the constraint comprises a set of spatial vectors of extreme orientations of an end tool, and determining an objective function for the design variable from the non-linear equation;
points are taken at equal intervals on a plane formed by a second coordinate axis and a third coordinate axis in a base coordinate system, and the maximum value and the minimum value on the first coordinate axis which can be reached by the end tool corresponding to each point on the plane are calculated by optimizing the objective function;
and determining the maximum effective working space according to the maximum value and the minimum value corresponding to all the points on the plane.
2. A planning method according to claim 1, wherein the constraints further include structural parameters of the robotic arm and end-tool parameters.
3. A method of planning as claimed in claim 1, characterized in that the non-linear equation is:
wherein, T tool Is a coordinate transformation matrix from a base coordinate system to a tool coordinate system, F (-) is a forward kinematics solution function of the robot arm, q is an axial angle vector of the robot arm, tool is an end tool parameter, [ z ] 1 ，z 2 ，z 3 ]For projection of the tip orientation of the tip tool under a base coordinate system, [ x ] q ，y q ，z q ]For seating under a base coordinate system determined according to the axial angular vector of the mechanical armAnd (4) marking.
4. A planning method according to claim 3, wherein the objective function is:
wherein min (x) is the minimum value of the first coordinate axis, max (x) is the maximum value of the first coordinate axis, f (·) is a function of the value of the first coordinate axis and the angular vector of the mechanical arm axis, q (·) a Is the optimal mechanical arm axial angle vector of the minimum value corresponding to the first coordinate axis, q b Is the optimal arm axis angle vector corresponding to the maximum value of the first coordinate axis.
5. The planning method of claim 4, wherein the step of calculating the maximum and minimum values on the first coordinate axis that can be achieved by the end tool corresponding to each point on the plane by optimizing the objective function comprises:
optimizing the objective function by sequential quadratic programming to obtain a group of optimal mechanical arm axial angle vectors corresponding to each point according to the KKT condition of the objective function by using the following equality constraint and inequality constraint;
q min ＜q＜q max
wherein [ z ] 1 ，z 2 ，z 3 ]For the projection of the tip orientation of the tip tool under the base coordinate system, [ z ] n1 ，z n2 ，z n3 ]For projection of the end of the n-th set of constraints towards the base coordinate system, y q And z q The coordinate on the second coordinate axis and the coordinate on the third coordinate axis are determined according to the angular vector of the mechanical arm axis, y and z are the coordinate of the point on the plane on the second coordinate axis and the coordinate on the third coordinate axis, q min Vector q formed by the minimum reachable angle of the mechanical arm joint max The vector is formed by the maximum reachable angle of the mechanical arm joint, and q is the axial angle vector of the mechanical arm;
and taking one of the values of the group of first coordinate axes corresponding to the group of optimal mechanical arm axis angle vectors as the maximum value, and taking the other value as the minimum value.
6. The method of planning of claim 1 wherein the step of taking points equally spaced on a plane formed by the second axis and the third axis in the base coordinate system comprises:
determining second coordinates of each point in a first step increment from the origin on the second coordinate axis;
and determining a third coordinate of each point by increasing the second step length from the origin on the third coordinate axis until the difference between the maximum value and the minimum value of the first coordinate axis corresponding to the point determined by the second coordinate and the third coordinate is less than a first threshold, and then continuously determining the third coordinate of each point by decreasing the second step length from the origin on the third coordinate axis.
7. The planning method of claim 1, further comprising: and determining a first working range required by the end tool in the second coordinate axis and a second working range required by the end tool in the third coordinate axis according to working condition requirements, and taking points at equal intervals on a plane formed by the second coordinate axis and the third coordinate axis in a base coordinate system according to the first working range and the second working range.
8. The method of planning of claim 7 wherein the step of taking points equally spaced in a plane comprised of a second axis and a third axis in a base coordinate system based on the first working range and the second working range comprises:
determining a second coordinate of each point in a first step increment mode in the first working range from an origin to one half on the second coordinate axis, wherein the effective working space is symmetrical about the origin on the second coordinate axis;
and determining the third coordinate of each point by taking the second working range as a step length from the origin on the third coordinate axis in an increasing manner until the difference value between the maximum value and the minimum value of the first coordinate axis corresponding to the point determined by the second coordinate and the third coordinate is less than a first threshold value, and then determining the third coordinate of each point by taking the second working range as a step length from the origin on the third coordinate axis in a decreasing manner.
9. The planning method of claim 1 wherein the step of determining a maximum available workspace from the maximum and minimum of all point correspondences on the plane comprises:
taking the minimum value from the maximum values corresponding to all the points on the plane as the maximum limit value, and taking the maximum value from the minimum values corresponding to all the points on the plane as the minimum limit value;
and determining the maximum effective working space according to the limit maximum value and the limit minimum value.
10. The method of planning according to claim 1, wherein the step of deriving the maximum available workspace from the limiting maximum and the limiting minimum comprises:
and generating a maximum limit curve surface corresponding to a plane formed by the second coordinate axis and the third coordinate axis, and generating a minimum limit curve surface corresponding to a plane formed by the second coordinate axis and the third coordinate axis to obtain a maximum effective working space.
11. The planning method of claim 2, further comprising: and judging whether the maximum effective working space is larger than the required working space, if not, adjusting the structural parameters of the mechanical arm and reacquiring the maximum effective working space.
12. An apparatus for planning an effective workspace for an end tool, comprising:
a memory for storing instructions executable by the processor;
a processor for executing the instructions to implement the method of any one of claims 1-11.
13. A computer-readable medium having stored computer program code which, when executed by a processor, implements the method of any one of claims 1-11.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210706510.5A CN114872053B (en) | 2022-06-21 | 2022-06-21 | Planning method, device and storage medium for effective working space of end tool |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210706510.5A CN114872053B (en) | 2022-06-21 | 2022-06-21 | Planning method, device and storage medium for effective working space of end tool |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114872053A true CN114872053A (en) | 2022-08-09 |
| CN114872053B CN114872053B (en) | 2023-10-13 |
Family
ID=82680920
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210706510.5A Active CN114872053B (en) | 2022-06-21 | 2022-06-21 | Planning method, device and storage medium for effective working space of end tool |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114872053B (en) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107598929A (en) * | 2017-10-25 | 2018-01-19 | 北京邮电大学 | A kind of simple joint defective space mechanical arm pose reachable tree method for solving |
| WO2020138587A1 (en) * | 2018-12-24 | 2020-07-02 | 경북대학교 산학협력단 | Method for calculating maximum rectangular prism workspace of multi-axis machine |
| US20200331143A1 (en) * | 2018-12-29 | 2020-10-22 | Cloudminds (Shenzhen) Robotics Systems Co., Ltd. | Method for calculating an arm angle range of a robot arm, computing device and robot |
| DE102019125326B3 (en) * | 2019-09-20 | 2020-12-03 | Franka Emika Gmbh | Predicted braking range of a robot manipulator |
| CN112238452A (en) * | 2019-07-19 | 2021-01-19 | 顺丰科技有限公司 | Mechanical arm path planning method and device, industrial control equipment and storage medium |
| CN113442144A (en) * | 2021-09-01 | 2021-09-28 | 北京柏惠维康科技有限公司 | Optimal pose determining method and device under constraint, storage medium and mechanical arm |
| DE102020116900B3 (en) * | 2020-06-26 | 2021-10-07 | Deutsches Zentrum für Luft- und Raumfahrt e.V. | Method for determining the manipulability of a handling device and method for determining the carrying capacity of a handling device and handling device |
| CN113787518A (en) * | 2021-09-06 | 2021-12-14 | 武汉库柏特科技有限公司 | Robot tail end attitude control method, device, equipment and storage medium |
| CN114391958A (en) * | 2022-02-15 | 2022-04-26 | 南京佗道医疗科技有限公司 | Method for calculating effective working space of mechanical arm and control method thereof |
-
2022
- 2022-06-21 CN CN202210706510.5A patent/CN114872053B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107598929A (en) * | 2017-10-25 | 2018-01-19 | 北京邮电大学 | A kind of simple joint defective space mechanical arm pose reachable tree method for solving |
| WO2020138587A1 (en) * | 2018-12-24 | 2020-07-02 | 경북대학교 산학협력단 | Method for calculating maximum rectangular prism workspace of multi-axis machine |
| US20200331143A1 (en) * | 2018-12-29 | 2020-10-22 | Cloudminds (Shenzhen) Robotics Systems Co., Ltd. | Method for calculating an arm angle range of a robot arm, computing device and robot |
| CN112238452A (en) * | 2019-07-19 | 2021-01-19 | 顺丰科技有限公司 | Mechanical arm path planning method and device, industrial control equipment and storage medium |
| DE102019125326B3 (en) * | 2019-09-20 | 2020-12-03 | Franka Emika Gmbh | Predicted braking range of a robot manipulator |
| DE102020116900B3 (en) * | 2020-06-26 | 2021-10-07 | Deutsches Zentrum für Luft- und Raumfahrt e.V. | Method for determining the manipulability of a handling device and method for determining the carrying capacity of a handling device and handling device |
| CN113442144A (en) * | 2021-09-01 | 2021-09-28 | 北京柏惠维康科技有限公司 | Optimal pose determining method and device under constraint, storage medium and mechanical arm |
| CN113787518A (en) * | 2021-09-06 | 2021-12-14 | 武汉库柏特科技有限公司 | Robot tail end attitude control method, device, equipment and storage medium |
| CN114391958A (en) * | 2022-02-15 | 2022-04-26 | 南京佗道医疗科技有限公司 | Method for calculating effective working space of mechanical arm and control method thereof |
Non-Patent Citations (1)
| Title |
|---|
| 杜健超;孙卫红;马冠宇;: "基于拉丁超立方体抽样的六自由度机械臂工作空间分析", 组合机床与自动化加工技术, no. 07, pages 30 - 33 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114872053B (en) | 2023-10-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Song et al. | A peg-in-hole robot assembly system based on Gauss mixture model | |
| Liu et al. | Grasping posture of humanoid manipulator based on target shape analysis and force closure | |
| US9321175B2 (en) | Robotic manipulator articulation techniques | |
| CN109986564A (en) | Industrial machinery arm paths planning method | |
| CN109895101A (en) | A kind of articulated manipulator inverse kinematics numerical value unique solution acquiring method | |
| CN111037561B (en) | Space registration method and device | |
| CN104866722B (en) | A kind of inverse kinematics method of seven-shaft industrial mechanical arm | |
| CN108582071B (en) | Industrial robot programming track diagnosis and speed optimization method | |
| Kana et al. | Impedance controlled human–robot collaborative tooling for edge chamfering and polishing applications | |
| Zhang et al. | A novel coordinated motion planner based on capability map for autonomous mobile manipulator | |
| Wang et al. | Optimal trajectory planning of grinding robot based on improved whale optimization algorithm | |
| Bouzgou et al. | Geometric modeling and singularity of 6 DOF Fanuc 200IC robot | |
| CN113894800A (en) | Regular grid welding method, device, terminal and storage medium based on multi-point teaching | |
| CN113119081A (en) | Inverse solution method and device for arm angle interval of mechanical arm and terminal equipment | |
| Gentilini et al. | Cycle time based multi-goal path optimization for redundant robotic systems | |
| CN113103240B (en) | Method, device and system for realizing C2 continuous robot trajectory planning | |
| Jiang et al. | Obstacle-avoidance path planning based on the improved artificial potential field for a 5 degrees of freedom bending robot | |
| CN114872053B (en) | Planning method, device and storage medium for effective working space of end tool | |
| CN117207200A (en) | Method and device for generating working space of mechanical arm and computer equipment | |
| Chen et al. | A whole-path posture optimization method of robotic grinding based on multi-performance evaluation indices | |
| CN112917479B (en) | Approximate pose calculation method and device of five-axis robot and storage medium | |
| Dikmenli | Forward & inverse kinematics solution of 6-dof robots those have offset & spherical wrists | |
| Al-Faiz et al. | Inverse kinematics analysis for manipulator robot with wrist offset based on the closed-form algorithm | |
| Hu et al. | A gantry robot system for cutting single Y-shaped welding grooves on plane workpieces | |
| Zhao et al. | Model and simulation of the mitsubishi rv-m1 robot using matlab |
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 |