Tool head posture adjustment method, apparatus and readable storage medium
View Patent ↗A tool head posture adjustment method and apparatus, and a readable storage medium are disclosed. Distances to laser points are measured by laser distance sensors, and the laser points can construct at least one plane, thereby determining each plane corresponding to a site to be checked; and a comprehensive normal vector of each plane is calculated, and posture parameters of a tool head of a robot are calculated by using a posture expression, thereby adjusting the tool head posture.
1. A tool head posture adjustment method, applicable to a robot, comprising:
controlling laser points of laser distance sensors to irradiate onto a site in a way that the laser points are not on a straight line, where there are more than two of the laser distance sensors;
acquiring a distance measured by each of the laser distance sensors, an initial coordinate of each of the laser distance sensors and a laser direction of each of the laser distance sensors, where the distance is a distance between each laser distance sensor and the corresponding laser point;
calculating a laser point coordinate of each of the laser points on the site based on the distance, the initial coordinate and the laser direction, and calculating a comprehensive plane normal vector of a plane determined by the laser point coordinates, where the comprehensive plane normal vector is obtained from a plane normal vector of the plane determined by the laser point coordinates; and
calculating posture parameters of a tool head posture of the robot to be adjusted based on a preset posture expression, the distances and the comprehensive plane normal vector, and controlling a tool head of the robot to be adjusted to an adjusted posture based on the posture parameters;
wherein the posture expression comprises an Euler angle, a quaternion, or a rotation matrix expression, and calculating posture parameters of the tool head posture of the robot to be adjusted based on a preset posture expression, the distances and the comprehensive plane normal vector comprises;
assuming the obtained comprehensive plane normal vector to be V(v x , v y , v z ), with the convention that v z >0, where when v z <0, the comprehensive plane normal vector is multiplied by −1;
presetting a formula M(α, β, γ)*[0, 0, 1] T =V norm T , where M(α, β, γ) is a rotation matrix in an Euler angle rotation formula, where V norm represents the comprehensive plane normal vector with modulo being 1, and α, β and γ respectively represent Euler angles, and
obtaining the posture parameters based on the preset formula, the distances and the Euler angle rotation formula.
2. The tool head posture adjustment method according to claim 1 , wherein when there are more than three of the laser distance sensors, calculating a comprehensive normal vector of a plane determined by the laser point coordinates comprises:
calculating plane normal vectors of corresponding planes determined by the laser point coordinates; and
calculating a weighted average value of the plane normal vectors using a weighted average algorithm, and taking the weighted average value as the comprehensive plane normal vector.
3. The tool head posture adjustment method according to claim 1 , wherein there are three of the laser distance sensors, and laser beams of the laser distance sensors are parallel to one another.
4. The tool head posture adjustment method according to claim 3 , wherein calculating a laser point coordinate of each of the laser points on the site based on the distance, the initial coordinate and the laser direction comprises:
assuming unit vectors of the laser directions to be R a (r x1 , r y1 , r z1 ), R b (r x2 , r y2 , r z2 ) and R c (r x3 , r y3 , r z3 ) respectively, the coordinates of the laser distance sensors in a tool head coordinate system to be P a (x 1 , y 1 , z 1 ), P b (x 2 , y 2 , z 2 ) and P c (x 3 , y 3 , z 3 ), the distances to be d 1 , d 2 and d 3 , and the coordinates of the laser points to be U a , U b and U c , then
U
a
·
x
=
x
1
+
r
x
1
*
d
1
,
U
a
·
y
=
y
1
+
r
y
1
*
d
1
,
U
a
·
z
=
z
1
+
r
z
1
*
d
1
;
U
b
·
x
=
x
2
+
r
x
2
*
d
2
,
U
b
·
y
=
y
2
+
r
y
2
*
d
2
,
U
b
·
z
=
z
2
+
r
z
2
*
d
2
;
U
c
·
x
=
x
3
+
r
x
3
*
d
3
,
U
c
·
y
=
y
3
+
r
y
3
*
d
3
,
U
c
·
z
=
z
3
+
r
z
3
*
d
3
;
to obtain the coordinates of the laser points Ua(U a.x , U a.y , U a.z ), U b (U b.x , U b.y , U b.z ) and Uc(U c.x , U c.y , U c.z ).
5. The tool head posture adjustment method according to claim 1 , wherein obtaining the posture parameters based on the preset formula, the distances and the Euler angle rotation formula comprises:
assuming r x to be equal to 0, and calculating r y and r z based on the preset formula and the Euler angle rotation formula, where r x , r y and r z are Euler angles α, β and γ, respectively;
calculating a comprehensive distance using a preset algorithm based on the distances, which is assumed to be d aver ;
adding d aver to an origin of the tool head coordinate system in z direction so that the origin is set at the site;
acquiring posture parameters of the origin with respect to a base coordinate system of a manipulator base when the origin is provided at the site, where the posture parameters are assumed to be x 0 , y 0 , z 0 , r x0 , r y0 and r z0 ; and
assuming Euler angle parameters of the tool head posture to be adjusted to be R x , R y and R z , then
R x =r x0 , R y =r y0 +r y , and R z =r z0 +r z , then the posture parameters of the tool head posture to be adjusted being x 0 , y 0 , z 0 , R x , R y and R z .
6. The tool head posture adjustment method according to claim 5 , wherein the preset algorithm comprises an average algorithm and a weighted average algorithm.
7. A tool head posture adjustment apparatus, comprising: a memory, a processor, and a tool head posture adjustment program stored on the memory and executable by the processor, when executed by the processor, the tool head posture adjustment program implements a tool head posture adjustment method, applicable to a robot, comprising:
controlling laser points of laser distance sensors to irradiate onto a site in a way that the laser points are not on a straight line, where there are more than two of the laser distance sensors;
acquiring a distance measured by each of the laser distance sensors, an initial coordinate of each of the laser distance sensors and a laser direction of each of the laser distance sensors, where the distance is a distance between each laser distance sensor and the corresponding laser point;
calculating a laser point coordinate of each of the laser points on the site based on the distance, the initial coordinate and the laser direction, and calculating a comprehensive plane normal vector of a plane determined by the laser point coordinates, where the comprehensive plane normal vector is obtained from a plane normal vector of the plane determined by the laser point coordinates; and
calculating posture parameters of a tool head posture of the robot to be adjusted based on a preset posture expression, the distances and the comprehensive plane normal vector, and controlling a tool head of the robot to be adjusted to the adjusted posture based on the posture parameters,
wherein the posture expression comprises an Euler angle, a quaternion, or a rotation matrix expression, and calculating posture parameters of the tool head posture of the robot to be adjusted based on a preset posture expression, the distances and the comprehensive plane normal vector comprises;
assuming the obtained comprehensive plane normal vector to be V(v x , v y , v z ), with the convention that v z >0, where when v z <0, the comprehensive plane normal vector is multiplied by −1;
presetting a formula M(α, β, γ)*[0, 0, 1] T =V norm T , where M(α, β, γ) is a rotation matrix in an Euler angle rotation formula, where V norm represents the comprehensive plane normal vector with modulo being 1, and α, β and γ respectively represent Euler angles, and
obtaining the posture parameters based on the preset formula, the distances and the Euler angle rotation formula.
8. A non-transitory computer readable storage medium, storing thereon a tool head posture adjustment program, which, when executed by a processor, implements a tool head posture adjustment method, applicable to a robot, comprising:
controlling laser points of laser distance sensors to irradiate onto a site in a way that the laser points are not on a straight line, where there are more than two of the laser distance sensors;
acquiring a distance measured by each of the laser distance sensors, an initial coordinate of each of the laser distance sensors and a laser direction of each of the laser distance sensors, where the distance is a distance between each laser distance sensor and the corresponding laser point;
calculating a laser point coordinate of each of the laser points on the site based on the distance, the initial coordinate and the laser direction, and calculating a comprehensive plane normal vector of a plane determined by the laser point coordinates, where the comprehensive plane normal vector is obtained from a plane normal vector of the plane determined by the laser point coordinates; and
calculating posture parameters of a tool head posture of the robot to be adjusted based on a preset posture expression, the distances and the comprehensive plane normal vector, and controlling a tool head of the robot to be adjusted to an adjusted posture based on the posture parameters;
wherein the posture expression comprises an Euler angle, a quaternion, or a rotation matrix expression, and calculating posture parameters of the tool head posture of the robot to be adjusted based on a preset posture expression, the distances and the comprehensive plane normal vector comprises;
assuming the obtained comprehensive plane normal vector to be V(v x , v y , v z ), with the convention that v z >0, where when v z <0, the comprehensive plane normal vector is multiplied by −1;
presetting a formula M(α, β, γ)*[0, 0, 1] T =V norm T , where M(α, β, γ) is a rotation matrix in an Euler angle rotation formula, where V norm represents the comprehensive plane normal vector with modulo being 1, and α, β and γ respectively represent Euler angles, and
obtaining the posture parameters based on the preset formula, the distances and the Euler angle rotation formula.