阅读说明：本技术 一种遥操作用超声仿型探头姿态自校准方法、系统及装置 (Method, system and device for self-calibration of ultrasonic profiling probe attitude for teleoperation ) 是由 闫琳 李淼 韩冬 付中涛 张少华 邓兆兴 刘辰 万志林 龙会才 马天阳 于 2021-08-23 设计创作，主要内容包括：本发明涉及一种遥操作用超声仿型探头姿态自校准方法、系统及装置,其方法包括,超声仿型探头在触控板上摆动,获取IMU传感器的实时姿态数据和触控板感应的实时平移数据；将实时姿态数据转换成欧拉角形式；根据实时平移数据计算出平移方向向量；根据欧拉角形式的实时姿态数据以及平移方向向量判断出摆动轴、摆动方向以及平移方向向量所在的象限；利用摆动轴、摆动方向以及平移方向向量所在的象限对应的姿态校准模型,对IMU传感器的航向角进行自校准。本发明可以对IMU传感器的航向角的漂移进行抑制,从而保证了使超声仿型探头到机器人末端超声探头的位置和姿态映射精度,进而提高了获取的超声图像质量。(The invention relates to a method, a system and a device for self-calibration of the posture of an ultrasonic profiling probe for teleoperation, wherein the method comprises the steps that the ultrasonic profiling probe swings on a touch pad to obtain real-time posture data of an IMU sensor and real-time translation data sensed by the touch pad; converting the real-time attitude data into an Euler angle form; calculating a translation direction vector according to the real-time translation data; judging a swing axis, a swing direction and a quadrant of a translation direction vector according to real-time attitude data in an Euler angle form and the translation direction vector; and self-calibrating the course angle of the IMU sensor by utilizing the attitude calibration model corresponding to the quadrant in which the oscillating axis, the oscillating direction and the translation direction vector are positioned. The invention can restrain the drift of the course angle of the IMU sensor, thereby ensuring the mapping precision of the position and the posture from the ultrasonic profiling probe to the ultrasonic probe at the tail end of the robot and further improving the quality of the obtained ultrasonic image.)

1. A method for self-calibration of the posture of an ultrasonic profiling probe for teleoperation is characterized by comprising the following steps: the method is used for calibrating the course angle of the IMU sensor when the ultrasonic profiling probe embedded with the IMU sensor swings on the touch pad through the arc-shaped tail end, and comprises the following steps,

s1, contacting the arc-shaped tail end of the ultrasonic profiling probe with the touch pad, and enabling the ultrasonic profiling probe to swing on the touch pad to obtain real-time posture data of the IMU sensor under an IMU coordinate system and real-time translation data of the touch pad under a touch pad coordinate system; the data form of the real-time attitude data of the IMU sensor under the IMU coordinate system is specifically an attitude quaternion form;

s2, converting the real-time attitude data of the IMU sensor in the form of attitude quaternion under an IMU coordinate system into real-time attitude data in the form of Euler angles; calculating a translation direction vector of the ultrasonic profiling probe when swinging on the touch pad according to real-time translation data of the touch pad under a touch pad coordinate system;

s3, judging a swing axis and a swing direction of the ultrasonic profiling probe when swinging on the touch pad and a quadrant where the translation direction vector is located according to real-time attitude data in the form of Euler angles of the IMU sensor and the translation direction vector of the ultrasonic profiling probe when swinging on the touch pad;

and S4, self-calibrating the heading angle of the IMU sensor by utilizing a posture calibration model which corresponds to the swinging axis, the swinging direction and the quadrant where the translation direction vector is located when the ultrasonic profiling probe swings on the touch pad.

2. The method for self-calibration of the attitude of the ultrasonic profiling probe for teleoperation according to claim 1, characterized in that: the swinging axis of the ultrasonic profiling probe when swinging on the touch pad is one of an X axis, a Y axis and multiple axes; wherein, the swinging axis of the ultrasonic profiling probe when swinging on the touch pad is an X axis, which represents that the ultrasonic profiling probe swings around the X axis on the touch pad, the swinging axis of the ultrasonic profiling probe when swinging on the touch pad is a Y axis, which represents that the ultrasonic profiling probe swings around the Y axis on the touch pad, the swinging axis of the ultrasonic profiling probe when swinging on the touch pad is a plurality of axes, which represents that the ultrasonic profiling probe swings around axes except the X axis and the Y axis on the touch pad;

the swinging direction of the ultrasonic profiling probe when swinging on the touch pad comprises a positive direction and a negative direction;

the quadrants where the translation direction vector of the ultrasonic profiling probe is located when the ultrasonic profiling probe swings on the touch pad include 14 quadrants and 23 quadrants.

3. The method for self-calibration of the pose of an ultrasonic profiling probe for teleoperation according to claim 2, characterized in that: the attitude calibration models which correspond to the swinging axis, the swinging direction and the quadrant where the translation direction vector is located when the ultrasonic profiling probe swings on the touch pad together comprise an X-axis negative 14-quadrant attitude calibration model, an X-axis negative 23-quadrant attitude calibration model, an X-axis positive 14-quadrant attitude calibration model, an X-axis positive 23-quadrant attitude calibration model, a Y-axis negative 14-quadrant attitude calibration model, a Y-axis negative 23-quadrant attitude calibration model, a Y-axis positive 14-quadrant attitude calibration model, a Y-axis positive 23-quadrant attitude calibration model and a multi-axis attitude calibration model.

4. The method for self-calibration of the pose of an ultrasonic profiling probe for teleoperation according to claim 3, wherein:

the X-axis negative 14-quadrant pose calibration model is q (t) ═ -acos [ (dy (t)/D ],

the negative-going 23-quadrant attitude calibration model for the X-axis is q (t) acos [ (dy (t)/D ],

the X-axis forward 14-quadrant attitude calibration model is Q (t) ═ pi-acos [ (dy (t)/D ],

the X-axis forward 23-quadrant attitude calibration model is q (t) ═ acos [ (dy (t)/D ] -pi;

q (t) is the angle of the IMU sensor after the course angle self-calibration at the moment t; d ═ sqrt [ dx (t) × (t) + dy (t) × (t) ((t), dx (t)) and dy (t)) are respectively the X-axis component and the Y-axis component of the translation direction in the translation direction vector at the time t when the ultrasonic profiling probe swings on the touch pad, dx (t) ═ X (t) -X (t-1), dy (t) ═ Y (t) — Y (t-1), X (t) and Y (t)) are respectively the X-axis component and the Y-axis component in the translation data sensed by the touch pad at the time t in the touch pad coordinate system; acos () is an inverse cosine function and pi is the circumferential ratio.

5. The method for self-calibration of the pose of an ultrasonic profiling probe for teleoperation according to claim 3, wherein:

the Y-axis negative 14-quadrant pose calibration model is q (t) ═ -acos [ (dy (t)/D ] -pi/2,

the Y-axis negative 23-quadrant pose calibration model is q (t) ═ acos [ (dy (t)/D ] -pi/2,

the Y-axis forward 14-quadrant attitude calibration model is q (t) ═ -acos [ (dy (t)/D ] + pi/2,

the Y-axis forward 23-quadrant attitude calibration model is q (t) ═ acos [ (dy (t)/D ] + pi/2;

q (t) is the angle of the IMU sensor after the course angle self-calibration at the moment t; d ═ sqrt [ dx (t) × (t) + dy (t) × (t) ((t), dx (t)) and dy (t)) are respectively the X-axis component and the Y-axis component of the translation direction in the translation direction vector at the time t when the ultrasonic profiling probe swings on the touch pad, dx (t) ═ X (t) -X (t-1), dy (t) ═ Y (t) — Y (t-1), X (t) and Y (t)) are respectively the X-axis component and the Y-axis component in the translation data sensed by the touch pad at the time t in the touch pad coordinate system; acos () is an inverse cosine function and pi is the circumferential ratio.

6. The method for self-calibration of the pose of an ultrasonic profiling probe for teleoperation according to any one of claims 3 to 5, wherein: when the ultrasonic profiling probe swings around axes except the X axis and the Y axis on the touch pad, firstly decomposing multi-axis swing of the ultrasonic profiling probe into swing around the X axis and swing around the Y axis, and then comprehensively performing self-calibration on the course angle of the IMU sensor according to a posture calibration model corresponding to the X axis and a posture calibration model corresponding to the Y axis;

the attitude calibration model corresponding to the X axis is an X-axis negative 14-quadrant attitude calibration model or an X-axis negative 23-quadrant attitude calibration model or an X-axis positive 14-quadrant attitude calibration model or an X-axis positive 23-quadrant attitude calibration model; the posture calibration model corresponding to the Y axis is a Y axis negative 14-quadrant posture calibration model or a Y axis negative 23-quadrant posture calibration model or a Y axis positive 14-quadrant posture calibration model or a Y axis positive 23-quadrant posture calibration model.

7. The method for self-calibration of the pose of an ultrasonic profiling probe for teleoperation according to any one of claims 1 to 5, wherein: after the step of S1, the method further comprises the following steps,

judging whether the ultrasonic profiling probe swings on the touch pad and has translation operation or not according to real-time attitude data of the IMU sensor in an IMU coordinate system and real-time translation data of the touch pad in the touch pad coordinate system; if so, stopping the course of self-calibration of the course angle of the IMU sensor and ending the course; if not, the steps S2 to S4 are executed in sequence.

8. An ultrasonic profiling probe posture self-calibration system for teleoperation is characterized in that: the device is used for calibrating the course angle of an IMU sensor when an ultrasonic profiling probe embedded with the IMU sensor swings on a touch pad through an arc-shaped tail end, and comprises the following modules,

the attitude data and translation data acquisition module is used for contacting the touch pad at the arc-shaped tail end of the ultrasonic profiling probe, and acquiring real-time attitude data of the IMU sensor under an IMU coordinate system and real-time translation data of the touch pad under a touch pad coordinate system, which are induced by the touch pad, when the ultrasonic profiling probe swings on the touch pad; the data form of the real-time attitude data of the IMU sensor under the IMU coordinate system is specifically an attitude quaternion form;

the data transformation module is used for converting the real-time attitude data of the IMU sensor in the attitude quaternion form under an IMU coordinate system into real-time attitude data in the Euler angle form; calculating a translation direction vector of the ultrasonic profiling probe when swinging on the touch pad according to real-time translation data of the touch pad under a touch pad coordinate system;

the swinging parameter calculation module is used for judging a swinging axis and a swinging direction of the ultrasonic profiling probe when the ultrasonic profiling probe swings on the touch pad and a quadrant where the translation direction vector is located according to real-time attitude data in an Euler angle form of the IMU sensor and the translation direction vector of the ultrasonic profiling probe when the ultrasonic profiling probe swings on the touch pad;

and the attitude calibration module is used for carrying out self calibration on the course angle of the IMU sensor by utilizing an attitude calibration model which corresponds to the swinging axis, the swinging direction and the quadrant where the translation direction vector is located when the ultrasonic profiling probe swings on the touch pad.

9. A computer storage medium, characterized in that: comprising a memory and a computer program stored in said memory for implementing the method for teleoperational ultrasound profiling probe pose self-calibration as claimed in any of claims 1 to 7 when executed by a computer processor.

10. The utility model provides an ultrasonic profiling probe gesture self calibration device for teleoperation which characterized in that: the ultrasonic profiling device comprises a shell, wherein an industrial personal computer is installed in the shell, a touch panel is installed on the surface of the shell, and a movable ultrasonic profiling probe is configured on the touch panel; the industrial personal computer is internally provided with the computer storage medium as claimed in claim 9, and the touch pad is electrically connected with the industrial personal computer.

Technical Field

The invention relates to the field of ultrasonic probe calibration, in particular to a method, a system and a device for self-calibration of the posture of an ultrasonic profiling probe for teleoperation.

Background

In the teleoperation of robot ultrasonic scanning, a doctor holds an ultrasonic profiling probe embedded with an IMU sensor in a working room and controls the work of the robot-end ultrasonic probe through network transmission. The position and posture mapping precision from the ultrasonic profiling probe embedded with the IMU sensor to the ultrasonic probe at the tail end of the robot is important for obtaining clear ultrasonic image quality. In the practical application process, the rotation angle of the IMU sensor around the Z axis is inaccurate, but the rotation angles around the X axis and the Y axis are accurate, namely the ultrasonic profiling probe is hung in the air, and the direction of the ultrasonic profiling probe cannot be determined all the time, so that the course angle of the IMU sensor drifts, the mapping precision of the position and the posture of the ultrasonic profiling probe to the ultrasonic probe at the tail end of the robot is reduced, and the quality of the obtained ultrasonic image is not good.

Disclosure of Invention

The invention aims to solve the technical problem of providing a method, a system and a device for self-calibration of the attitude of an ultrasonic profiling probe for teleoperation, which can inhibit the drift of the course angle of an IMU sensor, thereby ensuring the mapping precision of the position and the attitude of the ultrasonic profiling probe to the ultrasonic probe at the tail end of a robot and further improving the quality of an obtained ultrasonic image.

The technical scheme for solving the technical problems is as follows: a self-calibration method of the posture of an ultrasonic copying probe for teleoperation is used for calibrating the course angle of an IMU sensor when the ultrasonic copying probe embedded with the IMU sensor swings on a touch pad through an arc-shaped tail end, and comprises the following steps,

s1, contacting the arc-shaped tail end of the ultrasonic profiling probe with the touch pad, and enabling the ultrasonic profiling probe to swing on the touch pad to obtain real-time posture data of the IMU sensor under an IMU coordinate system and real-time translation data of the touch pad under a touch pad coordinate system; the data form of the real-time attitude data of the IMU sensor under the IMU coordinate system is specifically an attitude quaternion form;

s2, converting the real-time attitude data of the IMU sensor in the form of attitude quaternion under an IMU coordinate system into real-time attitude data in the form of Euler angles; calculating a translation direction vector of the ultrasonic profiling probe when swinging on the touch pad according to real-time translation data of the touch pad under a touch pad coordinate system;

s3, judging a swing axis and a swing direction of the ultrasonic profiling probe when swinging on the touch pad and a quadrant where the translation direction vector is located according to real-time attitude data in the form of Euler angles of the IMU sensor and the translation direction vector of the ultrasonic profiling probe when swinging on the touch pad;

and S4, self-calibrating the heading angle of the IMU sensor by utilizing a posture calibration model which corresponds to the swinging axis, the swinging direction and the quadrant where the translation direction vector is located when the ultrasonic profiling probe swings on the touch pad.

Based on the above method for self-calibrating the posture of the ultrasonic profiling probe for teleoperation, the invention also provides a system for self-calibrating the posture of the ultrasonic profiling probe for teleoperation.

An ultrasonic profiling probe posture self-calibration system for teleoperation is used for calibrating the course angle of an IMU sensor when an ultrasonic profiling probe embedded with the IMU sensor swings on a touch pad through an arc-shaped tail end, and comprises the following modules,

the attitude data and translation data acquisition module is used for contacting the touch pad at the arc-shaped tail end of the ultrasonic profiling probe, and acquiring real-time attitude data of the IMU sensor under an IMU coordinate system and real-time translation data of the touch pad under a touch pad coordinate system, which are induced by the touch pad, when the ultrasonic profiling probe swings on the touch pad; the data form of the real-time attitude data of the IMU sensor under the IMU coordinate system is specifically an attitude quaternion form;

the data transformation module is used for converting the real-time attitude data of the IMU sensor in the attitude quaternion form under an IMU coordinate system into real-time attitude data in the Euler angle form; calculating a translation direction vector of the ultrasonic profiling probe when swinging on the touch pad according to real-time translation data of the touch pad under a touch pad coordinate system;

the swinging parameter calculation module is used for judging a swinging axis and a swinging direction of the ultrasonic profiling probe when the ultrasonic profiling probe swings on the touch pad and a quadrant where the translation direction vector is located according to real-time attitude data in an Euler angle form of the IMU sensor and the translation direction vector of the ultrasonic profiling probe when the ultrasonic profiling probe swings on the touch pad;

and the attitude calibration module is used for carrying out self calibration on the course angle of the IMU sensor by utilizing an attitude calibration model which corresponds to the swinging axis, the swinging direction and the quadrant where the translation direction vector is located when the ultrasonic profiling probe swings on the touch pad.

Based on the above method for self-calibration of the posture of the ultrasonic profiling probe for teleoperation, the invention also provides a computer storage medium.

A computer storage medium comprising a memory and a computer program stored in the memory for, when executed by a computer processor, implementing the method for teleoperational ultrasound profiling probe pose self-calibration as described above.

Based on the computer storage medium, the invention also provides an ultrasonic profiling probe posture self-calibration device for teleoperation.

An ultrasonic profiling probe posture self-calibration device for teleoperation comprises a shell, wherein an industrial personal computer is installed in the shell, a display screen and a touch pad are installed on the surface of the shell, and a movable ultrasonic profiling probe is configured on the touch pad; the industrial personal computer is internally provided with the computer storage medium, and the display screen and the touch pad are electrically connected with the industrial personal computer.

The invention has the beneficial effects that: in the method, the system and the device for self-calibration of the posture of the ultrasonic profiling probe for teleoperation, a linear track is formed when the arc-shaped tail end of the ultrasonic profiling probe swings on the touch pad, and the direction of the linear track in the plane of a coordinate system of the touch pad can represent the rotation angle of the ultrasonic profiling probe around a Z axis, namely the course angle of an IMU sensor; therefore, the invention utilizes the structural characteristics of the ultrasonic profiling probe to calibrate the course angle of the IMU sensor according to the linear direction formed when the ultrasonic profiling probe swings on the touch pad, and realizes the automatic elimination of the IMU course drift, thereby ensuring the mapping precision of the position and the gesture from the ultrasonic profiling probe to the ultrasonic probe at the tail end of the robot, and further improving the quality of the obtained ultrasonic image.

Drawings

FIG. 1 is a flow chart of a method for self-calibration of the attitude of an ultrasonic profiling probe for teleoperation according to the present invention;

FIG. 2 is a state diagram of the ultrasonic profiling probe contacting the touch pad;

FIG. 3 is a state diagram of the ultrasonic profiling probe oscillating on the touch pad;

FIG. 4 is a schematic block diagram of a method for self-calibration of the attitude of an ultrasonic probe for teleoperation according to the present invention;

FIG. 5(a) is a schematic diagram of the directional coordinates of the rotation of the acoustic profiling probe about various axes from above;

FIG. 5(b) is a schematic view of the ultrasonic profiling probe rotated about the X-axis from the rear view;

FIG. 5(c) is a schematic diagram of a side view of an ultrasound profiling probe rotated about the Y-axis;

FIG. 6 is a block diagram of the self-calibration system for the attitude of an ultrasonic probe for teleoperation according to the present invention;

fig. 7 is a schematic structural diagram of an ultrasonic profiling probe posture self-calibration device for teleoperation according to the present invention.

In the drawings, the components represented by the respective reference numerals are listed below:

1. ultrasonic profiling probe, 2, touch pad.

Detailed Description

The principles and features of this invention are described below in conjunction with the following drawings, which are set forth by way of illustration only and are not intended to limit the scope of the invention.

As shown in FIG. 1, a method for self-calibration of the posture of an ultrasonic probe for teleoperation, which is used for calibrating the course angle of an IMU sensor when the ultrasonic probe embedded with the IMU sensor swings on a touch pad through a circular-arc tail end, comprises the following steps,

s1, contacting the arc-shaped tail end of the ultrasonic profiling probe with the touch pad, and enabling the ultrasonic profiling probe to swing on the touch pad to obtain real-time posture data of the IMU sensor under an IMU coordinate system and real-time translation data of the touch pad under a touch pad coordinate system; the data form of the real-time attitude data of the IMU sensor under the IMU coordinate system is specifically an attitude quaternion form;

s2, converting the real-time attitude data of the IMU sensor in the form of attitude quaternion under an IMU coordinate system into real-time attitude data in the form of Euler angles; calculating a translation direction vector of the ultrasonic profiling probe when swinging on the touch pad according to real-time translation data of the touch pad under a touch pad coordinate system;

s3, judging a swing axis and a swing direction of the ultrasonic profiling probe when swinging on the touch pad and a quadrant where the translation direction vector is located according to real-time attitude data in the form of Euler angles of the IMU sensor and the translation direction vector of the ultrasonic profiling probe when swinging on the touch pad;

and S4, self-calibrating the heading angle of the IMU sensor by utilizing a posture calibration model which corresponds to the swinging axis, the swinging direction and the quadrant where the translation direction vector is located when the ultrasonic profiling probe swings on the touch pad.

In this particular embodiment, as shown in fig. 2 and 3: contacting the arc-shaped tail end of the ultrasonic profiling probe 1 with the touch pad 2, wherein the state of the ultrasonic profiling probe 1 when contacting the touch pad 2 is shown in figure 2, an XY coordinate system represents a touch pad coordinate system, and an XYZ coordinate system represents an IMU coordinate system; one state in which the ultrasonic profiling probe 1 is oscillated on the touch pad 2 and the ultrasonic profiling probe 1 is oscillated on the touch pad 2 is shown in fig. 3, where a straight arrow represents a straight track formed by the oscillation, and an arc arrow represents an oscillation direction. When the ultrasonic profiling probe is rotated on the touch pad in situ, the arc part at the tail end of the ultrasonic profiling probe forms a straight line track on the touch pad, and the direction of the straight line in the plane of the coordinate system of the touch pad represents the rotation angle of the profiling probe around the Z axis, namely the heading angle of the IMU. Therefore, the course angle of the IMU can be calibrated according to the linear direction formed when the profiling probe swings on the touch pad, and the IMU course drift can be automatically eliminated.

FIG. 4 is a schematic block diagram of a method for self-calibration of the attitude of an ultrasonic probe for teleoperation according to the present invention; in this particular embodiment:

the swinging axis of the ultrasonic profiling probe when swinging on the touch pad is one of an X axis, a Y axis and multiple axes; wherein, the swinging axis of the ultrasonic profiling probe when swinging on the touch pad is an X axis, which represents that the ultrasonic profiling probe swings around the X axis on the touch pad, the swinging axis of the ultrasonic profiling probe when swinging on the touch pad is a Y axis, which represents that the ultrasonic profiling probe swings around the Y axis on the touch pad, the swinging axis of the ultrasonic profiling probe when swinging on the touch pad is a plurality of axes, which represents that the ultrasonic profiling probe swings around axes except the X axis and the Y axis on the touch pad; the swinging direction of the ultrasonic profiling probe when swinging on the touch pad comprises a positive direction and a negative direction; the quadrants where the translation direction vector of the ultrasonic profiling probe is located when the ultrasonic profiling probe swings on the touch pad include 14 quadrants and 23 quadrants.

FIG. 5(a) is a schematic diagram of the directional coordinates of the rotation of the ultrasound profiling probe about each axis in a top view, FIG. 5(b) is a schematic diagram of the rotation of the ultrasound profiling probe about the X-axis in a back view, and FIG. 5(c) is a schematic diagram of the rotation of the ultrasound profiling probe about the Y-axis in a side view; with reference to fig. 5(a), 5(b), and 5(c), a posture calibration model corresponding to the pivot axis, the pivot direction, and the quadrant in which the translation direction vector is located when the ultrasound profiling probe pivots on the touch pad as described below can be obtained.

Specifically, the attitude calibration models which correspond to the swing axis, the swing direction and the quadrant in which the translation direction vector is located when the ultrasonic profiling probe swings on the touch pad together include an X-axis negative 14-quadrant attitude calibration model, an X-axis negative 23-quadrant attitude calibration model, an X-axis positive 14-quadrant attitude calibration model, an X-axis positive 23-quadrant attitude calibration model, a Y-axis negative 14-quadrant attitude calibration model, a Y-axis negative 23-quadrant attitude calibration model, a Y-axis positive 14-quadrant attitude calibration model, a Y-axis positive 23-quadrant attitude calibration model and a multi-axis attitude calibration model.

In this particular embodiment:

the negative-X-axis 14-quadrant attitude calibration model is q (t) ═ -acos [ (dy (t)/D ];

the negative-going 23-quadrant attitude calibration model of the X axis is q (t) ═ acos [ (dy (t)/D ];

the X-axis forward 14-quadrant attitude calibration model is Q (t) ═ pi-acos [ (dy (t)/D ];

the X-axis forward 23-quadrant attitude calibration model is q (t) ═ acos [ (dy (t)/D ] -pi;

the Y-axis negative 14-quadrant attitude calibration model is q (t) ═ -acos [ (dy (t)/D ] -pi/2;

the Y-axis negative 23-quadrant attitude calibration model is q (t) ═ acos [ (dy (t)/D ] -pi/2;

the Y-axis forward 14-quadrant attitude calibration model is Q (t) ═ -acos [ (dy (t)/D ] + pi/2;

the Y-axis forward 23-quadrant attitude calibration model is q (t) ═ acos [ (dy (t)/D ] + pi/2;

wherein, q (t) is an angle obtained by self-calibrating the heading angle of the IMU sensor at time t (specifically, q (t) is a true heading angle calculated by the attitude calibration model, which is to calibrate the heading angle output by the IMU sensor to the value); d ═ sqrt [ dx (t) × (t) + dy (t) × (t) ((t), dx (t)) and dy (t)) are respectively the X-axis component and the Y-axis component of the translation direction in the translation direction vector at the time t when the ultrasonic profiling probe swings on the touch pad, dx (t) ═ X (t) -X (t-1), dy (t) ═ Y (t) — Y (t-1), X (t) and Y (t)) are respectively the X-axis component and the Y-axis component in the translation data sensed by the touch pad at the time t in the touch pad coordinate system; acos () is an inverse cosine function and pi is the circumferential ratio.

In this particular embodiment:

when the ultrasonic profiling probe swings around axes except the X axis and the Y axis on the touch pad, firstly decomposing multi-axis swing of the ultrasonic profiling probe into swing around the X axis and swing around the Y axis, and then comprehensively performing self-calibration on the course angle of the IMU sensor according to a posture calibration model corresponding to the X axis and a posture calibration model corresponding to the Y axis;

the attitude calibration model corresponding to the X axis is an X-axis negative 14-quadrant attitude calibration model or an X-axis negative 23-quadrant attitude calibration model or an X-axis positive 14-quadrant attitude calibration model or an X-axis positive 23-quadrant attitude calibration model; the posture calibration model corresponding to the Y axis is a Y axis negative 14-quadrant posture calibration model or a Y axis negative 23-quadrant posture calibration model or a Y axis positive 14-quadrant posture calibration model or a Y axis positive 23-quadrant posture calibration model.

In this particular embodiment:

after the step of S1, the method further comprises the following steps,

judging whether the ultrasonic profiling probe swings on the touch pad and has translation operation or not according to real-time attitude data of the IMU sensor in an IMU coordinate system and real-time translation data of the touch pad in the touch pad coordinate system; if so, stopping the course of self-calibration of the course angle of the IMU sensor and ending the course; if not, the steps S2 to S4 are executed in sequence.

Specifically, the basic idea of self-calibration of the course angle of the IMU sensor is to calibrate course angle drift according to a linear track formed by swinging the arc-shaped tail end of the ultrasonic profiling probe on the touch pad. However, when the probe is swung and the translation operation is carried out, the direction vector acquired from the touch pad cannot represent the rotation angle of the Z axis of the ultrasonic profiling probe; at this time, it is necessary to determine the scenes where the translation and rotation exist simultaneously, so as to avoid the false touch of the IMU for automatic calibration.

Operations that may occur when operating an ultrasound profiling probe for automatic IMU calibration mis-touches include:

1. the rotation direction and the translation direction are opposite, namely, the rotation direction and the translation direction are translated forwards, but the rotation direction swings backwards; this results in a calculated IMU heading angle that differs by 180 ° from the true heading angle. Considering that the actual IMU sensor does not drift abruptly by 180 °, for this case it can be solved by constraining the maximum degree of drift of the heading angle.

2. The rotation direction and the translation direction have a certain included angle (which is difficult to realize in practice), that is, the rotation in one direction and the translation in a more deviated direction are realized simultaneously. This results in a deviation of 90 ° at most. The rotation direction and the translation direction have a certain included angle, so that the translation distance calculated according to the rotation angle is smaller than the real translation distance measured by the touch pad, and the condition can be filtered according to the principle.

Based on the above method for self-calibrating the posture of the ultrasonic profiling probe for teleoperation, the invention also provides a system for self-calibrating the posture of the ultrasonic profiling probe for teleoperation.

As shown in FIG. 6, the system for self-calibration of the posture of an ultrasonic probe for teleoperation, which is used for calibrating the course angle of an IMU sensor when the ultrasonic probe embedded with the IMU sensor swings on a touch pad through a circular-arc tail end, comprises the following modules,

the attitude data and translation data acquisition module is used for contacting the touch pad at the arc-shaped tail end of the ultrasonic profiling probe, and acquiring real-time attitude data of the IMU sensor under an IMU coordinate system and real-time translation data of the touch pad under a touch pad coordinate system, which are induced by the touch pad, when the ultrasonic profiling probe swings on the touch pad; the data form of the real-time attitude data of the IMU sensor under the IMU coordinate system is specifically an attitude quaternion form;

the data transformation module is used for converting the real-time attitude data of the IMU sensor in the attitude quaternion form under an IMU coordinate system into real-time attitude data in the Euler angle form; calculating a translation direction vector of the ultrasonic profiling probe when swinging on the touch pad according to real-time translation data of the touch pad under a touch pad coordinate system;

the swinging parameter calculation module is used for judging a swinging axis and a swinging direction of the ultrasonic profiling probe when the ultrasonic profiling probe swings on the touch pad and a quadrant where the translation direction vector is located according to real-time attitude data in an Euler angle form of the IMU sensor and the translation direction vector of the ultrasonic profiling probe when the ultrasonic profiling probe swings on the touch pad;

and the attitude calibration module is used for carrying out self calibration on the course angle of the IMU sensor by utilizing an attitude calibration model which corresponds to the swinging axis, the swinging direction and the quadrant where the translation direction vector is located when the ultrasonic profiling probe swings on the touch pad.

Based on the above method for self-calibration of the posture of the ultrasonic profiling probe for teleoperation, the invention also provides a computer storage medium.

A computer storage medium comprising a memory and a computer program stored in the memory for, when executed by a computer processor, implementing the method for teleoperational ultrasound profiling probe pose self-calibration as described above.

Based on the computer storage medium, the invention also provides an ultrasonic profiling probe posture self-calibration device for teleoperation.

As shown in fig. 7, the attitude self-calibration device for the ultrasonic profiling probe for teleoperation comprises a casing, wherein an industrial personal computer is installed in the casing, a touch pad 2 is installed on the surface of the casing, and a movable ultrasonic profiling probe 1 is configured on the touch pad 2; the industrial personal computer is internally provided with the computer storage medium, and the touch pad 2 is electrically connected with the industrial personal computer.

In this embodiment, the surface of the housing is further provided with a display screen, and the display screen is electrically connected with the industrial personal computer and used for displaying the ultrasonic image.

In the specific embodiment, an industrial personal computer is connected and communicated with a remote robot through a network, an ultrasonic probe is fixed at the tail end of the robot, and the industrial personal computer controls the remote robot to drive the ultrasonic probe at the tail end to work by acquiring data information of the ultrasonic profiling probe moving on a touch pad; when the ultrasonic profiling probe moves on the touch pad, the posture self-calibration method can ensure the position and posture mapping precision from the ultrasonic profiling probe to the ultrasonic probe at the tail end of the robot, thereby improving the quality of the obtained ultrasonic image.

In the method, the system and the device for self-calibration of the posture of the ultrasonic profiling probe for teleoperation, a linear track is formed when the arc-shaped tail end of the ultrasonic profiling probe swings on the touch pad, and the direction of the linear track in the plane of a coordinate system of the touch pad can represent the rotation angle of the ultrasonic profiling probe around a Z axis, namely the course angle of an IMU sensor; therefore, the invention utilizes the structural characteristics of the ultrasonic profiling probe to calibrate the course angle of the IMU sensor according to the linear direction formed when the ultrasonic profiling probe swings on the touch pad, and realizes the automatic elimination of the IMU course drift, thereby ensuring the mapping precision of the position and the posture from the ultrasonic profiling probe to the ultrasonic probe at the tail end of the robot, and further improving the quality of the obtained ultrasonic image.

The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.