CN109732609A  A kind of redundant degree of freedom manipulator motion planning method and device  Google Patents
A kind of redundant degree of freedom manipulator motion planning method and device Download PDFInfo
 Publication number
 CN109732609A CN109732609A CN201910135283.3A CN201910135283A CN109732609A CN 109732609 A CN109732609 A CN 109732609A CN 201910135283 A CN201910135283 A CN 201910135283A CN 109732609 A CN109732609 A CN 109732609A
 Authority
 CN
 China
 Prior art keywords
 joint
 freedom
 redundant degree
 mechanical arm
 degree
 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.)
 Pending
Links
 238000005457 optimization Methods 0.000 claims abstract description 171
 230000000875 corresponding Effects 0.000 claims abstract description 14
 238000001514 detection method Methods 0.000 claims description 39
 239000011159 matrix material Substances 0.000 claims description 24
 210000001503 Joints Anatomy 0.000 claims description 14
 235000013399 edible fruits Nutrition 0.000 claims 2
 238000004422 calculation algorithm Methods 0.000 description 7
 238000004364 calculation method Methods 0.000 description 6
 238000000034 method Methods 0.000 description 6
 238000010586 diagram Methods 0.000 description 5
 238000004458 analytical method Methods 0.000 description 4
 238000004590 computer program Methods 0.000 description 4
 230000001276 controlling effect Effects 0.000 description 4
 239000002245 particle Substances 0.000 description 3
 238000005516 engineering process Methods 0.000 description 2
 230000002068 genetic Effects 0.000 description 2
 230000015572 biosynthetic process Effects 0.000 description 1
 239000000969 carrier Substances 0.000 description 1
 238000005755 formation reaction Methods 0.000 description 1
 230000004048 modification Effects 0.000 description 1
 238000006011 modification reaction Methods 0.000 description 1
 230000035772 mutation Effects 0.000 description 1
 238000002922 simulated annealing Methods 0.000 description 1
 238000006467 substitution reaction Methods 0.000 description 1
Abstract
The embodiment of the present application provides a kind of redundant degree of freedom manipulator motion planning method and device, and redundant degree of freedom manipulator motion planning method includes: to obtain pose data corresponding with the expected pose of end effector of the redundant degree of freedom mechanical arm；Dimensionreduction treatment is carried out to the 7 degree of freedom optimization problem in the redundant degree of freedom manipulator motion planning according to preset constraint condition, obtains two dimensional optimization problem；The two dimensional optimization problem is solved, solving result is obtained；According to the pose data and the solving result, the control program of the redundant degree of freedom mechanical arm is obtained.The application redundant degree of freedom manipulator motion planning method and device, dimensionreduction treatment can be carried out to the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning, it reduces and calculates the time, reduce difficulty in computation, so as to more rapidly, more easily obtain the control program of redundant degree of freedom mechanical arm, motion planning is completed, the total time of motion planning is reduced, improves planning efficiency.
Description
Technical field
This application involves field of intelligent control technology, in particular to a kind of redundant degree of freedom manipulator motion planning
Method and device.
Background technique
Redundant degree of freedom mechanical arm compares six connecting rods with 7 degree of freedom with the mechanical arm of seven connecting rods
Mechanical arm, additional connecting rod allows mechanical arm to hide certain specific targets, reaches specific position convenient for end effector,
More flexible it can adapt to certain particular job environment.
Motion planning (Motion Planning) is exactly to find one between given position A and position B for robot
Meet the path of constraint condition, this constraint can be collisionless, path is most short, mechanical work is minimum etc..
Currently, the motion planning of redundant degree of freedom mechanical arm passes through the progress of Moveit motion planning frame, Moveit mostly
Motion planning frame mainly uses Newton method and Sequential Quadratic Programming method to come in computing redundancy degreeoffreedom manipulator motion planning
Inverse Kinematics Solution, however, using Newton method and Sequential Quadratic Programming method come in computing redundancy degreeoffreedom manipulator motion planning
It is longer that Inverse Kinematics Solution often calculates the time, and difficulty in computation is higher, so as to cause redundant degree of freedom manipulator motion planning
Total time is longer, planning efficiency is lower.
Summary of the invention
The embodiment of the present application is designed to provide a kind of redundant degree of freedom manipulator motion planning method and device, can be right
Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning carries out dimensionreduction treatment, reduces and calculates the time, reduces difficulty in computation,
So as to more rapidly, more easily obtain the control program of redundant degree of freedom mechanical arm, motion planning is completed, movement rule are reduced
The total time drawn improve planning efficiency.
In a first aspect, the embodiment of the present application provides a kind of redundant degree of freedom manipulator motion planning method, comprising:
Obtain pose data corresponding with the expected pose of end effector of the redundant degree of freedom mechanical arm；
The 7 degree of freedom optimization problem in the redundant degree of freedom manipulator motion planning is carried out according to preset constraint condition
Dimensionreduction treatment obtains two dimensional optimization problem；
The two dimensional optimization problem is solved, solving result is obtained；
According to the pose data and the solving result, the control program of the redundant degree of freedom mechanical arm is obtained.
During abovementioned realization, by preset constraint condition to the movement in redundant degree of freedom manipulator motion planning
The 7 degree of freedom optimization problem for learning inverse solution carries out dimensionreduction treatment, obtains two dimensional optimization problem；Two dimensional optimization problem is solved again, is obtained
To solving result；According to the pose data corresponding with the expected pose of the end effector of redundant degree of freedom mechanical arm of acquisition
With aforementioned solving result, the control program of redundant degree of freedom mechanical arm is obtained, the solution time of two dimensional optimization problem is shorter, solves
Difficulty is lower, reduces the calculating time of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning, reduces redundancy certainly
By the difficulty in computation of the Inverse Kinematics Solution in degree manipulator motion planning, thus in the motion planning of redundant degree of freedom mechanical arm
The control program of redundant degree of freedom mechanical arm can more rapidly, be more easily obtained, motion planning is completed, reduces motion planning
Total time improve planning efficiency.
Further, the preset constraint condition includes:
Seven joints of the redundant degree of freedom mechanical arm are divided into three groups, wherein first group includes the redundant degree of freedom
First joint of mechanical arm to the 4th joint, second group include the redundant degree of freedom mechanical arm the 5th joint and the 6th close
Section, third group includes the 7th joint of the redundant degree of freedom mechanical arm；And
The position in the 5th joint of redundant degree of freedom mechanical arm is determined with the pose data and the angle in the 7th joint；
And
The third arthrodesis of the redundant degree of freedom mechanical arm；And
The 4th joint of redundant degree of freedom mechanical arm and the axis in the 5th joint and the axis in the 5th joint and the 6th joint
Line is perpendicular.
During abovementioned realization, seven joints of redundant degree of freedom mechanical arm are divided into three groups, first group include redundancy from
By the first joint to the 4th joint of degree mechanical arm, second group include redundant degree of freedom mechanical arm the 5th joint and the 6th close
Section, third group includes the 7th joint of redundant degree of freedom mechanical arm；The position in the 5th joint of redundant degree of freedom mechanical arm is with abovementioned
Pose data and the angle in the 7th joint determine；The third arthrodesis of redundant degree of freedom mechanical arm, i.e. third joint are opposite
It is fixed, thus the first joint of redundant degree of freedom mechanical arm, second joint, the 4th joint angle there are limited group of solutions；If
After the angle in the first joint of redundant degree of freedom mechanical arm to the 4th joint determines, the 5th joint of redundant degree of freedom mechanical arm with
The angle in the 6th joint can be according to the 4th joint and the axis in the 5th joint and the axis perpendicular in the 5th joint and the 6th joint
Limited group of solution is obtained, Such analysis is according to preset constraint condition to the kinematics in redundant degree of freedom manipulator motion planning
The 7 degree of freedom optimization problem of inverse solution carries out dimensionreduction treatment, the thinking of two dimensional optimization problem is obtained, under this constraint condition, it is possible to understand that
, the angle in the angle in third joint and the 7th joint is free variable, and the angle in remaining joint is parameter, and redundancy is free
The third joint of degree mechanical arm is affected to the arm type of mechanical arm, this constraint condition can not only be preferably by redundant degree of freedom machine
The 7 degree of freedom optimization problem dimensionality reduction of Inverse Kinematics Solution in tool arm motion planning is at two dimensional optimization problem, additionally it is possible to which guarantee constrains herein
Under the conditions of redundant degree of freedom mechanical arm flexibility ratio with higher.
Further, described according to preset constraint condition to seven in the redundant degree of freedom manipulator motion planning
It is described that the two dimensional optimization problem is carried out after tieing up the step of optimization problem carries out dimensionreduction treatment, obtains two dimensional optimization problem
Before the step of solving, obtaining solving result, the method also includes:
Establish the collision detection model in the 5th joint to the 7th joint of the redundant degree of freedom mechanical arm；
Using the collision detection model as foundation, to the 5th joint to the 7th joint of the redundant degree of freedom mechanical arm into
Row collision detection.
During abovementioned realization, the 7 degree of freedom optimization of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning is asked
After dimensionality reduction is inscribed into two dimensional optimization problem, the collision detection mould in the 5th joint to the 7th joint of redundant degree of freedom mechanical arm is established
Type, and using collision detection model as foundation, collision detection is carried out to the 5th joint to the 7th joint of redundant degree of freedom mechanical arm,
It can be reduced calculation amount when two dimensional optimization problem solves, to further shorten the solution time of two dimensional optimization problem, reduce
The solution difficulty of two dimensional optimization problem, and then further reduce the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning
The calculating time, reduce redundant degree of freedom manipulator motion planning in Inverse Kinematics Solution difficulty in computation, further to subtract
The total time of few motion planning, improve planning efficiency.
Further, the Mathematical Planning form of the two dimensional optimization problem are as follows:
s.t.m_{l14}=m_{r14},m_{l24}=m_{r24},m_{l34}=m_{r34}
Wherein, q_{3}For the angle in the third joint of the redundant degree of freedom mechanical arm, q_{7}For the redundant degree of freedom mechanical arm
The 7th joint angle, m_{r11}For M_{r}The element that the 1st row the 1st arranges in transfer matrix, m_{l21}For M_{l}The 2nd row the 1st arranges in transfer matrix
Element.
During abovementioned realization, the Mathematical Planning form of two dimensional optimization problem be under certain preset constraint condition,
It is superfluous using the angle in the third joint of redundant degree of freedom mechanical arm and the angle in the 7th joint as the Mathematical Planning form of free variable
The third joint of cofree degree mechanical arm is affected to the arm type of mechanical arm, in the Mathematical Planning form of this two dimensional optimization problem
Under, redundant degree of freedom mechanical arm can flexibility ratio with higher.
Further, described the step of solving to the two dimensional optimization problem, obtain solving result, includes:
By global optimization method and/or local optimization methods, the two dimensional optimization problem is solved, is solved
As a result.
During abovementioned realization, the 7 degree of freedom optimization of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning is asked
After dimensionality reduction is inscribed into two dimensional optimization problem, the solution difficulty of two dimensional optimization problem is low compared with the solution difficulty of two dimensional optimization problem to be obtained
More, the solution of two dimensional optimization problem can be solved by global optimization method and/or a variety of solution modes of local optimization methods.
Second aspect, the embodiment of the present application provide a kind of redundant degree of freedom manipulator motion planning device, comprising:
Acquiring unit, it is corresponding with the expected pose of end effector of the redundant degree of freedom mechanical arm for obtaining
Pose data；
Processing unit, for according to preset constraint condition to the 7 degree of freedom in the redundant degree of freedom manipulator motion planning
Optimization problem carries out dimensionreduction treatment, obtains two dimensional optimization problem；
It solves unit and obtains solving result for solving to the two dimensional optimization problem；
Generation unit, for obtaining the redundant degree of freedom mechanical arm according to the pose data and the solving result
Control program.
During abovementioned realization, by preset constraint condition to the movement in redundant degree of freedom manipulator motion planning
The 7 degree of freedom optimization problem for learning inverse solution carries out dimensionreduction treatment, obtains two dimensional optimization problem；Two dimensional optimization problem is solved again, is obtained
To solving result；According to the pose data corresponding with the expected pose of the end effector of redundant degree of freedom mechanical arm of acquisition
With aforementioned solving result, the control program of redundant degree of freedom mechanical arm is obtained, the solution time of two dimensional optimization problem is shorter, solves
Difficulty is lower, reduces the calculating time of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning, reduces redundancy certainly
By the difficulty in computation of the Inverse Kinematics Solution in degree manipulator motion planning, thus in the motion planning of redundant degree of freedom mechanical arm
The control program of redundant degree of freedom mechanical arm can more rapidly, be more easily obtained, motion planning is completed, reduces motion planning
Total time improve planning efficiency.
Further, the preset constraint condition includes:
Seven joints of the redundant degree of freedom mechanical arm are divided into three groups, wherein first group includes the redundant degree of freedom
First joint of mechanical arm to the 4th joint, second group include the redundant degree of freedom mechanical arm the 5th joint and the 6th close
Section, third group includes the 7th joint of the redundant degree of freedom mechanical arm；And
The position in the 5th joint of redundant degree of freedom mechanical arm is determined with the pose data and the angle in the 7th joint；
And
The third arthrodesis of the redundant degree of freedom mechanical arm；And
The 4th joint of redundant degree of freedom mechanical arm and the axis in the 5th joint and the axis in the 5th joint and the 6th joint
Line is perpendicular.
During abovementioned realization, seven joints of redundant degree of freedom mechanical arm are divided into three groups, first group include redundancy from
By the first joint to the 4th joint of degree mechanical arm, second group include redundant degree of freedom mechanical arm the 5th joint and the 6th close
Section, third group includes the 7th joint of redundant degree of freedom mechanical arm；The position in the 5th joint of redundant degree of freedom mechanical arm is with abovementioned
Pose data and the angle in the 7th joint determine；The third arthrodesis of redundant degree of freedom mechanical arm, i.e. third joint are opposite
It is fixed, thus the first joint of redundant degree of freedom mechanical arm, second joint, the 4th joint angle there are limited group of solutions；If
After the angle in the first joint of redundant degree of freedom mechanical arm to the 4th joint determines, the 5th joint of redundant degree of freedom mechanical arm with
The angle in the 6th joint can be according to the 4th joint and the axis in the 5th joint and the axis perpendicular in the 5th joint and the 6th joint
Limited group of solution is obtained, Such analysis is according to preset constraint condition to the kinematics in redundant degree of freedom manipulator motion planning
The 7 degree of freedom optimization problem of inverse solution carries out dimensionreduction treatment, the thinking of two dimensional optimization problem is obtained, under this constraint condition, it is possible to understand that
, the angle in the angle in third joint and the 7th joint is free variable, and the angle in remaining joint is parameter, and redundancy is free
The third joint of degree mechanical arm is affected to the arm type of mechanical arm, this constraint condition can not only be preferably by redundant degree of freedom machine
The 7 degree of freedom optimization problem dimensionality reduction of Inverse Kinematics Solution in tool arm motion planning is at two dimensional optimization problem, additionally it is possible to which guarantee constrains herein
Under the conditions of redundant degree of freedom mechanical arm flexibility ratio with higher.
Further, the redundant degree of freedom manipulator motion planning device further include:
Modeling unit, the collision detection mould in the 5th joint to the 7th joint for establishing the redundant degree of freedom mechanical arm
Type；
Detection unit, for being closed to the 5th of the redundant degree of freedom mechanical arm the using the collision detection model as foundation
Section to the 7th joint carries out collision detection.
During abovementioned realization, the 7 degree of freedom optimization of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning is asked
After dimensionality reduction is inscribed into two dimensional optimization problem, the collision detection mould in the 5th joint to the 7th joint of redundant degree of freedom mechanical arm is established
Type, and using collision detection model as foundation, collision detection is carried out to the 5th joint to the 7th joint of redundant degree of freedom mechanical arm,
It can be reduced calculation amount when two dimensional optimization problem solves, to further shorten the solution time of two dimensional optimization problem, reduce
The solution difficulty of two dimensional optimization problem, and then further reduce the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning
The calculating time, reduce redundant degree of freedom manipulator motion planning in Inverse Kinematics Solution difficulty in computation, further to subtract
The total time of few motion planning, improve planning efficiency.
Further, the Mathematical Planning form of the two dimensional optimization problem are as follows:
s.t.m_{l14}=m_{r14},m_{l24}=m_{r24},m_{l34}=m_{r34}
Wherein, q_{3}For the angle in the third joint of the redundant degree of freedom mechanical arm, q_{7}For the redundant degree of freedom mechanical arm
The 7th joint angle, m_{r11}For M_{r}The element that the 1st row the 1st arranges in transfer matrix, m_{l21}For M_{l}The 2nd row the 1st arranges in transfer matrix
Element.
During abovementioned realization, the Mathematical Planning form of two dimensional optimization problem be under certain preset constraint condition,
It is superfluous using the angle in the third joint of redundant degree of freedom mechanical arm and the angle in the 7th joint as the Mathematical Planning form of free variable
The third joint of cofree degree mechanical arm is affected to the arm type of mechanical arm, in the Mathematical Planning form of this two dimensional optimization problem
Under, redundant degree of freedom mechanical arm can flexibility ratio with higher.
Further, the solution unit is specifically used for:
By global optimization method and/or local optimization methods, the two dimensional optimization problem is solved, is solved
As a result.
During abovementioned realization, the 7 degree of freedom optimization of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning is asked
After dimensionality reduction is inscribed into two dimensional optimization problem, the solution difficulty of two dimensional optimization problem is low compared with the solution difficulty of two dimensional optimization problem to be obtained
More, the solution of two dimensional optimization problem can be solved by global optimization method and/or a variety of solution modes of local optimization methods.
The third aspect, the embodiment of the present application provides a kind of computer equipment, including memory and processor, described to deposit
Reservoir is for storing computer program, and the processor runs the computer program so that computer equipment execution is abovementioned
Redundant degree of freedom manipulator motion planning method.
Fourth aspect, the embodiment of the present application provide a kind of computer readable storage medium, are stored with abovementioned calculating
Computer program used in machine equipment.
Detailed description of the invention
Technical solution in ord to more clearly illustrate embodiments of the present application will make below to required in the embodiment of the present application
Attached drawing is briefly described, it should be understood that the following drawings illustrates only some embodiments of the application, therefore should not be seen
Work is the restriction to range, for those of ordinary skill in the art, without creative efforts, can be with
Other relevant attached drawings are obtained according to these attached drawings.
Fig. 1 is a kind of flow chart for redundant degree of freedom manipulator motion planning method that the embodiment of the present application one provides；
Fig. 2 is a kind of flow chart for redundant degree of freedom manipulator motion planning method that the embodiment of the present application two provides；
Fig. 3 is a kind of structural representation for redundant degree of freedom manipulator motion planning device that the embodiment of the present application three provides
Figure.
Specific embodiment
Below in conjunction with the attached drawing in the embodiment of the present application, technical solutions in the embodiments of the present application is described.
It should also be noted that similar label and letter indicate similar terms in following attached drawing, therefore, once a certain Xiang Yi
It is defined in a attached drawing, does not then need that it is further defined and explained in subsequent attached drawing.Meanwhile the application's
In description, term " first ", " second " etc. are only used for distinguishing description, are not understood to indicate or imply relative importance.
For the problems of the prior art, this application provides a kind of redundant degree of freedom manipulator motion planning method and dresses
Set, by preset constraint condition to the 7 degree of freedom optimization problem of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning into
Row dimensionreduction treatment, obtains two dimensional optimization problem；Two dimensional optimization problem is solved again, obtains solving result；According to acquisition
Pose data corresponding with the expected pose of the end effector of redundant degree of freedom mechanical arm and aforementioned solving result, obtain superfluous
The solution time of the control program of cofree degree mechanical arm, two dimensional optimization problem is shorter, and solution difficulty is lower, reduces redundancy certainly
By the calculating time of the Inverse Kinematics Solution in degree manipulator motion planning, reduce in redundant degree of freedom manipulator motion planning
The difficulty in computation of Inverse Kinematics Solution, to can more rapidly, more easily be obtained in the motion planning of redundant degree of freedom mechanical arm
To the control program of redundant degree of freedom mechanical arm, motion planning is completed, the total time of motion planning is reduced, improves planning efficiency.
Embodiment one
Referring to Fig. 1, Fig. 1 is a kind of process of redundant degree of freedom manipulator motion planning method provided by the embodiments of the present application
Figure.
The redundant degree of freedom manipulator motion planning method of the embodiment of the present application, includes the following steps:
Step S101 obtains pose number corresponding with the expected pose of the end effector of redundant degree of freedom mechanical arm
According to.
Redundant degree of freedom mechanical arm usually has seven connecting rods, an end effector and seven joints, redundant degree of freedom
Seven connecting rods of mechanical arm be respectively first connecting rod, second connecting rod, third connecting rod, fourth link, the 5th connecting rod, sixbar linkage and
Sevenlink assembly；The sevenlink assembly of the end effector connection redundant degree of freedom mechanical arm of redundant degree of freedom mechanical arm；Redundancy is free
Spending between the first connecting rod and second connecting rod of mechanical arm has the first joint, and the second connecting rod and third of redundant degree of freedom mechanical arm connect
There is second joint between bar, there is third joint between the third connecting rod and fourth link of redundant degree of freedom mechanical arm, redundancy is free
Spending between the fourth link and the 5th connecting rod of mechanical arm has the 4th joint, and the 5th connecting rod of redundant degree of freedom mechanical arm and the 6th connects
There is the 5th joint between bar, the of redundant degree of freedom mechanical arm has the 6th joint between sixbar linkage and sevenlink assembly, redundancy is free
The of degree mechanical arm has the 7th joint between sevenlink assembly and end effector.
The expected pose of end effector, the i.e. desired locations and posture of end effector, it can be understood as, redundancy is free
Degree mechanical arm carries out the expectation of the end effector of redundant degree of freedom mechanical arm when certain planning tasks under certain application scenarios
Position and posture, for example, redundant degree of freedom mechanical arm carries out redundancy freedom when cargo is taken under the application scenarios of goods sorting
Spend the desired locations and posture of the end effector of mechanical arm.
In the present embodiment, the present embodiment is said accordingly using sawyer redundant degree of freedom mechanical arm as example
It is bright.
Step S102, according to preset constraint condition to the 7 degree of freedom optimization problem in redundant degree of freedom manipulator motion planning
Dimensionreduction treatment is carried out, two dimensional optimization problem is obtained.
In the present embodiment, the angle in the ith joint of redundant degree of freedom mechanical arm is q_{i}, the of redundant degree of freedom mechanical arm
One joint is A, and the second joint of redundant degree of freedom mechanical arm is B, and the third joint of redundant degree of freedom mechanical arm is C, and redundancy is certainly
It is D by the 4th joint of degree mechanical arm, the 5th joint of redundant degree of freedom mechanical arm is E, the 6th of redundant degree of freedom mechanical arm the
Joint is F, and the 7th joint of redundant degree of freedom mechanical arm is G, and the end effector of redundant degree of freedom mechanical arm is T.
Redundant degree of freedom mechanical arm around the homogeneous spin matrix of three reference axis be respectively trotx (), troty (),
Trotz (), translation matrix are transl (x, y, z)； OA =l_{0},  AB =l_{1},  BC =l_{2},  CD =l_{3},  DE =l_{4},
 EF =l_{5},  FG =l_{6},  GT =l_{7}；Remember q=[q_{1} q_{2} … q_{7}]^{T}For joint angles composition column vector,Position and posture for the corresponding end effector of joint angles vector.
It is as follows to define coordinate system:
Oxyz basis coordinates system, wherein origin is located at base bottom center, and xaxis is directed straight ahead, yaxis straight up, z
Axis meets the righthand rule；
O_{A}Xyz origin is A point, Oxyz to O_{A}The homogeneous rotational translation matrix of xyz is T_{O2A}=transl (0,0, l_{0})；
O_{B}Xyz origin is B point, O_{A}Xyz to O_{B}The homogeneous rotational translation matrix of xyz is T_{A2B}=trotz (q_{1})transl
(0,l_{1},0)；
O_{C}Xyz origin is C point, O_{B}Xyz to O_{C}The homogeneous rotational translation matrix of xyz is T_{B2C}=troty (q_{2})transl
(l_{2},0,0)；
O_{D}Xyz origin is D point, O_{C}Xyz to O_{D}The homogeneous rotational translation matrix of xyz is T_{C2D}=trotx (q_{3})transl
(0,l_{3},0)；
O_{E}Xyz origin is E point, O_{D}Xyz to O_{E}The homogeneous rotational translation matrix of xyz is T_{D2E}=troty (q_{4})transl
(l_{4},0,0)；
O_{F}Xyz origin is F point, O_{E}Xyz to O_{F}The homogeneous rotational translation matrix of xyz is T_{E2F}=trotx (q_{5})transl
(0,l_{5},0)；
O_{G}Xyz origin is G point, O_{F}Xyz to O_{G}The homogeneous rotational translation matrix of xyz is T_{F2G}=troty (q_{6})transl
(l_{6},0,0)；
O_{T}Xyz origin is T point, O_{G}Xyz to O_{T}The homogeneous rotational translation matrix of xyz is T_{G2T}=trotx (q_{7})transl
(l_{7},0,0)；
Homogeneous rotational translation matrix of the note basis coordinates system to end effector are as follows:
T can be obtained according to chain rule_{O2T}=T_{O2A}T_{A2B}T_{B2C}T_{C2D}T_{D2E}T_{E2F}T_{F2G}T_{G2T}, then have
This is kinematical equation.
It should be noted that being calculated according to (1.1) formula superfluous in each joint angles q of known redundant degree of freedom mechanical arm
The pose of the end effector of cofree degree mechanical arm, as solution positive kinematics problem x=f_{fk}(q)；Known redundant degree of freedom
The pose x of the end effector of mechanical arm is as solved according to (1.1) formula each joint angles q of computing redundancy degreeoffreedom manipulator
Inverse Kinematics Problem q=f_{ik}(x)。
In the present embodiment, the expected pose of the end effector of redundant degree of freedom mechanical arm
Using sawyer redundant degree of freedom mechanical arm as example, sawyer redundant degree of freedom mechanical arm has 7 freedom degrees,
The Mathematical Planning form of the 7 degree of freedom optimization problem of Inverse Kinematics Solution in motion planning are as follows:
Q is the free variable of 7 dimensions, when carrying out the solution of 7 degree of freedom optimization problem, it is longer to solve the calculating time, and solve meter
It is higher to calculate difficulty.
In the present embodiment, abovementioned preset constraint condition includes:
Seven joints of redundant degree of freedom mechanical arm are divided into three groups, wherein first group includes redundant degree of freedom mechanical arm
First joint to the 4th joint, second group include redundant degree of freedom mechanical arm the 5th joint and the 6th joint, third group includes
7th joint of redundant degree of freedom mechanical arm；And
The above rheme appearance data in position in the 5th joint of redundant degree of freedom mechanical arm and the angle in the 7th joint determine；And
The third arthrodesis of redundant degree of freedom mechanical arm；And
The 4th joint of redundant degree of freedom mechanical arm and the axis in the 5th joint and the axis phase in the 5th joint and the 6th joint
Vertically.
Under abovementioned constraint condition, seven joints of redundant degree of freedom mechanical arm are divided into three groups, first group include redundancy from
By the first joint to the 4th joint of degree mechanical arm, second group include redundant degree of freedom mechanical arm the 5th joint and the 6th close
Section, third group includes the 7th joint of redundant degree of freedom mechanical arm；The position in the 5th joint of redundant degree of freedom mechanical arm is with abovementioned
Pose data and the angle in the 7th joint determine；The third arthrodesis of redundant degree of freedom mechanical arm, i.e. third joint are opposite
It is fixed, thus the first joint of redundant degree of freedom mechanical arm, second joint, the 4th joint angle there are limited group of solutions；If
After the angle in the first joint of redundant degree of freedom mechanical arm to the 4th joint determines, the 5th joint of redundant degree of freedom mechanical arm with
The angle in the 6th joint can be according to the 4th joint and the axis in the 5th joint and the axis perpendicular in the 5th joint and the 6th joint
Limited group of solution is obtained, Such analysis is according to preset constraint condition to the kinematics in redundant degree of freedom manipulator motion planning
The 7 degree of freedom optimization problem of inverse solution carries out dimensionreduction treatment, the thinking of two dimensional optimization problem is obtained, under this constraint condition, it is possible to understand that
, the angle in the angle in third joint and the 7th joint is free variable, and the angle in remaining joint is parameter, and redundancy is free
The third joint of degree mechanical arm is affected to the arm type of mechanical arm, this constraint condition can not only be preferably by redundant degree of freedom machine
The 7 degree of freedom optimization problem dimensionality reduction of Inverse Kinematics Solution in tool arm motion planning is at two dimensional optimization problem, additionally it is possible to which guarantee constrains herein
Under the conditions of redundant degree of freedom mechanical arm flexibility ratio with higher.
It should be noted that the other embodiments of the application can also use other preset constraint conditions, as long as other
Preset constraint condition is reasonable, and under other reasonable preset constraint conditions, in redundant degree of freedom manipulator motion planning
Inverse Kinematics Solution 7 degree of freedom optimization problem dimensionality reduction at two dimensional optimization problem.
Under abovementioned constraint condition, the threedimensional coordinate of note E point is e_{x}、e_{y}、e_{z}, (1.1) formula is changed to:
In (1.3) formula,It is known that the equation left side includes q_{5}、q_{6}、q_{7}Three unknown quantitys, the equation left side include q_{1}、q_{2}、
q_{3}、q_{4}Four unknown quantitys.
It enables,
M_{r}(q_{1},q_{2},q_{3},q_{4})=T_{O2A}T_{A2B}T_{B2C}T_{C2D}T_{D2E} (1.5)
(1.3) formula is equivalent to solve the Mathematical Planning form of 7 degree of freedom optimization problem:
Wherein, M_{l}And M_{r}For transfer matrix,
According to abovementioned geometrical constraint, the 7 degree of freedom optimization problem in redundant degree of freedom manipulator motion planning can be carried out
Dimensionreduction treatment obtains two dimensional optimization problem.
The Mathematical Planning form of two dimensional optimization problem are as follows:
s.t.m_{l14}=m_{r14},m_{l24}=m_{r24},m_{l34}=m_{r34}
Wherein, q_{3}For the angle in the third joint of redundant degree of freedom mechanical arm, q_{7}The 7th for redundant degree of freedom mechanical arm closes
The angle of section, m_{r11}For M_{r}The element that the 1st row the 1st arranges in transfer matrix, m_{l21}For M_{l}The element that the 2nd row the 1st arranges in transfer matrix.
In the present embodiment, the Mathematical Planning form of abovementioned two dimensional optimization problem is under abovementioned constraint condition, with redundancy
The angle in the third joint of degreeoffreedom manipulator and the angle in the 7th joint are the Mathematical Planning form of free variable, due to redundancy
The third joint of degreeoffreedom manipulator is affected to the arm type of mechanical arm, in the Mathematical Planning form of this two dimensional optimization problem
Under, redundant degree of freedom mechanical arm can flexibility ratio with higher.
It should be noted that be also applicable to other dimensionality reduction thinkings similar for the Mathematical Planning form of abovementioned two dimensional optimization problem
Constraint condition, for example, on the basis of abovementioned constraint condition, by the axis in the 4th joint and the 5th joint of redundant degree of freedom mechanical arm
The axis perpendicular of line and the 5th joint and the 6th joint is changed to the axis in the 4th joint and the 5th joint of redundant degree of freedom mechanical arm
Line and the axis in the 5th joint and the 6th joint are angled, can be 30 ° or 45 ° etc..
The other embodiments of the application can also use the Mathematical Planning form of other two dimensional optimization problems, and specific two dimension is excellent
The Mathematical Planning form of change problem free variable according to set by other preset constraint conditions and specific constraint condition obtain.
Step S103 solves abovementioned two dimensional optimization problem, obtains solving result.
In the present embodiment, can by global optimization method and/or local optimization methods, to abovementioned two dimensional optimization problem into
Row solves, and obtains solving result.
As an alternative embodiment, the solution of two dimensional optimization problem, can be solved, grain by particle swarm algorithm
Swarm optimization is a kind of global optimization method.
One kind that particle swarm algorithm belongs to evolution algorithm is similar with simulated annealing, it is also to lead to from RANDOM SOLUTION
It crosses iteration and finds optimal solution, it is also the quality of solution to be evaluated by fitness, but it is more simpler than genetic algorithm rule, it
There is no " intersection " (Crossover) of genetic algorithm and " variation " (Mutation) operation, what it was arrived by following current search
Optimal value finds global optimum.This algorithm has many advantages, such as that realization is easy, precision is high, convergence is fast, and is solving reality
There is superiority in problem.
It should be noted that in the present embodiment, the solution of two dimensional optimization problem can also pass through other global optimization sides
Method, for example, gridding method.
As an alternative embodiment, the solution of two dimensional optimization problem, can be asked by Sequential Quadratic Programming method
Solution, Sequential Quadratic Programming method is a kind of local optimization methods.
It should be noted that in the present embodiment, the solution of two dimensional optimization problem can also pass through other local optimum sides
Method, for example, Newton method.
As an alternative embodiment, the solution of two dimensional optimization problem, can first pass through particle swarm algorithm and obtain the overall situation
Approximate solution, then really solved by Sequential Quadratic Programming method refinement.
As an alternative embodiment, the solution of two dimensional optimization problem, can first pass through gridding method and obtain global approximation
Solution, then really solved by Sequential Quadratic Programming method refinement.
During abovementioned realization, the 7 degree of freedom optimization of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning is asked
After dimensionality reduction is inscribed into two dimensional optimization problem, the solution difficulty of two dimensional optimization problem is low compared with the solution difficulty of two dimensional optimization problem to be obtained
More, the solution of two dimensional optimization problem can be solved by global optimization method and/or a variety of solution modes of local optimization methods.
Step S104 obtains the control program of redundant degree of freedom mechanical arm according to abovementioned pose data and solving result.
The control program of redundant degree of freedom mechanical arm for control redundant degree of freedom mechanical arm under certain application scenarios into
Certain desired operation of row, with sawyer redundant degree of freedom mechanical arm as an example, can be free for controlling sawyer redundancy
Degree mechanical arm is taken the control program of certain cargo or the cargo of certain position under the application scenarios of goods sorting.
Outside abovementioned example, the control program of redundant degree of freedom mechanical arm is also possible to for controlling other redundancies freedom
Degree mechanical arm is completed to open or close the operation etc. of certain switch in premises, for example, closing premises master bedroom electric light
Switch.
It should be noted that the control program of the redundant degree of freedom mechanical arm obtained according to pose data and solving result,
It can not only be obtained simply by pose data and solving result, it can also be by combining other data or information to obtain.
The redundant degree of freedom manipulator motion planning method of the embodiment of the present application, by preset constraint condition to redundancy from
Dimensionreduction treatment is carried out by the 7 degree of freedom optimization problem of the Inverse Kinematics Solution in degree manipulator motion planning, obtains two dimensional optimization problem；
Two dimensional optimization problem is solved again, obtains solving result；According to the end execution with redundant degree of freedom mechanical arm of acquisition
The corresponding pose data of the expected pose of device and aforementioned solving result, obtain the control program of redundant degree of freedom mechanical arm, and two
The solution time for tieing up optimization problem is shorter, and solution difficulty is lower, reduces the movement in redundant degree of freedom manipulator motion planning
The calculating time for learning inverse solution, the difficulty in computation of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning is reduced, thus
The controlling party of redundant degree of freedom mechanical arm can more rapidly, be more easily obtained in the motion planning of redundant degree of freedom mechanical arm
Case completes motion planning, reduces the total time of motion planning, improves planning efficiency.
Embodiment two
Referring to fig. 2, Fig. 2 is a kind of process of redundant degree of freedom manipulator motion planning method provided by the embodiments of the present application
Figure.
The redundant degree of freedom manipulator motion planning method of the embodiment of the present application, includes the following steps:
Step S201 obtains pose number corresponding with the expected pose of the end effector of redundant degree of freedom mechanical arm
According to.
Step S202, according to preset constraint condition to the 7 degree of freedom optimization problem in redundant degree of freedom manipulator motion planning
Dimensionreduction treatment is carried out, two dimensional optimization problem is obtained.
Step S203 establishes the collision detection model in the 5th joint to the 7th joint of redundant degree of freedom mechanical arm.
In the present embodiment, abovementioned collision detection model is for the 5th joint to redundant degree of freedom mechanical arm to the
Seven joints carry out the model of collision detection, do not include examining to the first joint to the 4th joint of redundant degree of freedom mechanical arm
It surveys, therefore, the modeling time of abovementioned collision detection model can be shorter.
Step S204, using abovementioned collision detection model as foundation, to the 5th joint of redundant degree of freedom mechanical arm to the 7th
Joint carries out collision detection.
During abovementioned realization, the 7 degree of freedom optimization of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning is asked
After dimensionality reduction is inscribed into two dimensional optimization problem, the collision detection mould in the 5th joint to the 7th joint of redundant degree of freedom mechanical arm is established
Type, and using collision detection model as foundation, collision detection is carried out to the 5th joint to the 7th joint of redundant degree of freedom mechanical arm,
It can be reduced calculation amount when two dimensional optimization problem solves, to further shorten the solution time of two dimensional optimization problem, reduce
The solution difficulty of two dimensional optimization problem.
In the present embodiment, with sawyer redundant degree of freedom mechanical arm as an example, step S203 and step S204 can be more
For suitable for the application scenarios for carrying out goods sorting on the goods carrier of trellistype.
Step S205 solves abovementioned two dimensional optimization problem, obtains solving result.
Step S206 obtains the control program of redundant degree of freedom mechanical arm according to abovementioned pose data and solving result.
The redundant degree of freedom manipulator motion planning method of the embodiment of the present application, in redundant degree of freedom manipulator motion planning
In Inverse Kinematics Solution 7 degree of freedom optimization problem dimensionality reduction at two dimensional optimization problem after, establish redundant degree of freedom mechanical arm the 5th close
It saves to the collision detection model in the 7th joint, and using collision detection model as foundation, is closed to the 5th of redundant degree of freedom mechanical arm the
Section to the 7th joint carries out collision detection, can be reduced calculation amount when two dimensional optimization problem solves, to further shorten two
The solution time for tieing up optimization problem, the solution difficulty of two dimensional optimization problem is reduced, and then further reduce redundant degree of freedom machine
The calculating time of Inverse Kinematics Solution in tool arm motion planning reduces the Inverse Kinematics in redundant degree of freedom manipulator motion planning
The difficulty in computation of solution improves planning efficiency further to reduce the total time of motion planning.
Remaining content of the embodiment of the present application can refer to the content of abovedescribed embodiment one, in the present embodiment, no longer carry out
It repeats.
Embodiment three
Referring to Fig. 3, Fig. 3 is a kind of structure of redundant degree of freedom manipulator motion planning device provided by the embodiments of the present application
Schematic diagram.
The redundant degree of freedom manipulator motion planning device of the embodiment of the present application, comprising:
Acquiring unit 310, it is corresponding with the expected pose of the end effector of redundant degree of freedom mechanical arm for obtaining
Pose data.
Processing unit 320, for according to preset constraint condition to the 7 degree of freedom in redundant degree of freedom manipulator motion planning
Optimization problem carries out dimensionreduction treatment, obtains two dimensional optimization problem.
In the present embodiment, abovementioned preset constraint condition includes:
Seven joints of redundant degree of freedom mechanical arm are divided into three groups, wherein first group includes redundant degree of freedom mechanical arm
First joint to the 4th joint, second group include redundant degree of freedom mechanical arm the 5th joint and the 6th joint, third group includes
7th joint of redundant degree of freedom mechanical arm；And
The above rheme appearance data in position in the 5th joint of redundant degree of freedom mechanical arm and the angle in the 7th joint determine；And
The third arthrodesis of redundant degree of freedom mechanical arm；And
The 4th joint of redundant degree of freedom mechanical arm and the axis in the 5th joint and the axis phase in the 5th joint and the 6th joint
Vertically.
Under abovementioned constraint condition, seven joints of redundant degree of freedom mechanical arm are divided into three groups, first group include redundancy from
By the first joint to the 4th joint of degree mechanical arm, second group include redundant degree of freedom mechanical arm the 5th joint and the 6th close
Section, third group includes the 7th joint of redundant degree of freedom mechanical arm；The position in the 5th joint of redundant degree of freedom mechanical arm is with abovementioned
Pose data and the angle in the 7th joint determine；The third arthrodesis of redundant degree of freedom mechanical arm, i.e. third joint are opposite
It is fixed, thus the first joint of redundant degree of freedom mechanical arm, second joint, the 4th joint angle there are limited group of solutions；If
After the angle in the first joint of redundant degree of freedom mechanical arm to the 4th joint determines, the 5th joint of redundant degree of freedom mechanical arm with
The angle in the 6th joint can be according to the 4th joint and the axis in the 5th joint and the axis perpendicular in the 5th joint and the 6th joint
Limited group of solution is obtained, Such analysis is according to preset constraint condition to the kinematics in redundant degree of freedom manipulator motion planning
The 7 degree of freedom optimization problem of inverse solution carries out dimensionreduction treatment, the thinking of two dimensional optimization problem is obtained, under this constraint condition, it is possible to understand that
, the angle in the angle in third joint and the 7th joint is free variable, and the angle in remaining joint is parameter, and redundancy is free
The third joint of degree mechanical arm is affected to the arm type of mechanical arm, this constraint condition can not only be preferably by redundant degree of freedom machine
The 7 degree of freedom optimization problem dimensionality reduction of Inverse Kinematics Solution in tool arm motion planning is at two dimensional optimization problem, additionally it is possible to which guarantee constrains herein
Under the conditions of redundant degree of freedom mechanical arm flexibility ratio with higher.
In the present embodiment, the Mathematical Planning form of two dimensional optimization problem are as follows:
s.t.m_{l14}=m_{r14},m_{l24}=m_{r24},m_{l34}=m_{r34}
Wherein, q_{3}For the angle in the third joint of redundant degree of freedom mechanical arm, q_{7}The 7th for redundant degree of freedom mechanical arm closes
The angle of section, m_{r11}For M_{r}The element that the 1st row the 1st arranges in transfer matrix, m_{l21}For M_{l}The element that the 2nd row the 1st arranges in transfer matrix.
In the present embodiment, the Mathematical Planning form of abovementioned two dimensional optimization problem is under abovementioned constraint condition, with redundancy
The angle in the third joint of degreeoffreedom manipulator and the angle in the 7th joint are the Mathematical Planning form of free variable, due to redundancy
The third joint of degreeoffreedom manipulator is affected to the arm type of mechanical arm, in the Mathematical Planning form of this two dimensional optimization problem
Under, redundant degree of freedom mechanical arm can flexibility ratio with higher.
It solves unit 330 and obtains solving result for solving to abovementioned two dimensional optimization problem.
In the present embodiment, can by global optimization method and/or local optimization methods, to abovementioned two dimensional optimization problem into
Row solves, and obtains solving result.
During abovementioned realization, the 7 degree of freedom optimization of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning is asked
After dimensionality reduction is inscribed into two dimensional optimization problem, the solution difficulty of two dimensional optimization problem is low compared with the solution difficulty of two dimensional optimization problem to be obtained
More, the solution of two dimensional optimization problem can be solved by global optimization method and/or a variety of solution modes of local optimization methods.
Generation unit 340, for obtaining the control of redundant degree of freedom mechanical arm according to abovementioned pose data and solving result
Scheme.
As an alternative embodiment, the redundant degree of freedom manipulator motion planning device of the embodiment of the present application, also
Include:
Modeling unit, the collision detection model in the 5th joint to the 7th joint for establishing redundant degree of freedom mechanical arm.
Detection unit, for using abovementioned collision detection model as foundation, extremely to the 5th joint of redundant degree of freedom mechanical arm
7th joint carries out collision detection.
During abovementioned realization, the 7 degree of freedom optimization of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning is asked
After dimensionality reduction is inscribed into two dimensional optimization problem, the collision detection mould in the 5th joint to the 7th joint of redundant degree of freedom mechanical arm is established
Type, and using collision detection model as foundation, collision detection is carried out to the 5th joint to the 7th joint of redundant degree of freedom mechanical arm,
It can be reduced calculation amount when two dimensional optimization problem solves, to further shorten the solution time of two dimensional optimization problem, reduce
The solution difficulty of two dimensional optimization problem.
The redundant degree of freedom manipulator motion planning device of the embodiment of the present application, by preset constraint condition to redundancy from
Dimensionreduction treatment is carried out by the 7 degree of freedom optimization problem of the Inverse Kinematics Solution in degree manipulator motion planning, obtains two dimensional optimization problem；
Two dimensional optimization problem is solved again, obtains solving result；According to the end execution with redundant degree of freedom mechanical arm of acquisition
The corresponding pose data of the expected pose of device and aforementioned solving result, obtain the control program of redundant degree of freedom mechanical arm, and two
The solution time for tieing up optimization problem is shorter, and solution difficulty is lower, reduces the movement in redundant degree of freedom manipulator motion planning
The calculating time for learning inverse solution, the difficulty in computation of the Inverse Kinematics Solution in redundant degree of freedom manipulator motion planning is reduced, thus
The controlling party of redundant degree of freedom mechanical arm can more rapidly, be more easily obtained in the motion planning of redundant degree of freedom mechanical arm
Case completes motion planning, reduces the total time of motion planning, improves planning efficiency.
The redundancy of abovementioned redundant degree of freedom manipulator motion planning device implementable abovedescribed embodiment one and embodiment two
Degreeoffreedom manipulator motion planning method.Any option in abovedescribed embodiment one and embodiment two is also applied for this implementation
Example, I will not elaborate.
Remaining content of the embodiment of the present application can refer to the content of abovedescribed embodiment one and embodiment two, in the present embodiment
In, no longer repeated.
In several embodiments provided herein, it should be understood that disclosed device and method can also pass through
Other modes are realized.The apparatus embodiments described above are merely exemplary, for example, flow chart and block diagram in attached drawing
Show the device of multiple embodiments according to the application, the architectural framework in the cards of method and computer program product,
Function and operation.In this regard, each box in flowchart or block diagram can represent the one of a module, section or code
Part, a part of the module, section or code, which includes that one or more is for implementing the specified logical function, to be held
Row instruction.It should also be noted that function marked in the box can also be to be different from some implementations as replacement
The sequence marked in attached drawing occurs.For example, two continuous boxes can actually be basically executed in parallel, they are sometimes
It can execute in the opposite order, this depends on the function involved.It is also noted that every in block diagram and or flow chart
The combination of box in a box and block diagram and or flow chart can use the dedicated base for executing defined function or movement
It realizes, or can realize using a combination of dedicated hardware and computer instructions in the system of hardware.
In addition, each functional module in each embodiment of the application can integrate one independent portion of formation together
Point, it is also possible to modules individualism, an independent part can also be integrated to form with two or more modules.
It, can be with if the function is realized and when sold or used as an independent product in the form of software function module
It is stored in a computer readable storage medium.Based on this understanding, the technical solution of the application is substantially in other words
The part of the part that contributes to existing technology or the technical solution can be embodied in the form of software products, the meter
Calculation machine software product is stored in a storage medium, including some instructions are used so that a computer equipment (can be a
People's computer, server or network equipment etc.) execute each embodiment the method for the application all or part of the steps.
And storage medium abovementioned includes: that USB flash disk, mobile hard disk, readonly memory (ROM, ReadOnly Memory), arbitrary access are deposited
The various media that can store program code such as reservoir (RAM, Random Access Memory), magnetic or disk.
The above description is only an example of the present application, the protection scope being not intended to limit this application, for ability
For the technical staff in domain, various changes and changes are possible in this application.Within the spirit and principles of this application, made
Any modification, equivalent substitution, improvement and etc. should be included within the scope of protection of this application.It should also be noted that similar label and
Letter indicates similar terms in following attached drawing, therefore, once it is defined in a certain Xiang Yi attached drawing, then in subsequent attached drawing
In do not need that it is further defined and explained.
The above, the only specific embodiment of the application, but the protection scope of the application is not limited thereto, it is any
Those familiar with the art within the technical scope of the present application, can easily think of the change or the replacement, and should all contain
Lid is within the scope of protection of this application.Therefore, the protection scope of the application shall be subject to the protection scope of the claim.
It should be noted that, in this document, relational terms such as first and second and the like are used merely to a reality
Body or operation are distinguished with another entity or operation, are deposited without necessarily requiring or implying between these entities or operation
In any actual relationship or order or sequence.Moreover, the terms "include", "comprise" or its any other variant are intended to
Nonexclusive inclusion, so that the process, method, article or equipment including a series of elements is not only wanted including those
Element, but also including other elements that are not explicitly listed, or further include for this process, method, article or equipment
Intrinsic element.In the absence of more restrictions, the element limited by sentence "including a ...", it is not excluded that
There is also other identical elements in process, method, article or equipment including the element.
Claims (10)
1. a kind of redundant degree of freedom manipulator motion planning method characterized by comprising
Obtain pose data corresponding with the expected pose of end effector of the redundant degree of freedom mechanical arm；
Dimensionality reduction is carried out to the 7 degree of freedom optimization problem in the redundant degree of freedom manipulator motion planning according to preset constraint condition
Processing, obtains two dimensional optimization problem；
The two dimensional optimization problem is solved, solving result is obtained；
According to the pose data and the solving result, the control program of the redundant degree of freedom mechanical arm is obtained.
2. redundant degree of freedom manipulator motion planning method according to claim 1, which is characterized in that it is described it is preset about
Beam condition includes:
Seven joints of the redundant degree of freedom mechanical arm are divided into three groups, wherein first group includes that the redundant degree of freedom is mechanical
First joint of arm to the 4th joint, second group include the redundant degree of freedom mechanical arm the 5th joint and the 6th joint,
Three groups include the redundant degree of freedom mechanical arm the 7th joint；And
The position in the 5th joint of redundant degree of freedom mechanical arm is determined with the pose data and the angle in the 7th joint；And
The third arthrodesis of the redundant degree of freedom mechanical arm；And
The 4th joint of redundant degree of freedom mechanical arm and the axis in the 5th joint and the axis phase in the 5th joint and the 6th joint
Vertically.
3. redundant degree of freedom manipulator motion planning method according to claim 1, which is characterized in that pre in the basis
If constraint condition in the redundant degree of freedom manipulator motion planning 7 degree of freedom optimization problem carry out dimensionreduction treatment, obtain two
After the step of tieing up optimization problem, before described the step of solving to the two dimensional optimization problem, obtain solving result, institute
State method further include:
Establish the collision detection model in the 5th joint to the 7th joint of the redundant degree of freedom mechanical arm；
Using the collision detection model as foundation, the 5th joint to the 7th joint of the redundant degree of freedom mechanical arm is touched
Hit detection.
4. redundant degree of freedom manipulator motion planning method according to claim 1, which is characterized in that the twodimensional optimization
The Mathematical Planning form of problem are as follows:
s.t.m_{l14}=m_{r14},m_{l24}=m_{r24},m_{l34}=m_{r34}
Wherein, q_{3}For the angle in the third joint of the redundant degree of freedom mechanical arm, q_{7}It is the of the redundant degree of freedom mechanical arm
The angle in seven joints, m_{r11}For M_{r}The element that the 1st row the 1st arranges in transfer matrix, m_{l21}For M_{l}The member that the 2nd row the 1st arranges in transfer matrix
Element.
5. redundant degree of freedom manipulator motion planning method according to claim 1, which is characterized in that described to described two
Tieing up the step of optimization problem is solved, obtains solving result includes:
By global optimization method and/or local optimization methods, the two dimensional optimization problem is solved, obtains solving knot
Fruit.
6. a kind of redundant degree of freedom manipulator motion planning device characterized by comprising
Acquiring unit, for obtaining pose corresponding with the expected pose of end effector of the redundant degree of freedom mechanical arm
Data；
Processing unit, for being optimized according to preset constraint condition to the 7 degree of freedom in the redundant degree of freedom manipulator motion planning
Problem carries out dimensionreduction treatment, obtains two dimensional optimization problem；
It solves unit and obtains solving result for solving to the two dimensional optimization problem；
Generation unit, for obtaining the control of the redundant degree of freedom mechanical arm according to the pose data and the solving result
Scheme processed.
7. redundant degree of freedom manipulator motion planning device according to claim 6, which is characterized in that it is described it is preset about
Beam condition includes:
Seven joints of the redundant degree of freedom mechanical arm are divided into three groups, wherein first group includes that the redundant degree of freedom is mechanical
First joint of arm to the 4th joint, second group include the redundant degree of freedom mechanical arm the 5th joint and the 6th joint,
Three groups include the redundant degree of freedom mechanical arm the 7th joint；And
The position in the 5th joint of redundant degree of freedom mechanical arm is determined with the pose data and the angle in the 7th joint；And
The third arthrodesis of the redundant degree of freedom mechanical arm；And
The 4th joint of redundant degree of freedom mechanical arm and the axis in the 5th joint and the axis in the 5th joint and the 6th joint.
8. redundant degree of freedom manipulator motion planning device according to claim 6, which is characterized in that the redundancy is free
Spend manipulator motion planning device further include:
Modeling unit, the collision detection model in the 5th joint to the 7th joint for establishing the redundant degree of freedom mechanical arm；
Detection unit, for using the collision detection model as foundation, extremely to the 5th joint of the redundant degree of freedom mechanical arm
7th joint carries out collision detection.
9. redundant degree of freedom manipulator motion planning device according to claim 6, which is characterized in that the twodimensional optimization
The Mathematical Planning form of problem are as follows:
s.t.m_{l14}=m_{r14},m_{l24}=m_{r24},m_{l34}=m_{r34}
Wherein, q_{3}For the angle in the third joint of the redundant degree of freedom mechanical arm, q_{7}It is the of the redundant degree of freedom mechanical arm
The angle in seven joints, m_{r11}For M_{r}The element that the 1st row the 1st arranges in transfer matrix, m_{l21}For M_{l}The member that the 2nd row the 1st arranges in transfer matrix
Element.
10. redundant degree of freedom manipulator motion planning device according to claim 6, which is characterized in that the solution is single
Member is specifically used for:
By global optimization method and/or local optimization methods, the two dimensional optimization problem is solved, obtains solving knot
Fruit.
Priority Applications (1)
Application Number  Priority Date  Filing Date  Title 

CN201910135283.3A CN109732609A (en)  20190221  20190221  A kind of redundant degree of freedom manipulator motion planning method and device 
Applications Claiming Priority (1)
Application Number  Priority Date  Filing Date  Title 

CN201910135283.3A CN109732609A (en)  20190221  20190221  A kind of redundant degree of freedom manipulator motion planning method and device 
Publications (1)
Publication Number  Publication Date 

CN109732609A true CN109732609A (en)  20190510 
Family
ID=66368179
Family Applications (1)
Application Number  Title  Priority Date  Filing Date 

CN201910135283.3A Pending CN109732609A (en)  20190221  20190221  A kind of redundant degree of freedom manipulator motion planning method and device 
Country Status (1)
Country  Link 

CN (1)  CN109732609A (en) 
Cited By (2)
Publication number  Priority date  Publication date  Assignee  Title 

CN110370256A (en) *  20190625  20191025  达闼科技（北京）有限公司  Robot and its paths planning method, device and controller 
CN110371325A (en) *  20190702  20191025  西北工业大学  A kind of adaptive envelope of inert satellite based on super redundant mechanical arm arrests method 
Citations (5)
Publication number  Priority date  Publication date  Assignee  Title 

US20130339918A1 (en) *  20120613  20131219  Purdue Research Foundation  Microelectromechanical system design and layout 
CN106584461A (en) *  20161221  20170426  西安科技大学  Method for optimizing inverse kinematic humanoidarm configuration of 7freedomdegree humanoid mechanical arm under multiconstraint condition 
CN108326844A (en) *  20170120  20180727  香港理工大学深圳研究院  The motion planning method and device of the operable degree optimization of redundancy mechanical arm 
CN108656119A (en) *  20180715  20181016  宓建  A kind of control method of humanoid robot 
CN109015627A (en) *  20180524  20181218  湘潭大学  Big machinery arm posture perception method and system 

2019
 20190221 CN CN201910135283.3A patent/CN109732609A/en active Pending
Patent Citations (5)
Publication number  Priority date  Publication date  Assignee  Title 

US20130339918A1 (en) *  20120613  20131219  Purdue Research Foundation  Microelectromechanical system design and layout 
CN106584461A (en) *  20161221  20170426  西安科技大学  Method for optimizing inverse kinematic humanoidarm configuration of 7freedomdegree humanoid mechanical arm under multiconstraint condition 
CN108326844A (en) *  20170120  20180727  香港理工大学深圳研究院  The motion planning method and device of the operable degree optimization of redundancy mechanical arm 
CN109015627A (en) *  20180524  20181218  湘潭大学  Big machinery arm posture perception method and system 
CN108656119A (en) *  20180715  20181016  宓建  A kind of control method of humanoid robot 
Cited By (2)
Publication number  Priority date  Publication date  Assignee  Title 

CN110370256A (en) *  20190625  20191025  达闼科技（北京）有限公司  Robot and its paths planning method, device and controller 
CN110371325A (en) *  20190702  20191025  西北工业大学  A kind of adaptive envelope of inert satellite based on super redundant mechanical arm arrests method 
Similar Documents
Publication  Publication Date  Title 

CN109732609A (en)  A kind of redundant degree of freedom manipulator motion planning method and device  
Ho et al.  Character motion synthesis by topology coordinates  
Wang et al.  Robot manipulator calibration using neural network and a camerabased measurement system  
Pan et al.  Realtime collision detection and distance computation on point cloud sensor data  
Zacharia et al.  Task scheduling and motion planning for an industrial manipulator  
Saravanan et al.  Evolutionary multicriteria trajectory modeling of industrial robots in the presence of obstacles  
Pan et al.  Efficient configuration space construction and optimization for motion planning  
Lien et al.  Planning motion in environments with similar obstacles.  
Suh et al.  Tangent space RRT: A randomized planning algorithm on constraint manifolds  
Lim et al.  Nature inspired algorithms to optimize robot workcell layouts  
Tarokh  Real time forward kinematics solutions for general Stewart platforms  
De Angulo et al.  Learning inverse kinematics: Reduced sampling through decomposition into virtual robots  
Grasl et al.  GRAPE: using graph grammars to implement shape grammars.  
Gayle et al.  Constraintbased motion planning of deformable robots  
Lertkultanon et al.  A singlequery manipulation planner  
Ota et al.  Trajectory optimization for unknown constrained systems using reinforcement learning  
Wang  Application of genetic algorithms to robot kinematics calibration  
Prianto et al.  Path planning for multiarm manipulators using deep reinforcement learning: Soft actor–critic with hindsight experience replay  
Mirolo et al.  A solid modelling system for robot action planning  
Omran et al.  Mission‐based optimal control of Stewart manipulator  
Han et al.  Local structurization for the forward kinematics of parallel manipulators using extra sensor data  
CN109202904B (en)  Method and system for determining motion path of mechanical arm  
AbuDakka et al.  Evolutionary path planning algorithm for industrial robots  
Zhang et al.  Safe and Efficient Robot Manipulation: TaskOriented Environment Modeling and Object Pose Estimation  
ParraGonzalez et al.  A new object path planner for the box pushing problem 
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 