Robot calibration method and device based on equivalent kinematic model
The present disclosure relates to a robot calibration method based on an equivalent kinematic model and a device thereof. The method includes: calibrating a robot real system with a degree of freedom of f≤6 and containing multiple types of time-invariant error sources as a kinematic equivalent system with six degrees of freedom; wherein 6-dimensional pose vectors y of output members of the equivalent system and the real system are the same, and the equivalent system includes a forward position solution model: y=f(ρ(q,α),β 0 ); constructing a function relationship between ρ and α; acquiring a pose measurement value of the real system; calculating an estimator {circumflex over (α)} i , of the adjustable constant parameter vector α in a decoupling manner in a joint space; modifying a driving joint variable corresponding to a received motion trajectory instruction set; and driving the robot to move according to the modified driving joint variable.
1 . A robot calibration method based on an equivalent kinematic model, comprising:
calibrating a robot real system with a degree of freedom of f≤6 and containing multiple types of time-invariant error sources as a kinematic equivalent system with six degrees of freedom; wherein 6-dimensional pose vectors y of output members of the equivalent system and the real system are the same, and the equivalent system comprises a forward position solution model: y=f(ρ(q,α),β 0 ); where β 0 is a nominal value of a known kinematic parameter of the real system, ρ is a total joint variable of the equivalent system, q is a driving joint variable of the real system, and α is an adjustable constant parameter vector;
constructing a function relationship between ρ and α:
ρ
(
q
,
α
)
=
ρ
0
+
Z
(
q
)
α
,
ρ
0
=
(
q
0
)
,
Z
(
q
)
=
diaq
[
z
(
q
)
]
6
;
where z(q) indicates a function vector with the same dimension as α in which q is an independent variable;
acquiring a pose measurement value of the real system, and based on an iterative algorithm, calculating an estimator {circumflex over (α)} i of the adjustable constant parameter vector α in a decoupling manner in a joint space;
modifying a driving joint variable corresponding to a received motion trajectory instruction set according to z(q) and {circumflex over (α)} i ; and
driving the robot to move according to the modified driving joint variable.
2 . The robot calibration method based on the equivalent kinematic model according to claim 1 , wherein acquiring a pose measurement value of the real system, and based on an iterative algorithm, calculating an estimator {circumflex over (α)} i of the adjustable constant parameter vector α in a decoupling manner in a joint space comprises:
acquiring a pose measurement value y j of an output member at a configuration j (j=1~N) of the real system;
assuming that
α
^
i
k
(
i
=
1
∼
6
,
k
=
0
,
1
,
…
)
is a k-th iteration value of α i , and assuming that
α
^
i
0
=
0
;
performing first-order Taylor expansion on f(ρ(q j ,{circumflex over (α)}),β 0 ) in a neighborhood of
α
^
i
0
,
and expressing a residual of y j and f(ρ(q j ,{circumflex over (α)}),β 0 ) as:
e
j
k
=
Δ
y
j
k
-
T
j
k
Z
j
Δ
α
k
Δ
y
j
k
=
y
j
-
f
(
ρ
(
q
j
,
α
^
k
)
,
β
0
)
,
T
j
k
=
∂
f
(
ρ
(
q
j
,
α
)
,
β
0
)
∂
ρ
❘
"\[LeftBracketingBar]"
α
-
α
^
k
,
Z
j
=
Z
(
q
j
)
,
Δ
α
k
=
α
-
α
^
k
;
realizing decoupling in the joint space by simultaneous left multiplication of
(
W
j
k
)
T
=
(
T
j
k
)
-
1
at both ends of
e
j
k
=
Δ
y
j
k
-
T
j
k
Z
j
Δ
α
k
,
and grouping and integrating a multivariate linear regression equation;
Δ
ρ
i
k
=
H
Δα
i
k
+
ε
i
k
,
Δ
ρ
i
k
=
(
(
w
1
,
i
k
)
T
Δ
y
1
k
⋮
(
w
N
,
i
k
)
T
Δ
y
N
k
)
,
H
=
(
z
1
T
⋮
z
N
T
)
,
ε
i
k
=
(
(
w
1
,
i
k
)
T
e
1
k
⋮
(
w
N
,
i
k
)
T
e
N
k
)
;
wherein
w
j
,
i
k
indicates an i-th column of a matrix
W
j
k
;
obtaining
Δ
a
i
k
by calculation based on a preset algorithm;
updating
α
^
i
k
=:
α
^
i
k
+
Δ
α
^
i
k
,
executing iterative loop until convergence, and taking a convergence value as the estimator {circumflex over (α)} i .
3 . The robot method based on the equivalent kinematic model according to claim 2 , wherein acquiring a pose measurement value y j of an output member at the configuration j (j=1~N) comprises:
evenly selecting N>n=dim(α i ) measurement configurations in a robot workspace, and determining a pose instruction set {y 0 } corresponding to the configuration;
obtaining an inverse position solution model f −1 (·) of the equivalent system according to the forward position solution model f(·) of the equivalent system;
based on the inverse position solution model f −1 (·) of the equivalent system, mapping a pose vector y 0,j of the output member at a configuration j to the total joint variable ρ 0,j of the equivalent system;
constructing a driving joint variable q j of the real system according to the total joint variable ρ 0,j of the equivalent system;
driving the robot to traverse the configuration j;
measuring coordinates of a preset measuring target point of a calibration gauge on the robot in a calibration coordinate system;
obtaining a pose measurement value y j of the output member at a configuration j (j=1~N) based on the coordinate.
4 . The robot calibration method based on the equivalent kinematic model according to claim 3 , wherein modifying a driving joint variable corresponding to a received motion trajectory instruction set according to z(q) and {circumflex over (α)} i , comprises:
based on the inverse position solution model f −1 (·) of the equivalent system, mapping the received motion trajectory instruction set to an instruction set {ρ 0 } of the equivalent system for the driving joint variable;
constructing an instruction set {q} of the real system for the driving joint variables according to {ρ 0 };
modifying {q} according to z(q) and {circumflex over (α)} i based on a pre-constructed error compensation modification formula {q m,i }={q i −z T (q){circumflex over (α)} i }, in which i=1~5, and obtaining the modified instruction set {q m,i } of the driving joint variable.
5 . The robot calibration method based on the equivalent kinematic model according to claim 4 , wherein the method further comprises:
interpolating the modified instruction set {q m,i } of the driving joint variable.
6 . The robot calibration method based on the equivalent kinematic model according to claim 1 , wherein constructing a function relationship between ρ and α comprises:
expressing each component ρ i (i=1~6) of ρ as a quadratic response surface function with each component q 1 ~q 5 of q as an independent variable:
ρ
i
=
q
i
+
μ
0
,
i
+
∑
j
=
1
5
μ
j
,
i
q
j
+
∑
j
=
1
5
∑
k
=
j
5
μ
j
.
k
,
i
q
j
q
k
,
i
=
1
∼
6
,
q
6
=
0
;
where μ *,*,j indicates coefficients of the quadratic response surface;
obtaining the following formula by variable substitution:
ρ i =q i +z T ( q )α i , where i= 1~6, q 6 =0;
wherein:
z
(
q
)
=
(
1
z
l
…
z
n
)
T
=
(
1
q
1
…
q
5
q
1
2
q
1
q
2
…
q
5
2
)
T
,
α
i
=
(
α
0
,
i
α
1
,
i
…
α
n
,
i
)
T
=
(
μ
0
,
i
μ
1
,
i
…
μ
5
,
i
μ
1
,
1
i
μ
1
,
2
,
i
…
μ
5
,
5
,
i
)
T
,
i
=
1
∼
6.
7 . The robot calibration method based on the equivalent kinematic model according to claim 1 , wherein the function relationship between ρ and α is a linear function or a nonlinear function.
8 . The robot calibration method based on the equivalent kinematic model according to claim 2 , wherein obtaining
Δ
a
i
k
by calculation based on a preset algorithm comprises:
obtaining
Δ
a
i
k
by calculation based on a damped least square method.
9 . The robot calibration method based on the equivalent kinematic model according to claim 2 , wherein prior to acquiring a pose measurement value of the real system, and based on an iterative algorithm, calculating an estimator {circumflex over (α)} i of the adjustable constant parameter vector α in a decoupling manner in a joint space, the method further comprises:
constructing an analytical model or a numerical model of full kinematic Jacobian T=∂f/∂ρ and full dynamic Jacobian W=T −T of the equivalent system.
10 . A robot calibration device based on an equivalent kinematic model, comprising:
a processor and a memory;
wherein the processor is connected with the memory through a communication bus;
wherein the processor is configured to call and execute a program stored in the memory;
the memory is configured to store a program, and the program is at least configured to execute the robot calibration method based on the equivalent kinematic model according to claim 1 .