Gantry platform kinematics modeling method considering parallelism and perpendicularity errors
The invention relates to the technical field of intelligent control, and particularly to a gantry platform kinematics modeling method considering parallelism and perpendicularity errors, which is applied to a gantry platform device. The gantry platform device comprises a crossbeam, a first guide rail and a second guide rail, one end of the crossbeam is in sliding fit with the first guide rail, and the other end of the crossbeam is in sliding fit with the second guide rail; the first guide rail is provided with a first longitudinal motor, and the second guide rail is provided with a second longitudinal motor; and the crossbeam is provided with a transverse motor configured to drive the gantry platform to move along the crossbeam. The method comprises: establishing a two-dimensional coordinate system; constructing a forward kinematics solution model; and constructing an inverse kinematics solution model.
1 . A gantry platform control method applied to a gantry platform device which comprises a crossbeam, a first guide rail and a second guide rail, wherein one end of the crossbeam is in sliding fit with the first guide rail, and the other end of the crossbeam is in sliding fit with the second guide rail; the first guide rail is provided with a first longitudinal motor, the second guide rail is provided with a second longitudinal motor, and the first longitudinal motor and the second longitudinal motor are respectively connected to two ends of the crossbeam; and the crossbeam is provided with a transverse motor configured to drive the gantry platform to move along the crossbeam; and
a gantry platform kinematics modeling method considering parallelism and perpendicularity errors comprises the following steps:
(1) establishing a two-dimensional coordinate system
taking the first guide rail as a reference guide rail and an initial position of the first longitudinal motor as an origin to establish a two-dimensional coordinate system O 0 in a plane at which the first guide rail and the second guide rail are located, wherein in the coordinate system, a coordinate of the first longitudinal motor after movement is Y1′ (0, y 1 ), a coordinate of the second longitudinal motor after movement is Y2′ (x 2 , y 2 ), and a position coordinate of the gantry platform after movement is (x, y); and
a length of the crossbeam is 1, a parallelism error between the first guide rail and the second guide rail is α, and a perpendicularity error between the crossbeam and the first guide rail at the initial position of the first motor is β 0 ;
(2) constructing a forward kinematics solution model
it is given that moved displacements of the first longitudinal motor, the second longitudinal motor and the transverse motor are y e1 , y e2 , x e , and an initial offset of the transverse motor is x z0 , solving displacements x r , y r of the gantry platform as follows:
{
x
r
=
x
e
1
cos
β
0
y
r
=
xy
2
-
xy
1
+
x
2
y
1
x
2
-
x
z
0
β
0
;
equation
(
1
-
4
)
and
(3) constructing an inverse kinematics solution model
it is given that the displacements of the gantry platform are x r , y r solving required displacements y e1 , y e2 , x e for the first longitudinal motor, the second longitudinal motor and the transverse motor to move:
{
y
e
1
=
y
1
y
e
2
=
y
2
-
l
β
0
cos
α
x
e
=
x
r
cos
β
,
equation
(
1
-
9
)
wherein β is a perpendicularity error after movement;
wherein the gantry platform control method comprises the following steps:
acquiring expected displacements x r , y r of the gantry platform after movement;
obtaining the required displacements x e , y e1 , y e2 for the transverse motor, the first longitudinal motor and the second longitudinal motor to move by using the inverse kinematics solution mode;
controlling the movement of the first longitudinal motor, the second longitudinal motor and the transverse motor;
acquiring actual displacements x t1 , y t1 , y t2 of the transverse motor, the first longitudinal motor and the second longitudinal motor;
obtaining an actual position (x, y) of the gantry platform after movement by using the forward kinematics solution model; and
compensating for a deviation between an expected position (x r , y r ) and the actual position (x, y) of the gantry platform after movement.
2 . The method according to claim 1 , wherein, in the step (2), the step of constructing the forward kinematics solution model is as follows:
it is given that the moved displacements of the first longitudinal motor, the second longitudinal motor and the transverse motor are y e1 , y e2 , x e , and the initial offset of the transverse motor is x z0 ,
coordinate values of each point are as follows:
x
0
=
x
z
0
,
y
0
=
x
z
0
β
0
,
y
1
=
y
e
1
;
x
2
=
l
cos
β
0
+
y
e
2
α
,
y
2
=
l
β
0
+
y
e
2
cos
α
;
equation
(
1
-
1
)
the perpendicularity error β after movement is:
β
=
y
2
-
y
1
x
2
=
l
β
0
+
y
e
2
cos
α
-
y
e
1
l
cos
β
0
+
y
e
2
α
;
equation
(
1
-
2
)
an end position x after movement is:
x
=
x
e
1
cos
β
0
+
x
z
0
;
equation
(
1
-
3
)
then a forward kinematics solution value x r , y r is:
{
x
r
=
x
e
1
cos
β
0
y
r
=
xy
2
-
xy
1
+
x
2
y
1
x
2
-
x
z
0
β
0
.
equation
(
1
-
4
)
3 . The method according to claim 1 , wherein, in the step (3), the step of constructing the forward kinematics solution model is as follows:
it is given that the displacements of the gantry platform are x r , y r , values of x, y, x 2 are:
x
=
x
z
0
+
x
r
,
y
0
=
x
z
0
β
0
+
y
r
;
x
2
=
l
cos
β
0
+
y
r
α
;
equation
(
1
-
5
)
based on a geometrical relationship, an equation set is obtained:
{
-
x
x
-
x
2
=
y
1
-
y
y
-
y
2
x
2
2
+
(
y
2
-
y
1
)
2
=
l
2
;
equation
(
1
-
6
)
values of the coordinate (y 1 , y 2 ) are solved as follows:
{
y
1
=
-
x
2
y
-
x
(
x
2
y
-
x
(
l
-
x
2
)
(
l
-
x
2
)
+
x
2
(
l
+
x
2
)
(
l
-
x
2
)
)
x
2
x
-
x
2
y
2
=
x
2
y
-
x
(
l
+
x
2
)
(
l
-
x
2
)
+
x
2
(
l
+
x
2
)
(
l
-
x
2
)
x
2
;
equation
(
1
-
7
)
the perpendicularity error β after movement is:
β
=
y
2
-
y
1
x
2
;
equation
(
1
-
8
)
the required displacements y e1 , y e2 , x e for the first longitudinal motor, the second longitudinal motor and the transverse motor to move are:
{
y
e
1
=
y
1
y
e
2
=
y
2
-
l
β
0
cos
α
x
e
=
x
r
cos
β
.
equation
(
1
-
9
)
4 . The method according to claim 3 , wherein, in the equation (1-5), the value of x 2 is an estimated value, and y e2 ˜y r is set.
5 . The gantry platform control method according to claim 1 , wherein actual displacements x t1 , y t1 , y t2 of the transverse motor, the first longitudinal motor and the second longitudinal motor are acquired by using a displacement sensor.
6 . The gantry platform control method according to claim 1 , wherein the parallelism error a and the perpendicularity error β 0 are obtained by means of measurement.
7 . A gantry platform device applied to the gantry platform control method according to claim 1 , wherein the gantry platform device further comprises a controller and a displacement sensor, the displacement sensor is configured to sense the displacements of the transverse motor, the first longitudinal motor and the second longitudinal motor, and send displacement data to the controller;
the controller stores the forward kinematics solution model and the inverse kinematics solution model, the controller is configured to obtain the required displacements y e1 , y e2 , x e for the first longitudinal motor, the second longitudinal motor and the transverse motor to move based on the expected displacement of the gantry platform after movement and the inverse kinematics solution model, send a movement instruction to the transverse motor, the first longitudinal motor and the second longitudinal motor, and obtain a compensation deviation.