CN108705535B - Planar 3R operating arm inverse kinematics implementation method - Google Patents

Planar 3R operating arm inverse kinematics implementation method Download PDF

Info

Publication number
CN108705535B
CN108705535B CN201810561283.5A CN201810561283A CN108705535B CN 108705535 B CN108705535 B CN 108705535B CN 201810561283 A CN201810561283 A CN 201810561283A CN 108705535 B CN108705535 B CN 108705535B
Authority
CN
China
Prior art keywords
coordinates
point
coordinate system
calculating
joint
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810561283.5A
Other languages
Chinese (zh)
Other versions
CN108705535A (en
Inventor
王品
张迪
李栋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ludong University
Original Assignee
Ludong University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Ludong University filed Critical Ludong University
Priority to CN201810561283.5A priority Critical patent/CN108705535B/en
Publication of CN108705535A publication Critical patent/CN108705535A/en
Application granted granted Critical
Publication of CN108705535B publication Critical patent/CN108705535B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

The invention relates to a planar 3R operating arm inverse kinematics implementation method which is characterized in that the lengths of a large arm, a small arm and a tool are givenl 1、l 2、l 3Calculating the angles of the shoulder joint O1, the elbow joint A and the wrist joint O2 according to the position of the target point D and the posture alpha of the tool at the target pointθ 1、θ 2、θ 3. In calculating the coordinates and rotation angles of the elbow joint Aθ 2In the process, the problem of intersection of the circle O1 and the circle O2 is solved, and the coordinates of the point A can be quickly calculated by combining an algebraic method and a geometric method and adopting a method of interconversion of a static coordinate system and a dynamic coordinate system without judging and increasing roots. The method of the present invention has the advantages of less computer control program codes, less calculation amount and high running speed.

Description

Planar 3R operating arm inverse kinematics implementation method
Technical Field
The invention relates to the technical field of robots, in particular to a method for realizing inverse kinematics of a plane 3R operating arm.
Background
With the popularization of industrial robot application, the planar 3R manipulator is more and more widely applied as the simplest robot mechanism. The problem of inverse kinematics control of the plane 3R operating arm is solved as the problem of solving an intersection point of two intersected circles, the most direct method is to form a binary quadratic equation set according to the equation of the two circles, the binary quadratic equation is simplified into a unitary quadratic equation, and after two solutions are solved, root increase is removed according to actual conditions, and a group of solutions are reserved; in another method, two circular equation formulas are subtracted to obtain a linear equation where the intersection point is located, the linear equation and one of the circular equations are simultaneously controlled, finally the linear equation is simplified into a quadratic equation of one element, and after two solutions are solved, root increase is removed according to actual conditions, and a group of solutions is reserved. The two methods have the problems of judgment and root increase elimination, increase of calculated amount and reduction of working efficiency.
Disclosure of Invention
The invention aims to overcome the defects of the two methods, and the method combining the algebraic method and the geometric method can quickly and conveniently calculate the needed intersection point at one time and calculate the rotation angles of the shoulder joint, the elbow joint and the wrist joint.
The technical scheme for solving the technical problems is as follows:
the planar 3R operation arm inverse kinematics implementation method is characterized by comprising the following steps: the length of the large arm 1, the small arm 2 and the tool 3 is givenl 1、l 2、l 3And the position of the target point D and the posture alpha of the tool 3 at the target point are calculated to obtain the angles of the shoulder joint O1, the elbow joint A and the wrist joint O2θ 1、θ 2、θ 3The specific calculation method comprises the following steps:
step 1: calculating the coordinates and rotation angles of the wrist joint O2θ 3Coordinates of target point D in FIG. 1: (x D，y D) And the pose α of the tool 3 at the target point D are given, the coordinates of the wrist joint O2 can be determined ((D))x O2，y O2)=( x D +l 3cosα， y D +l 3sin α), wrist joint rotation angleθ 3=α+π；
Step 2: calculating the coordinates and rotation angles of the elbow joint Aθ 2(ii) a This problem is attributed to the circle O1 (radius)l 1) And the circle O2 (radius)l 2) Intersecting, finding the coordinates of point A of two intersection points (x A，y A) To obtain the elbow joint angleθ 2=atan2(y O2- y A，x O2- x A) (ii) a Wherein atan2 is an arctangent function in C language, the unit of the return value is radian, and the value range is (-pi, pi)];
And step 3: calculating the angle of rotation of the shoulder jointθ 1(ii) a Coordinates of shoulder joint O1: (x O1，y O1) Is known, so that the shoulder joint turnsθ 1=atan2(y A- y O1，x A- x O1)。
Further, the step 2 of calculating the coordinates of the elbow joint a, as shown in fig. 2, adopts a method of combining an algebraic method and a geometric method and interconverting a static coordinate system, and includes the following steps:
step 2.1: in the stationary coordinate systemXOYIn, the equation of two circles is
⊙O1：(x- x O1)2+(y- y O1)2= l 1 2，⊙O2：(x- x O2)2+(y- y O2)2= l 2 2；
Step 2.2: calculating the distance between the centers of two circlesl=((y O2- y O1)2+(x O2- x O1) 2)1/2If, ifl> l 1+ l 2The two circles are separated from each other, no intersection point exists, the calculation is finished, and otherwise, the next step is carried out;
step 2.3: establishing a moving coordinate systemX1O1Y1, the origin is located in a static coordinate systemXOYPoint O1 in (1) and horizontal axis O1X1 through the center line O1O2, and forms an X-axis included angle with the static coordinate systemθ=atan2(y O2- y O1，x O2- x O1)；
Step 2.4: in the moving coordinate system, an equation of two circles is [ O1 ]:u 2+v 2= l 1 2，⊙O2：(u- l)2+v 2=l 2 2；
step 2.5: in the moving coordinate system, the common chord crossed by the two circles is a line perpendicular to the horizontal axis O1X1, the equation of which is:u=( l 1 2- l 2 2+ l 2)/(2l) I.e. O1C = uAC = (according to pythagorean theorem) l 1 2-u 2)1/2Thus the coordinates of point A: (u A，v A)=( u，( l 1 2-u 2)1/2) (ii) a Similarly, the coordinates of point B: (u B，v B)=( u，-( l 1 2-u 2)1/2)；
Step 2.6: in the stationary coordinate systemXOYIn the method, the coordinates of the point A are calculated through coordinate transformationIs composed of
(x A，y A)= (u Acosθ+ v A sinθ+ x O1，v Acosθ- u A sinθ+y O1)
The coordinate calculation method of point B is the same as (1)x B，y B)= (u Bcosθ+ v B sinθ+ x O1，v Bcosθ- u B sinθ+y O1) The flow chart is shown in fig. 3.
In the planar 3R operating arm inverse kinematics control process, if the intersection point A is positioned above the horizontal axis O1X1 of the moving coordinate system under the initial condition, the point A is always positioned above the horizontal axis of the moving coordinate system in the changing process of the moving coordinate system; and vice versa. When the computer is used for control, the point A or the point B can be directly judged to be used in the initial condition, the problem of selecting the point A or the point B does not need to be judged again in the change process of the moving coordinate system, the program code is less, the calculated amount is less, and the running speed is high.
Drawings
FIG. 1 is a schematic structural view of a plane 3R operating arm;
FIG. 2 is a diagram illustrating a case where two circles intersect in a static and dynamic coordinate system;
FIG. 3 is a flow chart of calculating the intersection A of two circles;
fig. 4 is a schematic view of the four-bar linkage in embodiment 2.
Detailed Description
The principles and features of this invention are described below in conjunction with the following drawings, which are set forth by way of illustration only and are not intended to limit the scope of the invention.
Example 1:
the length of the large arm 1, the small arm 2 and the tool 3 is givenl 1、l 2、l 3And the position of the target point D and the posture alpha of the tool 3 at the target point are calculated to obtain the angles of the shoulder joint O1, the elbow joint A and the wrist joint O2θ 1、θ 2、θ 3The specific calculation method comprises the following steps:
step 1: calculating the coordinates and rotation angles of the wrist joint O2θ 3Coordinates of target point D in FIG. 1: (x D，y D) And the pose α of the tool 3 at the target point D are given, the coordinates of the wrist joint O2 can be determined ((D))x O2，y O2)=( x D +l 3cosα， y D +l 3sin α), wrist joint rotation angleθ 3=α+π；
Step 2: calculating the coordinates and rotation angles of the elbow joint Aθ 2(ii) a First, the coordinates of point A are calculated (x A，y A) The specific calculation method comprises the following steps:
step 2.1: in the stationary coordinate systemXOYIn, the equation of two circles is
⊙O1：(x- x O1)2+(y- y O1)2= l 1 2，⊙O2：(x- x O2)2+(y- y O2)2= l 2 2；
Step 2.2: calculating the distance between the centers of two circlesl=((y O2- y O1)2+(x O2- x O1) 2)1/2If, ifl> l 1+ l 2The two circles are separated from each other, no intersection point exists, the calculation is finished, and otherwise, the next step is carried out;
step 2.3: establishing a moving coordinate systemX1O1Y1, the origin is located in a static coordinate systemXOYPoint O1 in (1) and horizontal axis O1X1 through the center line O1O2, and forms an X-axis included angle with the static coordinate systemθ=atan2(y O2- y O1，x O2- x O1)；
Step 2.4: in the moving coordinate system, an equation of two circles is [ O1 ]:u 2+v 2= l 1 2，⊙O2：(u- l)2+v 2=l 2 2；
step 2.5: in the moving coordinate system, the common chord crossed by the two circles is a line perpendicular to the horizontal axis O1X1, the equation of which is:u=( l 1 2- l 2 2+ l 2)/(2l) I.e. O1C = uAC = (according to pythagorean theorem) l 1 2-u 2)1/2Thus the coordinates of point A: (u A，v A)=( u，( l 1 2-u 2)1/2) (ii) a Similarly, the coordinates of point B: (u B，v B)=( u，-( l 1 2-u 2)1/2)；
Step 2.6: in the stationary coordinate systemXOYIn (1), the coordinates of the point A are calculated as
(x A，y A)= (u Acosθ+ v A sinθ+ x O1，v Acosθ- u A sinθ+y O1) The flow chart is shown in fig. 3.
According to the coordinates of the point A, the elbow joint rotation angle is calculatedθ 2=atan2(y O2- y A，x O2- x A) (ii) a Wherein atan2 is an arctangent function in C language, the unit of the return value is radian, and the value range is (-pi, pi)];
And step 3: calculating the angle of rotation of the shoulder jointθ 1(ii) a Coordinates of shoulder joint O1: (x O1，y O1) Is known, so that the shoulder joint turnsθ 1=atan2(y A- y O1，x A- x O1)。
Example 2:
in the articulated four-bar mechanism shown in fig. 4, the bar 4 is a frame, and coordinates of O (0,0) and O2 (0,0)x O2，y O2) Is given by the rod 3 beingPrime mover, each rod being longl 1、l 2、l 3Andl 4and the input angle alpha is given, and the output angle phi is obtained, wherein the specific calculation method comprises the following steps:
step 1: calculating the coordinates of hinge O1: (x O1，y O1)=( l 3cosα， l 3sinα)；
Step 2: calculating the coordinates of hinge A: (x A，y A) The specific calculation method comprises the following steps:
step 2.1: in the stationary coordinate systemXOYIn, the equation of two circles is
⊙O1：(x- x O1)2+(y- y O1)2= l 1 2，⊙O2：(x- x O2)2+(y- y O2)2= l 2 2；
Step 2.2: calculating the distance between the centers of two circlesl=((y O2- y O1)2+(x O2- x O1) 2)1/2If, ifl> l 1+ l 2The two circles are separated from each other, no intersection point exists, the calculation is finished, and otherwise, the next step is carried out;
step 2.3: establishing a moving coordinate systemX1O1Y1, the origin is located in a static coordinate systemXOYPoint O1 in (1) and horizontal axis O1X1 through the center line O1O2, and forms an X-axis included angle with the static coordinate systemθ=atan2(y O2- y O1，x O2- x O1)；
Step 2.4: in the moving coordinate system, an equation of two circles is [ O1 ]:u 2+v 2= l 1 2，⊙O2：(u- l)2+v 2=l 2 2；
step 2.5: in the moving coordinate system, the common chord crossed by the two circles is a line perpendicular to the horizontal axis O1X1 straight lineThe equation is:u=( l 1 2- l 2 2+ l 2)/(2l) I.e. O1C = uAC = (according to pythagorean theorem) l 1 2-u 2)1/2Thus the coordinates of point A: (u A，v A)=( u，( l 1 2-u 2)1/2) (ii) a Similarly, the coordinates of point B: (u B，v B)=( u，-( l 1 2-u 2)1/2)；
Step 2.6: in the stationary coordinate systemXOYIn (1), the coordinates of the point A are calculated as
(x A，y A)= (u Acosθ+ v A sinθ+ x O1，v Acosθ- u A sinθ+y O1) The flow chart is shown in fig. 3.
And step 3: from the coordinates of point a, calculate the output angle phi = atan2(y A- y O2，x A- x O2)。
The protection scope of the present invention is not limited to the examples given above, and all technical solutions that can be realized according to the idea of the present invention belong to the protection scope of the present invention.

Claims (1)

1. The planar 3R operation arm inverse kinematics implementation method is characterized by comprising the following steps: the length of the big arm, the small arm and the tool is givenl 1、l 2、l 3Calculating the angles of the shoulder joint O1, the elbow joint A and the wrist joint O2 according to the position of the target point D and the posture alpha of the tool at the target pointθ 1、θ 2、θ 3The specific calculation method comprises the following steps:
step 1: calculating the coordinates and rotation angles of the wrist joint O2θ 3Coordinates of the target point D: (x D，y D) And the tool are on the eyeGiven the pose α of punctuation D, the coordinates of wrist O2 can be determined (R) ((R))x O2，y O2)=( x D +l 3cosα，y D +l 3sin α), wrist joint rotation angleθ 3=α+π；
Step 2: calculating the coordinates and rotation angles of the elbow joint Aθ 2(ii) a This problem is attributed to the circle O1 (radius)l 1) And the circle O2 (radius)l 2) Intersecting, finding the coordinates of point A of two intersection points (x A，y A) As shown in fig. 2, the method for converting the static and dynamic coordinate systems by combining the algebraic method and the geometric method includes the following steps:
step 2.1: in the stationary coordinate systemXOYIn, the equation of two circles is
⊙O1：(x- x O1)2+(y- y O1)2= l 1 2，⊙O2：(x- x O2)2+(y- y O2)2= l 2 2；
Step 2.2: calculating the distance between the centers of two circlesl=((y O2- y O1)2+(x O2- x O1) 2)1/2If, ifl> l 1+ l 2The two circles are separated from each other, no intersection point exists, the calculation is finished, and otherwise, the next step is carried out;
step 2.3: establishing a moving coordinate systemX1O1Y1, the origin is located in a static coordinate systemXOYPoint O1 in (1) and horizontal axis O1X1 through the center line O1O2, and forms an X-axis included angle with the static coordinate systemθ=atan2(y O2- y O1，x O2- x O1)；
Step 2.4: in the moving coordinate system, the equation of two circles is
⊙O1：u 2+v 2= l 1 2，⊙O2：(u- l)2+v 2= l 2 2；
Step 2.5: in the moving coordinate system, the common chord crossed by the two circles is a line perpendicular to the horizontal axis O1X1, and the horizontal axis O1X1 the intersection point is:u 0 =( l 1 2- l 2 2+ l 2)/(2l) I.e. O1C = u 0 . AC = (according to pythagorean theorem) l 1 2-u 0 2)1/2Thus the coordinates of point A: (u A，v A)=( u 0 ，( l 1 2-u 0 2)1/2) (ii) a Similarly, the coordinates of point B: (u B，v B)=( u 0 ，-( l 1 2-u 0 2)1/2)；
Step 2.6: in the stationary coordinate systemXOYIn (1), the coordinates of the point A are calculated as
(x A，y A)= (u Acosθ+ v A sinθ+ x O1，v Acosθ- u A sinθ+y O1)
The coordinate calculation method of point B is the same as (1)x B，y B)= (u Bcosθ+ v B sinθ+ x O1，v Bcosθ- u B sinθ+y O1)；
Step 2.7: calculating the elbow joint angleθ 2=atan2(y O2- y A，x O2- x A) (ii) a Wherein atan2 is an arctangent function in C language, the unit of the return value is radian, and the value range is (-pi, pi)]；
And step 3: calculating the angle of rotation of the shoulder jointθ 1(ii) a Coordinates of shoulder joint O1: (x O1，y O1) Is known, so that the shoulder joint turnsθ 1=atan2(y A- y O1，x A- x O1)。
CN201810561283.5A 2018-06-04 2018-06-04 Planar 3R operating arm inverse kinematics implementation method Active CN108705535B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810561283.5A CN108705535B (en) 2018-06-04 2018-06-04 Planar 3R operating arm inverse kinematics implementation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810561283.5A CN108705535B (en) 2018-06-04 2018-06-04 Planar 3R operating arm inverse kinematics implementation method

Publications (2)

Publication Number Publication Date
CN108705535A CN108705535A (en) 2018-10-26
CN108705535B true CN108705535B (en) 2021-04-16

Family

ID=63871212

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810561283.5A Active CN108705535B (en) 2018-06-04 2018-06-04 Planar 3R operating arm inverse kinematics implementation method

Country Status (1)

Country Link
CN (1) CN108705535B (en)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2168548A1 (en) * 2005-05-27 2010-03-31 Honda Motor Co., Ltd. Walking assisting device
CN105005656A (en) * 2015-07-13 2015-10-28 广州霞光技研有限公司 Autonomous grasp inverse solution engineering algorithm for six-degree-of-freedom manipulator
CN105183009A (en) * 2015-10-15 2015-12-23 哈尔滨工程大学 Trajectory control method for redundant mechanical arm
CN107953340A (en) * 2018-01-16 2018-04-24 广东凯宝机器人科技有限公司 A kind of inverse solution Engineering Algorithm of universal six degree of freedom manipulator

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2168548A1 (en) * 2005-05-27 2010-03-31 Honda Motor Co., Ltd. Walking assisting device
CN105005656A (en) * 2015-07-13 2015-10-28 广州霞光技研有限公司 Autonomous grasp inverse solution engineering algorithm for six-degree-of-freedom manipulator
CN105183009A (en) * 2015-10-15 2015-12-23 哈尔滨工程大学 Trajectory control method for redundant mechanical arm
CN107953340A (en) * 2018-01-16 2018-04-24 广东凯宝机器人科技有限公司 A kind of inverse solution Engineering Algorithm of universal six degree of freedom manipulator

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
2019管理类联考华明讲数学 教程篇;孙华明等;《2019管理类联考华明讲数学 教程篇》;北京理工大学出版社;20180228;第294-296页 *
三自由度关节式机械臂的结构设计与轨迹控制研究;常斌;《中国优秀硕士学位论文全文数据库 信息科技辑》;20180115;正文5-38页 *
机构运动学分析中若干问题的几何代数法研究;倪振松;《中国博士学位论文全文数据库 工程科技Ⅱ辑》;20101115;全文 *

Also Published As

Publication number Publication date
CN108705535A (en) 2018-10-26

Similar Documents

Publication Publication Date Title
CN103942427B (en) Quick and simple method for solving inverse kinematics of six-degree-of-freedom mechanical arm
CN109291046A (en) A kind of seven freedom personification configuration mechanical arm inverse kinematics planing method
CN109176494A (en) Rope drives Arm Flexible machine people self-calibrating method and system, storage medium
TWI704039B (en) Mechanical arm singular point control method and system
CN107378952B (en) A kind of solution that redundancy mechanical arm end effector posture is kept
CN108839025A (en) A kind of motion planning method and device of mobile mechanical arm
CN105773620A (en) Track planning and control method of free curve of industrial robot based on double quaternions
CN106844951B (en) Method and system for solving inverse kinematics of super-redundant robot based on segmented geometric method
US10105842B2 (en) Operation program creating method and control method of robot
CN104385283B (en) A kind of quick judgment method of sixdegree-of-freedom simulation Singularity
CN111890349A (en) Four-degree-of-freedom mechanical arm motion planning method
KR940003948B1 (en) Industrial robot control method and apparatus
CN111230866A (en) Calculation method for real-time pose of six-axis robot tail end following target object
CN108705535B (en) Planar 3R operating arm inverse kinematics implementation method
CN116330267A (en) Control method based on industrial robot wrist singular point calculation
CN109866224A (en) A kind of robot Jacobian matrix calculation method, device and storage medium
CN106003057A (en) Rapid judging method for configuration singularity of mechanical arm with redundant degree of freedom
CN116276990A (en) Two-degree-of-freedom parallel structure kinematics positive solution method based on neural network training
CN109648563B (en) Method for controlling motion of serial robot and computer storage medium
CN115525863A (en) Kinematics solving method and system for seven-degree-of-freedom teleoperation mechanical arm system
JPH0693209B2 (en) Robot's circular interpolation attitude control device
CN108890648B (en) Method for establishing multi-joint serial robot kinematics model
CN116141302A (en) Seven-degree-of-freedom mechanical arm movement control method and device and electronic equipment
CN108068108B (en) Method for controlling motion of in-plane swinging telescopic robot
CN117733872B (en) Series robot inverse kinematics control method based on directional performance

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant