一种姿态平滑方法及系统、电子设备与流程

本发明涉及姿态平滑方法，具体涉及一种姿态平滑方法及系统、电子设备。

背景技术：

1、在骨科手术机器人相关产品中，通常使用ndi相机对患者跟踪器位姿进行实时获取，以便控制机械臂对患者病灶进行实时跟踪，ndi相机的输出数据存在一定程度的噪声干扰，在使用其测量到的姿态数据进行实时跟踪前，需要对姿态数据进行平滑处理，一方面可以提高机械臂跟踪的稳定性，另一方面可以降低机械臂的振动，使运动更加流畅，未经平滑处理的姿态数据若直接使用，不光会增加机械臂的振动，也有可能导致较差的截骨精度，降低产品的系统级精度水平，另一方面，对于骨科手术机器人相关产品中软件界面上的病人、截骨工具实时显示功能，也要求系统获取相应位姿数据后，对位姿数据进行平滑处理，再进行显示，以提高显示的连续性，保证显示的图像是真实无噪声的。

2、姿态数据有多种表示形式，一般可通过欧拉角、轴角、旋转矩阵或四元数表示，其中，欧拉角表示姿态具有非唯一性，不同的欧拉角可以表示相同的姿态，在进行平滑时容易产生数值跳变问题，且欧拉角表示的姿态会出现万向节锁问题，影响与其他姿态表示形式的互相转换，轴角法是用空间旋转轴与绕轴旋转角度表示姿态，当绕轴旋转角度在0°(360°)附近时，容易产生数据的跳变，影响姿态平滑的数值解算,旋转矩阵需要9个变量来存储姿态数据，且其每列数据间存在正交关系，直接对数据进行平滑操作往往会影响其单位性和正交性,相比之下，四元数较为适合作为姿态数据平滑的原始数据形式,但常规的对四元数单通道数据的直接处理，也会影响四元数表示姿态的单位性，并且一般的四元数处理方法还会存在姿态数据的连续性(跳变)问题。

3、申请号为us9492237b2的专利公开了对位置数据的平滑，其采用三个位置分量分别通过低通滤波器进行滤波的方法实现；对姿态数据的平滑，采用先将姿态数据统一转换为四元数，再对四元数的四个分量分别通过低通滤波器进行滤波的方法实现,该方法容易存在平滑姿态跳变问题，且四个分量分别滤波，从原理上会产生相位同步问题，使得平滑后姿态数据失真,另外，滤波后的四个分量合成的四元数并非单位四元数，需要再次进行归一化才能用于表示姿态。

4、申请号为cn116476073a的专利公开了基于卡尔曼滤波原理的姿态平滑方法，使用机械臂反馈数据融合相机位姿数据，进行病人实际位姿估计，以期降低病人示踪器的噪声水平，卡尔曼滤波原理假设数据的噪声复合高斯分布，然而在医疗机器人领域，病人示踪器的振动往往非符合正态分布的随机振动，而是由于摆锯截骨或磨锉磨削引起的被动振动，具有一定的振动噪声特性,因此该方法无法实现姿态数据的有效平滑与精度提升，并会带来较大的不必要计算量。

5、因此，现有技术还有待进一步发展。

技术实现思路

1、本发明的目的在于克服上述技术不足，提供一种姿态平滑方法及系统、电子设备，以解决现有技术存在的问题。

2、为达到上述技术目的，根据本发明的第一方面，本发明提供了一种姿态平滑方法，包括：

3、s100、获取当前时刻原始姿态的姿态四元数，根据所获取的姿态四元数更新姿态缓存区，并根据预设缓存区权重系数计算方法确定缓存区的第一权重系数；

4、s200、通过预设姿态加权平滑方法计算原始姿态四元数的第一加权四元数累加和矩阵，对第一加权四元数累加和矩阵进行归一化处理，得到第二加权四元数累加和矩阵，并计算平滑后的姿态四元数；

5、s300、通过预设平滑姿态后处理方法对平滑后的姿态四元数进行平滑姿态后处理，得到平滑姿态后处理后的姿态四元数，并将处理后的姿态四元数作为当前时刻的平滑姿态四元数进行输出。

6、具体的，所述根据所获取的姿态四元数更新姿态缓存区，包括以下任意一种：

7、先入先出队列存储更新、移动数组索引存储更新。

8、具体的，所述根据预设缓存区权重系数计算方法确定缓存区的第一权重系数，具体步骤包括：

9、将缓存区的第一权重系数表示为，具体计算过程如下：

10、首先确定缓存区的第一权重系数的个数，具体计算公式如下：

11、；

12、；

13、；

14、其中，是平滑频率带内波动百分比，是过滤频率带内最小衰减，是采样频率，是平滑频率带最大值，是过滤频率带最小值，a、b为过程变量；

15、其次，计算过程设计系数，计算公式如下：

16、；

17、由以下公式确定缓存区的第一权重系数的分布系数，

18、；

19、其中，为过程变量，，为零阶第一类修正贝塞尔函数，则可由分布系数及以下公式计算缓存区的第一权重系数：

20、；

21、其中，为以采样频率的一半进行归一化后的过渡频率带中值；

22、则由平滑所引起的滞后时间可表示为：

23、；

24、其中，为由平滑所引起的滞后时间，为采样的时间间隔。

25、具体的，所述通过预设姿态加权平滑方法计算原始姿态四元数的第一加权四元数累加和矩阵，包括：

26、设姿态缓存区中，n个以四元数表示的姿态为，将个姿态的第一权重系数设为，则根据如下公式计算原始姿态的第一加权四元数累加和矩阵，

27、；

28、其中，为原始姿态的第一加权四元数累加和矩阵。

29、具体的，所述对第一加权四元数累加和矩阵进行归一化处理，得到第二加权四元数累加和矩阵，并计算平滑后的姿态四元数，包括：

30、计算第一权重系数的累加和，计算公式如下：

31、；

32、对第一加权四元数累加和矩阵进行归一化处理，得到第二加权四元数累加和矩阵，计算公式如下：

33、；

34、其中，为第二加权四元数累加和矩阵。

35、具体的，所述对第一加权四元数累加和矩阵进行归一化处理，得到第二加权四元数累加和矩阵，并计算平滑后的姿态四元数，还包括：

36、计算第二加权四元数累加和矩阵的特征值与特征向量，获取最大实特征值对应的特征向量，将最大实特征值对应的特征向量作为平滑后的姿态四元数，计算公式如下：

37、；

38、其中，为平滑后的姿态四元数，，代表待求解的单位四元数。

39、具体的，所述通过预设平滑姿态后处理方法对平滑后的姿态四元数进行平滑姿态后处理，得到平滑姿态后处理后的姿态四元数，并将处理后的姿态四元数作为当前时刻的平滑姿态四元数进行输出，包括：

40、将当前时刻的参考姿态表示为，根据平滑后的姿态四元数，对平滑后的姿态四元数进行平滑姿态后处理，得到处理后的姿态四元数，具体计算公式如下：

41、；

42、其中，。

43、具体的，所述对平滑后的姿态四元数进行平滑姿态后处理，得到平滑姿态后处理后的姿态四元数，并将处理后的姿态四元数作为当前时刻的平滑姿态四元数进行输出，还包括：

44、将平滑姿态后处理后的姿态四元数作为当前时刻的平滑姿态四元数进行输出，完成对参考姿态四元数的更新，以及将平滑姿态后处理后的姿态四元数作为下一时刻的参考姿态四元数。

45、根据本发明的第二方面，提供一种姿态平滑系统，包括：

46、获取模块：用于获取当前时刻原始姿态的姿态四元数，根据所获取的姿态四元数更新姿态缓存区，并根据缓存区权重系数计算方法确定缓存区的第一权重系数；

47、控制模块：用于通过预设姿态加权平滑方法计算原始姿态四元数的第一加权四元数累加和矩阵，对第一加权四元数累加和矩阵进行归一化处理，得到第二加权四元数累加和矩阵，并计算平滑后的姿态四元数；或用于通过预设平滑姿态后处理方法对平滑后的姿态四元数进行平滑姿态后处理，得到平滑姿态后处理后的姿态四元数，并将处理后的姿态四元数作为当前时刻的平滑姿态四元数进行输出。

48、根据本发明的第三方面，提供一种电子设备，包括：存储器；以及处理器，所述存储器上存储有计算机可读指令，所述计算机可读指令被所述处理器执行时实现上述的姿态平滑方法。

49、有益效果：

50、本发明提出了一种姿态平滑器，通过预设姿态加权平滑方法对存在噪声的四元数姿态进行实时平滑，保证了输出的姿态数据为单位四元数，且可以预先设定各输入姿态在缓存区中的所占比重，通过预设缓存区权重系数计算方法对缓存区的权重系数进行优化，并能准确评估平滑导致的滞后时间，大大提高了姿态平滑的有效性，利用预设平滑姿态后处理方法对平滑后的姿态四元数进行后处理，使得本发明提出的姿态平滑器具有线性相位，保证了在姿态数据无失真、无跳变的情况下，降低姿态数据噪声，减少时间滞后，以满足姿态实时平滑去噪的相关功能需求，极大的提高了使用机械臂跟踪的稳定性，使运动更加流畅，很大程度上提高了本发明的智能化程度、可用性和可靠性，大大拓展了本发明的应用场景。