本发明涉及内窥镜视野控制的，具体涉及一种基于器械追踪的视野控制装置及视野控制方法。
背景技术：
1、近年来，设计机器人来操纵内窥镜是一个流行的趋势，这些系统仍需要由一个单独的持镜助手来操纵，或由外科医生的外部信号来驱动，如眼睛注视跟踪，语音控制等。然而，额外的持镜助手仍然存在外科医生间的误解问题，而外科医生需要不断与助手沟通来调节视野在，这也会分散外科医生的注意力，从而增加手术负担。
2、公告号为cn113143461a的中国发明专利申请文件公开了一种人机协同的微创内窥镜持镜机器人系统，涉及内窥镜领域。通过获取内窥镜视图和机器人位姿；获取机器人各关节状态，并根据机器人正向运动学原理求解内窥镜摄像头位姿；采用yolov3算法检测内窥镜视图中的手术器械尖端位置；基于视图中每一个手术器械尖端位置，获取手术器械尖端到视图中心点的距离；基于手术器械尖端位置、手术器械尖端到视图中心点的距离和相机参数获取视觉追踪向量；获取内窥镜插入体内距离约束向量；基于视觉追踪向量和内窥镜插入体内距离约束向量获取协作型机械臂各关节速度。采用yolov3算法获取内窥镜视图中的手术器械尖端的定位，同时通过机器人正运动学建模求解内窥镜相机的位姿；在相机参数已知的情况下，追踪控制器输出相机运动向量。为了保证手术器械始终内窥镜视图内，追踪控制器根据手术器械尖端和视图中心点的距离来判断是否改变相机的位姿。同时为了保证内窥镜视图的稳定性，如果手术器械尖端中心与视图中心之间的距离小于阈值r，则内窥镜不动。反之，内窥镜需要根据手术器械尖端在视图中的位置和手术器械尖端与视图中心的距离进行运动。上述人机协同的微创内窥镜持镜机器人系统由于每次都运动到视图中心，若阈值r过小，则容易出现内窥镜运动频率过大的问题，不方便观察，若阈值r过大，则又会导致每次内窥镜视野调节的距离和角度过大，难以将视图角度调整到合适的位置，该矛盾造成了不方便调节内窥镜视野的问题。并且不能解决机器人运动过程中内窥镜因旋转产生的视野方向上的错向的问题，导致操作器械操作方向与内窥镜视野方向错开。
技术实现思路
1、为了解决上述技术问题，本发明的目的在于提供一种基于器械追踪的视野控制装置，其包括内窥镜、数据处理及控制设备、机器人，还提供一种基于器械追踪的视野控制方法，包括以下步骤：s1、器械的识别和追踪；s2、判断器械是否位于理想视野区域；s3、计算机器人执行速度；s4、机器人动作；s5、判断手术是否继续；s6、结束。该基于器械追踪的视野控制装置具有方便调节内窥镜视野和能够阻止内窥镜视野发生错向的优点。
2、为实现上述发明目的，本发明采取的技术方案如下：
3、一种基于器械追踪的视野控制装置，包括内窥镜、数据处理及控制设备、机器人，所述数据处理及控制设备分别与内窥镜和机器人通信连接，所述数据处理及控制设备设有规划器；
4、所述内窥镜用于采集图像数据并将图像数据发送至数据处理及控制设备；
5、所述数据处理及控制设备用于接收图像数据，所述图像数据包括视图画面，通过所述图像数据完成器械的识别和追踪；所述规划器在视图画面中增加内部平面参考区域和外部平面参考区域，所述内部平面参考区域位于外部平面参考区域内；所述数据处理及控制设备基于所述器械的识别和追踪执行机器人控制算法；内窥镜相对rcm坐标系的线速度和内窥镜相对rcm坐标系的角速度符合以下计算式：
6、
7、其中kr表示控制rcm误差er(t)的正定增益矩阵，ks表示控制器械距离规划中心框误差ep(t)的正定增益矩阵，jfov(t)＝jimg·jd∈r2×4表示控制field of view的雅克比矩阵，i4表示4x4的单位矩阵，代表矩阵j*的伪逆形式，以z轴为基础向量e3和从内窥镜到rcm帧的平移向量表示控制z轴对应的深度方向的速度，为控制绕z轴旋转用于抵消视野方向错向的角速度，er(t)表示远端不动点离内窥镜的距离的rcm误差，er(t)＝0，数据处理及控制设备通过计算机器人末端执行速度，通过机器人逆运动学将末端执行速度转化为包含机器人各关节的速度的指令信息，并将指令信息发送给机器人；
8、所述机器人用于根据指令信息运动并驱动内窥镜运动。
9、通过这样的设置：使器械从外部平面参考区域回到内部平面参考区域内即停止内窥镜运动，从而能够实现内窥镜的微调，有效减轻了镜头不稳定的问题，并且能够更方便将镜头角度调整至合适的方位，达到方便控制视野的优点。
10、实现对内窥镜的运动建立远端不动点的约束，从而能够在机器人带动内窥镜运动时，有效提高了机器人自动控制内窥镜运动过程中的安全性，达到安全性较高的优点。并且，由于本发明视觉伺服控制器的主要目标是调整器械在图像空间中的二维位置，辅助目标兼顾调整深度误差(即变焦水平误差)和机器人运动带动内窥镜旋转时产生的方向误差，本发明通过控制z轴对应的深度方向的速度和控制绕z轴旋转抵消了视野方向的错向角速度，从而实现了最小化机器人运动过程中内窥镜因旋转产生的视野方向上的错向，能够有效阻止内窥镜视野与器械操作方向的错开，达到了方便控制视野的优点。
11、一种基于器械追踪的视野控制方法，采用一种基于器械追踪的视野控制装置，所述基于器械追踪的视野控制装置包括内窥镜、数据处理及控制设备和机器人，所述数据处理及控制设备设有规划器；
12、所述方法包括以下步骤：
13、s1、器械的识别和追踪：使用所述内窥镜采集图像数据并将图像数据发送至控制模块，所述控制模块通过图像数据和完成器械的识别和追踪；
14、s2、判断器械是否位于理想视野区域：所述图像数据包括视图画面，所述规划器在视图画面中增加内部平面参考区域和外部平面参考区域，所述内部平面参考区域位于外部平面参考区域内，若所述器械位于外部平面参考区域外，则基于器械位置规划内窥镜运动路径并进入步骤s3；若所述器械位于外部平面参考区域内，则进入步骤s5；
15、s3、计算机器人执行速度：
16、内窥镜相对rcm坐标系的线速度和内窥镜相对rcm坐标系的角速度符合以下计算式：
17、
18、其中kr表示控制rcm误差er(t)的正定增益矩阵，ks表示控制器械距离规划中心框误差ep(t)的正定增益矩阵，jfov(t)＝jimg·jd∈r2×4表示控制field of view(fov)的雅克比矩阵，i4表示4x4的单位矩阵，代表矩阵j*的伪逆形式，以z轴为基础向量e3和从内窥镜到rcm帧的平移向量表示控制z轴对应的深度方向的速度，为控制绕z轴旋转用于抵消视野方向错向的角速度，er(t)表示远端不动点离内窥镜的距离的rcm误差，er(t)＝0，通过计算机器人末端执行速度；
19、s4、机器人动作：所述数据处理及控制设备通过机器人逆运动学将机器人末端执行速度转化为机器人各关节的速度qj，并发送给机器人，使机器人运动，直至视图画面中器械运动到内部平面参考区域内；
20、s5、判断手术是否继续：若继续手术则进入步骤s1，若手术结束则进入步骤s6；
21、s6、结束。
22、通过这样的设置：使器械从外部平面参考区域回到内部平面参考区域内即停止内窥镜运动，从而能够实现内窥镜的微调，有效减轻了镜头不稳定的问题，并且能够更方便将镜头角度调整至合适的方位，达到方便控制视野的优点。
23、实现对内窥镜的运动建立远端不动点的约束，从而能够在机器人带动内窥镜运动时，有效提高了机器人自动控制内窥镜运动过程中的安全性，达到安全性较高的优点。并且，由于本发明视觉伺服控制器的主要目标是调整器械在图像空间中的二维位置，辅助目标兼顾调整深度误差(即变焦水平误差)和机器人运动带动内窥镜旋转时产生的方向误差，本发明通过控制z轴对应的深度方向的速度和控制绕z轴旋转抵消了视野方向的错向角速度，从而实现了最小化机器人运动过程中内窥镜因旋转产生的视野方向上的错向，能够有效阻止内窥镜视野与器械操作方向的错开，达到了方便控制视野的优点。
24、作为优选，在所述步骤s2中，还包括以下步骤：
25、所述内部平面参考区域的大小scale＝max(scalex，scaley)
26、其中xe表示x轴方向上器械与外部平面参考区域的距离，ye表示在y轴方向上器械与外部平面参考区域的距离。
27、通过这样的设置：当器械与外部平面参考区域的距离较小时，减小器械在镜头画面移动的距离，提高镜头画面的稳定性；当器械与外部平面参考区域的距离较大时，使内窥镜运动至器械回到内部平面参考区域的运动幅度增大，起到快速调节画面方位的作用。提高了内窥镜的控制效率，达到了方便控制视野的优点。
28、作为优选，在所述步骤s2中，还包括以下步骤：
29、所述规划器增加内部深度参考区域和外部深度参考区域，所述内部深度参考区域位于外部深度参考区域和内窥镜之间，若所述器械位于外部深度参考区域外，则基于器械位置规划内窥镜运动路径并进入步骤s3；若所述器械位于外部深度参考区域内，则进入步骤s5；
30、在所述步骤s4中，还包括以下步骤：
31、直至器械运动到内部深度参考区域内。
32、通过这样的设置：有效减轻了镜头不稳定的问题，并且能够更方便将镜头角度调整至合适的方位，达到方便控制视野的优点。
33、作为优选，在所述步骤s2中，还包括以下步骤：
34、所述外部深度参考区域的区间长度
35、所述内部深度参考区域的区间长度
36、其中depthm表示内部深度参考区域和外部深度参考区域的中间值，r表示内部深度参考区域和外部深度参考区域的长度比值，r表示外部深度参考区域的长度。
37、通过这样的设置：当器械与外部深度参考区域的距离较小时，减小器械在镜头画面移动的距离，提高镜头画面的稳定性；当器械与外部深度参考区域的距离较大时，使内窥镜运动至器械回到内部深度参考区域的运动幅度增大，起到快速调节画面方位的作用。提高了内窥镜的控制效率，达到了方便控制视野的优点。
38、作为优选，在所述步骤s3中，还包括以下步骤：
39、所述数据处理及控制设备通过以下计算式计算出机器人末端执行速度
40、其中表示从rcm坐标系到ur5基本坐标系下的旋转矩阵，表示ur5末端执行器到rcm坐标系的平移分量，i3表示3x3的单位矩阵，skew(·)表示向量的斜对称矩阵形式，表示内窥镜摄像头相对rcm坐标系的线速度，表示内窥镜摄像头相对rcm坐标系的角速度。
41、通过这样的设置：实现数据处理及控制设备根据计算末端执行速度的功能，达到了保证安全性的同时，起到方便控制视野的作用。
42、作为优选，在所述步骤s3中，还包括以下步骤：
43、采用软rcm机制，通过运动学控制使得内窥镜绕远端不动点进行运动，记机器人基准坐标系原点为b，末端点为e，内窥镜杆夹持点s，内窥镜摄像头位置c，远端不动点r，通过正运动学计算机器人末端点的位姿标定内窥镜摄像头离机器人末端点相对位姿
44、计算内窥镜摄像头位姿夹持点位姿其中由单位阵的旋转矩阵及z方向长度为llaparoscope的基准向量构成；
45、同理得到沿着内窥镜杆长度为lrcm处不动点的位姿得到不动点离内窥镜杆的距离作为rcm误差
46、通过这样的设置：实现了远端不动点的约束限制，达到了提高安全性的作用。
47、作为优选，在所述步骤s3中，还包括以下步骤：
48、内窥镜的运动符合以下计算式的旋转角度约束：
49、
50、其中表示内窥镜运动初始时的参考方向，是内窥镜轴向旋转角度θ的方向，aθ∈r2×2，tθ∈r2×1分别表示仿射矩阵和内窥镜轴向旋转角度θ下的2d图像位移。
51、通过这样的设置：实现了基于仿生映射的最小化旋转约束，达到了最小化在机器人运动过程中视觉伺服错向的效果。
52、作为优选，在所述步骤s3中，还包括以下步骤：
53、aθ＝udvt＝(uvt)(vdvt)＝r(φ)·(vdvt)
54、其中u，d，vt代表矩阵aθ的奇异值分解的三个矩阵：u是2×2酉矩阵，d是2×2阶非负实数对角矩阵，vt是v的共轭转置矩阵；r(φ)表示由内窥镜轴向旋转角度θ引起的一个方位偏移。
55、通过这样的设置：能够通过找到一个最佳的θ来实现视觉伺服错向的最小化。
56、作为优选，在所述步骤s3中，还包括以下步骤：
57、根据θ*＝argminθ|φ|找到一个θ*来最小化|φ|。
58、通过这样的设置：实现了|φ|的最小化，从而能够找到一个最佳的θ来最小化视觉伺服错向。
59、相对于现有技术，本发明取得了有益的技术效果：
60、1、当器械只是超出外部平面参考区域较小的距离时，只需要以较小的幅度调节内窥镜的角度。本技术在调节内窥镜的过程中，使器械从外部平面参考区域回到内部平面参考区域内即停止内窥镜运动，从而能够实现内窥镜的微调，有效减轻了镜头不稳定的问题，并且能够更方便将镜头角度调整至合适的方位，达到方便控制视野的优点。
61、2、由于内窥镜是由机器人把持的，因此需要一个特定的机器人控制器策略，从图像中获取控制目标，包括理想位置和深度误差。以图像视觉伺服为基础，基于前段追踪的器械位置与实时规划的目标位置作为信息差，得到摄像头平面的移动速度，考虑到内窥镜移动受到rcm的约束，在此采用基于软rcm约束的方法以及机器人学坐标转换关系，将摄像头平面的速度传递到机器人进行速度控制。实现对内窥镜的运动建立远端不动点的约束，从而能够在机器人带动内窥镜运动时，有效提高了机器人自动控制内窥镜运动过程中的安全性，达到安全性较高的优点。并且，由于本发明视觉伺服控制器的主要目标是调整器械在图像空间中的二维位置，辅助目标兼顾调整深度误差(即变焦水平误差)和机器人运动带动内窥镜旋转时产生的方向误差，本发明通过控制z轴对应的深度方向的速度和控制绕z轴旋转抵消了视野方向的错向角速度，从而实现了最小化机器人运动过程中内窥镜因旋转产生的视野方向上的错向，能够有效阻止内窥镜视野与器械操作方向的错开，达到了方便控制视野的优点。
62、3、当器械与外部平面参考区域的距离较小时，减小器械在镜头画面移动的距离，提高镜头画面的稳定性；当器械与外部平面参考区域的距离较大时，使内窥镜运动至器械回到内部平面参考区域的运动幅度增大，起到快速调节画面方位的作用。提高了内窥镜的控制效率，达到了方便控制视野的优点。