CN110238848B - Method for calculating gravity vector under robot coordinate system - Google Patents
Method for calculating gravity vector under robot coordinate system Download PDFInfo
- Publication number
- CN110238848B CN110238848B CN201910461617.6A CN201910461617A CN110238848B CN 110238848 B CN110238848 B CN 110238848B CN 201910461617 A CN201910461617 A CN 201910461617A CN 110238848 B CN110238848 B CN 110238848B
- Authority
- CN
- China
- Prior art keywords
- vector
- gravity
- straight line
- gravity vector
- calculating
- 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
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
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The invention relates to the field of robot dynamics calculation, in particular to a method for calculating a gravity vector under a robot coordinate system, which comprises the following specific steps: s1: positioning a marker to indicate a direction of gravity; s2: acknowledgement P1(ii) a S3: acknowledgement P2(ii) a S4: calculating a unit vector; s5: calculating a gravity vector; the gravity vector direction is changed into a visual straight line by using a string hung with a heavy object, then a robot is used as a position sensor to measure coordinates of points on the straight line, a unit vector of the straight line is calculated based on the coordinates of the points, and finally the gravity vector is calculated according to the unit vector direction of the straight line and a local gravity acceleration value, so that the identification of the gravity vector at any installation angle can be realized compared with the traditional method which depends on the installation type given by a user; the gravity vector can be identified by measuring the coordinates of two or more points, the operation is simple, the kinematic model which is more accurate than the dynamic model is selected to identify the gravity vector, and the accuracy is higher.
Description
Technical Field
The invention relates to the field of robot dynamics calculation, in particular to a method for calculating a gravity vector under a robot coordinate system.
Background
With the development of robotics, dynamics-based related functions such as collision detection, drag teaching, and the like are widely used. In both of these applications, a description of the gravity vector in the robot coordinate system needs to be set. Whether the gravity vector is accurately set or not has a great influence on the effects of the dynamics applications, so that how to accurately measure the gravity vector is very important.
The following two schemes are generally adopted in the prior art to determine the gravity vector:
the first scheme is as follows: the installation mode selected by the user determines: when the user uses the dynamics function, the user can select an installation mode, such as forward installation, reverse installation, inclination at a certain angle and the like, in the demonstrator according to the installation mode of the robot, then the controller calculates a gravity vector according to the selected installation mode, for example, if the user selects forward installation, the controller considers that the direction of the gravity vector is opposite to the direction of the z-axis of a robot coordinate system, and the gravity vector is a gravity acceleration value. In this way, the user is required to ensure that the selected mounting method is very accurate, but in practice, due to problems of machining, assembly, measurement and the like, the user cannot ensure the accuracy of the selected mounting method of the robot, and thus cannot obtain an accurate description of the gravity vector in the robot coordinate system. And when the robot is installed on a platform with a certain inclination angle and the inclination angle of the platform is unknown, the method cannot be used.
Scheme II: determining based on the dynamic model and parameters: in the method, a dynamic model of the robot is established, the robot is operated to enable the pose of the robot to be changed continuously, information such as the position, the speed, the acceleration and the moment of each axis under the pose is recorded, and description of the gravity vector under a robot coordinate system is solved based on the dynamic model, dynamic parameters and the collected information such as the moment. The algorithm of the scheme is complex, accurate model parameters are needed to establish a dynamic model, and the method cannot be applied to robots without accurate dynamic model parameters.
The determination of the gravity vector direction in the first scheme can only depend on user input and cannot autonomously identify the gravity moment vector, and the second scheme solves the gravity vector by establishing a mathematical relationship between the gravity vector and the moments of all axes and then solving the gravity vector based on the relationship.
Disclosure of Invention
In order to solve the above problem, the present invention provides a method for calculating a gravity vector in a robot coordinate system.
A method for calculating a gravity vector under a robot coordinate system comprises the following specific steps:
s1: positioning a marker to indicate a direction of gravity;
s2: acknowledgement P1: adjusting the position of the sharp point of the tool at the execution end to ensure that the sharp point just contacts one point at the upper end of the string, and recording the position P of the sharp point of the tool at the moment in the coordinate system of the execution end1；
S3: acknowledgement P2: adjusting the position of the tool sharp point at the execution end to ensure that the sharp point just contacts one point at the lower end of the string, and then recording the position P of the tool sharp point under the coordinate system of the execution end at the moment2；
S4: calculating a unit vector: according to point P1And P2The unit vector direction V of the straight line is calculated:
wherein V is [ V ═ Vx vy vz]Is a unit direction vector of a straight line, P ═ Px py pz]Is a point on a straight line, Δ P12＝P1-P2；
S5: calculating a gravity vector: after the unit vector direction of the straight line is obtained, the unit vector direction of the straight line is consistent with the gravity vector direction, and the gravity vector G is calculated as follows:
G＝g·V；
where g is the local gravitational acceleration value, which may be obtained from a regional query.
As a further improvement of the invention, the marker is an object which is hung and the shape of the marker is consistent with the direction of gravity.
As a further improvement of the invention, the marker is a string with one end connected with a heavy object.
As a further improvement of the present invention, in order to further improve the measurement accuracy in the linear direction in step S4, more points may be measured and solved by using the least square algorithm, and three points are selected for description, and the following equation is listed:
d represents a distance vector between all the measurement points, as shown in the following formula:
the unit vector corresponding to the straight line is obtained by the least square method, and is shown as the following formula:
V＝(DT·D)-1·DT·ΔP。
the execution end is used as a measuring tool to measure the coordinates of each point on the straight line, and the position of the tool sharp point is solved through the kinematic positive solution of the execution end, as shown in the following formula:
P＝f(θ)；
wherein P is the position of the sharp point of the tool, theta is the position vector of each axis of the execution end, and f is the positive solution function of kinematics.
As a further improvement of the present invention, it is necessary to make the position error of the actuator 1mm or less.
As a further improvement of the invention, the execution end is a robot.
The beneficial effects of the invention are: the gravity vector direction is changed into a visual straight line by using a string hung with a heavy object, then the robot is used as a position sensor to measure the coordinates of points on the straight line, the unit vector of the straight line is calculated based on the coordinates of the points, and finally the gravity vector is calculated according to the unit vector direction of the straight line and the local gravity acceleration value; the gravity vector can be identified by measuring the coordinates of two or more points, the operation is simple, the kinematic model which is more accurate than the dynamic model is selected to identify the gravity vector, and the accuracy is higher.
Drawings
The invention is further illustrated with reference to the following figures and examples.
FIG. 1 is a schematic structural diagram of the present invention.
Detailed Description
In order to make the technical means, the creation characteristics, the achievement purposes and the effects of the invention easy to understand, the invention is further explained below.
As shown in fig. 1, a in the figure is a gravity direction, and a method for calculating a gravity vector in a robot coordinate system includes the following specific steps:
s1: the marker 4 is positioned to indicate the direction of gravity;
s2: acknowledgement P1: adjusting the position of the tip of the tool 3 of the actuator 1The tip is brought into contact with a point at the upper end of the marker 4, and the position P of the tip of the tool 3 in the coordinate system of the execution end is recorded1；
S3: acknowledgement P2: adjusting the position of the tip of the tool 3 of the actuating end 1 to make the tip contact with a point at the lower end of the marker 4, and recording the position P of the tip of the tool 3 in the coordinate system of the actuating end2；
S4: calculating a unit vector: according to point P1And P2The unit vector direction V of the straight line is calculated:
wherein V is [ V ═ Vx vy vz]Is a unit direction vector of a straight line, P ═ Px py pz]Is a point on a straight line, Δ P12＝P1-P2；
S5: calculating a gravity vector: after the unit vector direction of the straight line is obtained, the unit vector direction of the straight line is consistent with the gravity vector direction, and the gravity vector G is calculated by the following steps:
G＝g·V；
where g is the local gravitational acceleration value, which may be obtained from a regional query.
The tool 3 is any strip-shaped or other regular fixed-shape object with sharp points.
The marker 4 is an object which is hung and the shape direction of which is consistent with the gravity direction.
The marker 4 is a string with one end connected with a weight 5.
The marker 4 is light in weight and high in flexibility, and is straightened under the action of the weight 5, and the direction of the rope length after the marker 4 is straightened is consistent with the direction of the gravity vector according to physical knowledge, so that the direction of the gravity vector can be determined by directly measuring the length direction of the marker 4.
The gravity vector direction is changed into a visual straight line by using a marker 4 hung with a heavy object, then the robot 1 is used as a position sensor to measure the coordinates of points on the straight line, the unit vector of the straight line is calculated based on the coordinates of the points, and finally the gravity vector is calculated according to the unit vector direction of the straight line and the local gravity acceleration value.
In step S4, in order to further improve the measurement accuracy of the linear direction, more points may be measured and solved by using a least square algorithm, and three points are selected for description, and an equation shown in the following formula is listed:
d represents a distance vector between all the measurement points, as shown in the following formula:
the unit vector corresponding to the straight line is obtained by the least square method, and the unit vector is shown as the following formula:
V＝(DT·D)-1·DT·ΔP。
the execution end 1 is used as a measuring tool to measure the coordinates of each point on the straight line, and the position of the tool sharp point is solved through the kinematic positive solution of the execution end 1, as shown in the following formula:
P＝f(θ)；
where P is the position of the cusp of the tool 3, θ is the position vector of each axis of the actuation end, and f is the kinematics positive solution function.
The position error of the execution end 1 needs to be less than 1 mm.
The execution end 1 is a robot.
The unit direction vector of any straight line in space can be measured by measuring two points on the straight lineTo confirm that the two points in the present invention are P1、P2。
The gravity vector can be identified by measuring the coordinates of two or more points, the operation is simple, the kinematic model which is more accurate than the dynamic model is selected to identify the gravity vector, and the accuracy is higher.
The foregoing shows and describes the general principles, essential features, and advantages of the invention. It will be understood by those skilled in the art that the present invention is not limited to the embodiments described above, which are merely illustrative of the principles of the invention, but that various changes and modifications may be made without departing from the spirit and scope of the invention, which fall within the scope of the invention as claimed. The scope of the invention is defined by the appended claims and equivalents thereof.
Claims (5)
1. A method for calculating a gravity vector under a robot coordinate system is characterized by comprising the following steps: the method comprises the following specific steps:
s1: positioning a marker (4) to indicate the direction of gravity;
s2: confirmation
: adjusting the position of the tip of the tool (3) at the execution end (1) to make the tip just contact with one point at the upper end of the marker (4), and recording the position of the tip of the tool (3) at the execution end in the coordinate system
；
S3: confirmation
: adjusting the position of the sharp point of the tool (3) of the execution end (1) to ensure that the sharp point just contacts one point of the lower end of the marker (4), and recording the position of the sharp point of the tool (3) under the coordinate system of the execution end at the moment
；
S4: calculating a unit vector: according to the points
And
the unit vector direction of the straight line is calculated:
wherein
Is a unit direction vector of a straight line,
is a point on the straight line and is,
；
s5: calculating a gravity vector: after the unit vector direction of the straight line is obtained, the gravity vector is calculated by utilizing the coincidence of the unit vector direction of the straight line and the gravity vector direction
：
Wherein
The gravity acceleration value is a local gravity acceleration value, and the value can be obtained according to regional query;
in step S4, in order to further improve the measurement accuracy of the linear direction, more points may be measured and solved by using a least square algorithm, and three points are selected for description, and an equation shown in the following formula is listed:
the execution end (1) is a robot;
said
Represents the distance vector between all the measurement points, as shown in the following formula:
the unit vector corresponding to the straight line is obtained by the least square method, and the unit vector is shown as the following formula:
2. the method for calculating the gravity vector under the robot coordinate system according to claim 1, wherein the method comprises the following steps: the marker (4) is an object of which the shape direction is consistent with the gravity direction after being suspended.
3. The method for calculating the gravity vector under the robot coordinate system according to claim 1, wherein the method comprises the following steps: the marker (4) is a string with one end connected with a weight (5).
4. The method for calculating the gravity vector under the robot coordinate system according to claim 1, wherein the method comprises the following steps: the execution end (1) is used as a measuring tool to measure the coordinates of each point on a straight line, and the position of the tool sharp point is solved through the kinematic forward solution of the execution end (1), as shown in the following formula:
wherein
Is the position of the sharp point of the tool (3),
for the position vector of each axis of the execution end,
is a kinematic positive solution function.
5. The method for calculating the gravity vector under the robot coordinate system according to claim 4, wherein the method comprises the following steps: the position error of the execution end (1) needs to be less than 1 mm.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910461617.6A CN110238848B (en) | 2019-05-30 | 2019-05-30 | Method for calculating gravity vector under robot coordinate system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910461617.6A CN110238848B (en) | 2019-05-30 | 2019-05-30 | Method for calculating gravity vector under robot coordinate system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110238848A CN110238848A (en) | 2019-09-17 |
| CN110238848B true CN110238848B (en) | 2022-07-05 |
Family
ID=67885314
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910461617.6A Active CN110238848B (en) | 2019-05-30 | 2019-05-30 | Method for calculating gravity vector under robot coordinate system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110238848B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113492398B (en) * | 2020-04-02 | 2022-12-20 | 北京配天技术有限公司 | Calibration rod, calibration system for gravity acceleration direction and calibration method thereof |
| CN112476435B (en) * | 2020-11-25 | 2022-07-12 | 北京配天技术有限公司 | Calibration method and calibration device for gravity acceleration direction and storage medium |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104057290A (en) * | 2014-06-24 | 2014-09-24 | 中国科学院自动化研究所 | Method and system for assembling robot based on visual sense and force feedback control |
| CN104443448A (en) * | 2014-10-27 | 2015-03-25 | 清华大学 | Cable drive robot device for simulating zero-gravity and low-gravity environment |
| CN105447856A (en) * | 2015-11-17 | 2016-03-30 | 天津工业大学 | Marking point coupling method based on robot motion parameters and characteristic vectors |
Family Cites Families (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH05237782A (en) * | 1992-02-26 | 1993-09-17 | Daikin Ind Ltd | Work holding position setting method and device thereof |
| US6442439B1 (en) * | 1999-06-24 | 2002-08-27 | Sandia Corporation | Pendulation control system and method for rotary boom cranes |
| JP4968604B2 (en) * | 2006-06-21 | 2012-07-04 | トヨタ自動車株式会社 | Attitude angle determination device and determination method thereof |
| DE102006049957A1 (en) * | 2006-10-19 | 2008-04-24 | Abb Ag | System and method for calibration of a handling device |
| CN102175185A (en) * | 2011-01-20 | 2011-09-07 | 中南大学 | Fiber bragg grating tilt sensor and calibrating method thereof |
| JP5383760B2 (en) * | 2011-09-09 | 2014-01-08 | ファナック株式会社 | Robot with workpiece mass measurement function |
| US9140598B2 (en) * | 2011-09-20 | 2015-09-22 | Ishida Co., Ltd. | Mass measurement device |
| CN103808461A (en) * | 2012-11-14 | 2014-05-21 | 陈均钧 | Device for measuring rotational inertia of rigid body through magnetic suspension rotating platform |
| CN103019096B (en) * | 2012-11-23 | 2015-09-16 | 北京理工大学 | A kind of anthropomorphic robot inverse metabolic engineering device optimized based on acceleration |
| CN103018784B (en) * | 2012-11-28 | 2015-04-15 | 华中科技大学 | Simple-pendulum absolute gravimeter based on two-point fixed differential measurement |
| CN103482089B (en) * | 2013-10-09 | 2015-04-08 | 北京航空航天大学 | Unrestricted suspension type initiative gravity compensation system |
| DE102016000187B3 (en) * | 2016-01-11 | 2017-01-26 | Kuka Roboter Gmbh | Determining an orientation of a robot relative to a gravitational direction |
| CN105606127A (en) * | 2016-01-11 | 2016-05-25 | 北京邮电大学 | Calibration method for relative attitude of binocular stereo camera and inertial measurement unit |
| CN109211174B (en) * | 2017-07-06 | 2020-12-01 | 中国航空制造技术研究院 | Space vector pull rope measuring method and device |
| CN109520486B (en) * | 2019-01-02 | 2021-09-24 | 中国船舶重工集团公司第七0七研究所 | Vertical line deviation real-time calculation method based on horizontal tensor gravity gradient |
-
2019
- 2019-05-30 CN CN201910461617.6A patent/CN110238848B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104057290A (en) * | 2014-06-24 | 2014-09-24 | 中国科学院自动化研究所 | Method and system for assembling robot based on visual sense and force feedback control |
| CN104443448A (en) * | 2014-10-27 | 2015-03-25 | 清华大学 | Cable drive robot device for simulating zero-gravity and low-gravity environment |
| CN105447856A (en) * | 2015-11-17 | 2016-03-30 | 天津工业大学 | Marking point coupling method based on robot motion parameters and characteristic vectors |
Non-Patent Citations (2)
| Title |
|---|
| Weifu Wang ; Dmitry Berenson ; Devin Balkcom.An online method for tight-tolerance insertion tasks for string and rope.《Robotics and Automation》.2015, * |
| 宋韬.轮式移动机械臂倾覆与滑移问题研究.《中国博士学位论文全文数据库 (信息科技辑)》.2017,(第02期), * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110238848A (en) | 2019-09-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20220314445A1 (en) | Kinematics model-free trajectory tracking method for robotic arms and robotic arm system | |
| CN107042528B (en) | A kind of Kinematic Calibration system and method for industrial robot | |
| CN101424520B (en) | Method for detecting partial contour outline of object curved surface | |
| CN107650144A (en) | The demarcation calibration system and its method of a kind of industrial robot workpiece coordinate system | |
| CN110228067B (en) | Double-arm robot combined operation testing method based on laser tracker | |
| CN110238848B (en) | Method for calculating gravity vector under robot coordinate system | |
| CN102768006B (en) | Method for automatically measuring and adjusting large-curvature curve part in normal direction | |
| CN110936355B (en) | Mechanical arm guiding system and method based on visual speed measurement positioning | |
| CN112060085B (en) | Robot operation pose control method based on visual-touch multi-scale positioning | |
| CN107443388A (en) | A kind of mechanical arm absolute positioning error method of estimation based on general Ke Lijin | |
| KR101593330B1 (en) | Method to measure squareness of multi-axis precision control system using ball-bar data and straightness data | |
| CN104102173A (en) | Numerical Controller | |
| CN111515928B (en) | Mechanical arm motion control system | |
| CN109740270A (en) | The big L/D ratio peg-in-hole assembly system and method predicted and analyzed based on contact force and torque | |
| CN113799130B (en) | Robot pose calibration method in man-machine cooperation assembly | |
| CN108253884B (en) | Method and system for measuring non-contact space attitude of shield machine segment | |
| CN106248000B (en) | The measurement method of part axially bored line | |
| CN105588512B (en) | The measuring device and method of class rectangle tunnel tunnel segment component size | |
| CN110967522B (en) | Wind field gradient measuring method | |
| CN114659709B (en) | Three-dimensional centroid measurement method for large winged aerospace vehicle | |
| CN102172831A (en) | Method for detecting height of rivet | |
| CN111604900B (en) | Target parameter detection method suitable for manual operation of manipulator | |
| Zhao et al. | Center of mass and friction coefficient exploration of unknown object for a robotic grasping manipulation | |
| CN108107882A (en) | Service robot automatic Calibration and detecting system based on optical motion tracking | |
| CN112556649A (en) | Method and device for correcting dip angle of excavator during dip angle measurement and dip angle measuring instrument |
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 |