Specific embodiment
Technical solution of the present invention is illustrated with reference to the accompanying drawing.
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, according to distributing position calculating machine arm movement
Radius of safety, according to the real work radius of the radius of safety and the size computer tool arm of the tool pawl of mechanical arm;
In one embodiment, it can establish the coordinate system of mechanical arm and distribution of obstacles, it is preferable that the coordinate system is
Three-dimensional system of coordinate.The central point of mechanical arm pedestal can be set as origin by the three-dimensional system of coordinate, mutually by two on horizontal plane
Vertical direction is set as x-axis and y-axis, and vertical direction is set as z-axis.It, can be more easily to machinery by establishing coordinate system
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, may include multiple coordinate values in the coordinate set, each for example, can establish a coordinate set
Coordinate value represents the position distribution of a barrier, which is represented by the form of (x, y, z).It is each for the ease of distinguishing
Each barrier can also be numbered for barrier, and the coordinate value of barrier is bound with reference numeral.
The radius of safety and real work radius are as shown in Figure 4.In figure, RmaxAnd RminRespectively indicate radius of safety most
Big value and minimum value, WRmaxAnd WRminThe maximum value and minimum value of real work radius are respectively indicated, A and B respectively indicate starting
Position and target position.The maximum value of the radius of safety can be calculated according to distribution of obstacles, can be according to point of barrier
The distance between the central point of barrier and mechanical arm pedestal around cloth position acquisition mechanical arm, according to the minimum of the distance
It is worth the radius of safety of calculating machine arm movement.It in one embodiment, can be by the minimum value r of the distanceminIt is set as the peace
The maximum value of full radius can also be less than r for one in another embodimentminNumerical value be set as 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 pedestal.In one embodiment, it can incite somebody to action
Maximum value r of the mechanical arm pedestal each point at a distance from mechanical arm base center pointmaxIt is set as the minimum of the radius of safety
Value can also be greater than r for one in another embodimentmaxNumerical value be set as the minimum value of the radius of safety.Due to machine
The tool pawl of tool arm has certain size, and therefore, the real work radius of mechanical arm can be calculated according to following manner:
WRmax=Rmax-d;
WRmin=Rmin+d;
Wherein, d is the size of the tool pawl of the mechanical arm.
S2 is calculated among the first of manipulator motion according to the initial position of the tool pawl and the real work radius
Point position calculates the second point midway of manipulator motion according to the target position of the tool pawl and the real work radius
It sets;
In one embodiment, when the first mid-point position of calculating machine arm movement, available first straight line and institute
State the first intersection point of the corresponding border circular areas of real work radius;Wherein, the first straight line be the initial position with it is described
The straight line that the central point of the pedestal of mechanical arm is linked to be;Position where one of them first intersection point is set as the point midway
It sets.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 Fig. 5, A ' is the intersection point namely the intermediate point.It, can when the second mid-point position of calculating machine arm movement
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 central point of the pedestal of the target position and the mechanical arm is linked to be;By the position where one of them second intersection point
It installs as the mid-point position.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 Fig. 5, B ' is the intersection point namely the intermediate point.If establishing three-dimensional seat
Mark system, the coordinate of available first intermediate point and the second intermediate point in the three-dimensional system of coordinate, and save.
In one embodiment, the limitation due to the mechanical parameter of mechanical arm itself to mechanical arm, if mechanical arm is kept
Current pose possibly can not move to target position, at this point, during mechanical arm moves to target position from initial position
Need replacing right-hand man's posture.When mechanical arm replaces right-hand man's posture, need completely to stretch entire mechanical arm, the space needed
Maximum, hand-off point processing is bad to collide.Therefore, other than first mid-point position and the second mid-point position,
It may also need that hand-off position is arranged in the motion path of mechanical arm.The hand-off position can be set in hand-off region
Interior, the distance between central point of hand-off region, that is, barrier and mechanical arm pedestal is greater than the area of the maximum length of the mechanical arm
Domain.That is, if the target position of the tool pawl not in the motion range of the mechanical arm current pose, can be in hand-off region
It chooses to a little less and is used as hand-off point, obtain hand-off position;Wherein, the hand-off region is barrier and mechanical arm pedestal
The distance between central point is greater than the region of the maximum length of the mechanical arm.When maximum length, that is, the mechanical arm stretches completely
Length.Hand-off region is as shown in figure 5, C1 and C2 in figure are hand-off point, and in practical situations, the quantity of hand-off point can also be with
It is 1 or other quantity.Wherein, when judging that mechanical arm keeps current pose whether can reach target position, machine can be read
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 position
Not in the area, then determine to need switching right and left hand posture.
S3, if the target position of the tool pawl in the motion range of the mechanical arm current pose, according to described
Beginning position, the first mid-point position, the second mid-point position and target position generate the motion path of mechanical arm.
Assuming that the target position of the tool pawl does not need to switch in the motion range of the mechanical arm current pose
Right-hand man's posture can be generated directly according to the initial position, the first mid-point position, the second mid-point position and target position
The motion path of mechanical arm.Assuming that the target position of the tool pawl is in the motion range of the mechanical arm current pose,
Switching right and left hand posture is needed, it can be according to calculated hand-off in the initial 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
Position generates the motion path after mechanical arm switching posture.
In practical situations, due to mechanical arm special construction, need first to be promoted to Z axis the position of safe Z axis in starting point
It sets, terminal Z axis position is down to by safe Z axis again in terminal, all intermediate points are all run on safe Z axis, that is, walk one
" door " font trajectory.The position of safe Z axis can be stored in advance, and under normal circumstances, 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 prestored need to be only changed, without manually adjusting
Mechanical arm height.In this case it is necessary to be inserted into the centre right above an initial position in the motion path of mechanical arm
Point.Specifically, the initial position corresponding first safe altitude position can be calculated;Wherein, first safe altitude position
Set be right above the initial position, and with the vertical height of the initial position be greater than or equal to preset height value position
It sets;According to the initial position, the first safe altitude position, the first mid-point position, the second mid-point position and target position
Generate the motion path of mechanical arm.In this case it is also possible to according to first safe altitude position and the barrier
The radius of safety of distributed computer tool arm movement.At this point, the maximum value R of the radius of safetymaxIt can be calculated according to following manner:
Wherein, h is the height of safe Z axis, and l is the maximum distance of barrier and mechanical arm base center point.
Assuming that being increased at initial position to mechanical arm, as described above, then also needing in target position will be mechanical
Arm drops to former height, at this point it is possible to which an intermediate point is inserted into the surface in the target position of manipulator motion.Specifically,
The target position corresponding second safe altitude position can be calculated;Wherein, second safe altitude position is described
Right above target position, and the position with the vertical height of the target position more than or equal to the preset height value;Root
According to the initial position, the first safe altitude position, the first mid-point position, the second safe altitude position, the second point midway
Set the motion path that mechanical arm is generated with target position.
After the motion path for generating mechanical arm, manipulator motion can be controlled according to the motion path, make machinery
Arm moves to the aiming spot from the initial point position along the motion path automatically.When distribution of obstacles changes
When, distribution of obstacles only need to be reacquired, the motion path of mechanical arm can be cooked up again automatically.When mechanical arm replacement,
The mechanical parameter (such as size of tool pawl) that mechanical arm need to only be re-entered in systems, can cook up mechanical arm again automatically
Motion path.It is easy to operate without artificial teaching, it is high-efficient.
The actual motion path of mechanical arm is as shown in Figure 6 and Figure 7.Fig. 6 is not being added on vertical direction for one embodiment
Intermediate point when path schematic diagram, Fig. 7 be one embodiment be added vertical direction on intermediate point when path schematic diagram.
Dotted line indicates the motion path of mechanical arm in figure.
The invention has the following advantages that
(1) easy to operate quick, do not need manual teaching.
(2) intermediate point that intelligence insertion needs, operational efficiency are higher.
(3) planning of management Z axis, caller only need to input starting point and endpoint information automatically.
(4) whether intelligent decision needs replacing right-hand man's posture, and operational efficiency is higher.
(5) various disorders object layout is adapted to, only need to update environmental information after environmental change to use immediately.
As shown in figure 8, the present invention provides a kind of manipulator motion path generating system, it may include:
First computing module 10, for obtaining the distributing position of the barrier around mechanical arm, according to the distributing position
The radius of safety of calculating machine arm movement, according to the reality of the radius of safety and the size computer tool arm of the tool pawl of mechanical arm
Border working radius;
In one embodiment, it can establish the coordinate system of mechanical arm and distribution of obstacles, it is preferable that the coordinate system is
Three-dimensional system of coordinate.The central point of mechanical arm pedestal can be set as origin by the three-dimensional system of coordinate, mutually by two on horizontal plane
Vertical direction is set as x-axis and y-axis, and vertical direction is set as z-axis.It, can be more easily to machinery by establishing coordinate system
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, may include multiple coordinate values in the coordinate set, each for example, can establish a coordinate set
Coordinate value represents the position distribution of a barrier, which is represented by the form of (x, y, z).It is each for the ease of distinguishing
Each barrier can also be numbered for barrier, and the coordinate value of barrier is bound with reference numeral.
The radius of safety and real work radius are as shown in Figure 4.In figure, RmaxAnd RminRespectively indicate radius of safety most
Big value and minimum value, WRmaxAnd WRminRespectively indicate the maximum value and minimum value of real work radius.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 the central point of tool arm pedestal, the radius of safety moved according to the minimum value calculating machine arm of the distance.One
It, can be by the minimum value r of the distance in a embodimentminIt is set as the maximum value of the radius of safety, in another embodiment
In, r can also be less than by oneminNumerical value be set as the maximum value of the radius of safety.The minimum value of the radius of safety can be with
It is calculated according to the size of mechanical arm pedestal.It in one embodiment, can be by the mechanical arm pedestal each point and mechanical arm bottom
The maximum value r of the distance of seat central pointmaxIt is set as the minimum value of the radius of safety, it in another embodiment, can also be by one
It is a to be greater than rmaxNumerical value be set as the minimum value of the radius of safety.Since the tool pawl of mechanical arm has certain size, machine
The real work radius of tool arm can be calculated according to following manner:
WRmax=Rmax-d;
WRmin=Rmin+d;
Wherein, d is the size of the tool pawl of the mechanical arm.
Second computing module 20, for according to the tool pawl initial position and the real work radius calculating machine
First mid-point position of arm movement calculates mechanical arm fortune according to the target position of the tool pawl and the real work radius
The second dynamic mid-point position;
In one embodiment, when the first mid-point position of calculating machine arm movement, available first straight line and institute
State the first intersection point of the corresponding border circular areas of real work radius;Wherein, the first straight line be the initial position with it is described
The straight line that the central point of the pedestal of mechanical arm is linked to be;Position where one of them first intersection point is set as the point midway
It sets.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 Fig. 5, A ' is the intersection point namely the intermediate point.It, can when the second mid-point position of calculating machine arm movement
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 central point of the pedestal of the target position and the mechanical arm is linked to be;By the position where one of them second intersection point
It installs as the mid-point position.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 Fig. 5, B ' is the intersection point namely the intermediate point.If establishing three-dimensional seat
Mark system, the coordinate of available first intermediate point and the second intermediate point in the three-dimensional system of coordinate, and save.
In one embodiment, the limitation due to the mechanical parameter of mechanical arm itself to mechanical arm, if mechanical arm is kept
Current pose possibly can not move to target position, at this point, during mechanical arm moves to target position from initial position
Need replacing right-hand man's posture.When mechanical arm replaces right-hand man's posture, need completely to stretch entire mechanical arm, the space needed
Maximum, hand-off point processing is bad to collide.Therefore, other than first mid-point position and the second mid-point position,
It may also need that hand-off position is arranged in the motion path of mechanical arm.The hand-off position can be set in hand-off region
Interior, the distance between central point of hand-off region, that is, barrier and mechanical arm pedestal is greater than the area of the maximum length of the mechanical arm
Domain.That is, settable one obtains module, if the target position of the tool pawl is not in the motion range of the mechanical arm current pose
Interior, which can choose to a little less in hand-off region and be used as hand-off point, obtain hand-off position;Wherein, described to change
Hand region is the region for the maximum length that the distance between central point of barrier and mechanical arm pedestal is greater than the mechanical arm.Institute
Length when stating maximum length i.e. mechanical arm stretching completely.Hand-off region is as shown in Figure 5.Wherein, judge mechanical arm holding work as
When whether preceding posture can reach target position, the mechanical parameter of mechanical arm can be read, and according to the mechanical parameter computer
The region that tool arm current pose can reach, if target position is not in the area, judgement needs switching right and left hand posture.
First generation module 30, if for the tool pawl target position the mechanical arm current pose movement model
In enclosing, the movement of mechanical arm is generated according to the initial position, the first mid-point position, the second mid-point position and target position
Path.
Assuming that the target position of the tool pawl does not need to switch in the motion range of the mechanical arm current pose
Right-hand man's posture can be generated directly according to the initial position, the first mid-point position, the second mid-point position and target position
The motion path of mechanical arm.Assuming that the target position of the tool pawl is in the motion range of the mechanical arm current pose,
Need switching right and left hand posture, settable second generation module, for according to the initial position, the first mid-point position and
The calculated hand-off position of second computing module 20 generates the motion path of mechanical arm current pose, and according to the hand-off point
Position, the second mid-point position and target position generate the motion path after mechanical arm switching posture.
In practical situations, due to mechanical arm special construction, need first to be promoted to Z axis the position of safe Z axis in starting point
It sets, terminal Z axis position is down to by safe Z axis again in terminal, all intermediate points are all run on safe Z axis, that is, walk one
" door " font trajectory.The position of safe Z axis can be stored in advance, and under normal circumstances, 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 prestored need to be only changed, without manually adjusting
Mechanical arm height.In this case it is necessary to be inserted into the centre right above an initial position in the motion path of mechanical arm
Point.Specifically, the initial position corresponding first safe altitude position can be calculated;Wherein, first safe altitude position
Set be right above the initial position, and with the vertical height of the initial position be greater than or equal to preset height value position
It sets;According to the initial position, the first safe altitude position, the first mid-point position, the second mid-point position and target position
Generate the motion path of mechanical arm.In this case it is also possible to according to first safe altitude position and the barrier
The radius of safety of distributed computer tool arm movement.At this point, the maximum value R of the radius of safetymaxIt can be calculated according to following manner:
Wherein, h is the height of safe Z axis, and l is the maximum distance of barrier and mechanical arm base center point.
Assuming that being increased at initial position to mechanical arm, as described above, then also needing in target position will be mechanical
Arm drops to former height, at this point it is possible to which an intermediate point is inserted into the surface in the target position of manipulator motion.Specifically,
The target position corresponding second safe altitude position can be calculated;Wherein, second safe altitude position is described
Right above target position, and the position with the vertical height of the target position more than or equal to the preset height value;Root
According to the initial position, the first safe altitude position, the first mid-point position, the second safe altitude position, the second point midway
Set the motion path that mechanical arm is generated with target position.
After the motion path for generating mechanical arm, manipulator motion can be controlled according to the motion path, make machinery
Arm moves to the aiming spot from the initial point position along the motion path automatically.When distribution of obstacles changes
When, distribution of obstacles only need to be reacquired, the motion path of mechanical arm can be cooked up again automatically.When mechanical arm replacement,
The mechanical parameter (such as size of tool pawl) that mechanical arm need to only be re-entered in systems, can cook up mechanical arm again automatically
Motion path.It is easy to operate without artificial teaching, it is high-efficient.
The actual motion path of mechanical arm is as shown in Figure 6 and Figure 7.Fig. 6 is not being added on vertical direction for one embodiment
Intermediate point when path schematic diagram, Fig. 7 be one embodiment be added vertical direction on intermediate point when path schematic diagram.
Dotted line indicates the motion path of mechanical arm in figure.
The invention has the following advantages that
(1) easy to operate quick, do not need manual teaching.
(2) intermediate point that intelligence insertion needs, operational efficiency are higher.
(3) planning of management Z axis, caller only need to input starting point and endpoint information automatically.
(4) whether intelligent decision needs replacing right-hand man's posture, and operational efficiency is higher.
(5) various disorders object layout is adapted to, only need to update environmental information after environmental change to use immediately.
Manipulator motion path generating system of the invention and manipulator motion path generating method one of the invention are a pair of
It answers, is suitable for machinery in the technical characteristic and its advantages of the embodiment elaboration of above-mentioned manipulator motion path generating method
Arm motion path generates in the embodiment of system, hereby give notice that.
Each technical characteristic of embodiment described above can be combined arbitrarily, for simplicity of description, not to above-mentioned reality
It applies 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, all should be considered as described in this specification.
Those of ordinary skill in the art will appreciate that implement the method for the above embodiments be can be with
Relevant hardware is instructed to complete by program.The program can be stored in a computer readable storage medium.
The program when being executed, includes the steps that described in the above method.The storage medium, comprising: ROM/RAM, magnetic disk, CD
Deng.
The embodiments described above only express several embodiments of the present invention, and the description thereof is more specific and detailed, but simultaneously
It cannot therefore be construed as limiting the scope of the patent.It should be pointed out that coming for those of ordinary skill in the art
It says, without departing from the inventive concept of the premise, various modifications and improvements can be made, these belong to protection of the invention
Range.Therefore, the scope of protection of the patent of the invention shall be subject to the appended claims.