CN108789406A - A kind of inverse solution method for solving for redundant mechanical arm approachability analysis - Google Patents
A kind of inverse solution method for solving for redundant mechanical arm approachability analysis Download PDFInfo
- Publication number
- CN108789406A CN108789406A CN201810567298.2A CN201810567298A CN108789406A CN 108789406 A CN108789406 A CN 108789406A CN 201810567298 A CN201810567298 A CN 201810567298A CN 108789406 A CN108789406 A CN 108789406A
- Authority
- CN
- China
- Prior art keywords
- joint
- mechanical arm
- joint angle
- angle
- inverse solution
- 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
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/15—Correlation function computation including computation of convolution operations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Mathematical Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computational Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Mathematical Optimization (AREA)
- Databases & Information Systems (AREA)
- Algebra (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Computing Systems (AREA)
- Automation & Control Theory (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a kind of inverse solution method for solving for redundant mechanical arm approachability analysis.This method carries out the inverse solution of mechanical arm as nuisance parameter using joint angle and solves, in the case where considering the joint limit, obtained solution may not meet joint limit condition, it can determine whether distance of this solution apart from the joint limit at this time, find out the joint angle of the farthest distance of the abscission joint limit, joint angle current value is updated into joint angle by a fixed step size close to the joint limit, inverse solution is carried out as nuisance parameter using the newer joint angle again to solve, it repeats the above process until solving to the inverse solution in feasible joint for meeting joint limit, then judge that given terminal angle is the reachable posture of the mechanical arm.The present invention can be used in redundant mechanical arm approachability analysis, solve global mechanical arm posture in the case where considering that joint limits whether reachable problem, to understand the distribution situation of reachable pose of the mechanical arm in its entire space, help that mechanical arm is instructed preferably to complete task.
Description
Technical field
The invention belongs to Mechanical transmission test fields, and in particular to a kind of inverse solution for redundant mechanical arm approachability analysis
Method for solving.
Background technology
Currently, seven freedom mechanical arm is because of it, there are the features of redundancy, are ensureing the case where end task is realized ShiShimonoseki
Still there are one degree of freedom can be used for realizing that operational readiness optimization, joint moment optimization, avoidance etc. are appointed with self-movement for ganglionic layer
Business, such robot have become the hot spot and emphasis of current robot research.
However the presence of redundancy brings difficulty to the solution of the inverse solution of seven freedom mechanical arm, by inquiring document and comprehensive point
Analysis is found, although more to the research of the inverse solution of seven freedom mechanical arm both at home and abroad, be generally divided into numerical solution and analytic solutions solve,
When needing to consider in approachability analysis to solve the inverse solution of global all postures in the case of the limit of joint, numerical solution method for solving because
Whether it can quick and precisely obtain inverse solution dependent on the selection just solved therefore be not suitable for, and current common analytic solutions, which solve, is based on arm shape
The inverse solution at angle is solved because it can just need to obtain inverse solution to the value at fixed arm shape angle first, and also cannot be guaranteed to the size at fixed arm shape angle
Inverse solution meets joint limit condition, therefore is also not suitable for.It is solved also not based on the inverse solution of analytic solutions that single joint angle is nuisance parameter
The inverse solution that can guarantee meets joint limit condition.
In approachability analysis, we usually divide the sextuple working space of mechanical arm first, then carry out inverse solution and solve
It calculates, global posture need to be solved without artificially judging and whether there is the reachable inverse solution for meeting joint limit.Nowadays it deposits
The feasible scheme of possibility of inverse solution of the posture that global mechanical arm is judged in the case where considering the joint limit exist
Shimizu M,Kakuya H,Yoon W K,et al.Analytical Inverse Kinematic Computation
for 7-DOF Redundant Manipulators With Joint Limits and Its Application to
Redundancy Resolution[J].IEEE Transactions on Robotics,2008,24(5):1131-1142.
It is analyzed, has obtained the relationship at arm shape angle and the joint limit, acquire all arm shape angles for meeting joint limit condition.However
This method obtains the joint limit and the relationship situation at arm shape angle is more, more complex, and obtains the joint pole in each joint solving
It limits more difficult when the union at corresponding arm shape angle, usually needs human intervention to carry out union calculating, therefore this method is also not suitable for.
Invention content
In order to overcome the above-mentioned deficiencies of the prior art, the present invention provides a kind of for redundant mechanical arm approachability analysis
Inverse solution method for solving.Consider that joint angle is the inverse solution that may not be able to obtain meeting joint limit when nuisance parameter solves inverse solution, sentences
It is disconnected to limit maximum joint angle beyond joint, using the autokinesis of redundant mechanical arm, obtain close to the updated of joint limit
Joint angle re-starts inverse solution and calculates, until acquiring the feasible inverse solution for meeting joint and limiting condition.
The technical solution adopted in the present invention is：
A kind of inverse solution method for solving for redundant mechanical arm approachability analysis includes the following steps：
Step 1：The connecting rod coordinate relationship of mechanical arm, first three rotary joint of mechanical arm are established using standard D-H methods
(q1,q2,q3) joints axes intersect at a point s, therefore first three joint is equivalent to a virtual ball-joint；Mechanical arm
Three rotary joint (q afterwards5,q6,q7) joints axes also intersect at a point w, therefore also by rear three joints be equivalent to one it is virtual
Ball-joint；4th joint angle q4It is a rotary joint, link rod coordinate system center is point w；The end of mechanical arm and the
Seven joint link lever coordinate systems overlap；The relationship of the relevant parameter needed for the computation of inverse- kinematics is obtained according to Mechanical transmission test；
Step 2：For the mechanical arm of SRS configurations, terminal angle is fixed, and joint space remains able to self-movement, this
Movement is known as the autokinesis of redundant robot, however the position at two virtual ball-joint centers can uniquely determine at this time, root
According to the cosine formula of triangle, joint angle q can be obtained4, then using a joint angle as nuisance parameter progress inverse kinematics,
It is i.e. given to remove joint angle q4Any one outer joint angle, calculates other joint angles；
Step 3：For given terminal angle, first with joint angle q3=0 solves one group of inverse solution for nuisance parameter, is examining
In the case of considering joint limit, which is perhaps discontented with podarthrum positive stop strip part against solution, and the inverse solution as obtained by definition arrives the joint limit
The function of position relationship limits condition to judge whether current inverse solution meets joint, if being limited beyond joint, the value energy of the function
The currently inverse size solved beyond joint limit of enough descriptions and the direction more than joint limit, that is, be greater than the maximum of the joint limit
Value or less than the joint limit minimum value；
Step 4：Judgement is defined by above-mentioned function to obtain limiting farthest joint angle q apart from joint at this timej, with certain
Step-length update the size of the joint angle, make it close to the joint limit, obtain new joint angleAgain withFor redundant mechanical arm
Nuisance parameter carry out inverse kinematics and obtain one group of new inverse solution, judge that the group is against solution with the function defined in step 3
It is no to meet joint limit condition；If obtaining feasible inverse solution, solution procedure terminates, and given posture is the reachable posture of mechanical arm；
If the group is also discontented with the condition of podarthrum limit against solution, repeatedly step 4 until solve obtain meeting joint limits condition can
The inverse solution of row or maximum iteration of the iterations beyond setting, then think that the given mechanical arm tail end posture is not at this time
It is reachable.
Joint angle is that nuisance parameter carries out inverse kinematics in the step 2, i.e., given to remove joint angle q4It is any one
A joint angle calculates other joint angles；
For given terminal angle, the center of two virtual ball-joints can determine and the length of connecting rod of mechanical arm
Degree has obtained, therefore according to the cosine formula of triangle, first determines whether joint angle q4Size, and its not with mechanical arm from transport
It moves and changes；The position vector of the centre of sphere line of virtual ball-joint simultaneously0tswWith the direction transformation relationship of virtual ball-joint0R3
(q1,q2,q3) between relationship can obtain, for solve joint angle be nuisance parameter inverse solution, be as follows：
1) joint angle q1The inverse solution of redundant robot is sought for nuisance parameter：
By0tswWith0R3(q1,q2,q3) between relationship obtain：
F sin q3=swycos q1-swxsin q1 (1)
swz=E cos q2-F cos q3sin q2 (2)
swx,swy,swzIt is respectively0tswX, y, the component of z-axis, and E=dewcos q4+dse, F=dewsin q4；
According to formula (1) and formula (2), joint angle q is obtained2With q3, about joint angle q1Expression formula be：
sin q3=(swycos q1-swxsin q1)/F (3)
According to the transitive relation of connecting rod, the direction transformation matrix of another virtual ball-joint is obtained4R7, and because its about
q5,q6,q7Expression formula it is as follows：
q6=± arccos (4R7(3,3)) (6)
q5=arctan2 (sign (sinq6)4R7(2,3),sign(sinq6)4R7(1,3)) (7)
q7=arctan2 (- sign (sinq6)4R7(3,1),sign(sinq6)4R7(3,2)) (8)
Therefore, it completes to be based on joint angle q1It is solved for the inverse solution of nuisance parameter；
2) joint angle q2The inverse solution of redundant robot is sought for nuisance parameter：
When with joint angle q2For nuisance parameter when, by0tswWith0R3(q1,q2,q3) relationship abbreviation obtain：
swx=(E sin q2+F cos q2cos q3)cos q1-(F sin q3)sin q1 (9)
swy=(E sin q2+F cos q2cos q3)sin q1+(F sin q3)cos q1 (10)
swz=-E (cos q2)+F(sin q2cos q3) (11)
Joint angle q is obtained according to above formula at this time1And q3, about joint angle q2Expression formula be：
q3=± arccos ((swz+E cos q2)/F sin q2) (12)
q12 (H of=arctanq2(1),Hq2(2)) (13)
Wherein Hq2Expression formula it is as follows：
Ibid determine transformation matrix4R7Joint angle q is determined according to anticosine and arctan function afterwards5,q6,q7；
3) joint angle q3The inverse solution of redundant robot is sought for nuisance parameter：
When with joint angle q3For nuisance parameter when, according to formula (9), formula (10) and formula (11), q1,q2Also it is calculated：
q12 (H of=arctanq2(1),Hq2(2)) (15)
Ibid determine transformation matrix4R7Afterwards according to anticosine and arctan function, joint angle q is determined5,q6,q7；Joint angle q3
It asks the inverse solution of redundant robot to solve for nuisance parameter to complete；
4) joint angle q5,q6,q7The inverse solution of redundant robot is sought for nuisance parameter：
Because mechanical arm is symmetrical, joint of mechanical arm backward is calculated, at this time4R7It is inverse as0R3Processing, together
Sample obtains respectively with joint angle q5,q6,q7For the inverse solution of nuisance parameter.
In the step 3, judge whether current inverse solution meets joint by defining the function that inverse solution is limited to joint
Limit condition describes currently to limit against size of the solution beyond the joint limit and more than joint if limiting beyond joint；The table of function
It is as follows up to formula：
Maxδ=argmaxi{|δ1|,|δ2|,|δ3|,|δ5|,|δ6|,|δ7|} (18)
In formulaIndicate the limit on the left in i-th of joint,Indicate the limit on the right-right-hand limit in i-th of joint；Variable MaxδDefinition be
For obtaining 6 farthest joints of the joint angle abscission joint limit, if obtained δiIt is not all 0, then judges that this is unsatisfactory for closing against solution
Limit condition is saved, function Max is usedδIt obtains abscission joint and limits maximum joint angle j updating to obtain the joint angle q of next iterationj(k
+ 1)=qj(k)+sign(δj)Δqj, Δ qjFor the size of newer joint angle, and use qj(k+1) it is used as nuisance parameter to continue
Inverse solution is carried out to solve.
Using above-mentioned technical proposal, the present invention has such advantageous effect compared with prior art：
The method of the present invention can directly judge the inverse solution of global mechanical arm in the case where considering the joint limit without given just solution
It is whether reachable.The characteristics of this method is that arbitrarily given terminal angle can determine whether the posture is to meet joint limit condition
Up to pose.When solving the accessibility of mechanical arm, the pose of sextuple space is divided first, it can be without artificial using this method
Judge whether given pose is reachable in the case of intervention.
Description of the drawings
Fig. 1 is a kind of inverse solution method for solving algorithm flow chart for redundant mechanical arm approachability analysis of the invention.
Fig. 2 is the connecting rod coordinate schematic diagram of seven freedom mechanical arm.
Fig. 3 is seven freedom mechanical arm schematic diagram and its autokinesis schematic diagram.
Fig. 4 is the description figure of joint limit.
Specific implementation mode
Below in conjunction with the accompanying drawings and specific implementation mode is described further the present invention：
As shown in Figure 1, a kind of inverse solution method for solving for redundant mechanical arm approachability analysis, realizes that process is as follows：
Step 1：The connecting rod coordinate relationship of mechanical arm, connecting rod coordinate relationship such as Fig. 2 institutes are established using standard D-H methods
Show, and first three rotary joint (q of mechanical arm1,q2,q3) joints axes intersect at a point s, therefore can be by first three joint
It is equivalent to a virtual ball-joint；Rear three rotary joint (q of mechanical arm5,q6,q7) joints axes also intersect at a point
W, therefore rear three joints can also be equivalent to a virtual ball-joint；4th joint angle q4It is a rotary joint,
Link rod coordinate system center is w；The end of mechanical arm is overlapped with the 7th joint link lever coordinate system；Mechanical arm after equivalent can letter
Two ball-joints and a rotary joint are turned to, as shown in Figure 3.It is obtained needed for the computation of inverse- kinematics according to Mechanical transmission test
The relationship of relevant parameter.
Step 2：As shown in figure 3, for the mechanical arm of this configuration, terminal angle is fixed, and joint space still can be only
Vertical movement, we are known as the autokinesis of redundant robot for this movement, and in end, motionless joint of mechanical arm configuration is because certainly
Movement can be become bse ' w from bsew.The position s and w at two virtual ball-joint centers can be uniquely determined at this time, according to three
Angular cosine formula, joint angle q4It can obtain, then inverse kinematics are carried out by nuisance parameter of a joint angle, that is, give
Surely joint angle q is removed4Any one joint angle calculates other joint angles.
Step 3：For given terminal angle0R7,0t7, first with joint angle q3=0 is inverse for one group of nuisance parameter solution
Solution, in the case where considering joint limit, which may be discontented with podarthrum positive stop strip part against solution, the inverse solution as obtained by definition to pass
It saves the function of spacing relation and limits condition to judge whether current inverse solution meets joint, if being limited beyond joint, the value of the function
It can describe the direction that current inverse solution exceeds the size of joint limit and limited more than joint and (be greater than the joint limit most
Big value or the minimum value less than the joint limit).
Step 4：Judge the group limits condition against whether solution meets joint with the function defined in step 3.If obtaining feasible
Inverse solution, then solution procedure terminates, and given posture is the reachable posture of mechanical arm；If the inverse solution of the group is discontented with the item of podarthrum limit
Part, defines us by above-mentioned function and may determine that obtain and limit farthest joint angle q apart from joint at this timej, with certain
Step-length updates the size of the joint angle, makes it close to the joint limit, obtains new joint angleAgain withFor redundant mechanical arm
Nuisance parameter carries out inverse kinematics and obtains one group of new inverse solution, then repeatedly step 4 obtains meeting joint limit until solving
The feasible inverse solution or maximum iteration of the iterations beyond setting of condition, then think the given mechanical arm tail end at this time
Posture is unreachable.
Further, joint angle is that nuisance parameter carries out inverse kinematics in step 2, i.e., given to remove joint angle q4Appoint
One joint angle of meaning, calculates other joint angles.
For given terminal angle, the center of two virtual ball-joints can determine and the length of connecting rod of mechanical arm
Degree has obtained, therefore can first determine whether joint angle q according to the cosine formula of triangle4Size, and it is with mechanical arm
Autokinesis and change.The position vector of the centre of sphere line of virtual ball-joint simultaneously0tswWith the direction transformation relationship of virtual ball-joint0R3(q1,q2,q3) between relationship can obtain, can be used for solve joint angle be nuisance parameter inverse solution.
1) joint angle q1The inverse solution of redundant robot is sought for nuisance parameter：
By0tswWith0R3(q1,q2,q3) between relationship can obtain：
F sin q3=swycos q1-swxsin q1 (1)
swz=E cos q2-F cos q3sin q2 (2)
swx,swy,swzIt is respectively0tswX, y, the component of z-axis, and E=dewcos q4+dse, F=dewsin q4。
According to above formula (1) and formula (2), joint angle q can be obtained2With q3, about joint angle q1Expression formula
sin q3=(swycos q1-swxsin q1)/F (3)
According to the transitive relation of connecting rod, we can obtain the direction transformation matrix of another virtual ball-joint4R7, and because
It is it about q5,q6,q7Expression formula can be as follows：
q6=± arccos (4R7(3,3)) (6)
q5=arctan2 (sign (sinq6)4R7(2,3),sign(sinq6)4R7(1,3)) (7)
q7=arctan2 (- sign (sinq6)4R7(3,1),sign(sinq6)4R7(3,2)) (8)
Therefore, it completes to be based on joint angle q1It is solved for the inverse solution of nuisance parameter.
2) joint angle q2The inverse solution of redundant robot is sought for nuisance parameter：
When with joint angle q2For nuisance parameter when, by0tswWith0R3(q1,q2,q3) relationship abbreviation obtain：
swx=(E sin q2+F cos q2cos q3)cos q1-(F sin q3)sin q1 (9)
swy=(E sin q2+F cos q2cos q3)sin q1+(F sin q3)cos q1 (10)
swz=E (cos q2)-F(sin q2cos q3) (11)
Joint angle q can be obtained according to above formula at this time1And q3, about joint angle q2Expression formula be：
q3=± arccos ((swz+E cos q2)/F sin q2) (12)
q12 (H of=arctanq2(1),Hq2(2)) (13)
Wherein Hq2Expression formula it is as follows：
Ibid determine transformation matrix4R7Joint angle q can be determined according to anticosine and arctan function afterwards5,q6,q7。
3) joint angle q3The inverse solution of redundant robot is sought for nuisance parameter：
When with joint angle q3For nuisance parameter when, according to formula (9), formula (10) and formula (11), q1,q2Also it can be calculated：
q12 (H of=arctanq2(1),Hq2(2)) (15)
Ibid determine transformation matrix4R7Joint angle q can be determined according to anticosine and arctan function afterwards5,q6,q7.Joint
Angle q3It asks the inverse solution of redundant robot to solve for nuisance parameter to complete.
4) joint angle q5,q6,q7The inverse solution of redundant robot is sought for nuisance parameter：
Because mechanical arm is symmetrical, joint of mechanical arm backward can be calculated, at this time4R7It is inverse as 0R3Place
Reason, can equally obtain respectively with joint angle q5,q6,q7For the inverse solution of nuisance parameter.
Further, in step 3, judge whether current inverse solution meets by defining the function that inverse solution is limited to joint
Joint limits condition, if beyond joint limit, describes currently inverse solution and exceeds the size of the joint limit and limited more than joint, the letter
Several expression formulas is as follows, and function is with value with qiVariation is as shown in Figure 4.
Maxδ=argmax | δ1|,|δ2|,|δ3|,|δ5|,|δ6|,|δ7|} (18)
In formula (17)Indicate the limit on the left in i-th of joint,Indicate the limit on the right-right-hand limit in i-th of joint.Variable MaxδDetermine
Justice is for obtaining 6 farthest joints of the joint angle abscission joint limit.If obtained δiIt is not all 0, then judges that the inverse solution is discontented
Podarthrum limits condition, uses function MaxδIt obtains abscission joint and limits maximum joint angle j updating to obtain the joint of next iteration
Angle qj(k+1)=qj(k)+sign(δj)Δqj, Δ qjFor the size of newer joint angle, and use qj(k+1) it is used as redundancy to join
Number continues inverse solution and solves.
Claims (3)
1. a kind of inverse solution method for solving for redundant mechanical arm approachability analysis, it is characterised in that：Include the following steps：
Step 1：The connecting rod coordinate relationship of mechanical arm, first three rotary joint (q of mechanical arm are established using standard D-H methods1,
q2,q3) joints axes intersect at a point s, therefore first three joint is equivalent to a virtual ball-joint；Rear the three of mechanical arm
A rotary joint (q5,q6,q7) joints axes also intersect at a point w, therefore rear three joints are also equivalent to a virtual ball
Joint；4th joint angle q4It is a rotary joint, link rod coordinate system center is point w；The end of mechanical arm with the 7th
Joint link lever coordinate system overlaps；The relationship of the relevant parameter needed for the computation of inverse- kinematics is obtained according to Mechanical transmission test；
Step 2：For the mechanical arm of SRS configurations, terminal angle is fixed, and joint space remains able to self-movement, this movement
The referred to as autokinesis of redundant robot, however the position at two virtual ball-joint centers can uniquely determine at this time, according to three
Angular cosine formula can obtain joint angle q4, then using a joint angle as nuisance parameter progress inverse kinematics, that is, give
Surely joint angle q is removed4Any one outer joint angle, calculates other joint angles；
Step 3：For given terminal angle, first with joint angle q3=0 solves one group of inverse solution for nuisance parameter, is considering to close
In the case of section limit, which is perhaps discontented with podarthrum positive stop strip part against solution, and the inverse solution as obtained by definition arrives joint spacing relation
Function limit condition to judge whether current inverse solution meets joint, if being limited beyond joint, the value of the function can describe to work as
Preceding size of the inverse solution beyond joint limit and the direction more than joint limit, that is, be greater than the maximum value of the joint limit or be less than
The minimum value of the joint limit；
Step 4：Judgement is defined by above-mentioned function to obtain limiting farthest joint angle q apart from joint at this timej, with certain step
The long size for updating the joint angle, makes it close to the joint limit, obtains new joint angleAgain withFor the superfluous of redundant mechanical arm
Remaining parameter carries out inverse kinematics and obtains one group of new inverse solution, judges whether the group is full against solution with the function defined in step 3
Podarthrum limits condition；If obtaining feasible inverse solution, solution procedure terminates, and given posture is the reachable posture of mechanical arm；If should
The inverse solution of group is also discontented with the condition of podarthrum limit, then repeatedly step 4 obtains meeting joint and limit the feasible inverse of condition until solving
The maximum iteration of solution or iterations beyond setting, then thinking that the given mechanical arm tail end posture is at this time can not
It reaches.
2. the inverse solution method for solving according to claim 1 for redundant mechanical arm approachability analysis, it is characterised in that：?
Joint angle is that nuisance parameter carries out inverse kinematics in the step 2, i.e., given to remove joint angle q4Any one joint angle, meter
Calculate other joint angles；
For given terminal angle, the center of two virtual ball-joints can determine and the length of connecting rod of mechanical arm
Through obtaining, therefore according to the cosine formula of triangle, first determine whether joint angle q4Size, and its not with mechanical arm autokinesis and
Change；The position vector of the centre of sphere line of virtual ball-joint simultaneously0tswWith the direction transformation relationship of virtual ball-joint0R3(q1,q2,
q3) between relationship can obtain, for solve joint angle be nuisance parameter inverse solution, be as follows：
1) joint angle q1The inverse solution of redundant robot is sought for nuisance parameter：
By0tswWith0R3(q1,q2,q3) between relationship obtain：
Fsinq3=swycosq1-swxsinq1 (1)
swz=Ecosq2-Fcosq3sinq2 (2)
swx,swy,swzIt is respectively0tswX, y, the component of z-axis, and E=dewcosq4+dse, F=dewsinq4；
According to formula (1) and formula (2), joint angle q is obtained2With q3, about joint angle q1Expression formula be：
sinq3=(swycosq1-swxsinq1)/F (3)
According to the transitive relation of connecting rod, the direction transformation matrix of another virtual ball-joint is obtained4R7, and because of it about q5,q6,
q7Expression formula it is as follows：
q6=± arccos (4R7(3,3)) (6)
q5=arctan2 (sign (sinq6)4R7(2,3),sign(sinq6)4R7(1,3)) (7)
q7=arctan2 (- sign (sinq6)4R7(3,1),sign(sinq6)4R7(3,2)) (8)
Therefore, it completes to be based on joint angle q1It is solved for the inverse solution of nuisance parameter；
2) joint angle q2The inverse solution of redundant robot is sought for nuisance parameter：
When with joint angle q2For nuisance parameter when, by0tswWith0R3(q1,q2,q3) relationship abbreviation obtain：
swx=(Esinq2+Fcosq2cosq3)cosq1-(Fsinq3)sinq1 (9)
swy=(Esinq2+Fcosq2cosq3)sinq1+(Fsinq3)cosq1 (10)
swz=-E (cosq2)+F(sinq2cosq3) (11)
Joint angle q is obtained according to above formula at this time1And q3, about joint angle q2Expression formula be：
q3=± arccos ((swz+Ecosq2)/Fsinq2) (12)
q1=arctan2 (Hq2(1),Hq2(2)) (13)
Wherein Hq2Expression formula it is as follows：
Ibid determine transformation matrix4R7Joint angle q is determined according to anticosine and arctan function afterwards5,q6,q7；
3) joint angle q3The inverse solution of redundant robot is sought for nuisance parameter：
When with joint angle q3For nuisance parameter when, according to formula (9), formula (10) and formula (11), q1,q2Also it is calculated：
q1=arctan2 (Hq2(1),Hq2(2)) (15)
Ibid determine transformation matrix4R7Afterwards according to anticosine and arctan function, joint angle q is determined5,q6,q7；Joint angle q3It is superfluous
Remaining parameter is asked the inverse solution of redundant robot to solve and is completed；
4) joint angle q5,q6,q7The inverse solution of redundant robot is sought for nuisance parameter：
Because mechanical arm is symmetrical, joint of mechanical arm backward is calculated, at this time4R7It is inverse as0R3Processing, is similarly obtained
Respectively with joint angle q5,q6,q7For the inverse solution of nuisance parameter.
3. the inverse solution method for solving according to claim 1 for redundant mechanical arm approachability analysis, it is characterised in that：?
In the step 3, judge whether current inverse solution meets joint and limit condition by defining the function that inverse solution is limited to joint, if
It limits beyond joint, describe the currently inverse size solved beyond the joint limit and is limited more than joint；The expression formula of function is as follows：
Maxδ=argmaxi{|δ1|,|δ2|,|δ3|,|δ5|,|δ6|,|δ7|} (18)
In formulaIndicate the limit on the left in i-th of joint,Indicate the limit on the right-right-hand limit in i-th of joint；Variable MaxδDefinition be for
6 farthest joints of the joint angle abscission joint limit are obtained, if obtained δiIt is not all 0, then judges that the inverse solution is discontented with podarthrum limit
Position condition, uses function MaxδIt obtains abscission joint and limits maximum joint angle j updating to obtain the joint angle q of next iterationj(k+1)
=qj(k)+sign(δj)Δqj, Δ qjFor the size of newer joint angle, and use qj(k+1) continue as nuisance parameter
Inverse solution solves.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810567298.2A CN108789406A (en) | 2018-06-05 | 2018-06-05 | A kind of inverse solution method for solving for redundant mechanical arm approachability analysis |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810567298.2A CN108789406A (en) | 2018-06-05 | 2018-06-05 | A kind of inverse solution method for solving for redundant mechanical arm approachability analysis |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108789406A true CN108789406A (en) | 2018-11-13 |
Family
ID=64088679
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810567298.2A Pending CN108789406A (en) | 2018-06-05 | 2018-06-05 | A kind of inverse solution method for solving for redundant mechanical arm approachability analysis |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108789406A (en) |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112091979A (en) * | 2020-10-10 | 2020-12-18 | 哈尔滨工业大学 | Seven-degree-of-freedom mechanical arm limiting optimization method based on position-level inverse kinematics |
| CN112364458A (en) * | 2020-11-17 | 2021-02-12 | 苏州睿友智能装备有限公司 | Inverse solution solving method and medium for right-angle steel rail polishing unit |
| CN112597437A (en) * | 2020-12-29 | 2021-04-02 | 深圳市优必选科技股份有限公司 | Method, device and equipment for analyzing inverse kinematics of mechanical arm |
| CN113084791A (en) * | 2019-12-23 | 2021-07-09 | 深圳市优必选科技股份有限公司 | Mechanical arm control method, mechanical arm control device and terminal equipment |
| CN113119104A (en) * | 2019-12-31 | 2021-07-16 | 深圳市优必选科技股份有限公司 | Mechanical arm control method, mechanical arm control device, computing equipment and system |
| CN113119131A (en) * | 2021-05-08 | 2021-07-16 | 北京爱康宜诚医疗器材有限公司 | Robot control method and device, computer readable storage medium and processor |
| CN113487655A (en) * | 2021-09-07 | 2021-10-08 | 成都博恩思医学机器人有限公司 | Indoor model generation method, electronic equipment and storage medium |
| CN113858202A (en) * | 2021-09-29 | 2021-12-31 | 中铁工程装备集团有限公司 | Inverse solution analysis method, device, equipment and medium for anchor rod trolley drill arm |
| CN114310915A (en) * | 2022-02-16 | 2022-04-12 | 哈尔滨工业大学 | Space manipulator butt joint end tool trajectory planning method based on visual feedback |
| CN116587289A (en) * | 2023-07-17 | 2023-08-15 | 西北工业大学太仓长三角研究院 | Seven-degree-of-freedom wrist joint bias mechanical arm inverse solution method, system and medium |
| CN118143948A (en) * | 2024-04-10 | 2024-06-07 | 北京天智航医疗科技股份有限公司 | Seven-degree-of-freedom mechanical arm movement control method and device and electronic equipment |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101719172A (en) * | 2009-11-03 | 2010-06-02 | 上海大学 | Method for constructing microporous structure of bionic support |
| US20110172819A1 (en) * | 2010-01-14 | 2011-07-14 | Samsung Electronics Co., Ltd. | Manipulator and control method thereof |
| CN107066645A (en) * | 2016-12-01 | 2017-08-18 | 西北工业大学 | A kind of seven freedom biasing mechanism arm is against solution method |
| CN107729637A (en) * | 2017-10-09 | 2018-02-23 | 燕山大学 | Redundant degree of freedom manipulator motion planning and evaluation method based on arm shape angle range |
-
2018
- 2018-06-05 CN CN201810567298.2A patent/CN108789406A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101719172A (en) * | 2009-11-03 | 2010-06-02 | 上海大学 | Method for constructing microporous structure of bionic support |
| US20110172819A1 (en) * | 2010-01-14 | 2011-07-14 | Samsung Electronics Co., Ltd. | Manipulator and control method thereof |
| CN107066645A (en) * | 2016-12-01 | 2017-08-18 | 西北工业大学 | A kind of seven freedom biasing mechanism arm is against solution method |
| CN107729637A (en) * | 2017-10-09 | 2018-02-23 | 燕山大学 | Redundant degree of freedom manipulator motion planning and evaluation method based on arm shape angle range |
Non-Patent Citations (1)
| Title |
|---|
| QINGHUA LIU 等: "Common workspace analysis for a dual-arm robot based on reachability", 《2017 IEEE INTERNATIONAL CONFERENCE ON CYBERNETICS AND INTELLIGENT SYSTEMS (CIS) AND IEEE CONFERENCE ON ROBOTICS, AUTOMATION AND MECHATRONICS (RAM)》 * |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113084791A (en) * | 2019-12-23 | 2021-07-09 | 深圳市优必选科技股份有限公司 | Mechanical arm control method, mechanical arm control device and terminal equipment |
| CN113119104A (en) * | 2019-12-31 | 2021-07-16 | 深圳市优必选科技股份有限公司 | Mechanical arm control method, mechanical arm control device, computing equipment and system |
| CN113119104B (en) * | 2019-12-31 | 2022-08-05 | 深圳市优必选科技股份有限公司 | Mechanical arm control method, mechanical arm control device, computing equipment and system |
| CN112091979B (en) * | 2020-10-10 | 2021-11-19 | 哈尔滨工业大学 | Seven-degree-of-freedom mechanical arm limiting optimization method based on position-level inverse kinematics |
| CN112091979A (en) * | 2020-10-10 | 2020-12-18 | 哈尔滨工业大学 | Seven-degree-of-freedom mechanical arm limiting optimization method based on position-level inverse kinematics |
| CN112364458A (en) * | 2020-11-17 | 2021-02-12 | 苏州睿友智能装备有限公司 | Inverse solution solving method and medium for right-angle steel rail polishing unit |
| CN112364458B (en) * | 2020-11-17 | 2023-12-29 | 苏州睿友智能装备有限公司 | Inverse solution method and medium for right-angle steel rail grinding unit |
| CN112597437A (en) * | 2020-12-29 | 2021-04-02 | 深圳市优必选科技股份有限公司 | Method, device and equipment for analyzing inverse kinematics of mechanical arm |
| CN113119131B (en) * | 2021-05-08 | 2022-08-16 | 北京壹点灵动科技有限公司 | Robot control method and device, computer readable storage medium and processor |
| CN113119131A (en) * | 2021-05-08 | 2021-07-16 | 北京爱康宜诚医疗器材有限公司 | Robot control method and device, computer readable storage medium and processor |
| CN113487655A (en) * | 2021-09-07 | 2021-10-08 | 成都博恩思医学机器人有限公司 | Indoor model generation method, electronic equipment and storage medium |
| CN113858202A (en) * | 2021-09-29 | 2021-12-31 | 中铁工程装备集团有限公司 | Inverse solution analysis method, device, equipment and medium for anchor rod trolley drill arm |
| CN114310915B (en) * | 2022-02-16 | 2022-09-09 | 哈尔滨工业大学 | Space manipulator butt joint end tool trajectory planning method based on visual feedback |
| CN114310915A (en) * | 2022-02-16 | 2022-04-12 | 哈尔滨工业大学 | Space manipulator butt joint end tool trajectory planning method based on visual feedback |
| CN116587289A (en) * | 2023-07-17 | 2023-08-15 | 西北工业大学太仓长三角研究院 | Seven-degree-of-freedom wrist joint bias mechanical arm inverse solution method, system and medium |
| CN116587289B (en) * | 2023-07-17 | 2024-03-29 | 西北工业大学太仓长三角研究院 | Seven-degree-of-freedom wrist joint bias mechanical arm inverse solution method, system and medium |
| CN118143948A (en) * | 2024-04-10 | 2024-06-07 | 北京天智航医疗科技股份有限公司 | Seven-degree-of-freedom mechanical arm movement control method and device and electronic equipment |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108789406A (en) | A kind of inverse solution method for solving for redundant mechanical arm approachability analysis | |
| CN109895101B (en) | Unique solution method for inverse kinematics numerical value of joint type mechanical arm | |
| CN106166749B (en) | A kind of motion track planing method of space movement multi-arm robot | |
| CN108324373B (en) | Accurate positioning implementation method of puncture surgery robot based on electromagnetic positioning system | |
| CN111037561B (en) | Space registration method and device | |
| CN109344477B (en) | 6-degree-of-freedom mechanical arm inverse kinematics solving method | |
| CN108908347B (en) | Fault-tolerant repetitive motion planning method for redundant mobile mechanical arm | |
| CN106845037B (en) | A kind of general method for solving of inverse kinematics of five degree of freedom serial manipulator | |
| CN111230866B (en) | Calculation method for real-time pose of six-axis robot tail end following target object | |
| CN106844951B (en) | Method and system for solving inverse kinematics of super-redundant robot based on segmented geometric method | |
| CN110695988A (en) | Method and system for cooperative motion of double mechanical arms | |
| CN114161425B (en) | Error compensation method for industrial robot | |
| CN111546376A (en) | Rapid self-collision detection method of space combination mechanical arm | |
| Zheng et al. | Kinematics analysis and trajectory planning of 6-DOF robot | |
| CN111216138A (en) | Robot calibration method, robot calibration system and readable storage medium | |
| CN111113425B (en) | Inverse solution method for kinematics of five-degree-of-freedom series-parallel robot with parasitic motion | |
| CN108481324A (en) | A kind of inverse solution engineering and its collision detection algorithm of eight shaft multifunctionals machinery arm | |
| CN113722864B (en) | 7-degree-of-freedom redundant mechanical arm inverse kinematics solving method and system | |
| CN114589701A (en) | Multi-joint mechanical arm obstacle avoidance inverse kinematics method based on damping least squares | |
| CN113043278A (en) | Mechanical arm track planning method based on improved whale searching method | |
| CN111546379B (en) | Sensor layout method for monitoring operating state of industrial robot | |
| CN105856240B (en) | A kind of simple joint failure manipulator model reconstructing method based on perspective geometry method | |
| CN114784701A (en) | Power distribution network live working autonomous navigation method, system, equipment and storage medium | |
| CN116985147B (en) | Mechanical arm inverse kinematics solving method and device | |
| Xiang et al. | An optimal trajectory control strategy for underwater welding robot |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20181113 |