Indoor mobile robot position and posture measurement system based on photoelectric scanning and measurement method

Abstract

An indoor mobile robot position and posture measurement system based on photoelectric scanning and the measurement method thereof, the measurement system includes: a mobile robot (1) which is arranged with a laser transmitter (2), the peripheral of the laser transmitter (2) is provided with no less than three receivers (3) for receiving the light signals emitted by the laser transmitter (2), and at least one signal processor (4) connected to the receivers (3) for processing signals received by the receivers (3) to determine precise coordinates of the receivers in laser transmitter coordinate system, and a terminal computer (5) wirelessly connected with the signal processor (4) to determine the posture angle and the position of the mobile robot through the distances between the laser transmitter (2) and each receiver (3). Without arranging multiple transmitters when measuring and performing tedious global orientation, the operators by using the measurement system and the measurement method of the present invention may measure the 3D position and posture of the indoor mobile robot in real time by multiple guidance signals consisting of photoelectric receiver and a high-speed laser scanning turntable fixed on the mobile robot.

Claims

1. An indoor mobile robot position and posture measurement system based on photoelectric scanning, including a mobile robot (1) which is arranged with a laser transmitter (2), a peripheral of the laser transmitter (2) provided with at least three receivers (3) for receiving light signals emitted by the laser transmitter (2), and at least one signal processor (4) connected to the at least three receivers (3) for processing signals received by the at least three receivers (3) to determine precise coordinates of the at least three receivers in a laser transmitter coordinate system, and a terminal computer (5) wirelessly connected with the at least one signal processor (4) to determine a posture angle and a position of the mobile robot by determining distances between the laser transmitter and each receiver, wherein the measurement method comprises the following steps: Step 1: establishing &global navigation coordinate system, and measuring 3D coordinates of each receiver in the global navigation coordinate system by a laser tracker or an indoor GPS; Step 2: receiving, by each receiver, a scanning laser signal emitted by an infrared laser of the laser transmitter and a synchronous pulse laser signal emitted by a synchronous pulse laser, and sending the scanning laser signal and the synchronous pulse laser signal to the at least one signal processor; calculating, by the at least one signal processor, the precise coordinates of each receiver in a laser transmitter coordinate system, including two scanning angles .sub.i1, .sub.i2, a horizontal angle .sub.i and a vertical angle .sub.i of each receiver i corresponding to the laser transmitter coordinate system, and storing the precise coordinates into the terminal computer; Step 3: calculating, by the terminal computer, the distances between the laser transmitter and each receiver according to the precise coordinates of each receiver, respectively: Step 4: expressing approximate coordinates of each receiver in the laser transmitter coordinate system as i=di (cos .sub.i, cos .sub.i, cos .sub.i, sin .sub.i, sin .sub.i) taking the approximate coordinates as initial values, and performing an iteration calculation to update the precise coordinates of each receiver in the laser transmitter coordinate system: Step 5: performing coordinates conversion according to the precise coordinates achieved by step 4 and the 3D coordinates of the at least three receivers achieved by step 1 to obtain a posture matrix and a translation matrix of the laser transmitter coordinate system corresponding to the global navigation coordinate system: Step 6: acquiring the posture angle of the mobile robot according to the posture matrix and acquiring the position of the mobile robot according to the translation matrix.

2. The indoor mobile robot position and posture measurement system based on photoelectric scanning according to claim 1, wherein each of the at least one signal processor (4) is connected with 3 to 8 receivers.

3. The indoor mobile robot position and posture measurement system based on photoelectric scanning according to claim 1, wherein the two scanning angles .sub.i1, .sub.i2, the horizontal angle .sub.i and the vertical angle i in step 2 are obtained by the following formulae: ${}_{i1}=.Math.t_{i1}$ ${}_{i2}=.Math.t_{i2}$ $l_i=n_{i1}{n}_{i2}=\left(\begin{array}{ccc}{r}_{\mathrm{ix}}& r_{\mathrm{iy}}& r_{\mathrm{iz}}\end{array}\right)$ ${}_i=\arctan \left(\frac{r_{\mathrm{iy}}}{r_{\mathrm{ix}}}\right)$ ${}_i=\arctan \left(\frac{r_{\mathrm{iz}}}{\sqrt{r_{\mathrm{ix}}^2+r_{\mathrm{iy}}^2}}\right)$ wherein, represents a rotational angular velocity of a rotor of the laser transmitter; t.sub.i1 and t.sub.i2 represent time lengths when a first light-plane and a second light-plane of the laser transmitter scan the i.sup.th receiver, respectively; n.sub.i1 and n.sub.i2 represent the normal vectors of the first light-plane and the second light-plane when the first light-plane and the second light-plane of the laser transmitter scan the i.sup.th receiver, respectively, a cross multiply algorithm result l.sub.i thereof is expressed by (r.sub.ixr.sub.iy r.sub.iz).

4. The indoor mobile robot position and posture measurement system based on photoelectric scanning according to claim 1, wherein the distances, which are calculated in Step 3, between the laser transmitter and each receiver may be obtained by the following formula achieved by arbitrary two receivers: $\cos \left({}_{\mathrm{ij}}\right)=\frac{d_i^2+d_j^2-L_{\mathrm{ij}}^2}{2d_i{d}_j}$ wherein if no less than three receivers receive the scanning laser signal and the synchronous pulse laser signal, three formulae according to the above formula can be obtained, and a number of unknown values is equal to that of the formulae, so that the distances di respectively between the laser transmitter and the at least three receivers may be calculated, i=1,2,3; wherein, .sub.ij represents an included angle between the laser transmitter and a connecting line between two receivers i, j, which can be obtained by the following formula: ${}_{\mathrm{ij}}=\arccos \left(\frac{l_i.Math.l_j}{.Math.l_i.Math..Math..Math.l_j.Math.}\right).$

5. The indoor mobile robot position and posture measurement system based on photoelectric scanning according to claim 1, wherein the posture matrix and the translation matrix in Step 5 may be obtained by:
[C.sub.b.sup.nT.sub.b.sup.n]=G.Math.S.sup.T.Math.(S.Math.S.sup.T).sup.1 wherein, C.sub.b.sup.n represents the posture matrix, T.sub.b.sup.n represents the translation matrix, assuming i receivers receiving the light signals from the laser transmitter; G and S represent 3D coordinates point matrixes of the at least three receivers in the global navigation coordinate system and the laser transmitter coordinate system, respectively, which can be expressed as: $G=\left[\begin{array}{cccc}{P}_1^n& P_2^n& \mathrm{.Math.}& P_i^n\\ 1& 1& \mathrm{.Math.}& 1\end{array}\right]$ $S=\left[\begin{array}{cccc}{P}_1^b& P_2^b& \mathrm{.Math.}& P_i^b\\ 1& 1& \mathrm{.Math.}& 1\end{array}\right]$ wherein a least-square problem is calculated to obtain final results of C.sub.b.sup.n and T.sub.b.sup.n as follows: $\{\frac{\min F=\underset{i=1}{\overset{N}{.Math.}}{.Math.P_i^n-\left[C_b^n{T}_b^n\right].Math.P_i^b.Math.}^2}{C_b^n{T}_b^n-l=0}.$

6. The indoor mobile robot position and posture measurement system based on photoelectric scanning according to claim 1, wherein the posture angle of the mobile robot in Step 6 is obtained by the posture matrix obtained in Step 5, the formulae are as follows: $=\arctan \left(\frac{C_b^n\left(1,2\right)}{C_b^n\left(2,2\right)}\right)$ $=\arctan \left(C_b^n\left(3,2\right)\right)$ $=\arctan \left(-\frac{C_b^n\left(3,1\right)}{C_b^n\left(3,3\right)}\right)$ wherein, represents a heading angle, represents a pitch angle, and represents a roll angle.

7. The indoor mobile robot position and posture measurement system based on photoelectric scanning according to claim 1, wherein the position of the mobile robot in Step 6 is obtained by the translation matrix directly, the translation matrix T.sub.b.sup.n may be expressed as:
T.sub.b.sup.n=(X Y Z) wherein the components of the translation matrix are the 3D coordinates of the mobile robot in the global navigation coordinate system.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 is the schematic diagram of the indoor mobile robot position and posture measurement system based on photoelectric scanning and measurement method; and

(2) FIG. 2 is the schematic diagram of the structure of the laser transmitter of the present invention; and

(3) FIG. 3 is the schematic diagram of the structure of the receiver in the present invention; and

(4) FIG. 4 shows the global navigation coordinate system;

(5) FIG. 5 shows the laser transmitter coordinate system;

(6) FIG. 6 shows the coordinates to be solved.

(7) In which:

(8) TABLE-US-00001 1: mobile robot 2: laser transmitter 3: receiver 4: Signal processor 5: terminal computer 21: light-plane 22: rotation axis 23: rotor 24: synchronous pulse laser 25: base 26: infrared laser 27: housing of the receiver 28: photosensitive center of the receiver

DETAILED DESCRIPTION OF THE EMBODIMENTS

(9) The present invention will be further described accompanying with the figures.

(10) Based on the precise 3D coordinates of each guiding beacon in the global navigation coordinate system, the present invention uses a small high-speed rotating platform as the transmitter and a plurality of guiding beacons to measure the position and posture of the transmitter by considering the precise parameter of which and measuring the spatial scanning angle of the transmitter corresponding to the respective guiding beacon.

(11) In the present invention, the small high-speed rotating platform (acts as the transmitter) capable of transmitting laser signals to the whole space is fixed on the body of the mobile robot, so that the transmitter can continuously transmit laser signal to the workspace in the way of scanning while following the mobile robot to move. Meanwhile, the multiple guiding beacons (acts as receivers) are distributed at the working areas that can be detected by the transmitter. When the laser signals emitted by the transmitter scan the guiding beacons, the photoelectric sensors inside the beacons convert the laser signal into the electrical signal, and the electric signals are converted into the scanning angle information by the subsequent processing. The present invention takes the scanning angles as the basic observations, and finally optimized solving the 3D coordinates and posture angle of the transmitter in the global navigation coordinate system according to the parameters of the transmitter and the precise coordinates of the guiding beacons in the global navigation coordinate system.

(12) As shown in FIG. 1 and FIG. 2, the indoor mobile robot position and posture measurement system based on photoelectric scanning in the present invention, including a mobile robot 1 which is arranged with a laser transmitter 2, the peripheral of the laser transmitter 2 is provided with no less than three receivers 3 for receiving the light signals emitted by the laser transmitter 2, and at least one signal processor 4 connected to the receivers 3 for processing signals received by the receivers 3 to determine the precise coordinates of the receivers 3 in laser transmitter coordinate system, and a terminal computer 5 wireless connected with the signal processor 4 to determine the posture angle and the position of the mobile robot by determining the distances between the laser transmitter and each receiver. Each signal processor 4 connects with 3 to 8 receivers 3.

(13) The laser transmitter 2 and the receivers 3 employ the structure disclosed in Chinese application No. 201210126759.5. Wherein:

(14) As shown in FIG. 2, the laser transmitter mainly comprises a base 25 and a rotor 23; the rotor 23, which is rotatable at a high-speed, is arranged at the top of the laser transmitter, and two linear infrared lasers 26 are mounted on the rotor so as to obtain two fan-shaped infrared laser planes which can be scanned at high speed to the space to be measured; a circular synchronous pulse laser 24 is arranged on the base, and may emit pulse laser when the rotor rotates around a preset position to form a timing reference. In this way, the receivers receive the synchronous pulse laser and fan-shaped scanning laser so as to measure the rotation angles .sub.i1 and .sub.i2 of the rotor when the scanning laser scans the receiver, and measured data may be used as the basic observation for the subsequent calculation.

(15) As shown in FIG. 2, fast photosensitive devices, such as PIN photodiodes or avalanche photodiodes, are selected as sensors for converting the light signals from the base into photocurrent signals. The preprocessing circuit amplifies the raw photocurrent signal and converts it into a logic pulse by thresholding, the logic pulse is then input into the timing circuit for timing. The size of a housing 27 of the receiver is exactly the same as that of a laser tracker reflection target ball (SMR) with the diameter of 1.5-inch, and a deviation between a photosensitive center 28 of the receiver and the central position of an SMR cone prism is less than 0.1 mm, which allows accuracy comparisons with the laser tracker or other global navigation coordinates measurement systems. In order to increase anti-jamming capacity of the system, the sensitive wavelength of the photosensitive unit is identical to the laser wavelength from the transmitter, and the output signals from the terminal of the receivers are differential signals.

(16) The present invention also provides a measurement method according to the indoor mobile robot position and posture measurement system based on photoelectric scanning on the basis of the angle measurement method by biplane coaxial rotating scanning. The measurement method mainly comprises a photoelectric scanning transmitter fixed on the mobile robot and the receivers positioned at known points, each receiver is arranged with a computer for coordinate computation. The measurement system adopts the unidirectional broadcasting mode of transmitter-to-receiver broadcast for measuring, and adopts laser transmitter based on the biplane coaxial rotating scanning to broadcast scanning light signals with angle information to the whole space. Furthermore, the position and posture measurement accuracy of the present invention may reach 2 mm.

(17) The precise 3D coordinates of the guiding beacons are required to obtain a high-accuracy position and posture measurement of the mobile robot. The present invention adopts a laser tracker or other precise equipments for precisely measuring the 3D coordinates of the guiding beacons, the accuracy of which is better than 0.03 mm, which may completely meet a millimeter-sized measurement requirements and an arcminute-level posture measurement requirements of the mobile robot.

(18) A measurement method of the indoor mobile robot position and posture measurement system based on photoelectric scanning, including:

(19) Step 1: establishing global navigation coordinate system, precisely measuring the 3D coordinates in global navigation coordinate system by a laser tracker or an indoor GPS;

(20) Step 2: receiving, by each receiver, a scanning laser signal emitted by an infrared laser of the laser transmitter and the synchronous pulse laser signal emitted by the synchronous pulse laser, and sending the signals to each connected signal processor; calculating, by each signal processor, precise coordinates of each connected receiver in laser transmitter coordinate system, including two scanning angles .sub.i1, .sub.i2 and the horizontal angle .sub.i and vertical angle .sub.i of the receiver corresponding to the laser transmitter coordinate system, and storing the results into the terminal computer;

(21) Wherein, two scanning angles .sub.i1, .sub.i2, the horizontal angle .sub.i and vertical angle .sub.i in step 2 are obtained by the following formulae:

(22) ${}_{i1}=.Math.t_{i1}$${}_{i2}=.Math.t_{i2}$$l_i=n_{i1}{n}_{i2}=\left(\begin{array}{ccc}{r}_{\mathrm{ix}}& r_{\mathrm{iy}}& r_{\mathrm{iz}}\end{array}\right)$${}_i=\arctan \left(\frac{r_{\mathrm{iy}}}{r_{\mathrm{ix}}}\right)$${}_i=\arctan \left(\frac{r_{\mathrm{iz}}}{\sqrt{r_{\mathrm{ix}}^2+r_{\mathrm{iy}}^2}}\right)$

(23) Wherein, represents a rotational angular velocity of the rotor of the laser transmitter; t.sub.i1 and t.sub.i2 represent the time lengths when the first and second light-plane of the laser transmitter scan the i.sup.th receiver, respectively; n.sub.i1 and n.sub.i2 represent the normal vectors of the light-planes when the first and second light-plane of the laser transmitter scan the i.sup.th receiver, respectively, the cross multiply algorithm results l.sub.i thereof are expressed by (r.sub.ix r.sub.iy r.sub.iz);

(24) Step 3: calculating, by the terminal computer, the distance values between the laser transmitter and each receiver according to the precise coordinates of each receiver, respectively;

(25) The distance values, which are calculated in Step 3, between the laser transmitter and each receiver may be obtained by the following formula achieved by arbitrary two receivers:

(26) $\cos \left({}_{\mathrm{ij}}\right)=\frac{d_i^2+d_j^2-L_{\mathrm{ij}}^2}{2d_i{d}_j}$

(27) If no less than three receivers received the laser signals, three formulae according to the above formula can be obtained, and the number of unknown values is equal to that of the formulae, so that the distances d.sub.i respectively between the transmitter and the three receivers may be calculated, i=1,2,3. Wherein, .sub.ij represents an included angle between the laser transmitter and a connecting line of the two receivers i, j, which can be obtained by the following formula:

(28) ${}_{\mathrm{ij}}=\arccos \left(\frac{l_i.Math.l_j}{.Math.l_i.Math..Math..Math.l_j.Math.}\right);$

(29) Step 4: expressing the approximate coordinates of each receiver in laser transmitter coordinate system as p.sub.i=d.sub.i(cos .sub.i cos .sub.i, cos .sub.i sin .sub.i, sin .sub.i), and by taking the approximate coordinates as iteration initial value, performing optimal calculation by an Levenberg-Marquardt algorithm to obtain the precise coordinates of each receiver in laser transmitter coordinate system;

(30) Step 5: performing coordinates conversion according to the precise coordinates achieved by step 4 and the 3D coordinates of all the receivers achieved by step 1 to obtain a posture matrix and a translation matrix of the laser transmitter coordinate system corresponding to the global navigation coordinate system;

(31) The posture matrix and the translation matrix in Step 5 may be obtained by:
[C.sub.b.sup.nT.sub.b.sup.n]=G.Math.S.sup.T.Math.(S.Math.S.sup.T).sup.1

(32) Wherein, C.sub.b.sup.n represents the posture matrix, T.sub.b.sup.n represents the translation matrix, assuming i receivers receiving the light signals from the laser transmitter; G and S represent the 3D coordinates point matrixes of the receivers in the global navigation coordinate system and the laser transmitter coordinate system, respectively, which can be expressed as:

(33) 0$G=\left[\begin{array}{cccc}{P}_1^n& P_2^n& \mathrm{.Math.}& P_i^n\\ 1& 1& \mathrm{.Math.}& 1\end{array}\right]$$S=\left[\begin{array}{cccc}{P}_1^b& P_2^b& \mathrm{.Math.}& P_i^b\\ 1& 1& \mathrm{.Math.}& 1\end{array}\right]$

(34) Then the least-square problem is calculated to obtain the final results of C.sub.b.sup.n and T.sub.b.sup.n as follows:

(35) $\{\begin{array}{c}\min F=\underset{i=1}{\overset{N}{.Math.}}{.Math.P_i^n-\left[C_b^n{T}_b^n\right].Math.P_i^b.Math.}^2\\ C_b^{\mathrm{nT}}{C}_b^n-I=0\end{array};$

(36) Step 6: acquiring a posture angle of the mobile robot according to the posture matrix and acquiring a position of the mobile robot according to the translation matrix;

(37) Wherein, the posture angle of the mobile robot is obtained by the posture matrix obtained in Step 5, the formulae are as follows:

(38) $=\arctan \left(\frac{C_b^n\left(1,2\right)}{C_b^n\left(2,2\right)}\right)$$=\arctan \left(C_b^n\left(3,2\right)\right)$$=\arctan \left(-\frac{C_b^n\left(3,1\right)}{C_b^n\left(3,3\right)}\right)$

(39) Wherein, represents a heading angle, represents a pitch angle, and represents a roll angle.

(40) The position of the mobile robot is obtained by the translation matrix directly, the translation matrix T.sub.b.sup.n may be expressed as:
T.sub.b.sup.n=(X Y Z)

(41) The components of the translation matrix are the 3D coordinates of the mobile robot in global navigation coordinate system.

(42) Although the functions and working processes of the present invention have been described above with reference to the accompanying drawings, the present invention is not limited thereto. The foregoing specific implementations are merely illustrative but not limiting. A person of ordinary skill in the art may make various forms under the teaching of the present invention without departing from the purpose of the present invention and the protection scope of the appended claims, and all the forms shall fall into the protection scope of the present invention.