Tool head posture adjustment method, apparatus and readable storage medium

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(vx, vy, vz), with the convention that vz>0, where when vz<0, the comprehensive plane normal vector is multiplied by −1;
• presetting a formula M(α, β, γ)*[0, 0, 1]T=VnormT, where M(α, β, γ) is a rotation matrix in an Euler angle rotation formula, where Vnorm 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.

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(vx, vy, vz), with the convention that vz>0, where when vz<0, the comprehensive plane normal vector is multiplied by −1;
• presetting a formula M(α, β, γ)*[0, 0, 1]T=VnormT, where M(α, β, γ) is a rotation matrix in an Euler angle rotation formula, where Vnorm 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.