一种基于平行四边形集员估计的手术机器人轨迹定位方法与流程

本发明属于多传感器数据融合领域，特别设计了一种基于平行四边形集员估计的手术机器人轨迹定位方法。

背景技术：

1、随着机器人技术的快速发展，骨科手术机器人在微创截骨术中因切口小、出血少和术后恢复快而得到广泛使用。为了保证微创截骨术的安全性和可重复性，需要具有极高精度的轨迹跟踪，使得重复定位精度和导航精度小于1毫米。尽管截骨手术机器人具有自定位能力，但其自身的定位信息无法与复杂的手术环境相结合，导致术中轨迹存在定位偏差。因此，手术机器人定位需要通过外部传感器定位信息进行状态估计和定位。

2、常用的状态估计方法分为概率滤波估计算法和集员状态估计算法。其中，概率滤波器估计在很大程度上取决于噪声分布的准确信息，当噪声不满足假设分布条件或采样信息过少时，状态估计精度将受到严重影响。事实上，术中传感器噪声存在分布未知但有界特点，因此术中噪声很难通过某种特定分布进行准确建模。此外，传感器漂移、遮挡、某一时刻信息缺失也会造成概率滤波估计发散，影响机器人位置估计结果。因此，将未知但有界噪声建模的集员估计方法引入术中机器人轨迹估计，可以避免数据缺失带来的估计偏差，有效提升手术机器人位置估计精度。

3、现有的机器人状态集员估计方法大多基于椭球外包络形式进行状态集合描述，通过融合多个观测集合和状态集合中的定位信息进行机器人轨迹状态值更新与估计，并优化求解最小状态估计集合作为真值最优解。基于椭球的外包络集合在对状态真值和有界噪声进行建模的过程中引入冗余信息多、交集优化易发散，多椭球进行融合优化时很难得到唯一确定的最小体积椭球，次优融合结果导致机器人轨迹估计收敛慢、偏差大，限制了多传感器定位轨迹估计精度。

技术实现思路

1、为了解决上述背景技术提到的技术问题，本发明提出了一种基于平行四边形集员估计的手术机器人轨迹定位方法，通过多观测集员估计算法克服术中传感器故障、ndi观测漂移引起的定位偏差，利用有界噪声建模方法实现定位真值的平行四边形集合全包络以减小原包络中噪声集合引入的冗余信息，并利用平行四边形交集融合方程对集员框架下平行四边形集合进行求交与优化以解决集员估计优化不收敛问题，实现截骨机器人定位精度的提升。

2、为了实现上述目的，本发明采用的技术方案为：

3、一种基于平行四边形集员估计的手术机器人轨迹定位方法，包括以下步骤：

4、(1)标定两个ndi传感器，使机器人标记点运行范围在ndi传感器观测范围内，并依据规划轨迹和传感器参数构建任意k时刻观测模型yk；

5、(2)初始化机器人轨迹状态值x0，根据机器人运动轨迹对状态估计值xk进行运动状态建模，预测任意时刻状态估计值得到预测状态集合并求解任意时刻预测状态估计集合的平行四边形外包络，得到预测状态集合的外包络集合

6、(3)采集任意时刻ndi传感器观测位置信息，判断观测位置信息是否缺失，若缺失则默认下一时刻状态真值为预测状态集合求解得到的预测估计值，返回步骤(2)；若无缺失则利用观测模型和观测信息反解得到由观测量表示的一致状态集合及对应的外包络

7、(4)将步骤(3)得到的一致状态集合与步骤(2)得到的预测状态集合进行交集融合，求解融合后所有符合条件的交集集合及对应的外包络集合即求解所以满足条件的状态估计集合解集；

8、(5)对状态估计集合解集进行优化，求解最小面积状态估计集合，并将对应的最优外包络集合表示为pk(xk)，将pk(xk)中点所表示的估计真值输入步骤(2)中，作为下一时刻机器人轨迹状态值进行迭代，计算下一时刻状态预测集合；

9、(6)输出状态集合对应的最优外包络集合pk(xk)、状态估计集合xk、状态估计值xk和状态集合面积。

10、进一步的，步骤(1)中ndi传感器误差边界条件为依据机器人轨迹与ndi传感器观察值关系构建观测方程yk＝cxk+vk，其中xk为k时刻状态值，c为观测矩阵，vk为k时刻ndi传感器噪声；其中，为设定值。

11、进一步的，步骤(2)具体为：

12、构建机器人状态方程如下：

13、

14、其中，xk-1为k-1时刻机器人状态值，uk-1为k-1时刻手术机器人在轨迹估计中的轨迹插值，为常量，wk-1为k-1时刻状态噪声，满足有界条件为xk-1预测得到的下一时刻状态估计值，为设定值，a和b为方程系数；

15、然后求解预测估计值的平行四边形外包络：

16、在手术机器人轨迹规划中，在轨迹截骨平面建立位置坐标系，在位置坐标系中通过两条不平行直线的动态平移得到包络预测估计值的平行四边形p(θ,x0,e)：

17、

18、式中，θ＝(θ1,θ2)，x0＝(x0,y0)，e＝(e1,e2)；θ＝(θ1,θ2)为两条不平行直线与坐标系纵轴的夹角，(x,y)为两条不平行直线与纵轴的交点，e1,e2为平行四边形p(θ,x0,e)两条不平行直线距离中线距离，(x0,y0)为平行四边形p(θ,x0,e)的中心点，也为对应时刻预测的状态估计值

19、因此所有预测估计值形成的预测状态集合的外包络平行四边形集合表示为：

20、

21、式中，为预测状态集合中所有外包络平行四边形两边到中线距离集合，为预测状态集合中所有外包络平行四边形的外包络旋转角集合。

22、进一步的，步骤(3)中利用观测模型和观测信息反解得到由观测量表示的一致状态集合及对应的外包络具体过程为：

23、利用观测模型和ndi传感器采集到的位置信息求解得到一致状态和一致状态集合

24、

25、

26、利用平行四边形包络方法对一致状态集合进行描述：

27、

28、式中，为非规则包络的一致状态集合，x′k为平行四边形一致状态集合中心，为一致状态集合边界到中心线距离，表示一致状态方程的平行四边形外包络参数，ck为观测矩阵，表示观测噪声边界，vk表示噪声集合。

29、进一步的，步骤(4)具体过程如下：

30、利用预测状态集合和一致状态集合中包含的状态真值信息，分别为从模型角度和观测角度求解融合后所有符合条件的交集集合则两集合交集融合过程满足如下方程：

31、

32、其中，xk为k时刻的状态真值，为状态真值xk的非规则包络集合，为实现下一时刻真值估计集合求解，需将非规则集合包络为平行四边形状态估计集合：

33、

34、

35、式中，为k-1时刻由状态集合预测求得的平行四边形包络的预测估计集合，为规则包络的一致状态集合，p(conv(xk))为平行四边形包络的状态估计集合；

36、对平行四边形预测状态集合p(θk,xk,ek)和一致状态集合进行交集求解，得到如下融合方程：

37、

38、式中，θ＝(θ1,θ2)为状态集合p(θk,xk,ek)参数θk，(x,y)为状态集合参数xk，λ表示满足融合条件的不同面积估计集合组参数，pλ(θ′k,x′k,e′k,λ)表示融合后的带参状态估计值，θ′k,x′k,e′k分别表示交集融合后符合条件的平行四边形组参数，其中真值必被每一个集合组中的解集合包络；

39、求解上述交集方程可得：

40、

41、

42、

43、其中，参数θ求解中表示为p(θ)＝θ·i2，i2为二维单位矩阵。

44、进一步的，步骤(5)中对状态估计集合解集进行优化，求解最小面积状态估计集合，并将对应的最优外包络集合表示为pk(xk)，具体为：

45、

46、其中，λ为步骤(4)中的估计集合解集组参数，n表示状态值的维度。

47、采用上述技术方案带来的有益效果为：

48、本发明克服了术中视觉传感器定位点漂移、噪声干扰、镜头遮挡、信息缺失造成的定位误差过大问题，并在集员框架下减小了融合、优化引入的冗余估计信息，提高了术中机器人轨迹估计精度。