External parameter calibration method for robot sensors and apparatus and robot with the same
The present disclosure provides an external parameter calibration method for robot sensors as well as an apparatus, robot and storage medium with the same. The method includes: obtaining first sensor data and second sensor data obtained through a first sensor and a second sensor of the robot by collecting position information of a calibration reference object and converting to a same coordinate system to obtain corresponding first converted sensor data and second converted sensor data, thereby determining a first coordinate and a second coordinate of a reference point of the calibration reference object; using the first coordinate and the second coordinate are as a set of coordinate data; repeating the above-mentioned steps to obtain N sets of the coordinate data to calculate the external parameter between the first sensor and the second sensor in response to a relative positional relationship between the robot and the calibration reference object being changed.
1. An external parameter calibration method for sensors of a robot, comprising:
obtaining first sensor data and second sensor data obtained through a first sensor and a second sensor of the robot by collecting position information of a calibration reference object;
converting the first sensor data and the second sensor data to a same coordinate system to obtain corresponding first converted sensor data and second converted sensor data;
determining a first coordinate of a reference point of the calibration reference object based on the first converted sensor data, and determining a second coordinate of the reference point of the calibration reference object based on the second converted sensor data, and using the first coordinate and the second coordinate as a set of coordinate data;
returning to the step of obtaining the first sensor data and the second sensor data obtained through the first sensor and the second sensor of the robot by collecting the position information of the calibration reference object in response to a relative positional relationship between the robot and the calibration reference object being changed until N sets of the coordinate data are obtained, wherein N is a positive integer not less than an amount of unknown degrees of freedom in an external parameter; and
calculating the external parameter between the first sensor and the second sensor based on the N sets of the coordinate data, wherein the external parameter is a relative positional relationship parameter between the first sensor and the second sensor;
wherein the first sensor is a single-line radar, the second sensor is a visual sensor, and the step of obtaining the first sensor data and the second sensor data obtained through the first sensor and the second sensor of the robot by collecting the position information of the calibration reference object comprises:
obtaining visual sensor data obtained through the visual sensor by collecting the position information of the calibration reference object; and
extracting corresponding row data from the visual sensor data to take as the second sensor data based on a measured height of the single-line radar, in response to a height of the single-line radar being within a measurement range of the vision sensor; and
selecting data with a closest height from the visual sensor data to take as the second sensor data, in response to the height of the single-line radar not being within the measurement range of the vision sensor;
wherein the calculating the external parameter between the first sensor and the second sensor based on the N sets of the coordinate data, wherein the external parameter is the relative positional relationship parameter between the first sensor and the second sensor comprises:
obtaining a conversion relationship between positions of the first sensor and the second sensor; and
calculating the relative positional relationship parameter between the first sensor and the second sensor based on the N sets of the coordinate data and the conversion relationship; and
wherein the conversion relationship is represented by a parameter to be solved, a data set of the first sensor is (x i ,y i ) and a data set of the second sensor is (x i ′,y i ′), and for a 2D navigation application of the robot, the parameter to be solved is (Δx, Δy, Δyaw), the conversion relationship is expressed as following matrix equation:
[
x
i
y
i
1
]
=
[
cos
(
Δ
yaw
)
sin
(
Δ
yaw
)
Δ
x
-
sin
(
Δ
yaw
)
cos
(
Δ
yaw
)
Δ
y
0
0
1
]
[
x
i
′
y
i
′
1
]
;
the matrix equation is solved to obtain;
[
x
i
y
i
1
]
=
[
x
i
′
cos
(
Δ
yaw
)
+
y
i
′
*
sin
(
Δ
yaw
)
+
Δ
x
-
x
i
′
sin
(
Δ
yaw
)
+
y
i
′
*
cos
(
Δ
yaw
)
+
Δ
y
1
]
;
the solved matrix equation is converted to obtain;
[
x
i
y
i
1
]
=
[
x
i
′
y
i
′
1
0
y
i
′
-
x
i
′
0
1
]
[
cos
(
Δ
yaw
)
sin
(
Δ
yaw
)
Δ
x
Δ
y
]
;
the obtained matrix equation is described as a problem of solving a linear equation of Ax=b:
A
=
[
x
0
′
y
0
′
1
0
y
0
′
-
x
0
′
0
1
⋮
x
n
′
y
n
′
1
0
y
n
′
-
x
n
′
0
1
]
2
n
*
4
;
b
=
[
x
0
y
0
⋮
x
n
y
n
]
2
n
*
1
;
and
the following formula is obtained:
[
cos
(
Δ
yaw
)
sin
(
Δ
yaw
)
Δ
x
Δ
y
]
=
[
x
0
′
y
0
′
1
0
y
0
′
-
x
0
′
0
1
·
·
·
x
n
′
y
n
′
1
0
y
n
′
-
x
n
′
0
1
]
2
n
*
4
-
1
[
x
0
y
0
·
·
·
x
n
y
n
]
2
n
*
1
;
Δ
yaw
=
arctan
(
sin
(
Δ
yaw
)
cos
(
Δ
yaw
)
)
;
each value in the external parameter (Δx, Δy, Δyaw) is solved, where n represents n sets of coordinate data.
2. The method of claim 1 , wherein the reference point is a turning point of two associated sides of the calibration reference object; the determining the first coordinate of the reference point of the calibration reference object based on the first converted sensor data, and determining the second coordinate of the reference point of the calibration reference object based on the second converted sensor data comprises:
fitting the two associated sides with respect to the turning point of the collected calibration reference object based on the first converted sensor data, and determining the first coordinate of the turning point based on a result of the fitting; and
fitting the two associated sides with respect to the turning point of the collected calibration reference object based on the second convened sensor data, and determining the second coordinate of the turning point based on a result of the fitting.
3. The method of claim 2 , wherein the two associated sides with respect to the turning point of the calibration reference object are straight lines; the performing the fitting on the two associated sides with respect to the turning point of the collected calibration reference object based on the first converted sensor data, and determining the first coordinate of the turning point based on the result of the fitting comprises:
performing a straight line fitting on each of the two associated sides with respect to the turning point of the collected calibration reference object based on the first converted sensor data, and using a coordinate of an intersection point of two straight lines obtained by the fitting as the first coordinate of the turning point;
the performing the fitting on the two associated sides with respect to the turning point of the collected calibration reference object based on the second converted sensor data, and determining the second coordinate of the turning point based on the result of the fitting comprises:
performing a straight line fitting on each of the two associated sides with respect to the turning point of the collected calibration reference object based on the second convened sensor data, and using a coordinate of an intersection point of two straight lines obtained by the fitting as the second coordinate of the turning point.
4. The method of claim 1 , wherein the converting the first sensor data and the second sensor data to the same coordinate system to obtain the corresponding first converted sensor data and the second converted sensor data comprises:
converting the first sensor data from a coordinate system of the first sensor to a coordinate system of the robot to obtain first converted sensor data; and
convening the second sensor data from a coordinate system of the second sensor to the coordinate system of the robot to obtain second converted sensor data.
5. The method of claim 1 , wherein the calibration reference object is a triangular cylinder and the reference point is an apex of an included angle of the triangular cylinder.
6. An external parameter calibration apparatus for sensors of a robot, comprising:
an obtaining module configured to obtain first sensor data and second sensor data obtained through a first sensor and a second sensor of the robot by collecting position information of a calibration reference object;
a conversion module configured to convert the first sensor data and the second sensor data to a same coordinate system to obtain corresponding first converted sensor data and second converted sensor data;
a determination module configured to determine a first coordinate of a reference point of the calibration reference object based on the first converted sensor data, and determining a second coordinate of the reference point of the calibration reference object based on the second converted sensor data, and using the first coordinate and the second coordinate as a set of coordinate data;
a loop module configured to return to the obtaining the first sensor data and the second sensor data obtained through the first sensor and the second sensor of the robot by collecting the position information of the calibration reference object in response to a relative positional relationship between the robot and the calibration reference object being changed until N sets of the coordinate data are obtained, wherein N is a positive integer not less than an amount of unknown degrees of freedom in an external parameter; and
a calculation module configured to calculate the external parameter between the first sensor and the second sensor based on the N sets of the coordinate data, wherein the external parameter is a relative positional relationship parameter between the first sensor and the second sensor;
wherein the first sensor is a single-line radar, the second sensor is a visual sensor, and the obtaining module is further configured to:
obtain visual sensor data obtained through the visual sensor by collecting the position information of the calibration reference object; and
extract corresponding row data from the visual sensor data to take as the second sensor data based on a measured height of the single-line radar, in response to a height of the single-line radar being within a measurement range of the vision sensor; and
select data with a closest height from the visual sensor data to take as the second sensor data, in response to the height of the single-line radar not being within the measurement range of the vision sensor;
wherein the calculation module is further configured to:
obtain a conversion relationship between positions of the first sensor and the second sensor; and
calculate the relative positional relationship parameter between the first sensor and the second sensor based on the N sets of the coordinate data and the conversion relationship; and
wherein the conversion relationship is represented by a parameter to be solved, a data set of the first sensor is (x i ,y i ) and a data set of the second sensor is (x i ′,y i ′), and for a 2D navigation application of the robot, the parameter to be solved is (Δx, Δy, Δyaw), the conversion relationship is expressed as following matrix equation;
[
x
i
y
i
1
]
=
[
cos
(
Δ
yaw
)
sin
(
Δ
yaw
)
Δ
x
-
sin
(
Δ
yaw
)
cos
(
Δ
yaw
)
Δ
y
0
0
1
]
[
x
i
′
y
i
′
1
]
;
the matrix equation is solved to obtain;
[
x
i
y
i
1
]
=
[
x
i
′
cos
(
Δ
yaw
)
+
y
i
′
*
sin
(
Δ
yaw
)
+
Δ
x
-
x
i
′
sin
(
Δ
yaw
)
+
y
i
′
*
cos
(
Δ
yaw
)
+
Δ
y
1
]
;
the solved matrix equation is converted to obtain;
[
x
i
y
i
1
]
=
[
x
i
′
y
i
′
1
0
y
i
′
-
x
i
′
0
1
]
[
cos
(
Δ
yaw
)
sin
(
Δ
yaw
)
Δ
x
Δ
y
]
;
the obtained matrix equation is described as a problem of solving a linear equation of Ax=b;
A
=
[
x
0
′
y
0
′
1
0
y
0
′
-
x
0
′
0
1
⋮
x
n
′
y
n
′
1
0
y
n
′
-
x
n
′
0
1
]
2
n
*
4
;
b
=
[
x
0
y
0
⋮
x
n
y
n
]
2
n
*
1
;
and
the following formula is obtained:
[
cos
(
Δ
yaw
)
sin
(
Δ
yaw
)
Δ
x
Δ
y
]
=
[
x
0
′
y
0
′
1
0
y
0
′
-
x
0
′
0
1
·
·
·
x
n
′
y
n
′
1
0
y
n
′
-
x
n
′
0
1
]
2
n
*
4
-
1
[
x
0
y
0
·
·
·
x
n
y
n
]
2
n
*
1
;
Δ
yaw
=
arctan
(
sin
(
Δ
yaw
)
cos
(
Δ
yaw
)
)
;
each value in the external parameter (Δx, Δy, Δyaw) is solved, where n represents a sets of coordinate data.
7. A robot comprising a non-transitory storage and a processor, wherein the non-transitory storage stores a computer program executable on the processor and the computer program comprises:
instructions for obtaining first sensor data and second sensor data obtained through a first sensor and a second sensor of the robot by collecting position information of a calibration reference object;
instructions for converting the first sensor data and the second sensor data to a same coordinate system to obtain corresponding first converted sensor data and second converted sensor data;
instructions for determining a first coordinate of a reference point of the calibration reference object based on the first converted sensor data, and determining a second coordinate of the reference point of the calibration reference object based on the second converted sensor data, and using the first coordinate and the second coordinate as a set of coordinate data;
instructions for returning to execute the instructions for obtaining the first sensor data and the second sensor data obtained through the first sensor and the second sensor of the robot by collecting the position information of the calibration reference object in response to a relative positional relationship between the robot and the calibration reference object being changed until N sets of the coordinate data re obtained, wherein N is a positive integer not less than an amount of unknown degrees of freedom in an external parameter; and
instructions for calculating the external parameter between the first sensor and the second sensor based on the N sets of the coordinate data, wherein the external parameter is a relative positional relationship parameter between the first sensor and the second sensor;
wherein the first sensor is a single-line radar, the second sensor is a visual sensor, and the instructions for obtaining the first sensor data and the second sensor data obtained through the first sensor and the second sensor of the robot by collecting the position information of the calibration reference object comprises:
instructions for obtaining visual sensor data obtained through the visual sensor by collecting the position information of the calibration reference object; and
instructions for extracting corresponding row data from the visual sensor data to take as the second sensor data based on a measured height of the single-line radar, in response to a height of the single-line radar being within a measurement range of the vision sensor; and
instructions for selecting data with a closest height from the visual sensor data to take as the second sensor data, in response to the height of the single-line radar not being within the measurement range of the vision sensor;
wherein the instructions for calculating the external parameter between the first sensor and the second sensor based on the N sets of the coordinate data, wherein the external parameter is the relative positional relationship parameter between the first sensor and the second sensor comprise:
instructions for obtaining, a conversion relationship between positions of the first sensor and the second sensor; and
instructions for calculating the relative positional relationship parameter between the first sensor and the second sensor based on the N sets of the coordinate data and the conversion relationship; and
wherein the conversion relationship is represented by a parameter to: be solved a data set of the first sensor is (x i ,y i ) and a data set of the second sensor is (x i ′,y i ′), and for a 2D navigation application of the robot, the parameter to be solved is (Δx, Δy, Δyaw), the conversion relationship is expressed as following matrix equation:
[
x
i
y
i
1
]
=
[
cos
(
Δ
yaw
)
sin
(
Δ
yaw
)
Δ
x
-
sin
(
Δ
yaw
)
cos
(
Δ
yaw
)
Δ
y
0
0
1
]
[
x
i
′
y
i
′
1
]
;
the matrix equation is solved to obtain;
[
x
i
y
i
1
]
=
[
x
i
′
cos
(
Δ
yaw
)
+
y
i
′
*
sin
(
Δ
yaw
)
+
Δ
x
-
x
i
′
sin
(
Δ
yaw
)
+
y
i
′
*
cos
(
Δ
yaw
)
+
Δ
y
1
]
;
the solved matrix equation is converted to obtain;
[
x
i
y
i
1
]
=
[
x
i
′
y
i
′
1
0
y
i
′
-
x
i
′
0
1
]
[
cos
(
Δ
yaw
)
sin
(
Δ
yaw
)
Δ
x
Δ
y
]
;
the obtained matrix equation is described as a problem of Solving a linear equation of Ax=b;
A
=
[
x
0
′
y
0
′
1
0
y
0
′
-
x
0
′
0
1
⋮
x
n
′
y
n
′
1
0
y
n
′
-
x
n
′
0
1
]
2
n
*
4
;
b
=
[
x
0
y
0
⋮
x
n
y
n
]
2
n
*
1
;
and
the following formula is obtained;
[
cos
(
Δ
yaw
)
sin
(
Δ
yaw
)
Δ
x
Δ
y
]
=
[
x
0
′
y
0
′
1
0
y
0
′
-
x
0
′
0
1
·
·
·
x
n
′
y
n
′
1
0
y
n
′
-
x
n
′
0
1
]
2
n
*
4
-
1
[
x
0
y
0
·
·
·
x
n
y
n
]
2
n
*
1
;
Δ
yaw
=
arctan
(
sin
(
Δ
yaw
)
cos
(
Δ
yaw
)
)
;
each value in the external parameter (Δx, Δy, Δyaw) is solved, where n represents n sets of coordinate data.
8. The robot of claim 7 , wherein the reference point is a turning point of two associated sides of the calibration reference object; the instructions for determining the first coordinate of the reference point of the calibration reference object based on the first converted sensor data, and determining the second coordinate of the reference point of the calibration reference object based on the second converted sensor data comprise:
instructions for fitting the two associated sides with respect to the turning point of the collected calibration reference object based on the first converted sensor data, and determining the first coordinate of the turning point based on a result of the fitting; and
instructions for fitting the two associated sides with respect to the turning point of the collected calibration reference object based on the second converted sensor data, and determining the second coordinate of the turning point based on a result of the fitting.
9. The robot of claim 7 , wherein the two associated sides with respect to the turning point of the calibration reference object are straight lines; the instructions for performing the fitting on the two associated sides with respect to the turning point of the collected calibration reference object based on the first converted sensor data, and determining the first coordinate of the turning point based on the result of the fitting comprise:
instructions for performing a straight line fitting on each of the two associated sides with respect to the turning point of the collected calibration reference object based on the first converted sensor data, and using a coordinate of an intersection point of two straight lines obtained by the fitting as the first coordinate of the turning point;
the instructions for performing the fitting on the two associated sides with respect to the turning point of the collected calibration reference object based on the second converted sensor data, and determining the second coordinate of the turning point based on the result of the fitting comprise:
instructions for performing a straight line fitting on each of the two associated sides with respect to the turning point of the collected calibration reference object based on the second converted sensor data, and using a coordinate of an intersection point of two straight lines obtained by the fitting as the second coordinate of the turning point.
10. The robot of claim 7 , wherein the instructions for converting the first sensor data and the second sensor data to the same coordinate system to obtain the corresponding first converted sensor data and the second convened sensor data comprise:
instructions for converting the first sensor data from a coordinate system of the first sensor to a coordinate system of the robot to obtain first convened sensor data; and
instructions for converting the second sensor data from a coordinate system of the second sensor to the coordinate system of the robot to obtain second convened sensor data.
11. The robot of claim 7 , wherein the calibration reference object is a triangular cylinder and the reference point is an apex of an included angle of the triangular cylinder.
12. The method of claim 1 , wherein the external parameter is calculated using least squares.
13. The method of claim 1 , wherein the relative positional relationship between the robot and the calibration reference object is changed by moving the calibration reference object.
14. The method of claim 5 , wherein the calibration reference object is an isosceles triangle cylinder, and an angle of an isosceles included angle of the isosceles triangle cylinder is in a range of 90°-160°.
15. The method of claim 1 , wherein a range of measuring distance is 0.3 m-2.0 m, a middle part of a vision field of the visual sensor is selected as an area range of the visual sensor.
16. The method of claim 1 , wherein the step of converting the first sensor data and the second sensor data to the same coordinate system to obtain the corresponding first converted sensor data and the second converted sensor data comprises:
converting the first sensor data and the second sensor data uniformly to a coordinate system of the first sensor or a coordinate system of the second sensor.
17. The apparatus of claim 6 , wherein the calculation module is further configured to:
obtain a conversion relationship between positions of the first sensor and the second sensor; and
calculate the relative positional relationship parameter between the first sensor and the second sensor based on the N sets of the coordinate data and the conversion relationship.