Specific embodiment
Technical scheme is illustrated below in conjunction with the accompanying drawings.
As shown in figure 1, the present invention provides a kind of manipulator motion path generating method, it may include following steps:
S1, obtains the distributing position of the barrier around mechanical arm, is moved according to the distributing position calculating machine arm
Radius of safety, the real work radius of the Size calculation machinery arm according to the radius of safety and the instrument pawl of mechanical arm;
In one embodiment, the coordinate system of mechanical arm and distribution of obstacles can be set up, it is preferable that the coordinate system is
Three-dimensional system of coordinate.The central point of mechanical arm base can be set to origin by the three-dimensional system of coordinate, by two on horizontal plane mutually
Vertical direction is set to x-axis and y-axis, and vertical direction is set into z-axis.By setting up coordinate system, can more easily to machinery
Arm position and distribution of obstacles are described.Under the three-dimensional system of coordinate, the distributing position of the barrier can use coordinate
Form show, for example, a coordinate set can be set up, multiple coordinate values can be included in the coordinate set, each
Coordinate value represents a position distribution for barrier, and the coordinate value is represented by the form of (x, y, z).For the ease of distinguishing each
, can also be numbered for each barrier, and the coordinate value of barrier and reference numeral are bound by barrier.
The radius of safety is as shown in Figure 4 with real work radius.In figure, RmaxAnd RminRepresent radius of safety most respectively
Big value and minimum value, WRmaxAnd WRminThe maximum and minimum value of real work radius are represented respectively, and A and B represents starting respectively
Position and target location.The maximum of the radius of safety can be calculated according to distribution of obstacles, can dividing according to barrier
The distance between central point of barrier and mechanical arm base around cloth position acquisition mechanical arm, according to the minimum of the distance
The radius of safety of value calculating machine arm motion.In one embodiment, can be by the minimum value r of the distanceminIt is set to the peace
The maximum of full radius, in another embodiment, it is also possible to be less than r by oneminNumerical value be set to the radius of safety most
Big value.The minimum value of the radius of safety can be calculated according to the size of mechanical arm base.In one embodiment, can be by
The maximum r of the distance of the mechanical arm base each point and mechanical arm base central pointmaxIt is set to the minimum of the radius of safety
Value, in another embodiment, it is also possible to be more than r by onemaxNumerical value be set to the minimum value of the radius of safety.Due to machine
The instrument pawl of tool arm has certain size, therefore, the real work radius of mechanical arm can be calculated according in the following manner:
WRmax=Rmax-d;
WRmin=Rmin+d;
Wherein, d is the size of the instrument pawl of the mechanical arm.
S2, original position according to the instrument pawl and in the middle of the first of the real work radius calculation manipulator motion
Point position, the second point midway of target location and the real work radius calculation manipulator motion according to the instrument pawl
Put;
In one embodiment, during the first mid-point position of calculating machine arm motion, first straight line and institute can be obtained
State the first intersection point of the corresponding border circular areas of real work radius;Wherein, the first straight line be the original position with it is described
The straight line that the central point of the base of mechanical arm is linked to be;Position where one of them first intersection point is set to the point midway
Put.Preferably, first intersection point can be the intersection point of first straight line circle corresponding with the real work radius, such as scheme
Shown in 5.In Figure 5, A ' is the intersection point, namely the intermediate point.During the second mid-point position of calculating machine arm motion, can
To obtain the second intersection point of second straight line border circular areas corresponding with the real work radius;Wherein, the second straight line is
The straight line that the target location is linked to be with the central point of the base of the mechanical arm;By the position where one of them second intersection point
It is the mid-point position to install.Preferably, second intersection point can be the second straight line and the real work radius
The intersection point of corresponding circle, as shown in Figure 5.In Figure 5, B ' is the intersection point, namely the intermediate point.If establishing three-dimensional seat
Mark system, can obtain the coordinate of first intermediate point and the second intermediate point in the three-dimensional system of coordinate, and preserve.
In one embodiment, the limitation due to mechanical arm mechanical parameter in itself to mechanical arm, if mechanical arm keeps
Current pose possibly cannot move to target location, now, during mechanical arm moves to target location from original position
Need to change right-hand man's attitude.Mechanical arm change right-hand man's attitude when, it is necessary to whole mechanical arm is stretched completely, it is necessary to space
Maximum, hand-off point treatment is bad to collide.Therefore, in addition to first mid-point position and the second mid-point position,
May also need to set hand-off position in the motion path of mechanical arm.The hand-off position can be arranged on hand-off region
Interior, hand-off region is area of the distance between the central point of barrier and mechanical arm base more than the maximum length of the mechanical arm
Domain.That is, if the target location of described instrument pawl, can be in hand-off region not in the range of movement of the mechanical arm current pose
Choose to a little less as hand-off point, obtain hand-off position;Wherein, the hand-off region is barrier and mechanical arm base
Region of the distance between the central point more than the maximum length of the mechanical arm.When the maximum length is that mechanical arm is stretched completely
Length.As shown in figure 5, C1 and C2 in figure are hand-off point, in practical situations both, the quantity of hand-off point can also in hand-off region
It is 1 or other quantity.Wherein, when judging that whether mechanical arm keeps current pose can reach target location, can read machine
The mechanical parameter of tool arm, and the region that can be reached according to the mechanical parameter calculating machine arm current pose, if target location
Not in the region, then judge to need switching right and left hand attitude.
S3, if the target location of the instrument pawl is in the range of movement of the mechanical arm current pose, according to described
Beginning position, the first mid-point position, the second mid-point position and target location generate the motion path of mechanical arm.
Assuming that the target location of the instrument pawl need not switch in the range of movement of the mechanical arm current pose
Right-hand man's attitude, directly can generate according to the original position, the first mid-point position, the second mid-point position and target location
The motion path of mechanical arm.Assuming that the target location of the instrument pawl is not in the range of movement of the mechanical arm current pose,
Switching right and left hand attitude is needed, can be hand-off according to what is calculated in the original position, the first mid-point position and step S2
Point position generates the motion path of mechanical arm current pose, and according to the hand-off position, the second mid-point position and target
Motion path after position generation mechanical arm switching attitude.
In practical situations both, due to mechanical arm special construction, need that Z axis are first promoted to the position of safe Z axis in starting point
Put, terminal Z axis position is down to by safe Z axis again in terminal, all intermediate points run all on safe Z axis, that is, walk one
" door " font trajectory.The position of safe Z axis can prestore, and in general, the position of safe Z axis is without often becoming
It is dynamic.When needing to change the hoisting depth of mechanical arm, the position of the safe Z axis for prestoring need to be only changed, without manually adjusting
Mechanical arm is highly.In this case it is necessary to insert the centre directly over an original position in the motion path of mechanical arm
Point.Specifically, the corresponding first safe altitude position of the original position can be calculated;Wherein, the first safe altitude position
It is position directly over the original position, and with the vertical height of the original position more than or equal to default height value to put
Put;According to the original position, the first safe altitude position, the first mid-point position, the second mid-point position and target location
Generate the motion path of mechanical arm.In this case it is also possible to according to the first safe altitude position and the barrier
The radius of safety of distributed computer tool arm motion.Now, the maximum R of the radius of safetymaxCan be calculated according in the following manner:
Wherein, h is the height of safe Z axis, and l is the ultimate range of barrier and mechanical arm base central point.
Assuming that mechanical arm is raised in original position, as described above, then being also needed to machinery in target location
Arm drops to former height, at this point it is possible to insert an intermediate point in the surface of the target location of manipulator motion.Specifically,
The corresponding second safe altitude position in the target location can be calculated;Wherein, the second safe altitude position is described
Directly over target location, and it is more than or equal to the position of the default height value with the vertical height of the target location;Root
According to the original position, the first safe altitude position, the first mid-point position, the second safe altitude position, the second point midway
Put the motion path that mechanical arm is generated with target location.
After the motion path of generation mechanical arm, can be moved according to the motion path control machinery arm, make machinery
Arm moves to the aiming spot along the motion path from the initial point position automatically.When distribution of obstacles changes
When, only need to reacquire distribution of obstacles, you can the automatic motion path for cooking up mechanical arm again.When mechanical arm is changed,
Only need in systems re-enter the mechanical parameter (such as size of instrument pawl) of mechanical arm, you can cook up mechanical arm again automatically
Motion path.Without artificial teaching, simple to operate, efficiency high.
The actual motion path of mechanical arm is as shown in Figure 6 and Figure 7.Fig. 6 is not added on vertical direction for one embodiment
Intermediate point when path schematic diagram, Fig. 7 for one embodiment add vertical direction on intermediate point when path schematic diagram.
Dotted line represents the motion path of mechanical arm in figure.
The present invention has advantages below:
(1) it is simple to operate quick, it is not necessary to manual teaching.
(2) intermediate point that intelligence insertion needs, operational efficiency is higher.
(3) automatic management Z axis planning, caller only needs to be input into starting point and endpoint information.
(4) whether intelligent decision needs to change right-hand man's attitude, and operational efficiency is higher.
(5) various disorders thing layout is adapted to, only needs to be used immediately by updating environmental information after environmental change.
As shown in figure 8, the present invention provides a kind of manipulator motion path generating system, it may include:
First computing module 10, the distributing position for obtaining the barrier around mechanical arm, according to the distributing position
The radius of safety of calculating machine arm motion, the reality of the Size calculation machinery arm according to the radius of safety and the instrument pawl of mechanical arm
The border radius of clean-up;
In one embodiment, the coordinate system of mechanical arm and distribution of obstacles can be set up, it is preferable that the coordinate system is
Three-dimensional system of coordinate.The central point of mechanical arm base can be set to origin by the three-dimensional system of coordinate, by two on horizontal plane mutually
Vertical direction is set to x-axis and y-axis, and vertical direction is set into z-axis.By setting up coordinate system, can more easily to machinery
Arm position and distribution of obstacles are described.Under the three-dimensional system of coordinate, the distributing position of the barrier can use coordinate
Form show, for example, a coordinate set can be set up, multiple coordinate values can be included in the coordinate set, each
Coordinate value represents a position distribution for barrier, and the coordinate value is represented by the form of (x, y, z).For the ease of distinguishing each
, can also be numbered for each barrier, and the coordinate value of barrier and reference numeral are bound by barrier.
The radius of safety is as shown in Figure 4 with real work radius.In figure, RmaxAnd RminRepresent radius of safety most respectively
Big value and minimum value, WRmaxAnd WRminThe maximum and minimum value of real work radius are represented respectively.The radius of safety is most
Big value can be calculated according to distribution of obstacles, and barrier and machine around mechanical arm can be obtained according to the distributing position of barrier
The distance between central point of tool arm base, the radius of safety of the minimum value calculating machine arm motion according to the distance.One
In individual embodiment, can be by the minimum value r of the distanceminThe maximum of the radius of safety is set to, in another embodiment
In, it is also possible to it is less than r by oneminNumerical value be set to the maximum of the radius of safety.The minimum value of the radius of safety can be with
Size according to mechanical arm base is calculated.In one embodiment, can be by the mechanical arm base each point and mechanical arm bottom
The maximum r of the distance of seat central pointmaxThe minimum value of the radius of safety is set to, in another embodiment, it is also possible to by one
It is individual more than rmaxNumerical value be set to the minimum value of the radius of safety.Because the instrument pawl of mechanical arm has certain size, therefore, machine
The real work radius of tool arm can be calculated according in the following manner:
WRmax=Rmax-d;
WRmin=Rmin+d;
Wherein, d is the size of the instrument pawl of the mechanical arm.
Second computing module 20, for the original position according to the instrument pawl and real work radius calculation machinery
First mid-point position of arm motion, target location and the real work radius calculation mechanical arm fortune according to the instrument pawl
The second dynamic mid-point position;
In one embodiment, during the first mid-point position of calculating machine arm motion, first straight line and institute can be obtained
State the first intersection point of the corresponding border circular areas of real work radius;Wherein, the first straight line be the original position with it is described
The straight line that the central point of the base of mechanical arm is linked to be;Position where one of them first intersection point is set to the point midway
Put.Preferably, first intersection point can be the intersection point of first straight line circle corresponding with the real work radius, such as scheme
Shown in 5.In Figure 5, A ' is the intersection point, namely the intermediate point.During the second mid-point position of calculating machine arm motion, can
To obtain the second intersection point of second straight line border circular areas corresponding with the real work radius;Wherein, the second straight line is
The straight line that the target location is linked to be with the central point of the base of the mechanical arm;By the position where one of them second intersection point
It is the mid-point position to install.Preferably, second intersection point can be the second straight line and the real work radius
The intersection point of corresponding circle, as shown in Figure 5.In Figure 5, B ' is the intersection point, namely the intermediate point.If establishing three-dimensional seat
Mark system, can obtain the coordinate of first intermediate point and the second intermediate point in the three-dimensional system of coordinate, and preserve.
In one embodiment, the limitation due to mechanical arm mechanical parameter in itself to mechanical arm, if mechanical arm keeps
Current pose possibly cannot move to target location, now, during mechanical arm moves to target location from original position
Need to change right-hand man's attitude.Mechanical arm change right-hand man's attitude when, it is necessary to whole mechanical arm is stretched completely, it is necessary to space
Maximum, hand-off point treatment is bad to collide.Therefore, in addition to first mid-point position and the second mid-point position,
May also need to set hand-off position in the motion path of mechanical arm.The hand-off position can be arranged on hand-off region
Interior, hand-off region is area of the distance between the central point of barrier and mechanical arm base more than the maximum length of the mechanical arm
Domain.That is, an acquisition module can be set, if the target location of the instrument pawl is not in the range of movement of the mechanical arm current pose
Interior, the acquisition module can be chosen to a little less as hand-off point in hand-off region, obtain hand-off position;Wherein, it is described to change
Hand region is region of the distance between the central point of barrier and mechanical arm base more than the maximum length of the mechanical arm.Institute
Length when stating maximum length i.e. mechanical arm being stretched completely.Hand-off region is as shown in Figure 5.Wherein, judging that mechanical arm holding ought
When whether preceding attitude can reach target location, the mechanical parameter of mechanical arm is can read, and according to the mechanical parameter computer
The region that tool arm current pose can be reached, if target location is not in the region, judgement needs switching right and left hand attitude.
First generation module 30, if for the instrument pawl target location the mechanical arm current pose motion model
In enclosing, the motion of mechanical arm is generated according to the original position, the first mid-point position, the second mid-point position and target location
Path.
Assuming that the target location of the instrument pawl need not switch in the range of movement of the mechanical arm current pose
Right-hand man's attitude, directly can generate according to the original position, the first mid-point position, the second mid-point position and target location
The motion path of mechanical arm.Assuming that the target location of the instrument pawl is not in the range of movement of the mechanical arm current pose,
Need switching right and left hand attitude, can be set the second generation module, for according to the original position, the first mid-point position and
The hand-off position that second computing module 20 is calculated generates the motion path of mechanical arm current pose, and according to the hand-off point
Motion path after position, the second mid-point position and target location generation mechanical arm switching attitude.
In practical situations both, due to mechanical arm special construction, need that Z axis are first promoted to the position of safe Z axis in starting point
Put, terminal Z axis position is down to by safe Z axis again in terminal, all intermediate points run all on safe Z axis, that is, walk one
" door " font trajectory.The position of safe Z axis can prestore, and in general, the position of safe Z axis is without often becoming
It is dynamic.When needing to change the hoisting depth of mechanical arm, the position of the safe Z axis for prestoring need to be only changed, without manually adjusting
Mechanical arm is highly.In this case it is necessary to insert the centre directly over an original position in the motion path of mechanical arm
Point.Specifically, the corresponding first safe altitude position of the original position can be calculated;Wherein, the first safe altitude position
It is position directly over the original position, and with the vertical height of the original position more than or equal to default height value to put
Put;According to the original position, the first safe altitude position, the first mid-point position, the second mid-point position and target location
Generate the motion path of mechanical arm.In this case it is also possible to according to the first safe altitude position and the barrier
The radius of safety of distributed computer tool arm motion.Now, the maximum R of the radius of safetymaxCan be calculated according in the following manner:
Wherein, h is the height of safe Z axis, and l is the ultimate range of barrier and mechanical arm base central point.
Assuming that mechanical arm is raised in original position, as described above, then being also needed to machinery in target location
Arm drops to former height, at this point it is possible to insert an intermediate point in the surface of the target location of manipulator motion.Specifically,
The corresponding second safe altitude position in the target location can be calculated;Wherein, the second safe altitude position is described
Directly over target location, and it is more than or equal to the position of the default height value with the vertical height of the target location;Root
According to the original position, the first safe altitude position, the first mid-point position, the second safe altitude position, the second point midway
Put the motion path that mechanical arm is generated with target location.
After the motion path of generation mechanical arm, can be moved according to the motion path control machinery arm, make machinery
Arm moves to the aiming spot along the motion path from the initial point position automatically.When distribution of obstacles changes
When, only need to reacquire distribution of obstacles, you can the automatic motion path for cooking up mechanical arm again.When mechanical arm is changed,
Only need in systems re-enter the mechanical parameter (such as size of instrument pawl) of mechanical arm, you can cook up mechanical arm again automatically
Motion path.Without artificial teaching, simple to operate, efficiency high.
The actual motion path of mechanical arm is as shown in Figure 6 and Figure 7.Fig. 6 is not added on vertical direction for one embodiment
Intermediate point when path schematic diagram, Fig. 7 for one embodiment add vertical direction on intermediate point when path schematic diagram.
Dotted line represents the motion path of mechanical arm in figure.
The present invention has advantages below:
(1) it is simple to operate quick, it is not necessary to manual teaching.
(2) intermediate point that intelligence insertion needs, operational efficiency is higher.
(3) automatic management Z axis planning, caller only needs to be input into starting point and endpoint information.
(4) whether intelligent decision needs to change right-hand man's attitude, and operational efficiency is higher.
(5) various disorders thing layout is adapted to, only needs to be used immediately by updating environmental information after environmental change.
A pair of manipulator motion path generating system of the invention and manipulator motion path generating method one of the invention
Should, the technical characteristic and its advantage illustrated in the embodiment of above-mentioned manipulator motion path generating method are applied to machinery
In the embodiment of arm motion path generation system, hereby give notice that.
Each technical characteristic of embodiment described above can be combined arbitrarily, to make description succinct, not to above-mentioned reality
Apply all possible combination of each technical characteristic in example to be all described, as long as however, the combination of these technical characteristics is not deposited
In contradiction, the scope of this specification record is all considered to be.
One of ordinary skill in the art will appreciate that all or part of step in realizing above-described embodiment method can be
The hardware of correlation is instructed to complete by program.Described program can be stored in a computer read/write memory medium.
The program upon execution, including the step described in the above method.Described storage medium, including:ROM/RAM, magnetic disc, CD
Deng.
Embodiment described above only expresses several embodiments of the invention, and its description is more specific and detailed, but simultaneously
Can not therefore be construed as limiting the scope of the patent.It should be pointed out that coming for one of ordinary skill in the art
Say, without departing from the inventive concept of the premise, various modifications and improvements can be made, these belong to protection of the invention
Scope.Therefore, the protection domain of patent of the present invention should be determined by the appended claims.