CN114872053B - Planning method, device and storage medium for effective working space of end tool - Google Patents
Planning method, device and storage medium for effective working space of end tool Download PDFInfo
- Publication number
- CN114872053B CN114872053B CN202210706510.5A CN202210706510A CN114872053B CN 114872053 B CN114872053 B CN 114872053B CN 202210706510 A CN202210706510 A CN 202210706510A CN 114872053 B CN114872053 B CN 114872053B
- Authority
- CN
- China
- Prior art keywords
- coordinate axis
- coordinate
- axis
- value
- maximum
- 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
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 for an effective working space of an end tool, wherein the method comprises the following steps: setting a limit value of a first coordinate axis in a base coordinate system as a design variable of an effective working space, wherein the limit value comprises a limit maximum value and a limit minimum value; determining a nonlinear equation of the effective workspace based on a constraint, and determining an objective function of the design variable from the nonlinear equation, wherein the constraint comprises a set of spatial vectors of a limit orientation of the end tool; equally-spaced point taking is carried out on a plane formed by the second coordinate axis and the third coordinate axis in the base coordinate system, and the maximum value and the minimum value of the first coordinate axis which can be reached by the end tool corresponding to each point on the plane are calculated through optimizing an 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 application realizes the planning of the maximum effective working space when the end tool is not coaxial with the tail end of the mechanical arm.
Description
Technical Field
The application mainly relates to the field of industrial robots, in particular to a planning method, a planning device and a storage medium for an effective working space of an end tool.
Background
In the practical application scenario of the mechanical arm, there is a certain constraint requirement on the orientation of the end tool, taking a 6-axis surgical mechanical 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 requirement of the surgery, but the orientations of the x axis and the y axis of the end tool are not limited, i.e. the needle can rotate around the z axis by 360 degrees. The spatial range that the tool center point can reach is defined as the effective working space with the workpiece orientation guaranteed. Therefore, when the mechanical arm structure is designed, the effective working space of the needle head needs to be calculated, namely, the effective space range which can be achieved by the needle head is obtained while all the directions required by the needle head under the design working condition are met.
CN111844027a discloses a method and apparatus for determining an optimal working space for a robotic arm. The method comprises the following steps: establishing a kinematic 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 sets to be determined according to the characteristic parameter table; according to a kinematic equation, a Jacobian matrix and a plurality of parameter sets to be determined, acquiring a terminal Cartesian space corresponding to each parameter set to be determined and the Jacobian matrix condition number of the terminal Cartesian space respectively; the optimal working space of the target mechanical arm is determined according to the Jacobian matrix condition number 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 constraint of the orientation of the end tool, so the method cannot be suitable for the scene with the orientation requirement of the end tool.
Therefore, there is a need for a method, apparatus and storage medium for planning an effective working space of an end tool.
Disclosure of Invention
The application aims to provide a planning method, a planning device and a storage medium for an effective working space of an end tool, and solves the problem that the existing working space determining method cannot meet the scene of the end tool with the oriented requirement.
In order to solve the above technical problems, the present application provides a planning method for an effective working space of an end tool, including: setting a limit value of a first coordinate axis in a base coordinate system as a design variable of an effective working space, wherein the limit value comprises a limit maximum value and a limit minimum value; determining a nonlinear equation of the effective workspace based on a constraint, and determining an objective function of the design variable from the nonlinear equation, wherein the constraint comprises a set of spatial vectors of a limiting orientation of an end tool; equally-spaced point taking is performed 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 of 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 points on the plane.
In an embodiment of the application, the constraints further include structural parameters of the robotic arm and end tool parameters.
In one embodiment of the present application, the nonlinear equation is:
wherein T is tool For the coordinate transformation matrix of the base coordinate system to the tool coordinate system, F (·) is the mechanical arm kinematic orthometric function, q is the mechanical arm axis angle vector, tool is the end tool parameter, [ z ] 1 ，z 2 ，z 3 ]For projection of the tip tool towards under a base coordinate system, [ x ] q ，y q ，z q ]Is the coordinate in the base coordinate system determined from the mechanical arm axis angle vector.
In one 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 the relation function of the value of the first coordinate axis and the mechanical arm axis angle vector, q a Is the optimal mechanical arm axis angle vector corresponding to the minimum value of the first coordinate axis, q b Is the optimal mechanical 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 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 by optimizing the objective function includes: the following equation constraint and inequality constraint are adopted as KKT conditions of the objective function, and the objective function is optimized through sequential quadratic programming to obtain a group of optimal mechanical arm shaft angle vectors corresponding to each point;
q min <q<q max
wherein [ z 1 ，z 2 ，z 3 ]For projection of the tip tool towards under a base coordinate system, [ z ] n1 ，z n2 ，z n3 ]For projection of the end-point orientation of the nth group constraint in the base coordinate system, y q And z q For the coordinates on the second coordinate axis and the coordinates on the third coordinate axis determined according to the mechanical arm axis angle vector, y and z are the coordinates on the second coordinate axis and the coordinates on the third coordinate axis of the point on the plane, q min Vector q which is formed by minimum reachable angle of mechanical arm joint max The vector is formed by the maximum reachable angle of the mechanical arm joint, and q is the mechanical arm shaft angle vector;
and taking one of the values of a group of first coordinate axes corresponding to the group of optimal mechanical arm shaft angle vectors as the maximum value and the other value as the minimum value.
In an embodiment of the present application, the step of equally spacing the points on the plane formed by the second coordinate axis and the third coordinate axis in the base coordinate system includes: incrementally determining a second coordinate of each point in a first step starting from the origin on the second coordinate axis; and determining the third coordinates of each point in a second step size from the origin point 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 points determined by the second coordinates and the third coordinates is smaller than a first threshold value, and continuing to determine the third coordinates of each point in a second step size from the origin point on the third coordinate axis.
In an embodiment of the present application, the method for planning an effective working space of an end tool further includes: and determining a first working range required by the end tool on the second coordinate axis and a second working range required by the end tool on the third coordinate axis according to working condition requirements, and equally spacing and taking points on a plane formed by 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.
In an embodiment of the present application, the step of equally spacing the points on a plane formed by 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 includes: incrementally determining a second coordinate of each point in a first step size within the first working range starting from an origin to one half of the second coordinate axis, wherein the effective working space is symmetrical about the origin on the second coordinate axis; and determining the third coordinates of all points in an increasing way by taking the second working range as a 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 coordinates and the third coordinates is smaller than a first threshold value, and continuously determining the third coordinates of all points in a decreasing way by taking the second working range as the step length from the origin on the third coordinate axis.
In one embodiment of the present application, the step of determining the maximum effective workspace from the maximum and minimum values corresponding to all points on the plane includes: taking the minimum value from the maximum values corresponding to all points on the plane as the limit maximum value, and taking the maximum value from the minimum values corresponding to all points on the plane as the limit minimum value; and determining the maximum effective working space according to the limit maximum value and the limit minimum value.
In one embodiment of the application, the step of obtaining the maximum effective workspace from the limit maximum and the limit minimum comprises: and generating a limit maximum value curved surface corresponding to a plane formed by the second coordinate axis and the third coordinate axis, and generating a limit minimum value curved surface corresponding to a plane formed by the second coordinate axis and the third coordinate axis, thereby obtaining a maximum effective working space.
In an embodiment of the present application, the method for planning an effective working space of an 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 re-acquiring the maximum effective working space.
In order to solve the above technical problems, 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; and a processor for executing the instructions to implement the end tool effective workspace planning method described above.
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 end tool efficient workspace planning method described above.
The planning method of the effective working space of the end tool provided by the application has the advantages that the limit value of the first coordinate axis is set as a design variable of the effective working space, the maximum effective working space is planned by taking the space vector set of the limit orientation of the end tool as a constraint condition, the planning of the maximum effective working space under the condition that the end tool is not coaxial with the tail end of the mechanical arm is realized, and the application scene is wide; compared with the conventional Monte Carlo method for randomly taking points in the working space, after the direction of the end tool is required, the point on the plane only needs to take the coordinates of two coordinate axes in the base coordinate system, so that the calculated amount is greatly reduced.
Drawings
In order to make the above objects, features and advantages of the present application more comprehensible, embodiments accompanied with figures are described in detail below, wherein:
FIG. 1 is a schematic view of a robot arm and end tool according to an embodiment of the present application;
FIG. 2 is a flow chart of a method for planning an active workspace of an end tool according to an embodiment of the application;
FIG. 3 is a system block diagram of an end tool effective workspace planning apparatus in accordance with an embodiment of the application.
Detailed Description
In order to make the above objects, features and advantages of the present application more comprehensible, embodiments accompanied with figures 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 as described herein, and therefore the present application is not limited to the specific embodiments disclosed below.
A flowchart is used in the present application to describe the operations performed by a system according to embodiments of the present application. It should be understood that the preceding or following operations are not necessarily performed in order precisely. Rather, the various steps may be processed in reverse order or simultaneously. At the same time, other operations are added to or removed from these processes.
FIG. 1 is a schematic view of a robot arm and an end tool according to an embodiment of the present application. Fig. 1 shows two different poses of the mechanical 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. The end tool 20 is mounted on the robot arm end 105 by a 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 center axis of the base passing through the origin and perpendicular to the surface of the base 101 as Z B An axis establishing X parallel to the surface of the base 101 B Axes and Y B An axis, wherein X B Axes and Y B The axes are perpendicular to each other, resulting in a base coordinate system B. The tool center point 201 of the end tool 20 is taken as an origin, and a straight line passing through the tool center point 201 and coinciding with the tool shaft axis is taken as Z T Axis X T Axes and Y T The axis only needs to be guaranteed to be consistent with Z T The axes are perpendicular to each other, and a tool coordinate system T is obtained. When the tool link 30 is connected to the arm end 105 at a fixed angle, the arm end 105 is oriented at a fixed angle other than collinear with the end tool 20. Thus, when the end tool 20 is not translating and is about Z T As the shaft rotates, the wrist 104 will rotate one revolution in space along the dashed line, i.e., for the distal endThe same orientation of the tool 20, the robotic arm may have a myriad of poses. There is a need for a method to determine the position of the wrist 104 by planning the maximum effective working space possible while meeting the orientation requirements of the end tool 20.
FIG. 2 is a flow chart of a method for planning an active workspace for an end tool in accordance with an embodiment of the application. As shown in fig. 2, the end tool active workspace planning method 200 includes the steps of:
step S201: the limit value of the first coordinate axis in the base coordinate system is set as a design variable of the effective working space, wherein the limit value comprises a limit maximum value and a limit minimum value.
In an embodiment of the present application, X of the base coordinate system is selected B The axis is a first coordinate axis, Y of a base coordinate system B Axis and Z B The axes are a second coordinate axis and a third coordinate axis, respectively. The coordinates of the tool center point of the end tool in the base coordinate system may be expressed as (x, y, z). Given the y and z coordinates, planning an end tool effective workspace requires planning 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 Axes or Z B The axes are the first coordinate axis, and the remaining two coordinate axes are the second coordinate axis and the third coordinate axis respectively. Correspondingly, the limit value of the axis as the first coordinate axis is selected as the design variable of the effective working space.
Step S202: the method includes determining a nonlinear equation of the effective workspace based on constraints, and determining an objective function of the design variable from the nonlinear equation, wherein the constraints include a set of spatial vectors of a limit orientation of the end tool.
In an embodiment of the application, the nonlinear equation of the effective workspace determined based on the constraints is:
wherein T is tool Is a base coordinate systemCoordinate transformation matrix to tool coordinate system, F (·) is mechanical arm kinematics orthometric function, q is mechanical arm axis angle vector, tool is end tool parameter, [ z ] 1 ，z 2 ，z 3 ]For projection of the tip tool towards under the base coordinate system, [ x ] q ，y q ，z q ]Is the coordinate in the base coordinate system determined from the mechanical arm axis angle vector.
In an embodiment of the application, the constraint comprises a set of spatial vectors for the limit orientation of the end tool. The set of limit orientations of the end tool refers to the set of limit orientations that the tool center point needs to reach at all locations within the workspace. The set of spatial vectors for the limit orientation of the end tool can be expressed as:
where A is the set of spatial vectors of the limiting orientation of the end tool, [ z ] 11 ，z 12 ，z 13 ]Representing the projection of the end of the group 1 constraint towards under the base coordinate system, [ z ] n1 ，z n2 ，z n3 ]Representing the projection of the end orientation of the nth set of constraints under 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 comprise mechanical arm DH parameters (Denavit Hartenberg parameters) and limiting ranges of joint angles of the mechanical arm. The DH parameter is a mechanical arm mathematical model and a coordinate system determining system which express the position angle relation between two pairs of joint connecting rods by four parameters. The limit range of each joint angle of the mechanical arm can be expressed as:wherein->Andthe 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 vector is consistent with the number of the mechanical arm joints. End tool parameters include, but are not limited to, the tool coordinate system Z T And the included angle between the axis and the z axis of the coordinate system of the tail end of the mechanical arm and the position of the center point of the tool relative to the tail end of the mechanical arm.
In step S202, for the nonlinear equation, given the y-coordinate and z-coordinate, the acquisition of the maximum x-value and the minimum x-value may be translated into solving the following objective function:
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 sub-function of the mechanical arm kinematic forward function, f (·) can also be expressed as x (q), which describes the relationship between x and q. qa is an optimal mechanical arm axis angle vector corresponding to the minimum value of the first coordinate axis, and qb is an optimal mechanical arm axis angle vector corresponding to the maximum value of the first coordinate axis.
In the embodiment of the present application, taking a six-axis mechanical arm as an example, 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 arm end coordinate system and the position [ dx, dy, dz ] of the tool center point relative to the arm end]The calculation formula of the function x (q) can be obtained:
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 taking points at equal intervals on a plane formed by the second coordinate axis and the third coordinate axis in the base coordinate system, and calculating the maximum value and the minimum value of the first coordinate axis which can be achieved by the end tool corresponding to each point on the plane by optimizing an objective function.
And equally spacing points 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 divided into two separate pairs of min (x (q a ) And min (1/x (q) b ) The two functions are optimally solved.
In some embodiments, the step of calculating the maximum and minimum values on the first coordinate axis that can be reached by the end tool corresponding to each point on the plane by optimizing the objective function includes:
optimizing the objective function by sequential quadratic programming (Sequential quadratic programming, SQP) to obtain a set of optimal mechanical arm shaft angle vectors q corresponding to each point by using KKT conditions (Karush-Kuhn-Tucker Conditions, KKT conditions) of the objective function with the following equation constraint and inequality constraint a And q 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 in the base coordinate system, [ z ] n1 ，z n2 ，z n3 ]For projection of the end-point orientation of the nth group constraint in the base coordinate system, y q And z q For the coordinates on the second coordinate axis and the coordinates on the third coordinate axis determined from the mechanical arm axis angle vector, y and z are the coordinates on the second coordinate axis and the coordinates on the third coordinate axis of the point on the plane, q min Vector q which is formed by minimum reachable angle of mechanical arm joint max Vector q which is formed by maximum reachable angle of mechanical arm joint a Is one of the optimal mechanical arm shaft angle vectors, q b Is another optimal mechanical arm shaft angle vector.
Taking the optimal mechanical arm shaft angle vector q a Corresponding x (q a ) The value of (1) is the minimum value of the first coordinate axis corresponding to the point, and the optimal mechanical arm shaft angle vector q is taken b Corresponding 1/x (q b ) The value of (2) is the maximum value of the first coordinate axis corresponding to the point. Since the space vector set A of the limit orientations of the end tool has n groups of limited end orientations, the coordinate transformation matrix from the mechanical arm to the end of the workpieceIt can be seen that when the limited spatial vector set of end tool has n sets of limited end orientations, it is necessary to substitute T for each set of limited end orientations in the limited spatial vector set a of end tool (i.e., each row of vectors in matrix a) tool Z of (2) 1 ，z 2 ，z 3 Obtaining n groups of xmax and xmin, wherein each point corresponds to a maximum value and a minimum value on n groups of first coordinate axes.
In some embodiments, the step of equally spacing the points on a plane formed by the second coordinate axis and the third coordinate axis in the base coordinate system comprises:
first, a second point is determined in increments of a first step from the origin on a second coordinate axisCoordinates. In Y form B Axis and Z B The axes are a second coordinate axis and a third coordinate axis, the first step is 0.01, and the y coordinates of the points are 0,0.01,0.02 and … in sequence. Next, a third coordinate of each point is determined in a second step from the origin on the third coordinate axis, and the z coordinates of each point are 0,0.1,0.2, … in order, taking the second step as an example. 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 smaller than the first threshold, namely, the minimum value xmin of the x coordinate corresponding to the point determined by the y coordinate and the z coordinate and the maximum value xmax of the x coordinate are calculated by optimizing the objective function. If the difference xmax-xmin is less than the first threshold, then the determination of the third coordinate of each point is continued starting from the origin in the third coordinate axis in decreasing second step, i.e. the z-coordinates of each point are in turn 0, -0.1, -0.2, ….
In some embodiments, the method of planning an end tool active workspace further comprises: and determining a first working range required by the end tool on the second coordinate axis and a second working range required by the end tool on 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 the base coordinate system according to the first working range and the second working range. For example, the needle is required to have a working range of 0.2m (in meters) on the second axis and a working range of 0.1m on the third axis.
In some embodiments, the step of equally spacing the points on a plane formed by 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 includes:
incrementally determining a second coordinate of each point in a first step size within a first working range starting from the origin to one half of the second coordinate axis, wherein the effective working space is symmetrical about the origin on the second coordinate axis; incrementally determining a third coordinate of each point on the third coordinate axis starting from the origin in a second working range as a step size until a difference between a maximum value and a minimum value of the first coordinate axis corresponding to the point determined by the second coordinate and the third coordinate is smaller than a first threshold value, and starting from the origin on the third coordinate axis in the second working range as a step sizeThe long decrement continues to determine the third coordinates of the respective points. Continue to use Y B The axis is the second coordinate axis, Z B The axis is a third coordinate axis, and the effective working space of the end tool is Y in the basic coordinate system due to the symmetry of the mechanical structure B The axis is symmetrical about the origin, so that only half of the first working range is needed for calculation, i.e. the working range of the y-coordinate should satisfy 0-0.1 m in this embodiment. Taking the first step of 0.01 as an example, the y coordinates of each point are 0,0.01,0.02, … and 0.1 in sequence. In the present embodiment, the working range of the Z coordinate should not be smaller than the second working range (i.e. 0.1 m), and since the effective working space is in Z B Can translate in the axial direction, so that it is required to be in Z B On axis a continuous interval of length 0.1m is found and within this interval along X B The movable range of the axial direction is larger than other sections. Thus requiring extensive searching and computation. First, the z coordinates of the points are sequentially 0,0.1,0.2, … and 0.5, and when the difference 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 smaller than the first threshold, the z coordinates of the points are sequentially 0, -0.1, -0.2, … and-0.5.
In some embodiments, structural features of the robotic arm may be incorporated when the end tool is at 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, … and-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 points on the plane. In this embodiment, the minimum value is taken as the limit maximum value from the maximum values on the first coordinate axes corresponding to all the points on the plane, and the maximum value is taken as the limit minimum value from the minimum values on the first coordinate axes corresponding to all the points on the plane; the maximum effective workspace is determined from the limit maximum and limit minimum. As can be seen from step S203, each point on the plane corresponds to a maximum value and a minimum value on n groups of first coordinate axes. All points on the plane correspond to a maximum value xmax of 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 limit maximum; similarly, all points on the plane correspond to the minimum xmin of the m x coordinates, and the maximum of the m x coordinates is selected as the final limit minimum.
In some embodiments, the step of deriving the maximum effective workspace from the limit maximum and limit minimum comprises: and generating a limit maximum value curved surface corresponding to a plane formed by the second coordinate axis and the third coordinate axis, and generating a limit minimum value curved surface corresponding to a plane formed by the second coordinate axis and the third coordinate axis, thereby obtaining the maximum effective working space.
In some embodiments, the method for planning an effective working space of an end tool further includes determining whether the maximum effective working space is greater than a required working space, and if not, adjusting the structural parameters of the mechanical arm and re-acquiring the maximum effective working space. The mechanical arm structure parameters can be adjusted by using a genetic algorithm, the planned maximum effective working space volume is used as a scoring function, the DH parameters which can be modified by the mechanical arm are used as individual parameters, and iterative optimization is carried out to obtain the mechanical arm structure parameters corresponding to the maximum effective working space.
The planning method of the effective working space of the end tool provided by the application has the advantages that the limit value of the first coordinate axis is set as a design variable of the effective working space, the maximum effective working space is planned by taking the space vector set of the limit orientation of the end tool as a constraint condition, the planning of the maximum effective working space under the condition that the end tool is not coaxial with the tail end of the mechanical arm is realized, and the application scene is wide; compared with the conventional Monte Carlo method for randomly taking points in the working space, after the direction of the end tool is required, the point on the plane only needs to take the coordinates of two coordinate axes in the base coordinate system, so that the calculated amount is greatly reduced.
The application also includes a planning apparatus for an end tool effective workspace, comprising a memory and a processor. Wherein the memory is for storing instructions executable by the processor; the processor is configured to execute the instructions to implement the end tool efficient workspace planning method described above. An embodiment of an end tool active workspace planning apparatus of the present application can refer to fig. 3, and fig. 3 is a system block diagram of an end tool active workspace planning apparatus according to an embodiment of the present application. Referring to fig. 3, the planning apparatus 300 for an end tool effective working space (hereinafter, referred to as the 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. The planning apparatus 300 may also include a hard disk 306 when applied on a personal computer. An internal communication bus 301 may enable data communication between the components of the planning apparatus 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. The communication port 305 may enable the planning apparatus 300 to communicate data with the outside. In some embodiments, the planning apparatus 300 may send and receive information and data from the network via the communication port 305. The programming device 300 may also include various forms of program storage units as well as 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 part of the method. The result processed by the processor is transmitted to the user equipment through the communication port and displayed on the user interface.
The text classification method described above may be implemented as a computer program stored on the hard disk 306 and loadable into the processor 302 for execution in order to implement the end tool efficient workspace planning method of the present application.
The application also includes a computer readable medium storing computer program code which, when executed by a processor, implements the end tool effective workspace planning method described above.
When the end tool effective workspace planning method is implemented as a computer program, it may also be stored in a computer readable storage medium as an article of manufacture. For example, computer-readable storage media may 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), cards, sticks, key drives). Moreover, 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 embodiments described above 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 processors 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.
Some aspects of the application may be performed entirely by hardware, entirely by software (including firmware, resident software, micro-code, etc.) or by a combination of hardware and software. The above hardware or software may be referred to as a "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 application may take the form of a computer product, comprising 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, tape … …), optical disk (e.g., compact disk CD, digital versatile disk DVD … …), smart card, 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 on a variety of forms, including electro-magnetic, optical, etc., or any suitable combination thereof. A computer readable medium can be any computer readable medium that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device. Program code located on a computer readable medium may be propagated through any suitable medium, including radio, cable, fiber optic cable, radio frequency signals, or the like, or a combination of any of the foregoing.
As used in the specification and in the claims, the terms "a," "an," "the," and/or "the" are not specific to a singular, but may include a plurality, unless the context clearly dictates otherwise. In general, the terms "comprises" and "comprising" merely indicate that the steps and elements are explicitly identified, and they do not constitute an exclusive list, as other steps or elements may be included in a method or apparatus.
The relative arrangement of the components and steps, numerical expressions and numerical values set forth in these embodiments do not limit the scope of the present application unless it is specifically stated otherwise. Meanwhile, it should be understood that the sizes of the respective parts shown in the drawings are not drawn in actual scale for convenience of description. Techniques, methods, and apparatus known to one of ordinary skill in the relevant art may not be discussed in detail, but should be considered part of the authorization specification where appropriate. In all examples shown and discussed herein, any specific values should be construed as merely illustrative, and not a limitation. Thus, other examples of the exemplary embodiments may have different values. It should be noted that: like reference numerals and letters denote like items in the following figures, and thus once an item is defined in one figure, no further discussion thereof is necessary in subsequent figures.
Furthermore, although terms used in the present application are selected from publicly known and commonly used terms, some terms mentioned in the present specification 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. Furthermore, it is required that the present application is understood, not simply by the actual terms used but by the meaning of each term lying within.
Meanwhile, the present application uses specific words to describe embodiments of the present application. Reference to "one embodiment," "an embodiment," and/or "some embodiments" means that a particular feature, structure, or characteristic is associated with at least one embodiment of the application. Thus, it should be emphasized and should be appreciated that two or more references to "an embodiment" or "one embodiment" or "an alternative embodiment" in various positions in this specification are not necessarily referring to the same embodiment. Furthermore, certain features, structures, or characteristics of one or more embodiments of the application may be combined as suitable.
While the basic concepts have been described above, it will be apparent to those skilled in the art that the foregoing application disclosure is by way of example only and is not intended to be limiting. Although not explicitly described herein, various modifications, improvements and adaptations of the application may occur to one skilled in the art. Such modifications, improvements, and modifications are intended to be suggested within the present disclosure, and therefore, such modifications, improvements, and adaptations are intended to be within the spirit and scope of the exemplary embodiments of the present disclosure.
Claims (13)
1. A method for planning an effective workspace for an end tool, comprising:
setting a limit value of a first coordinate axis in a base coordinate system as a design variable of an effective working space, wherein the limit value comprises a limit maximum value and a limit minimum value;
determining a nonlinear equation of the effective workspace based on a constraint, and determining an objective function of the design variable from the nonlinear equation, wherein the constraint comprises a set of spatial vectors of a limiting orientation of an end tool;
equally-spaced point taking is performed 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 of 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 on the first coordinate axis which can be reached by the end tool and corresponds to all points on the plane.
2. The planning method of claim 1 wherein the constraints further comprise structural parameters of the robotic arm and end tool parameters.
3. The planning method of claim 1, wherein the nonlinear equation is:
wherein T is tool For the coordinate transformation matrix of the base coordinate system to the tool coordinate system, F (·) is the mechanical arm kinematic orthometric function, q is the mechanical arm axis angle vector, tool is the end tool parameter, [ z ] 1 ，z 2 ，z 3 ]For projection of the tip tool towards under a base coordinate system, [ x ] q ，y q ，z q ]Is the coordinate in the base coordinate system determined from the mechanical arm axis angle vector.
4. A planning method according to claim 3, wherein the objective function is:
wherein min (x) is the minimum of the first coordinate axisThe value max (x) is the maximum value of the first coordinate axis, f (·) is the relation function of the value of the first coordinate axis and the mechanical arm axis angle vector, q a Is the optimal mechanical arm axis angle vector corresponding to the minimum value of the first coordinate axis, q b Is the optimal mechanical 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 maximum and minimum values on a first coordinate axis achievable by the end tool for each point on the plane by optimizing the objective function comprises:
the following equation constraint and inequality constraint are adopted as KKT conditions of the objective function, and the objective function is optimized through sequential quadratic programming to obtain a group of optimal mechanical arm shaft angle vectors corresponding to each point;
q min <q<q max
wherein [ z 1 ，z 2 ，z 3 ]For projection of the tip tool towards under a base coordinate system, [ z ] n1 ，z n2 ，z n3 ]For projection of the end-point orientation of the nth group constraint in the base coordinate system, y q And z q For the coordinates on the second coordinate axis and the coordinates on the third coordinate axis determined according to the mechanical arm axis angle vector, y and z are the coordinates on the second coordinate axis and the coordinates on the third coordinate axis of the point on the plane, q min Vector q which is formed by minimum reachable angle of mechanical arm joint max The vector is formed by the maximum reachable angle of the mechanical arm joint, and q is the mechanical arm shaft angle vector;
and taking one of the values of a group of first coordinate axes corresponding to the group of optimal mechanical arm shaft angle vectors as the maximum value and the other value as the minimum value.
6. The method of planning of claim 1 wherein the step of equally spacing the points on a plane formed by the second coordinate axis and the third coordinate axis in the base coordinate system comprises:
incrementally determining a second coordinate of each point in a first step starting from the origin on the second coordinate axis;
and determining the third coordinates of each point in a second step size from the origin point 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 points determined by the second coordinates and the third coordinates is smaller than a first threshold value, and continuing to determine the third coordinates of each point in a second step size from the origin point 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 on the second coordinate axis and a second working range required by the end tool on the third coordinate axis according to working condition requirements, and equally spacing and taking points on a plane formed by 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.
8. The method of planning of claim 7 wherein the step of equally spacing points on a plane comprised 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:
incrementally determining a second coordinate of each point in a first step size within the first working range starting from an origin to one half of the second coordinate axis, wherein the effective working space is symmetrical about the origin on the second coordinate axis;
and determining the third coordinates of all points in an increasing way by taking the second working range as a 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 coordinates and the third coordinates is smaller than a first threshold value, and continuously determining the third coordinates of all points in a decreasing way by taking the second working range as the step length from the origin on the third coordinate axis.
9. The planning method of claim 1 wherein the step of determining the maximum effective workspace from the maximum and minimum values on the first coordinate axis that can be reached by the end tool for all points on the plane comprises:
taking the minimum value from the maximum values on the first coordinate axes corresponding to all the points on the plane as the limit maximum value, and taking the maximum value from the minimum values on the first coordinate axes corresponding to all the points on the plane as the limit minimum value;
and determining the maximum effective working space according to the limit maximum value and the limit minimum value.
10. The planning method of claim 1 wherein the step of deriving a maximum effective workspace from the limit maximum and the limit minimum comprises:
and generating a limit maximum value curved surface corresponding to a plane formed by the second coordinate axis and the third coordinate axis, and generating a limit minimum value curved surface corresponding to a plane formed by the second coordinate axis and the third coordinate axis, thereby obtaining 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 re-acquiring the maximum effective working space.
12. A planning apparatus for an end tool effective workspace, 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 storing computer program code which, when executed by a processor, implements the method of any 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 CN114872053A (en) | 2022-08-09 |
| CN114872053B true 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 (8)
| 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 |
| 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 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109676606B (en) * | 2018-12-29 | 2021-07-23 | 达闼机器人有限公司 | Method for calculating arm angle range of mechanical arm, mechanical arm and robot |
-
2022
- 2022-06-21 CN CN202210706510.5A patent/CN114872053B/en active Active
Patent Citations (8)
| 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 |
| 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 |
|---|
| 基于拉丁超立方体抽样的六自由度机械臂工作空间分析;杜健超;孙卫红;马冠宇;;组合机床与自动化加工技术(07);第30-33页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114872053A (en) | 2022-08-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9321175B2 (en) | Robotic manipulator articulation techniques | |
| CN108582071B (en) | Industrial robot programming track diagnosis and speed optimization method | |
| CN111037561B (en) | Space registration method and device | |
| CN109986564A (en) | Industrial machinery arm paths planning method | |
| Li et al. | Path planning of 6-DOF venipuncture robot arm based on improved a-star and collision detection algorithms | |
| KR20150142796A (en) | Method and system for controlling elbow of robot | |
| Zhang et al. | An ergonomic shared workspace analysis framework for the optimal placement of a compact master control console | |
| Zhao et al. | Improved manipulator obstacle avoidance path planning based on potential field method | |
| CN114872053B (en) | Planning method, device and storage medium for effective working space of end tool | |
| CN113103240B (en) | Method, device and system for realizing C2 continuous robot trajectory planning | |
| Wang et al. | A method of robot base frame calibration by using dual quaternion algebra | |
| CN113084792A (en) | Method for determining joint singular area, robot and storage device | |
| CN108687774B (en) | Solving method for working space of hybrid robot | |
| CN117381790A (en) | Dynamic optimization method for flexibility of mechanical arm and master-slave teleoperation mechanical arm system | |
| CN111482968A (en) | Six-degree-of-freedom offset robot inverse solution method based on BFS algorithm | |
| Ostanin et al. | Programming by Demonstration Using Two-Step Optimization for Industrial Robot. | |
| CN112917479B (en) | Approximate pose calculation method and device of five-axis robot and storage medium | |
| DİKMENLİ | Forward & inverse kinematics solution of 6-dof robots those have offset & spherical wrists | |
| Zhao et al. | Model and simulation of the mitsubishi rv-m1 robot using matlab | |
| Al-Faiz et al. | Inverse kinematics analysis for manipulator robot with wrist offset based on the closed-form algorithm | |
| WO2019114630A1 (en) | Method and device for obtaining coordinates of tcp of robot | |
| Szkodny et al. | Planning and realization of singular trajectories of Adept Six 300 robot | |
| US20240051123A1 (en) | Robot programming device | |
| CN116652972B (en) | Series robot tail end track planning method based on bidirectional greedy search algorithm | |
| CN113001537A (en) | Mechanical arm control method, mechanical arm control device and terminal equipment |
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 |