定位追踪系统、基于线性趋势预测的时延补偿方法

本申请属于机器人控制领域，尤其涉及一种定位追踪系统、基于线性趋势预测的时延补偿方法。

背景技术：

机器人定位跟踪系统主要由发送端、控制端与对象端组成，发送端采集目标位置信息至控制端，控制端进行对象端跟踪系统进行同步跟踪。在信息的传输过程中，在发送端至控制端、控制端与对象端的通信链路的传输过程中会产生网络时延的存在，当发送端采集的图像数据传输至控制端后，控制端接收到的目标位置数据已经不再是最新的目标数据；同理，对象端跟踪分系统位置数据传输至控制端后，控制端接收到的跟踪数据也不是最新的跟踪数据。同时，信号的各个处理阶段也会产生较大的处理时延，从而导致信号不能实时的得到传输和利用。所以需要控制端进行处理运算来对对象端跟踪系统的跟踪数据进行外推预测，以消除网络时延对对象端实时跟踪所造成的影响。而现有的数据同步与时延消除所采用的算法控制精度相对较低，不能实时精确计算外推预测位置以及对目标位置进行修正。

因此有必要设计先进的控制策略对时延进行补偿，保证信号的实时传输，克服时延对机器人定位跟踪系统的影响。

技术实现要素：

有鉴于此，本申请提供了一种定位追踪系统、基于线性趋势预测的时延补偿方法，通过采集不同类型的网络时延，并通过拟合算法有效消除任意长度的时延，并对时延部分的跟踪信息进行前馈补偿外推，使目标图像数据与跟踪数据同步。

本申请提供一种定位跟踪系统，包括：发送端、控制端、对象端，所述发送端与所述控制端单向通信，所述控制端与所述对象端双向通信；

所述发送端获取捕获目标的图像数据，并将捕获目标的初始目标位置信息、目标实时位置偏差信息发送至控制端；

所述对象端将对象端原始位置信息发送至控制端；

所述控制端根据捕获目标的初始目标位置信息、目标实时位置偏差信息进行实时时延拟合，确定网络总延迟时间；并根据所述对象端的原始位置信息、目标实时位置偏差信息、网络总延迟时间，进行线性预测外推，确定对象端的实时前馈预测位置信息，并发送至对象端。

优选的，所述发送端采用双目视觉探测器，所述控制端采用主控分系统，所述对象端采用机械臂跟踪分系统；所述双目视觉探测器与所述主控分系统通过网线直连，所述主控分系统与所述机械臂跟踪分系统通过网线直连。

本申请还提供了一种基于线性趋势预测的时延补偿方法，采用所述的定位跟踪系统，包括：

获取捕获目标的初始目标位置信息、目标实时位置偏差信息、对象端的原始位置信息，并进行三维坐标投影处理；

根据控制端、对象端接收到数据报文的当前时钟信息，以及数据报文的发送时间戳信息，确定第一网络时延；

测试控制端与发送端、对象端的模拟网络通信时延，确定第二网络时延；

测量发送时延、传播时延、处理时延，确定第三网络时延；

根据第一网络时延、第二网络时延、第三网络时延，实时进行多元线性拟合，确定网络总延迟时间；

根据网络总延迟时间、对象端的原始位置信息、目标实时位置偏差信息、xyz方向的设定速度，进行线性预测外推，确定对象端的实时前馈预测位置信息。

优选的，所述捕获目标的初始目标位置信息为捕获目标相对于给定原点的实时三维位置坐标数据，所述目标实时位置偏差信息为目标实时位置相对于初始目标位置的偏差信息；所述对象端的原始位置信息为对象端相对于给定原点的实时三维位置坐标数据。

优选的，所述第一网络时延包括：

控制端接收到初始目标位置信息、目标实时位置偏差信息的当前时钟信息与发送端发送初始目标位置信息、目标实时位置偏差信息的时间戳的差值为时延ta1；

控制端接收到对象端原始位置信息的当前时钟信息与对象端发送原始位置信息的时间戳的差值为时延ta2；

对象端接收的实时前馈预测位置信息的当前时钟信息与控制端发送的实时前馈预测位置信息的时间戳的差值为时延ta3。

优选的，所述第二网络时延包括：

发送端与控制端的ping命令确定的时延tb1，即pingip1，ip1为发送端ip地址；

对象端与控制端的ping命令确定的时延tb2，即pingip2，ip2为控制端ip地址；

控制端与对象端的ping命令确定的时延tb3，即pingip3，ip3为对象端ip地址。

优选的，所述第三网络时延包括：

根据发送端至控制端的发送时延、传播时延、处理时延，求和计算发送端至控制端时延tc1；

根据对象端至控制端的发送时延、传播时延、处理时延，求和计算对象端至控制端时延tc2；

根据控制端至对象端的发送时延、传播时延、处理时延，求和计算控制端至对象端的时延tc3。

优选的，计算发送时延、传播时延、处理时延的方法为：

设发送时延为sd，数据块长度为data，信道带宽为cb，则发送时延为：

设传播时延为pd，信道长度为，电磁波在此信道上的传播速度为v，则传播时延为：

使用omp_get_wtime函数，确定处理延时cd。

优选的，将第一网络时延ta1、ta2、ta3，第二网络时延tb1、tb2、tb3，第三网络时延tc1、tc2、tc3，进行多元线性拟合，即：

t1＝βa0+βa1ta1+βa2ta2+βa3ta3

t2＝βb0+βb1tb1+βb2tb2+βb3tb3

t3＝βc0+βc1tc1+βc2tc2+βc3tc3

确定系统网络总延迟时间模型；

其中，t1为发送端与控制端的单向通信时延，t2为对象端至控制端的通信时延，t3为控制端至对象端的通信时延；βa0、βb0、βc0为常数项，βa1、βa2、βa3、βb1、βb2、βb3、βc1、βc2、βc3为偏回归系数。

优选的，设对象端原始位置信息为(xt1,yt1,zt1)，在x方向的速度为vx，在y方向的速度为vy，在z方向的速度为vz，根据：

xt2＝xt1+(t1+t2+t3)vx

yt2＝yt1+(t1+t2+t3)vv

zt2＝zt1+(t1+t2+t3)vz

将对象端原始位置信息修正为实时前馈预测位置信息(xt2,yt2,zt2)。

与现有技术相比，本申请的优点和积极效果在于：

本申请针对现有机器人定位跟踪系统存在的同步性较差的问题，提供了一种定位跟踪系统、及基于线性趋势预测的时延补偿方法。定位跟踪系统发送端获取捕获目标的图像数据，并将捕获目标的初始目标位置信息、目标实时位置偏差信息发送至控制端；对象端将对象端原始位置信息发送至控制端；控制端根据捕获目标的初始目标位置信息、目标实时位置偏差信息进行实时时延拟合，确定网络总延迟时间；并根据对象端的原始位置信息、目标实时位置偏差信息、网络总延迟时间，进行线性预测外推，确定对象端的实时前馈预测位置信息，并发送至对象端，使目标图像数据与跟踪数据同步。同时，充分考虑各项时延误差，采用三种方法共同对网络时延进行数据采集，并对各项误差通过多元线性回归的方法进行数据拟合，可以快速有效消除任意长度的时延，保证了图像跟踪的质量；并对时延部分的跟踪信息进行线性趋势预测外推法进行前馈补偿外推，提升控制整体工作的实时性，使得跟踪过程更加平滑，跟踪系统与图像系统的匹配度更高。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本申请的定位跟踪系统结构框图；

其中，1-发送端，2-控制端，3-对象端；

图2为本申请实施例的手术机器人定位跟踪系统原理框图；

图3为本申请的基于线性趋势预测的时延补偿方法流程图；

图4为本申请的基于线性趋势预测的时延补偿方法缩略流程图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚地描述，显然，所描述的实施例是本申请部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。

本申请的说明书和权利要求书及上述附图中的术语“包括”以及它们任何变形，意图在于覆盖不排他的包含。例如包含一系列步骤或单元的过程、方法或系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。

参考图1所示，本申请提供了一种定位跟踪系统，包括：发送端1、控制端2、对象端3，发送端1与控制端2通过网线直连，实现单向通信，控制端2与对象端3通过网线直连，实现双向通信。

发送端获取捕获目标的图像数据，并将捕获目标的初始目标位置信息、目标实时位置偏差信息发送至控制端；

对象端将对象端原始位置信息发送至控制端；

控制端根据捕获目标的初始目标位置信息、目标实时位置偏差信息进行实时时延拟合，确定网络总延迟时间；并根据对象端的原始位置信息、目标实时位置偏差信息、网络总延迟时间，进行线性预测外推，确定对象端的实时前馈预测位置信息，并发送至对象端，实现对象端跟踪数据与发送端目标的图像数据同步。

本申请实施例将上述定位跟踪系统具体应用到手术机器人定位跟踪系统中，参考图2所示，发送端采用双目视觉探测器，控制端采用主控分系统，对象端采用机械臂跟踪分系统；双目视觉探测器与主控分系统通过网线直连，实现单向通信；主控分系统与机械臂跟踪分系统通过网线直接连接，双向通信。双目视觉探测器获取捕获目标的图像数据，并将捕获目标的初始目标位置信息、目标实时位置偏差信息发送至主控分系统；机械臂跟踪分系统将其原始位置信息发送至机械臂跟踪分系统；主控分系统根据捕获目标的初始目标位置信息、目标实时位置偏差信息，进行实时时延拟合，确定网络总延迟时间；并根据对象端的原始位置信息、目标实时位置偏差信息、网络总延迟时间，进行线性预测外推，确定对象端的实时前馈预测位置信息，并发送至机械臂跟踪分系统，以使目标图像数据与跟踪数据同步。

由于传统手术机器人定位跟踪系统存在网络时延(发送时延、传播时延与处理时延等)，当双目视觉图像数据传输至主控分系统后，主控分系统接收到的目标位置数据已经不再是最新的目标数据；同理，机械臂跟踪分系统位置数据传输至主控分系统后，主控分系统接收到的机械臂跟踪数据也不是最新的跟踪数据。所以需要主控分系统进行处理运算来对机械臂跟踪系统的跟踪数据进行外推预测，以消除网络时延对机械臂实时跟踪所造成的影响。

本申请实施例还提供了一种基于线性趋势预测的时延补偿方法，参考图3、图4所示，具体为：

(ss1)获取捕获目标的初始目标位置信息、目标实时位置偏差信息、对象端的原始位置信息，并进行三维坐标投影处理。捕获目标的初始目标位置信息为捕获目标相对于给定原点的实时三维位置坐标数据，目标实时位置偏差信息为目标实时位置相对于初始目标位置的偏差信息，对象端的原始位置信息为对象端相对于给定原点的实时三维位置坐标数据。当双目视觉探测器捕获到目标后，将相对于给定原点(x0,y0,z0)的初始目标位置(xt0,yt0,zt0)以固定频率实时发送至主控分系统，并投影为三个二维的直角坐标，坐标系分别为xoy、yoz、xoz，机械臂跟踪分系统将当前位置为(xt1,yt1,zt1)发送至主控分系统。

由于发送端双目视觉探测器与控制端主控分系统通过网线直连，实现单向通信；控制端主控分系统与对象端机械臂跟踪分系统通过网线直接连接，双向通信。系统未设置路由器互联，不需要考虑分组排队时延。因此，可以将系统的网络总延迟时间划分为以下三类：

(ss2)计算第一网络延时，具体为：

根据控制端、对象端接收到数据报文的当前时钟信息，以及数据报文的发送时间戳信息，确定第一网络时延。即控制端接收到初始目标位置信息与目标实时位置偏差信息的当前时钟信息与发送端发送初始目标位置信息与目标实时位置偏差信息的时间戳的差值为时延ta1；控制端接收到对象端原始位置信息的当前时钟信息与对象端发送原始位置信息的时间戳的差值为时延ta2；对象端接收的实时前馈预测位置信息的当前时钟信息与控制端发送的实时前馈预测位置信息的时间戳的差值为时延ta3。

(ss3)计算第二网络时延，具体为：

进行模拟数据通讯时，通过使用networkpinger网络工具进行测试。在主控分系统中测试与双方通信的网络时延时，选择双方ip，进行通信可在界面上实时显示出双方的递归ping值。即发送端与控制端的ping命令确定的时延tb1，即pingip1，ip1为发送端ip地址；对象端与控制端的ping命令确定的时延tb2，即pingip2，ip2为控制端ip地址；控制端与对象端的ping命令确定的时延tb3，即pingip3，ip3为对象端ip地址。

(ss4)计算第三网络时延，具体为：

第三网络时延包括系统的发送时延、传播时延、处理时延。发送时延是指结点在发送数据时使数据块从结点进入到传输媒体所需的时间，也就是从数据块的第一个比特开始发送算起，到最后一个比特发送完毕所需的时间。设发送时延为sd，数据块长度为data，信道带宽为cb，则发送时延为：传播时延是指从发送端发送数据开始，到接收端收到数据(或者从接收端发送确认帧，到发送端收到确认帧)总共经历的时间。设传播时延为pd，信道长度为l，电磁波在此信道上的传播速度为v，则传播时延为：处理时延是指计算机处理数据所需的时间，与计算机cpu的性能、网卡的数据处理性能以及程序计算量有关。使用omp_get_wtime函数，确定处理延时cd，即在数据处理的函数中加入以下代码：

doubleftest1＝::omp_get_wtime()；

数据处理函数func()；

doubleftest2＝::omp_get_wtime()；

doublecd＝ftest2-ftest1；

因此，根据发送端至控制端的发送时延、传播时延、处理时延，求和就可以计算出发送端至控制端时延tc1；根据对象端至控制端的发送时延、传播时延、处理时延，求和就可以计算出对象端至控制端时延tc2；根据控制端至对象端的发送时延、传播时延、处理时延，求和就可以计算控制端至对象端的时延tc3。

因为上述三种方法确定的时延都存在误差，所以三种方法测到的时延数据可根据实时计算与存储数据进行拟合，得出最优的时延曲线数据。

(ss5)将上述确定的第一网络时延ta1、ta2、ta3，第二网络时延tb1、tb2、tb3，第三网络时延tc1、tc2、tc3，进行多元线性拟合，即：

t1＝βa0+βa1ta1+βa2ta2+βa3ta3

t2＝βb0+βb1tb1+βb2tb2+βb3tb3

t3＝βc0+βc1tc1+βc2tc2+βc3tc3

即可确定系统网络总延迟时间模型，系统网络总时延为t1+t2+t3。其中，t1为发送端与控制端的单向通信时延，t2为对象端至控制端的通信时延，t3为控制端至对象端的通信时延；βa0、βb0、βc0为常数项，βa1、βa2、βa3、βb1、βb2、βb3、βc1、βc2、βc3为偏回归系数。

本申请中对于目标跟踪的依据不仅为目标相对于原点的位置，还包括目标的实时位置偏差信息，对目标位置偏差的修正可以保证目标实时位于视场中心位置，保证整个跟踪的稳定性。同时，充分考虑各项时延误差，采用三种方法共同对网络时延进行数据采集，并对各项误差通过多元线性回归的方法进行数据拟合，可以快速有效地消除任意长度的时延，保证了图像跟踪的质量。当然，也可以采用rbf神经网络、支持向量机的方法进行数据计算与拟合。

(ss6)根据网络总延迟时间、对象端的原始位置信息、目标实时位置偏差信息、xyz方向的设定速度，进行线性预测外推，确定对象端的实时前馈预测位置信息。即：

设对象端原始位置信息为(xt1,yt1,zt1)，在x方向的速度为vx，在y方向的速度为vy，在z方向的速度为vz，则具体的线性预测外推公式为：

xt2＝xt1+(t1+t2+t3)vx

yt2＝yt1+(t1+t2+t3)vy

zt2＝zt1+(t1+t2+t3)vz

将对象端原始位置信息修正为实时前馈预测位置信息(xt2,yt2,zt2)，对象端机械臂跟踪分系统根据计算出的实时前馈预测信息(xt2,yt2,zt2)进行跟踪，以此来消除多个时延对跟踪造成的误差，达到相当于零时延的跟踪品质。

例如，假设设数据块长度1000字节，信道带宽20m/s，信道长度2m，电磁波在此信道上的传播速度2.8×10⁸m/s，cd≈0.13ms，计算得到的系统网络总时延t1+t2+t3＜2ms。对于机械臂跟踪信息(xt2,yt2,zt2)，以投影xoz坐标系为例，该坐标x方向的运动轨迹为曲线，设该时延时间范围内小段曲线x的值为a，以直线代替x的值为a′，对于时延远小于周期的数据，其曲线长度那么对于x来说，该段曲线则可以使用直线代替，y，z方向同理。因此，本实施例的线性趋势预测外推算法使用了微分极限的原理，不仅可以保证外推数据的精度，还使得处理时延大大减少，减少了函数处理时间长对时延造成的影响。当然，除了使用线性趋势外推法，还可以使用微分平滑外推或卡尔曼滤波法。

综上，本申请对于目标跟踪的依据不仅为目标相对于原点的位置，还包括目标的实时位置偏差信息，对目标位置偏差的修正可以保证目标实时位于视场中心位置，保证整个跟踪的稳定性。同时，充分考虑各项时延误差，采用三种方法共同对网络时延进行数据采集，并对各项误差通过多元线性回归的方法进行数据拟合，可以快速有效地消除任意长度的时延，保证了图像跟踪的质量；并对时延部分的跟踪信息进行线性趋势预测外推法进行前馈补偿外推，提升控制整体工作的实时性，使得跟踪过程更加平滑，跟踪系统与图像系统的匹配度更高。

以上所述，仅是本申请的较佳实施例而已，并非是对本申请作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本申请技术方案内容，依据本申请的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本申请技术方案的保护范围。