一种机械臂光学定位方法及装置

本发明涉及一种机械臂光学定位方法及装置。

背景技术：

1、当前超声引导下经皮穿刺活检技术是一种重要的诊断方式，该手术目前依靠医生徒手进行下针操作，之后再由医学超声影像进行确认，手术精度低、风险高，导致该手术时间长，容易引起并发症。为解决上述问题，手术机器人辅助下针已经有多年研究，其关键技术是解决机器人、穿刺器械、医学影响设备、外部测量设备等多系统间的高精度定位问题，以保证穿刺手术针的位置精度和姿态精度。

2、已有相近似方案论文名称为胸腔穿刺手术机器人的光学配准方法研究，参见图1，使标记物安装于机械臂末端持针装置，标记物上带有4个涂有红外反射涂层的小球可以强烈反射红外双目设备发出的红外线，从而标记物的位置姿态信息可以被红外设备稳定地跟踪。

3、对于机械臂末端的标记物与穿刺针的配准，该方案采用旋转方法配准针尖以及标记物，如图2。其方法具体为：将针尖位置保持在一个固定点上，然后旋转末端执行器，即将针尖作为校准点，穿刺针不同姿态下保持针尖位置固定，求出标记物与针尖的平移关系。

4、旋转关系可以通过主动测量末端执行器的导向管来进行求解，由于针的方向与导向管方向一致，那么一旦测得导向管方向，那么针的方向同样可以求解，如图3中主动探针工具的末端为一个圆球，圆球刚好可以卡在导向管的入口a和出口b，双目定位设备可以直接获取主动探针的末端位置，将末端置于在出入口处，这样就可以求得导管出入口的位置从而进一步求出手术针的方向。

5、另有中国专利公开号cn111956329a、名称为一种双臂机器人标定方法、系统、终端以及存储介质，公开了一种双臂机器人标定方法、系统，其通过设计和加工一个高精度的顶尖，将其安装到双臂机器人任一机械臂的末端法兰盘上，通过机械臂控制顶尖到达预设参考点，并计算参考点在机器人基座标系下的位置；计算公式为：

6、rprtip＝rter·erpmtip

7、式中erpmtip为顶尖到末端法兰盘坐标，可根据预设顶尖参考数获得；rter为末端法兰在机器人基座标系下的位姿，可以在机器人操作系统中直接读取。卸下尖顶将穿刺管末端执行器安装到双臂机器人任一法兰盘上，通过机械臂控制穿刺针到达预设参考点，并记录该末端法兰盘在机器人坐标系下的位姿矩阵。建立表达式：

8、erpntip＝inv(rter)·rprtip

9、erpntip为穿刺针在末端法兰盘坐标系下的坐标，对上式进行求逆可获得。参见图4，对于姿态获取，可通过控制穿刺针到任一个点，将该点设为针尖坐标系原点，计算出其在机器人基座标系下的坐标。通过机械臂控制穿刺针尖到达任意第二个点，定义该点为y轴上的点，计算出y轴方向向量。再通过同样方式计算出定义z轴的方向向量，通过两向量叉乘得出x轴方向向量，依次组合x、y、z三轴方向向量获得穿刺针姿态矩阵。

10、综上，现有技术中针尖触碰到物体时，接触力会使针变形，且机械臂重复性动作由于自身额定dh参数，存在一定误差。手动操作也会造成一定误差。

技术实现思路

1、本发明实施例提供了一种机械臂光学定位方法及装置，以至少解决现有引导机器人高精度定位时针尖形变导致误差的技术问题。

2、根据本发明的一实施例，提供了一种机械臂光学定位方法，包括以下步骤：

3、基于机械臂基座标、机械臂末端坐标设置多组初始点，将机械臂设立多个不同的初始点，分别沿着基于机械臂基座标系与机械臂末端坐标系的x、y、z轴移动若干次，记录在红外双目设备坐标下的标记物位移与旋转姿态的数据，同时记录机械臂末端位移和旋转姿态的数据；

4、计算出不同初始点沿基于机械臂基座标系与机械臂末端坐标系的x、y、z轴的平均单位向量，通过向量拼接，获得机械臂基座标与红外双目设备坐标的旋转姿态关系；

5、通过机械臂基座标、红外双目设备、标记物、机械臂末端坐标建立闭环转移矩阵公式，通过求取广义逆矩阵，获得机械臂基座标与红外双目设备以及标记物到机械臂末端的位移关系；

6、通过获得的机械臂基座标与红外双目设备坐标的旋转姿态关系、机械臂基座标与红外双目设备以及标记物到机械臂末端的位移关系与机械臂自身dh参数，计算出标记物的位移与姿态值，其值为计算值，将计算值与红外双目光学设备的参考值做差值；

7、通过对偶四元数方法，将差值迭代缩小至全局最小值，获得最优转换关系。

8、进一步地，方法还包括：

9、在机械臂上安装穿刺针机构，根据穿刺针机构的机械制图参数，并根据获取到的正向运动学关系，计算出穿刺针在机械臂基座标系下的位移与姿态。

10、进一步地，计算出不同初始点沿基于机械臂基座标系与机械臂末端坐标系的x、y、z轴的平均单位向量，通过向量拼接，获得机械臂基座标与红外双目设备坐标的旋转姿态关系包括：

11、

12、

13、上述公式中，rbox、rboy、rboz分别为在红外双目设备下，机械臂沿自身基座标向三轴移动的单位法向量，rbo为机械臂自身基座标与红外双目设备坐标的旋转矩阵，并获得不同初始点状态的机械臂末端与标记物的旋转矩阵关系ret。

14、进一步地，计算出不同初始点沿基于机械臂基座标系与机械臂末端坐标系的x、y、z轴的平均单位向量，通过向量拼接，获得机械臂基座标与红外双目设备坐标的旋转姿态关系包括：

15、机械臂末端与标记物的转移矩阵，通过建立闭环关系公式见下式来确定：

16、pe＝vbe·vob·vto·vet·pe

17、pe为基于机械臂末端坐标系的随机点，vbe代表由机械臂基座标cb到机械臂末端坐标系ce的转移矩阵，vob代表由机械臂基座标co到机械臂末端坐标系cb的转移矩阵，vto代表由机械臂基座标ct到机械臂末端坐标系co的转移矩阵，vet代表由机械臂基座标ce到机械臂末端坐标系ct的转移矩阵；

18、通过上述闭环等式，由求伪逆矩阵的方法得到机械臂基座标与红外双目光学设备、机械臂末端与标记物的位移关系，由此获得转移矩阵vob、vet。

19、进一步地，通过获得的机械臂基座标与红外双目设备坐标的旋转姿态关系、机械臂基座标与红外双目设备以及标记物到机械臂末端的位移关系与机械臂自身dh参数，计算出标记物的位移与姿态值，其值为计算值包括：

20、将vet与机械臂自身dh参数、采集记录的关节角度数据作为矫正系统的输入，通过机械臂正向运动学代入dh参数、关节角度数、机械臂末端坐标系与标记物自身坐标系的转移矩阵ve，算出标记物在机械臂基坐标下的位移与旋转姿态，再由计算出的机械臂基座标与红外双目光学设备自身坐标的转移矩阵vbo，计算出标记物在红外双目设备坐标参考系下的位移与旋转姿态，其值为计算值；

21、红外双目设备在自身坐标参考下，捕捉标记物的姿态与位移，其值为参考值。

22、进一步地，dh参数是一个用四个参数表达两对关节连杆之间位置角度关系的机械臂数学模型和坐标系确定的系统参数。

23、进一步地，通过对偶四元数方法，将差值迭代缩小至全局最小值，获得最优转换关系包括：

24、通过借助对偶四元数，迭代多轮数据组进行误差优化，不断更新dh参数的值，使其误差至全局最小值，让计算值与参考值误差降到最小，获得优化后的机械臂基座标系与红外双目设备坐标系的转移矩阵vob。

25、根据本发明的另一实施例，提供了一种机械臂光学定位装置，包括：

26、基础数据获取单元，用于基于机械臂基座标、机械臂末端坐标设置多组初始点，将机械臂设立多个不同的初始点，分别沿着基于机械臂基座标系与机械臂末端坐标系的x、y、z轴移动若干次，记录在红外双目设备坐标下的标记物位移与旋转姿态的数据，同时记录机械臂末端位移和旋转姿态的数据；

27、旋转姿态关系获取单元，用于计算出不同初始点沿基于机械臂基座标系与机械臂末端坐标系的x、y、z轴的平均单位向量，通过向量拼接，获得机械臂基座标与红外双目设备坐标的旋转姿态关系；

28、位移关系获取单元，用于通过机械臂基座标、红外双目设备、标记物、机械臂末端坐标建立闭环转移矩阵公式，通过求取广义逆矩阵，获得机械臂基座标与红外双目设备以及标记物到机械臂末端的位移关系；

29、差值获取单元，用于通过获得的机械臂基座标与红外双目设备坐标的旋转姿态关系、机械臂基座标与红外双目设备以及标记物到机械臂末端的位移关系与机械臂自身dh参数，计算出标记物的位移与姿态值，其值为计算值，将计算值与红外双目光学设备的参考值做差值；

30、最优转换关系获取单元，用于通过对偶四元数方法，将差值迭代缩小至全局最小值，获得最优转换关系。

31、一种存储介质，存储介质存储有能够实现上述任意一项机械臂光学定位方法的程序文件。

32、一种处理器，处理器用于运行程序，其中，程序运行时执行上述任意一项的机械臂光学定位方法。

33、本发明实施例中的机械臂光学定位方法及装置，其主要包括闭环矩阵计算以及机械臂dh参数辨识矫正部分。第一部分闭环矩阵计算是为了获取标记物与机械臂末端坐标系、机械臂基座标系与红外双目设备坐标系的旋转与平移关系；第二部分通过第一部分得出的转换矩阵，使用基于对偶四元数的方法进行机械臂dh参数矫正，更新上述转换矩阵内部参数。本发明中机械臂测取数据为自动测取，相较于手动更稳定，且精度更高。同时，本发明仅需运行一遍标定程序即可快速完成对应位置标定，效率较高，减少了因为光学设备的移动对于机械臂标定的影响。