本发明涉及医疗机器人技术领域，具体涉及一种基于视觉导航的牙科机器人路径规划系统及方法。
背景技术：
在使用机器人进行种牙操作的过程中一个关键的步骤是要控制机械臂从合适的初始位置开始运动到患者种植初始位置，这一过程要避免机器人与外界环境发生碰撞。如何规划出一条无碰撞的路径有多种方法，可以采用合适的算法由计算机自动计算出路径，也可由医生进行人工示教，考虑到手术的安全性与可靠性，人工示教的方法更合理。
目前计算机自动路径规划，需要对操作对象及环境进行精确建模，实现起来较为复杂和繁琐，尤其对于动态、不确定的场合。
技术实现要素：
针对现有技术中的缺陷，本发明提供一种基于视觉导航的牙科机器人路径规划系统及方法，本发明可以由视觉控制子系统检测记录医生手持探针的运动路径和轨迹信息，然后在需要时发出控制指令使机器人沿着同样的路径复现这一运动。
为实现上述目的，本发明提供以下技术方案：
第一方面，本发明提供了一种基于视觉导航的牙科机器人路径规划系统，包括：机器人子系统和视觉控制子系统；
所述机器人子系统包括：六自由度机器人和机器人控制器；
所述视觉控制子系统包括：视觉导航装置和工业控制计算机；
其中，所述六自由度机器人的末端执行器上安装有第一光学标记，医生手持探针安装有第二光学标记，患者口腔适当位置上安装有第三光学标记；
所述视觉导航装置固定在工作空间的预设位置，所述视觉导航装置以预设频率连续采集医生手持所述医生手持探针由示教初始位置移动至患者口腔操作位置这一过程中所述第二光学标记和所述第三光学标记的相对位置关系图像，并将采集的图像发送给所述工业控制计算机；
所述工业控制计算机根据所述视觉导航装置采集的图像以及预设视觉处理模型及投影变换关系获取所述第二光学标记相对于所述第三光学标记的运动路径及轨迹；
所述工业控制计算机将获取的所述第二光学标记相对于所述第三光学标记的运动路径及轨迹发送给所述机器人控制器；
所述机器人控制器在接收到用户指示机器人进行操作复现的指令后，控制所述六自由度机器人的末端执行器运动到所述示教初始位置，然后根据所述第二光学标记相对于所述第三光学标记的运动路径及轨迹，控制所述六自由度机器人的末端执行器运动，以使得所述六自由度机器人的末端执行器上的第一光学标记相对于所述第三光学标记的运动路径及轨迹与所述第二光学标记相对于所述第三光学标记的运动路径及轨迹相同。
进一步地，所述第三光学标记通过咬合或粘贴的方式与下颌骨固定。
进一步地，所述预设频率为50ms/次。
进一步地，所述工业控制计算机在根据所述视觉导航装置采集的图像以及预设视觉处理模型及投影变换关系获取所述第二光学标记相对于所述第三光学标记的运动路径及轨迹之后，对该运动路径及轨迹进行滤波平滑处理。
第二方面，本发明还提供了一种基于如上面第一方面所述的基于视觉导航的牙科机器人路径规划系统的牙科机器人路径规划方法，包括：
s1、将所述视觉导航装置固定在工作空间的预设位置，控制所述视觉导航装置以预设频率连续采集医生手持所述医生手持探针由示教初始位置移动至患者口腔操作位置这一过程中所述第二光学标记和所述第三光学标记的相对位置关系图像，所述视觉导航装置将采集的图像发送给所述工业控制计算机；
s2、控制所述工业控制计算机进行相关处理：根据所述视觉导航装置采集的图像以及预设视觉处理模型及投影变换关系获取所述第二光学标记相对于所述第三光学标记的运动路径及轨迹；所述工业控制计算机将获取的所述第二光学标记相对于所述第三光学标记的运动路径及轨迹发送给所述机器人控制器；
s3、向所述机器人控制器发送指示机器人进行操作复现的指令；
s4、所述机器人控制器在接收到用户指示机器人进行操作复现的指令后，控制所述六自由度机器人的末端执行器运动到所述示教初始位置，然后根据所述第二光学标记相对于所述第三光学标记的运动路径及轨迹，控制所述六自由度机器人的末端执行器运动，以使得所述六自由度机器人的末端执行器上的第一光学标记相对于所述第三光学标记的运动路径及轨迹与所述第二光学标记相对于所述第三光学标记的运动路径及轨迹相同。
由上述技术方案可知，本发明提供的基于视觉导航的牙科机器人路径规划系统，包括：机器人子系统和视觉控制子系统；所述机器人子系统包括：六自由度机器人和机器人控制器；所述视觉控制子系统包括：视觉导航装置和工业控制计算机；所述六自由度机器人的末端执行器上安装有第一光学标记，医生手持探针安装有第二光学标记，患者口腔上安装有第三光学标记；所述视觉导航装置固定在工作空间的预设位置，所述视觉导航装置以预设频率连续采集医生手持所述医生手持探针由示教初始位置移动至患者口腔操作位置这一过程中所述第二光学标记和所述第三光学标记的相对位置关系图像，并将采集的图像发送给所述工业控制计算机；所述工业控制计算机根据所述视觉导航装置采集的图像以及预设视觉处理模型及投影变换关系获取所述第二光学标记相对于所述第三光学标记的运动路径及轨迹；所述工业控制计算机将获取的所述第二光学标记相对于所述第三光学标记的运动路径及轨迹发送给所述机器人控制器；所述机器人控制器在接收到用户指示机器人进行操作复现的指令后，控制所述六自由度机器人的末端执行器运动到所述示教初始位置，然后根据所述第二光学标记相对于所述第三光学标记的运动路径及轨迹，控制所述六自由度机器人的末端执行器运动，以使得所述六自由度机器人的末端执行器上的第一光学标记相对于所述第三光学标记的运动路径及轨迹与所述第二光学标记相对于所述第三光学标记的运动路径及轨迹相同。可见，本发明可以由视觉控制子系统检测记录医生手持探针的运动路径和轨迹信息，然后在需要时发出控制指令使机器人沿着同样的路径复现这一运动。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明一实施例提供的基于视觉导航的牙科机器人路径规划系统的结构示意图；
图2是对路径点进行插补的示意图；
图3是本发明另一实施例提供的基于视觉导航的牙科机器人路径规划方法的流程图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明一实施例提供了一种基于视觉导航的牙科机器人路径规划系统，包括：机器人子系统和视觉控制子系统，两子系统通过线缆彼此相连；
参见图1，所述机器人子系统包括：六自由度机器人2和机器人控制器1；其中，六自由度机器人2固定在工作空间的地面上，并通过电缆与机器人控制器1相连，机器人控制器1可给六自由度机器人提供控制信号和动力。
所述视觉控制子系统包括：视觉导航装置6和工业控制计算机5；
其中，所述六自由度机器人2的末端执行器上安装有第一光学标记3，医生手持探针安装有第二光学标记4，患者口腔上安装有第三光学标记7；其中，所述第一光学标记3在末端执行器坐标系中的位置和姿态可以通过测量准确得到。
其中，第一光学标记3，第二光学标记4和第三光学标记7的表面上按照一定的模式印有高对比度的特征图像，以便于图像处理程序提取特征，严格来讲，第一光学标记3，第二光学标记4和第三光学标记7的表面的特征图像互不相同，以便于所述工业控制计算机后期进行标记区别和路径跟踪；
所述视觉导航装置6固定在工作空间的预设位置，使得待测量的光学标记能落在视觉采集视野内，所述视觉导航装置6以预设频率连续采集医生手持所述医生手持探针由示教初始位置移动至患者口腔操作位置这一过程中所述第二光学标记4和所述第三光学标记7的相对位置关系图像，并将采集的图像发送给所述工业控制计算机；
所述工业控制计算机5根据所述视觉导航装置6采集的图像以及预设视觉处理模型及投影变换关系获取所述第二光学标记4相对于所述第三光学标记7的运动路径及轨迹；
具体地，在每一个采样周期，视觉导航装置记录下第三光学标记7相对于视觉坐标系v的位置和姿态，用齐次矩阵
每个采样周期获取的相对位置姿态矩阵
在一种优选实施方式中，所述工业控制计算机在根据所述视觉导航装置采集的图像以及预设视觉处理模型及投影变换关系获取所述第二光学标记相对于所述第三光学标记的运动路径及轨迹之后，对该运动路径及轨迹进行滤波平滑处理，以滤除医生手部抖动等干扰，提高路径规划的准确度。
可以理解的是，与任何测量系统一样，视觉导航系统获取到的光学标记坐标数据包含了许多噪声。一般采用移动平均法进行滤波平滑处理，即收集一组观察值，计算这组值的均值，利用这一均值作为下一期的预测值。但是移动平均法有两个主要限制：1、计算移动平均必须具有n个过去观察值，当需要预测大量数据时，就必须存储大量数据，造成延迟；2、n个过去观察值中每一个权值都相等，而早期的观察值的权值为0，但实际上往往是最近观察的数据包含更多信息，应具有更大权重。
考虑传统方法的缺陷，本发明实施例采用霍尔特(holt)指数平滑法，平滑同时进行预测以减小延迟。这是一种线性指数平滑方法，对离预测期较近的观察值赋予较大的权数，对离预测值较远的观察值赋予较小的权数。霍尔特指数平滑法有两个基本平滑公式和一个预测公式，两个平滑公式分别对时间序列的两种因素进行平滑，它们是：
st＝αxt+(1-α)(st-1+bt-1)公式1
bt＝γ(st-st-1)+(1-γ)bt-1公式2
ft+t＝st+btt公式3
公式中：α、γ为平滑参数；xt为实际观察值；t为外推预测时期数，公式3中的ft+t为t+t期的预测值。公式1利用前一期的趋势值bt-1直接修正平滑值st，即将bt-1加在前一期平滑值st-1上，这就消除了滞后，并使st近似达到最新数据值；公式2是来修正趋势值bt，趋势值用相邻两次平滑值之差来表示，由于随机性，可以利用平滑系数γ对两次相邻平滑值之差进行修正，并将修正值加上前期趋势值乘以(1-γ)。
初始值s1通常设为x1，b1的初值可以设定为b1＝x2-x1。用霍尔特指数平滑法进行平滑时，调整平滑参数α、γ的取值，可以调整平滑及预测效果。
对采集到的路径点上的原始数据进行滤波时可以将位置和姿态分量分别代入上述公式中，计算出ft+t作为滤波预测输出量，之后将滤波平滑后的路径存入计算机中。
所述机器人控制器1在接收到用户指示机器人进行操作复现的指令后，控制所述六自由度机器人2的末端执行器运动到所述示教初始位置，然后根据所述第二光学标记4相对于所述第三光学标记7的运动路径及轨迹，控制所述六自由度机器人2的末端执行器运动，以使得所述六自由度机器人2的末端执行器上的第一光学标记3相对于所述第三光学标记7的运动路径及轨迹与所述第二光学标记4相对于所述第三光学标记7的运动路径及轨迹相同。
在获取了相对于患者的路径后可以根据坐标转换关系计算出机器人末端应处的位置：
式中，视觉坐标系v相对于机器人基座标系b的变换矩阵为
末端执行器上的第一光学标记3在机器人基座标系中的位置和姿态可通过标定和机器人实时反馈的数据得到，其位置姿态齐次矩阵为
根据上面两个公式计算出机器人末端执行器在机器人基座标系中的位置和姿态p后，可将其发送给机器人控制器来执行运动。
受传感器和图像处理速度等因素的限制，视觉采集频率为20hz，以这样低的频率控制机器人运动可能会发生振动，需要对路径点进行插补。利用线性插补,在路径上两点之间生成一系列中间点，简单地计算出由这两点确定的直线段上的一系列点的位姿信息。如图2所示，可在相邻的两个路径点a和b之间再进行线性细分，以提高机器人运动时的平稳度。对于位置分量，可以直接进行线性插值；对于姿态分量可将其转换为四元数后进行球面线性插值。本发明在相邻的两个路径点之间插补10次。
可见，本实施例将视觉导航引入，从而改变了机器人对操作对象及环境必须精确建模的要求。依靠视觉信息的反馈，机器人可实现对动态、不确定的场合操作。例如，不管在任何环境下，只要医生手持示教探针示教完成相应的动作示教(如入口动作)，由视觉传感器记录下示教探针的运动路径和轨迹，在需要机器人重现该动作过程时，只需发出控制指令便可使机器人沿着同样的路径复现这一运动。
在一种优选实施方式中，所述第三光学标记通过咬合或粘贴的方式与下颌骨固定。
在一种优选实施方式中，所述预设频率为50ms/次。
在一种优选实施方式中，所述工业控制计算机通过以太网接口、usb接口、1394、无线wifi等方式与所述视觉导航装置相连。
在一种优选实施方式中，所述工业控制计算机通过以太网接口、usb接口、1394、无线wifi等方式与所述机器人控制器相连。
由上面分析可知，本实施例提供的基于视觉导航的牙科机器人路径规划系统，包括：机器人子系统和视觉控制子系统；所述机器人子系统包括：六自由度机器人和机器人控制器；所述视觉控制子系统包括：视觉导航装置和工业控制计算机；所述六自由度机器人的末端执行器上安装有第一光学标记，医生手持探针安装有第二光学标记，患者口腔上安装有第三光学标记；所述视觉导航装置固定在工作空间的预设位置，所述视觉导航装置以预设频率连续采集医生手持所述医生手持探针由示教初始位置移动至患者口腔操作位置这一过程中所述第二光学标记和所述第三光学标记的相对位置关系图像，并将采集的图像发送给所述工业控制计算机；所述工业控制计算机根据所述视觉导航装置采集的图像以及预设视觉处理模型及投影变换关系获取所述第二光学标记相对于所述第三光学标记的运动路径及轨迹；所述工业控制计算机将获取的所述第二光学标记相对于所述第三光学标记的运动路径及轨迹发送给所述机器人控制器；所述机器人控制器在接收到用户指示机器人进行操作复现的指令后，控制所述六自由度机器人的末端执行器运动到所述示教初始位置，然后根据所述第二光学标记相对于所述第三光学标记的运动路径及轨迹，控制所述六自由度机器人的末端执行器运动，以使得所述六自由度机器人的末端执行器上的第一光学标记相对于所述第三光学标记的运动路径及轨迹与所述第二光学标记相对于所述第三光学标记的运动路径及轨迹相同。可见，本实施例可以由视觉控制子系统检测记录医生手持探针的运动路径和轨迹信息，然后在需要时发出控制指令使机器人沿着同样的路径复现这一运动。
基于相同的发明构思，本发明另一实施例提供了一种基于如上面实施例所述的基于视觉导航的牙科机器人路径规划系统的牙科机器人路径规划方法，参见图3，该方法包括如下步骤：
步骤101：将所述视觉导航装置固定在工作空间的预设位置，控制所述视觉导航装置以预设频率连续采集医生手持所述医生手持探针由示教初始位置移动至患者口腔操作位置这一过程中所述第二光学标记和所述第三光学标记的相对位置关系图像，所述视觉导航装置将采集的图像发送给所述工业控制计算机。
步骤102：控制所述工业控制计算机进行相关处理：根据所述视觉导航装置采集的图像以及预设视觉处理模型及投影变换关系获取所述第二光学标记相对于所述第三光学标记的运动路径及轨迹；所述工业控制计算机将获取的所述第二光学标记相对于所述第三光学标记的运动路径及轨迹发送给所述机器人控制器。
步骤103：向所述机器人控制器发送指示机器人进行操作复现的指令。
步骤104：所述机器人控制器在接收到用户指示机器人进行操作复现的指令后，控制所述六自由度机器人的末端执行器运动到所述示教初始位置，然后根据所述第二光学标记相对于所述第三光学标记的运动路径及轨迹，控制所述六自由度机器人的末端执行器运动，以使得所述六自由度机器人的末端执行器上的第一光学标记相对于所述第三光学标记的运动路径及轨迹与所述第二光学标记相对于所述第三光学标记的运动路径及轨迹相同。
可以理解的是，本实施例提供的牙科机器人路径规划方法基于上面实施例所述的基于视觉导航的牙科机器人路径规划系统实现，故其实现原理和有益效果类似，此处不再详述。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。