TOOL HEAD POSTURE ADJUSTMENT METHOD AND APPARATUS, AND READABLE STORAGE MEDIUM
20240261044 ยท 2024-08-08
Inventors
Cpc classification
B25J9/1694
PERFORMING OPERATIONS; TRANSPORTING
B25J9/1679
PERFORMING OPERATIONS; TRANSPORTING
A61H2201/1659
HUMAN NECESSITIES
B25J13/089
PERFORMING OPERATIONS; TRANSPORTING
G05B2219/40613
PHYSICS
A61H39/08
HUMAN NECESSITIES
International classification
A61H39/08
HUMAN NECESSITIES
Abstract
A tool head posture adjustment method and apparatus, and a readable storage medium are disclosed. Distances to laser points are measured by means of 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.
Claims
1. A tool head posture adjustment method, applicable to a robot, comprising: controlling laser points of laser distance sensors to irradiate onto a site to be checked 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 to be checked 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 a posture to be adjusted based on the posture parameters.
2. The tool head posture adjustment method according to claim 1, wherein if 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 the posture expression comprises an Euler angle, a quaternion, or a rotation matrix expression.
4. 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.
5. The tool head posture adjustment method according to claim 4, wherein calculating a laser point coordinate of each of the laser points on the site to be checked based on the distance, the initial coordinate and the laser direction comprises: assuming unit vectors of the laser directions to be Ra(rx1, ry1, rz1), Rb(rx2, ry2, rz2) and Rc(rx3, ry3, rz3) respectively, the coordinates of the laser distance sensors in a tool head coordinate system to be Pa(x1, y1, z1), Pb(x2, y2, z2) and Pc(x3, y3, z3), the distances to be d1, d2 and d3, and the coordinates of the laser points to be Ua, Ub and Uc, then
6. The tool head posture adjustment method according to claim 3, wherein 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 comprises: assuming the obtained comprehensive plane normal vector to be V(vx, vy, vz), with the convention that vz>0, where if 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.
7. The tool head posture adjustment method according to claim 6, wherein obtaining the posture parameters based on the preset formula, the distances and the Euler angle rotation formula comprises: assuming rx to be equal to 0, and calculating ry and rz based on the preset formula and the Euler angle rotation formula, where rx, ry and rz are Euler angles ?, ? and ?, respectively; calculating a comprehensive distance using a preset algorithm based on the distances, which is assumed to be daver; adding daver to an origin of the tool head coordinate system in z direction so that the origin is set at the site to be checked; 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 to be checked, where the posture parameters are assumed to be x0, y0, z0, rx0, ry0 and rz0; and assuming Euler angle parameters of the tool head posture to be adjusted to be Rx, Ry and Rz, then Rx=rx0, Ry=ry0+ry, and Rz=rz0+rz, then the posture parameters of the tool head posture to be adjusted being x0, y0, z0, Rx, Ry and Rz.
8. The tool head posture adjustment tool head posture adjustment method according to claim 7, wherein the preset algorithm comprises an average algorithm and a weighted average algorithm.
9. 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 to be checked 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 to be checked 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 a posture to be adjusted based on the posture parameters.
10. 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 to be checked 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 to be checked 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 a posture to be adjusted based on the posture parameters.
Description
BRIEF DESCRIPTION OF THE DRAWINGS
[0035]
[0036]
[0037]
[0038] The objects, features and advantages of the disclosure will be further described with reference to the accompanying drawings in conjunction with the embodiments.
DETAILED DESCRIPTION
[0039] In order to make the purpose, technical solutions and advantages of the disclosure clearer, the disclosure will be described in further detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the disclosure and not intended to limit the disclosure.
[0040] The disclosure further provides a tool head posture adjustment method. Referring to
[0041] In the present embodiment, the tool head posture adjustment method is executed by a tool head posture adjustment system, which includes a tool head posture adjustment apparatus, and the tool head posture adjustment apparatus may be a robot device, such as a moxibustion instrument and a massage instrument, but may also be a terminal device, such as a PC and a palmtop computer. According to the disclosure, distances to laser points are measured by means of laser distance sensors, and the various 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 the planes 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. According to the tool head posture adjustment method and apparatus, posture parameters of a tool head posture to be adjusted can be accurately determined. The present embodiment takes the moxibustion instrument as an example for illustration. The tool head posture adjustment method includes the following steps.
[0042] At S10, laser points of laser distance sensors are controlled to irradiate onto a site to be checked in a way that the laser points are not on a straight line, where the number of the laser distance sensors is more than 2.
[0043] In the present embodiment, referring to
[0044] At S20, 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 are acquired, where the distance is a distance between each laser distance sensor and the corresponding laser point.
[0045] In the present embodiment, the laser distance sensors 2 can measure a distance to a laser point, set a tool head coordinate system, and acquire initial coordinates of the laser distance sensors 2 in the tool head coordinate system and laser directions of the laser distance sensors 2, where the laser directions of the laser distance sensors 2 can be represented by a unit vector. At S30, a laser point coordinate of each of the laser points on the site to be checked is calculated based on the distance, the initial coordinate and the laser direction, and a comprehensive plane normal vector of a plane determined by the laser point coordinates are calculated, where the comprehensive plane normal vector is obtained from a plane normal vector of a plane determined by the laser point coordinates.
[0046] In the present embodiment, referring to
[0047] If the number of the laser distance sensors 2 is 3, the laser beams of the laser distance sensors 2 are parallel to each other, unit vectors of the respective laser directions are set as R.sub.a(r.sub.x1, r.sub.y1, r.sub.z1), R.sub.b(r.sub.x2, r.sub.y2, r.sub.z2) and R.sub.c(r.sub.x3, r.sub.y3, r.sub.z3), respectively, the coordinates of the laser distance sensors in the tool head coordinate system are set as P.sub.a(x.sub.1, y.sub.1, z.sub.1), P.sub.b(x.sub.2, y.sub.2, z.sub.2) and P.sub.c(x.sub.3, y.sub.3, z.sub.3), the respective distances are set as d.sub.1, d.sub.2 and d.sub.3, and the coordinates of the laser points are set as U.sub.a, U.sub.b and U.sub.c, then
[0049] At S40, posture parameters of a tool head posture of the robot to be adjusted are calculated based on a preset posture expression, the distances and the comprehensive plane normal vectors, and the tool head of the robot to be adjusted is controlled to the posture to be adjusted based on the posture parameters.
[0050] In the present embodiment, the preset posture expression includes expressions such as an Euler angle, a quaternion or a rotation matrix, and the posture parameters of the posture of the robot to be adjusted are obtained by using the posture expression according to the comprehensive plane normal vector and the distances.
[0051] The Euler angle rotation formula is used to calculate the posture parameters of the posture of the robot to be adjusted hereinafter.
[0052] The obtained comprehensive plane normal vector is assumed to be V(v.sub.x, v.sub.y, v.sub.z), with the convention that v.sub.z>0, where if v.sub.z<0, the comprehensive plane normal vector is multiplied by ?1, which is to calculate the unit normal vector of the comprehensive plane normal vector, so as to determine the direction of the comprehensive normal vector.
[0053] The Euler angle rotation formula is:
[0054] Assuming the preset formula M(?, ?, ?)*[0, 0, 1].sup.T=V.sub.norm.sup.T, where M(?, ?, ?) is a rotation matrix in the Euler angle rotation formula, ?, ?, and ? respectively represent Euler angles, V.sub.norm represents the comprehensive plane normal vector with modulo being 1, and ?, ? and ? respectively represent Euler angles.
[0055] Let r.sub.x to be equal to 0, r.sub.y and r.sub.z are obtained based on the preset formula and the Euler angle rotation formula, where r.sub.x, r.sub.y and r.sub.z are Euler angles ?, ? and ?, respectively, a comprehensive distance is calculated using a preset algorithm based on a distance, and is set as d.sub.aver: d.sub.aver is added to an origin of a tool head coordinate system in a z direction so that the origin is set at a site to be checked: then posture parameters of the origin with respect to a base coordinate system of a manipulator base are acquired when the origin is provided at the site to be checked, and the posture parameters are set as x.sub.0, y.sub.0, z.sub.0, r.sub.x0, r.sub.y0 and r.sub.z0; and the Euler angle parameters of the tool head posture to be adjusted are set as R.sub.x, R.sub.y and R.sub.z, then R.sub.x=r.sub.x0, R.sub.y=r.sub.y0+r.sub.y, and R.sub.z r.sub.z0+r.sub.z, then the posture parameters of the tool head posture to be adjusted are x.sub.0, y.sub.0, z.sub.0, R.sub.x, R.sub.y and R.sub.z, where the preset algorithm includes an average algorithm and a weighted average algorithm, for example, an average value of d.sub.1, d.sub.2 and ds is calculated using the average algorithm to obtain d.sub.aver.
[0056] The tool head posture is adjusted according to the posture parameters of the posture to be adjusted, so that the robot can accurately perform acupuncture and moxibustion care on the site to be checked.
[0057] The present embodiment provides a tool head posture adjustment method, including: controlling laser points of laser distance sensors to irradiate onto a site to be checked in a way that the laser points are not on a straight line, where the number of the laser distance sensors is more than 2: 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 to be checked 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 the tool head of the robot to be adjusted to the posture to be adjusted based on the posture parameters. In this way, the posture parameters of the tool head posture to be adjusted can be accurately determined, thereby improving the operation efficiency of the moxibustion instrument. In addition, the laser distance sensors of the disclosure are low in cost, high in precision and strong in real-time performance when the robot works, and can adapt to a scenario where a tool head is in contact with a human body or a scenario where the tool head is kept at a certain distance from the human body.
[0058] In addition, the embodiments of the disclosure also provide a readable storage medium.
[0059] The readable storage medium of the disclosure stores thereon a tool head posture adjustment program, which, when executed by a processor, implements the following steps: [0060] controlling laser points of laser distance sensors to irradiate onto a site to be checked in a way that the laser points are not on a straight line, where the number of the laser distance sensors is more than 2; [0061] 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; [0062] calculating a laser point coordinate of each of the laser points on the site to be checked 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 [0063] 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 a posture to be adjusted based on the posture parameters.
[0064] Further, if there are more than three of the laser distance sensors, calculating a comprehensive normal vector of a plane determined by the laser point coordinates includes: [0065] calculating plane normal vectors of corresponding planes determined by the laser point coordinates; and [0066] 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.
[0067] Further, the posture expression includes an Euler angle, a quaternion, or a rotation matrix expression.
[0068] Further, there are three of the laser distance sensors, and laser beams of the laser distance sensors are parallel to one another.
[0069] Further, calculating a laser point coordinate of each of the laser points on the site to be checked based on the distance, the initial coordinate and the laser direction includes: [0070] assuming unit vectors of the laser directions to be R.sub.a(r.sub.x1, r.sub.y1, r.sub.z1), R.sub.b(r.sub.x2, r.sub.y2, r.sub.z2) and R.sub.c(r.sub.x3, r.sub.y3, r.sub.z3) respectively, the coordinates of the laser distance sensors in a tool head coordinate system to be P.sub.a(x.sub.1, y.sub.1, z.sub.1), P.sub.b(x.sub.2, y.sub.2, z.sub.2) and P.sub.c(x.sub.3, y.sub.3, z.sub.3), the distances to be d.sub.1, d.sub.2 and d.sub.3, and the coordinates of the laser points to be U.sub.a, U.sub.b and U.sub.c, then
[0072] Further, 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 includes: [0073] assuming the obtained comprehensive plane normal vector to be V(v.sub.x, v.sub.y, v.sub.z), with the convention that v.sub.z>0, where if v.sub.z<0, the comprehensive plane normal vector is multiplied by ?1; [0074] presetting a formula M(?, ?, ?)*[0, 0, 1].sup.T=V.sub.norm.sup.T, where M(?, ?, ?) is a rotation matrix in an Euler angle rotation formula; and [0075] obtaining the posture parameters based on the preset formula, the distances and the Euler angle rotation formula, where V.sub.norm represents the comprehensive plane normal vector with modulo being 1, and ?, ? and ? respectively represent Euler angles.
[0076] Further, obtaining the posture parameters based on the preset formula, the distances and the Euler angle rotation formula includes: [0077] assuming r.sub.x to be equal to 0, and calculating r.sub.y and r.sub.z based on the preset formula and the Euler angle rotation formula, where r.sub.x, r.sub.y and r.sub.z are Euler angles ?, ? and ?, respectively; [0078] calculating a comprehensive distance using a preset algorithm based on the distances, which is assumed to be d.sub.aver; [0079] adding d.sub.aver to an origin of the tool head coordinate system in z direction so that the origin is set at the site to be checked; [0080] 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 to be checked, where the posture parameters are assumed to be x.sub.0, y.sub.0, z.sub.0, r.sub.x0, r.sub.y0 and r.sub.z0; and [0081] assuming Euler angle parameters of the tool head posture to be adjusted to be R.sub.x, R.sub.y and R.sub.z, then [0082] R.sub.x=r.sub.x0, R.sub.y=r.sub.y0+r.sub.y, and R.sub.z=r.sub.z0+r.sub.z, then the posture parameters of the tool head posture to be adjusted being x.sub.0, y.sub.0, z.sub.0, R.sub.x, R.sub.y and R.sub.z.
[0083] Further, the preset algorithm includes an average algorithm and a weighted average algorithm.
[0084] It should be noted that, as used herein, the terms comprising, including or any other variation thereof are intended to encompass non-exclusive inclusion, such that a process, method, article or system including a series of elements includes not only those elements, but also other elements that are not explicitly listed, or also includes elements inherent to such a process, method, article, or system. Without further limitation, an element defined by the statement includes a . . . does not preclude the existence of additional identical elements in the process, method, article or system that includes the element.
[0085] The above-mentioned serial numbers of the embodiments of the disclosure are merely for the purpose of description and do not represent the advantages and disadvantages of the embodiments.
[0086] From the description of the embodiments given above, it will be clear to a person of ordinary skill in the art that the method of the embodiments described above can be implemented by means of software plus a necessary general hardware platform, and certainly also by means of hardware, but in many cases the former is a better way. Based on such understanding, the technical solutions of the disclosure essentially or the part that contributes to the prior art can be reflected in the form of software products, and the computer software products are stored in a storage medium (such as an ROM/RAM, a magnetic diskette and an optical disk) as described above, and include several instructions for making a terminal device (which may be a mobile phone, a computer, a server, an air conditioner, or a network device, etc.) to execute the method according to various embodiments of the disclosure.
[0087] The above are only preferred embodiments of the disclosure, and do not therefore limit the patent scope of the disclosure. All equivalent structures or equivalent process changes made by using the description of the disclosure and the accompanying drawings, or directly or indirectly used in other related technical fields, are equally included in the scope of patent protection of the disclosure.