具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

如图1所示，一种遥操作用超声仿型探头姿态自校准方法，用于校准内嵌有IMU传感器的超声仿型探头通过圆弧状末端在触控板上摆动时IMU传感器的航向角，包括如下步骤，

S1，将所述超声仿型探头的圆弧状末端接触所述触控板，并使所述超声仿型探头在所述触控板上摆动，获取所述IMU传感器在IMU坐标系下的实时姿态数据和所述触控板感应的在触控板坐标系下的实时平移数据；其中，所述IMU传感器在IMU坐标系下的实时姿态数据的数据形式具体为姿态四元数形式；

S2，将所述IMU传感器在IMU坐标系下的姿态四元数形式的实时姿态数据转换成欧拉角形式的实时姿态数据；根据所述触控板感应的在触控板坐标系下的实时平移数据，计算出所述超声仿型探头在所述触控板上摆动时的平移方向向量；

S3，根据所述IMU传感器的欧拉角形式的实时姿态数据以及所述超声仿型探头在所述触控板上摆动时的平移方向向量，判断出所述超声仿型探头在所述触控板上摆动时的摆动轴、摆动方向以及所述平移方向向量所在的象限；

S4，利用所述超声仿型探头在所述触控板上摆动时的摆动轴、摆动方向以及所述平移方向向量所在的象限共同对应的姿态校准模型，对所述IMU传感器的航向角进行自校准。

在本具体实施例中，如图2和图3所示：将超声仿型探头1的圆弧状末端接触触控板2，超声仿型探头1接触触控板2时的状态如图2所示，XY坐标系代表触控板坐标系，XYZ坐标系代表IMU坐标系；使超声仿型探头1在触控板2上摆动，超声仿型探头1在触控板2上摆动的一种状态如图3所示，直线箭头代表摆动形成的直线轨迹，弧形箭头代表摆动方向。在触控板上原地旋转超声仿型探头时，超声仿型探头末端圆弧部分会在触控板上形成一条直线轨迹，该直线在触控板坐标系平面内的方向即表示仿型探头绕Z轴的旋转角度，也就是IMU的航向角。因此，我们可以根据仿型探头在触控板上摆动时形成的直线方向，校准IMU的航向角，实现对IMU航向漂移的自动消除。

图4为本发明一种遥操作用超声仿型探头姿态自校准方法的原理框图；在本具体实施例中：

所述超声仿型探头在所述触控板上摆动时的摆动轴为X轴、Y轴和多轴中的一种；其中，所述超声仿型探头在所述触控板上摆动时的摆动轴为X轴，表征所述超声仿型探头在所述触控板上绕X轴摆动，所述超声仿型探头在所述触控板上摆动时的摆动轴为Y轴，表征所述超声仿型探头在所述触控板上绕Y轴摆动，所述超声仿型探头在所述触控板上摆动时的摆动轴为多轴，表征所述超声仿型探头在所述触控板上绕除X轴以及Y轴以外的轴摆动；所述超声仿型探头在所述触控板上摆动时的摆动方向包括正向和负向；所述超声仿型探头在所述触控板上摆动时的所述平移方向向量所在的象限包括14象限和23象限。

图5(a)为俯视的超声仿型探头绕各轴旋转的方向坐标示意图，图5(b)为后视的超声仿型探头绕X轴旋转的示意图，图5(c)为侧视的超声仿型探头绕Y轴旋转的示意图；结合图5(a)、图5(b)和图5(c)，可以得出如下所述超声仿型探头在所述触控板上摆动时的摆动轴、摆动方向以及所述平移方向向量所在的象限共同对应的姿态校准模型。

具体的，所述超声仿型探头在所述触控板上摆动时的摆动轴、摆动方向以及所述平移方向向量所在的象限共同对应的姿态校准模型包括X轴负向14象限姿态校准模型、X轴负向23象限姿态校准模型、X轴正向14象限姿态校准模型、X轴正向23象限姿态校准模型、Y轴负向14象限姿态校准模型、Y轴负向23象限姿态校准模型、Y轴正向14象限姿态校准模型、Y轴正向23象限姿态校准模型和多轴姿态校准模型。

在本具体实施例中：

所述X轴负向14象限姿态校准模型为，Q(t)＝-acos[(dy(t)/D]；

所述X轴负向23象限姿态校准模型为，Q(t)＝acos[(dy(t)/D]；

所述X轴正向14象限姿态校准模型为，Q(t)＝π-acos[(dy(t)/D]；

所述X轴正向23象限姿态校准模型为，Q(t)＝acos[(dy(t)/D]-π；

所述Y轴负向14象限姿态校准模型为，Q(t)＝-acos[(dy(t)/D]-π/2；

所述Y轴负向23象限姿态校准模型为，Q(t)＝acos[(dy(t)/D]-π/2；

所述Y轴正向14象限姿态校准模型为，Q(t)＝-acos[(dy(t)/D]+π/2；

所述Y轴正向23象限姿态校准模型为，Q(t)＝acos[(dy(t)/D]+π/2；

其中，Q(t)为t时刻IMU传感器的航向角自校准后的角度(具体的，Q(t)为通过姿态校准模型计算得到的真实的航向角，姿态校准模型就是需要将IMU传感器输出的航向角校准为这个值)；D＝sqrt[dx(t)*dx(t)+dy(t)*dy(t)]，dx(t)和dy(t)分别为所述超声仿型探头在所述触控板上摆动时t时刻的平移方向向量中的平移方向X轴分量和平移方向Y轴分量，dx(t)＝x(t)-x(t-1)，dy(t)＝y(t)-y(t-1)，x(t)和y(t)分别为所述触控板感应的在触控板坐标系下t时刻的平移数据中的X轴分量和Y轴分量；acos()为反余弦函数，π为圆周率。

在本具体实施例中：

当所述超声仿型探头在所述触控板上绕除X轴以及Y轴以外的轴摆动时，先将超声仿型探头的多轴摆动进行分解，分解成绕X轴摆动以及绕Y轴摆动，然后根据X轴对应的姿态校准模型以及Y轴对应的姿态校准模型对所述IMU传感器的航向角综合进行自校准；

其中，X轴对应的姿态校准模型为X轴负向14象限姿态校准模型或X轴负向23象限姿态校准模型或X轴正向14象限姿态校准模型或X轴正向23象限姿态校准模型；Y轴对应的姿态校准模型为Y轴负向14象限姿态校准模型或Y轴负向23象限姿态校准模型或Y轴正向14象限姿态校准模型或Y轴正向23象限姿态校准模型。

在本具体实施例中：

在所述S1后，还包括如下步骤，

根据所述IMU传感器在IMU坐标系下的实时姿态数据和所述触控板感应的在触控板坐标系下的实时平移数据，判断所述超声仿型探头在所述触控板上摆动的同时是否存在平移操作；若是，则停止对所述IMU传感器的航向角进行自校准的过程并结束；若否，则依次执行所述S2至所述S4。

具体的，IMU传感器的航向角进行自校准的基本思路是根据超声仿型探头圆弧状末端在触控板上摆动形成的直线轨迹来校准航向角漂移的。但当摆动探头的同时进行平移操作时，从触控板获取的方向向量就不能表示超声仿型探头Z轴的旋转角了；此时，就需要对这些平移和旋转同时存在的场景进行判断，以避免误触IMU自动校准。

在操作超声仿型探头时，可能会出现的IMU自动校准误触的操作包括：

1.旋转方向和平移方向相反，即向前平移，但向后摆动；这样会造成计算的IMU航向角与真实航向角差180°。考虑到实际IMU传感器不会突然漂移180°，因此对于这种情况，可通过约束航向角的最大漂移度数来解决。

2.旋转方向和平移方向存在一定夹角(实际中比较难实现)，即往一个方向旋转同时往一个比较偏的方向平移。这样最多会造成90°的偏差。旋转方向和平移方向存在一定夹角，会造成根据旋转角计算出来的平移距离小于触控板测得的真实平移距离，根据该原则可对该类情况进行过滤。

基于上述一种遥操作用超声仿型探头姿态自校准方法，本发明还提供一种遥操作用超声仿型探头姿态自校准系统。

如图6所示，一种遥操作用超声仿型探头姿态自校准系统，用于校准内嵌有IMU传感器的超声仿型探头通过圆弧状末端在触控板上摆动时IMU传感器的航向角，包括如下模块，

姿态数据及平移数据获取模块，其用于在所述超声仿型探头的圆弧状末端接触所述触控板，且所述超声仿型探头在所述触控板上摆动时，获取所述IMU传感器在IMU坐标系下的实时姿态数据和所述触控板感应的在触控板坐标系下的实时平移数据；其中，所述IMU传感器在IMU坐标系下的实时姿态数据的数据形式具体为姿态四元数形式；

数据变换模块，其用于将所述IMU传感器在IMU坐标系下的姿态四元数形式的实时姿态数据转换成欧拉角形式的实时姿态数据；根据所述触控板感应的在触控板坐标系下的实时平移数据，计算出所述超声仿型探头在所述触控板上摆动时的平移方向向量；

摆动参数计算模块，其用于根据所述IMU传感器的欧拉角形式的实时姿态数据以及所述超声仿型探头在所述触控板上摆动时的平移方向向量，判断出所述超声仿型探头在所述触控板上摆动时的摆动轴、摆动方向以及所述平移方向向量所在的象限；

姿态校准模块，其用于利用所述超声仿型探头在所述触控板上摆动时的摆动轴、摆动方向以及所述平移方向向量所在的象限共同对应的姿态校准模型，对所述IMU传感器的航向角进行自校准。

基于上述一种遥操作用超声仿型探头姿态自校准方法，本发明还提供一种计算机存储介质。

一种计算机存储介质，包括存储器以及存储在所述存储器内的计算机程序，所述计算机程序被计算机处理器执行时用于实现如上述所述的遥操作用超声仿型探头姿态自校准方法。

基于上述一种计算机存储介质，本发明还提供一种遥操作用超声仿型探头姿态自校准装置。

如图7所示，一种遥操作用超声仿型探头姿态自校准装置，包括机壳，所述机壳内安装有工控机，所述机壳表面安装有触控板2，所述触控板2上配置有可移动的超声仿型探头1；所述工控机内安装有如上述所述的计算机存储介质，所述触控板2与所述工控机电连接。

在本具体实施例中，机壳表面还配置有显示屏，显示屏与工控机电连接，用于显示超声图像。

在本具体实施例中，工控机通过网络与远程的机器人连接并通信，机器人末端固定超声探头，工控机通过获取超声仿型探头在触控板上移动的数据信息控制远程的机器人带动末端的超声探头工作；而在超声仿型探头在触控板上移动时，通过本发明的姿态自校准方法可以保证超声仿型探头到机器人末端超声探头的位置和姿态映射精度，进而提高了获取的超声图像质量。

在本发明一种遥操作用超声仿型探头姿态自校准方法、系统及装置中，超声仿型探头的圆弧状末端在触控板上摆动时会形成一条直线轨迹，该直线在触控板坐标系平面内的方向即可表示超声仿型探头绕Z轴的旋转角度，也就是IMU传感器的航向角；因此，本发明利用超声仿型探头的结构特点，根据超声仿型探头在触控板上摆动时形成的直线方向来校准IMU传感器的航向角，实现对IMU航向漂移的自动消除，从而保证了超声仿型探头到机器人末端超声探头的位置和姿态映射精度，进而提高了获取的超声图像质量。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。