CN114353802A - Robot three-dimensional space positioning method based on laser tracking - Google Patents
Robot three-dimensional space positioning method based on laser tracking Download PDFInfo
- Publication number
- CN114353802A CN114353802A CN202210002047.6A CN202210002047A CN114353802A CN 114353802 A CN114353802 A CN 114353802A CN 202210002047 A CN202210002047 A CN 202210002047A CN 114353802 A CN114353802 A CN 114353802A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- total station
- robot
- working
- coordinate
- 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
- 238000000034 method Methods 0.000 title claims abstract description 20
- 239000011159 matrix material Substances 0.000 claims abstract description 22
- 230000009466 transformation Effects 0.000 claims description 13
- 239000013598 vector Substances 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 abstract description 6
- 238000004364 calculation method Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
Abstract
The invention discloses a robot three-dimensional space positioning method based on laser tracking, which comprises the following steps: constructing a total station coordinate system and a working coordinate system, collecting n points in the working coordinate system, wherein the coordinate of each point in the total station coordinate system is
The coordinate of each point in the working coordinate system is
Calculating to obtain a coordinate conversion matrix according to coordinates in a coordinate system of the total station and coordinates in a working coordinate system, installing a reflecting prism on the surface of the robot, automatically tracking the reflecting prism by the total station through laser, outputting coordinates of the robot in the coordinate system of the total station along with the movement of the robot by the total station, and obtaining a working seat of the robot according to the coordinate conversion matrixThe invention can avoid the condition of sensor failure or weak signals caused by environment, and improve the reliability and the accuracy of positioning.
Description
Technical Field
The invention relates to the field of robot positioning, in particular to a robot three-dimensional space positioning method based on laser tracking.
Background
The robot has a more common positioning and navigation method outdoors and indoors at present, for example, an indoor common laser radar and a machine vision camera are used as main sensors, and positioning and navigation are performed based on an SLAM algorithm; the GPS satellite signal is often used outdoors, and the robot positioning navigation is carried out by combining an encoder odometer and a gyroscope inertial navigation, however, no matter indoors or outdoors, some special conditions and scenes exist, so that common positioning sensors are invalid, for example, a laser radar cannot find enough reference objects due to an excessively spacious or monotonous environment; in the situations such as tunnels, bridge bottoms, basements and the like, no GPS signal or weak GPS signal exists, and under the situation, the traditional robot positioning mode cannot work normally, so that a novel laser tracking-based robot three-dimensional space positioning method is needed.
Disclosure of Invention
Aiming at the problems, the invention provides a robot three-dimensional space positioning method based on laser tracking, which has the advantages that the robot in any working area can be accurately positioned in real time, and the positioning accuracy can reach within 1 cm.
The technical scheme of the invention is as follows:
a robot three-dimensional space positioning method based on laser tracking comprises the following steps:
s1, constructing a total station coordinate system and a working coordinate system, wherein the total station coordinate system and the working coordinate system are both three-dimensional rectangular coordinate systems;
s2, collecting n points in a working plane coordinate system, wherein the coordinate of each point in a total station coordinate system is
The coordinate of each point in the working coordinate system is
S3, calculating to obtain a coordinate conversion matrix according to the coordinates and the working coordinates in the total station coordinate system;
s4, installing a reflecting prism on the surface of the robot, automatically tracking the reflecting prism by the total station through laser, and outputting the coordinates of the robot in a total station coordinate system along with the movement of the robot by the total station;
and S5, obtaining the coordinates of the robot under the working coordinate system according to the coordinate transformation matrix, and completing the real-time positioning of the robot.
In S1, a total station coordinate system a and a working coordinate system B are defined.
The transformation matrix R is formulated as follows:
z＝(0，0，0)
where T is a rotation matrix of 3 × 3 and T is a translation vector.
The rotation matrix T is obtained through SVD rigid transformation.
The total station needs to be calibrated before the total station coordinate system is established.
The invention has the beneficial effects that:
by changing the matrix, the real-time accurate positioning of the robot on any plane can be realized, the positioning accuracy can reach within 1cm, the defect that no environmental reference object exists or GPS signals are weak can be eliminated, and the positioning accuracy and the positioning reliability are improved.
Drawings
Fig. 1 is a schematic coordinate system diagram of a robot three-dimensional space positioning method based on laser tracking according to an embodiment of the present invention;
fig. 2 is a schematic diagram of a total station coordinate system of a robot three-dimensional space positioning method based on laser tracking according to an embodiment of the present invention.
Detailed Description
The embodiments of the present invention will be further described with reference to the accompanying drawings.
Example (b):
as shown in fig. 1 and fig. 2, a robot three-dimensional space positioning method based on laser tracking includes the following steps:
s1, constructing a total station coordinate system and a working coordinate system:
1) and calibrating the total station, and establishing a rectangular coordinate system of the total station: the total station is first mounted in position, typically on a tripod, and then leveled. The total station outputs a three-dimensional polar coordinate (d, ha, va), the origin of the coordinate is in the center of the total station body, d is the distance from the measured point to the origin, ha is the angle of rotation (yaw angle) of the total station along the axis perpendicular to the ground plane, and va is the pitch angle of the total station. A three-dimensional rectangular coordinate system is established as a total station coordinate system, the origin of the coordinate system is still at the midpoint of a total station body, the vertical horizontal plane faces upwards to serve as the positive direction of a z axis, any direction is set on a plane which is parallel to the horizontal plane and passes through the origin to serve as the positive direction of a y axis, the direction which is vertical to the right direction of the y axis on the plane is the positive direction of an x axis, and the three axes of xyz meet the left-hand rule.
Assuming that the polar coordinates of the measured point are (d)t,hat,vat) The coordinate of the total station rectangular coordinate system is (x)t,yt,zt) Then (x)t,yt,zt) The calculated relationship of (a) is as follows:
zt＝dt·sin(vat)
xt＝dt·cos(vat)·sin(hat)
yt＝dt·cos(vat)·cos(hat)
2) the method comprises the steps of selecting a working area of the robot, establishing a working coordinate system in the working area, wherein the working area of the robot is not necessarily the ground and can be any surface, such as a vertical wall surface, a slope surface, a tunnel vault, a bridge surface and the like, and the working coordinate system is established firstly when the robot is positioned in the areas. The working coordinate system needs to be selected to respectively determine the directions of an original point, an x axis and a y axis, the x axis and the y axis determine a plane, the z axis is perpendicular to the plane, particularly, if the robot moves in the plane, the working coordinate system is simplified into a plane coordinate system, the z axis coordinate is constant zero, if the condition exists, the original point, the x axis and the y axis of the working coordinate system can be respectively marked, and the total station can conveniently collect points.
S2, the special point collecting method provided by the invention: the first special point is the origin O of the working coordinate system B, whose coordinates are known
Measuring the coordinate of the origin O of the working coordinate system B in the coordinate system A of the total station by the total station as
The coordinates of point O in both coordinate systems have been acquired;
the second special point is any point on the x axis of the working coordinate system B, any point on the x axis of the working coordinate system B is taken as a second acquisition point, if the point is P, because the coordinates of the point P on the x axis and the y axis and the z axis are both 0, the coordinate of the point P in the working coordinate system B is made to be P
Where dx is the length of the line segment OP, and the coordinate of the point P in the coordinate system a of the total station measured by the total station is
In the total station coordinate system a, the length of the line segment OP can be calculated from the coordinates of the point O and the point P, so the value of dx is calculated by the following equation:
the coordinates of the point P in the two coordinate systems are also obtained;
the third special point is any point on the y axis of the working coordinate system B, and any point on the y axis of the working coordinate system B is taken as the third special pointThe third acquisition point is assumed to be a point Q, and since the point Q is on the y axis, the coordinates of the point Q on the x axis and the z axis are both 0, the coordinate of the point Q in the working coordinate system B is made to be the same
Where dy is the length of the line segment OQ, and the total station is used to measure the coordinate of point Q in the total station coordinate system A as
In the total station coordinate system a, the length of the line segment OQ can be calculated from the coordinates of the point O and the point Q, and thus the value of dy is calculated by the following equation:
the coordinates of the point Q in the two coordinate systems are also obtained;
up to now, there are three groups of point coordinates which are not on the same straight line, and theoretically, the transformation matrix of the total station coordinate system a and the working coordinate system B can be calculated, but in order to make the result of the fitting calculation more accurate, more point coordinates can be continuously acquired, and it is assumed here that more points are acquired:
starting from the fourth point, points can be arbitrarily taken within the working coordinate system. Taking any point R in the working coordinate system B to make the coordinate in the working coordinate system B be
Measuring the coordinate of the point R in a total station coordinate system A by using the total station as
Now the values of dx, dy and dz need to be found, possibly from vectors
And
to obtain:
dx is a vector
In the vector
The projected length in the direction, therefore, can be:
dy is a vector
In the vector
The projected length in the direction, therefore, can be:
the calculation yields dx and dy, then dz can be based on the vector
Solving for length of (c):
thus, the coordinates of the R point in the two coordinate systems are obtained. It should be noted that, for convenience of description, the default is to take the point P on the positive half axis of the x-axis of the working coordinate system B, the point Q on the positive half axis of the y-axis of the working coordinate system B, and the point R is a region in which the coordinate of the z-axis of the working coordinate system B is positive, that is, dx, dy, and dz are all greater than zero, actually, the point P or the point Q may also be taken on the negative half axis of the coordinate axis of the working coordinate system B, and the point R is a region in which the coordinate of the z-axis is negative, so that the signs of dx, dy, and dz need to be changed according to the situation;
so far, four groups of point coordinates are obtained, if a more accurate conversion matrix is needed to be obtained, the points can be continuously collected, and the step of collecting the R points is repeated.
S3, assuming that n points (n ≧ 3) have been collected, each point corresponds to two sets of coordinates, which are coordinates in the total station coordinate system a and the working coordinate system B, respectively, and requires a transformation matrix from the coordinate system a to the coordinate system B, where a mature SVD rigid body transformation method is adopted, which is briefly described as follows:
let the two sets of coordinate matrices be
Wherein the content of the first and second substances,
now to find PATo PBT and T are transformed such that,
and minimum.
The calculation of this problem is divided into the following steps:
let p beAAnd pBAre respectively PAAnd PBOf center of mass, i.e.
Then, the centroid-removed coordinates are obtained
And
wherein
Removing the coordinates of mass center to form two new coordinate matrixes
Solving a rotation matrix T: order to
W is a 3 × 3 matrix, and it can be proved that W is full rank as long as any three points in the n sets of points are not on a straight line, and when W is full rank, it is possible to perform SVD decomposition on W to obtain W ═ U Σ VTThen T ═ UVT；
Solving a translation vector t: t ═ pB-TpA；
T and T synthesize a transformation matrix R:
wherein z is (0,0,0)
Obtaining a coordinate conversion matrix according to the coordinates in the total station coordinate system and the coordinates in the working coordinate system;
s4, installing a reflecting prism on the surface of the robot, automatically tracking the reflecting prism by the total station through laser, and outputting the coordinates of the robot in a total station coordinate system along with the movement of the robot by the total station;
and S5, obtaining the coordinates of the robot under the working coordinate system according to the coordinate transformation matrix, and completing the real-time positioning of the robot.
The method for calculating the coordinate transformation matrix R can be solved by utilizing a space rigid three-dimensional transformation model, and theoretically, the coordinates of at least three points which are not on the same straight line in two coordinate systems A and B need to be known. The more points are obtained, the more accurate the calculated conversion matrix is, and by the method, the condition that a sensor fails or a GPS signal is weak due to monotonous environment can be avoided, and the positioning reliability and the positioning accuracy are improved.
The above-mentioned embodiments only express the specific embodiments of the present invention, and the description thereof is more specific and detailed, but not construed as limiting the scope of the present invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention.
Claims (5)
1. A robot three-dimensional space positioning method based on laser tracking is characterized by comprising the following steps:
s1, constructing a total station coordinate system and a working coordinate system, wherein the total station coordinate system and the working coordinate system are both three-dimensional rectangular coordinate systems;
s2, collecting n points (n is more than or equal to 3) in the working coordinate system, wherein the coordinate of each point in the total station coordinate system is
The coordinate of each point in the working coordinate system is
S3, calculating to obtain a coordinate transformation matrix according to the coordinates in the total station coordinate system and the coordinates in the working coordinate system;
s4, installing a reflecting prism on the surface of the robot, automatically tracking the reflecting prism by the total station through laser, and outputting the coordinates of the robot in a total station coordinate system along with the movement of the robot by the total station;
and S5, obtaining the coordinates of the robot under the working coordinate system according to the coordinate transformation matrix, and completing the real-time positioning of the robot.
2. The method of claim 1, wherein in step S1, a total station coordinate system a and a working coordinate system B are defined.
3. The robot three-dimensional space positioning method based on laser tracking according to claim 2, characterized in that the transformation matrix R is as follows:
z＝(0,0,0)
where T is a rotation matrix of 3 × 3 and T is a translation vector.
4. The robot three-dimensional space positioning method based on laser tracking according to claim 3, characterized in that the rotation matrix T is obtained by SVD rigid transformation.
5. The method of claim 1, wherein said total station coordinate system is calibrated before establishing said total station coordinate system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210002047.6A CN114353802A (en) | 2022-01-04 | 2022-01-04 | Robot three-dimensional space positioning method based on laser tracking |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210002047.6A CN114353802A (en) | 2022-01-04 | 2022-01-04 | Robot three-dimensional space positioning method based on laser tracking |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114353802A true CN114353802A (en) | 2022-04-15 |
Family
ID=81104951
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210002047.6A Pending CN114353802A (en) | 2022-01-04 | 2022-01-04 | Robot three-dimensional space positioning method based on laser tracking |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114353802A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114740511A (en) * | 2022-06-13 | 2022-07-12 | 广州地铁设计研究院股份有限公司 | Unmanned aerial vehicle positioning method and unmanned aerial vehicle positioning system based on measuring robot |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5363185A (en) * | 1992-12-23 | 1994-11-08 | The United States Of America As Represented By The Secretary Of The Air Force | Method and apparatus for identifying three-dimensional coordinates and orientation to a robot |
| JPH08132373A (en) * | 1994-11-08 | 1996-05-28 | Fanuc Ltd | Coordinate system coupling method in robot-sensor system |
| CN105058387A (en) * | 2015-07-17 | 2015-11-18 | 北京航空航天大学 | Industrial robot base coordinate system calibration method based on laser tracker |
| CN105180940A (en) * | 2015-09-08 | 2015-12-23 | 北京控制工程研究所 | Determination method of indoor target astronomical coordinates, based on wMPS |
| CN105716525A (en) * | 2016-03-30 | 2016-06-29 | 西北工业大学 | Robot end effector coordinate system calibration method based on laser tracker |
| CN107782293A (en) * | 2017-11-09 | 2018-03-09 | 北京卫星环境工程研究所 | Spacecraft equipment posture information measuring method based on six degree of freedom laser tracking target |
| CN109115191A (en) * | 2018-08-03 | 2019-01-01 | 华南农业大学 | The multi-faceted coordinate measuring method of total station |
| CN109341701A (en) * | 2018-12-06 | 2019-02-15 | 上海圭目机器人有限公司 | Ground robot course angle real time acquiring method based on laser tracking total station |
-
2022
- 2022-01-04 CN CN202210002047.6A patent/CN114353802A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5363185A (en) * | 1992-12-23 | 1994-11-08 | The United States Of America As Represented By The Secretary Of The Air Force | Method and apparatus for identifying three-dimensional coordinates and orientation to a robot |
| JPH08132373A (en) * | 1994-11-08 | 1996-05-28 | Fanuc Ltd | Coordinate system coupling method in robot-sensor system |
| CN105058387A (en) * | 2015-07-17 | 2015-11-18 | 北京航空航天大学 | Industrial robot base coordinate system calibration method based on laser tracker |
| CN105180940A (en) * | 2015-09-08 | 2015-12-23 | 北京控制工程研究所 | Determination method of indoor target astronomical coordinates, based on wMPS |
| CN105716525A (en) * | 2016-03-30 | 2016-06-29 | 西北工业大学 | Robot end effector coordinate system calibration method based on laser tracker |
| CN107782293A (en) * | 2017-11-09 | 2018-03-09 | 北京卫星环境工程研究所 | Spacecraft equipment posture information measuring method based on six degree of freedom laser tracking target |
| CN109115191A (en) * | 2018-08-03 | 2019-01-01 | 华南农业大学 | The multi-faceted coordinate measuring method of total station |
| CN109341701A (en) * | 2018-12-06 | 2019-02-15 | 上海圭目机器人有限公司 | Ground robot course angle real time acquiring method based on laser tracking total station |
Non-Patent Citations (2)
| Title |
|---|
| 范百兴: "高性能全站仪的研究及其在动态测量中的应用", 中国优秀硕士论文电子期刊, pages 26 - 30 * |
| 谭舸: "基于激光跟踪仪的多测站地面激光扫描点云配准方法", 中国激光, vol. 48, no. 17, pages 1 - 5 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114740511A (en) * | 2022-06-13 | 2022-07-12 | 广州地铁设计研究院股份有限公司 | Unmanned aerial vehicle positioning method and unmanned aerial vehicle positioning system based on measuring robot |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| He et al. | An integrated GNSS/LiDAR-SLAM pose estimation framework for large-scale map building in partially GNSS-denied environments | |
| CN111156998B (en) | Mobile robot positioning method based on RGB-D camera and IMU information fusion | |
| KR100728377B1 (en) | Method for real-time updating gis of changed region vis laser scanning and mobile internet | |
| US11441899B2 (en) | Real time position and orientation tracker | |
| Li | Mobile mapping: An emerging technology for spatial data acquisition | |
| CN110837080B (en) | Rapid calibration method of laser radar mobile measurement system | |
| US20060167648A1 (en) | 3-Dimensional measurement device and electronic storage medium | |
| CN108648242B (en) | Two-camera calibration method and device without public view field based on assistance of laser range finder | |
| CN109887041B (en) | Method for controlling position and posture of shooting center of digital camera by mechanical arm | |
| CN111077907A (en) | Autonomous positioning method of outdoor unmanned aerial vehicle | |
| CN108594255B (en) | Laser ranging auxiliary optical image joint adjustment method and system | |
| CN110986888A (en) | Aerial photography integrated method | |
| CN107727118B (en) | Method for calibrating GNC subsystem equipment attitude measurement system in large aircraft | |
| El-Hakim et al. | A mobile system for indoors 3-D mapping and positioning | |
| CN114353802A (en) | Robot three-dimensional space positioning method based on laser tracking | |
| CN114459345A (en) | System and method for detecting position and attitude of airplane body based on visual space positioning | |
| Bikmaev et al. | Improving the accuracy of supporting mobile objects with the use of the algorithm of complex processing of signals with a monocular camera and LiDAR | |
| CN109489642B (en) | Dynamic measurement method for relative attitude of two cube mirrors under any spatial attitude | |
| CN114088019B (en) | Portable device and method for measuring two-dimensional deflection angle of axis | |
| CN115930948A (en) | Orchard robot fusion positioning method | |
| Yu et al. | Flexible camera series network for deformation measurement of large scale structures | |
| Wang et al. | A mobile stereo vision system with variable baseline distance for three-dimensional coordinate measurement in large FOV | |
| KR100874425B1 (en) | System for measuring size of signboard and method for measuring size of signboard using the same | |
| Grejner-Brzezinska | Direct platform orientation with tightly integrated GPS/INS in airborne applications | |
| CN113376573B (en) | Fusion positioning system based on radio ranging and artificial light source angle measurement |
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 |