CN106529587A - 基于目标点识别的视觉航向识别方法 - Google Patents

Abstract

本发明公开了一种基于目标点识别的视觉航向识别方法，包括：载入通过相机标定获得的方位角与像素对应关系表，在第一帧图像中选择初始目标点，提取并描述第一与第二兴趣区域中的SURF特征点，并将此帧图像存储为第一幅参考图像；在采集的下一帧图像中，调整兴趣区域的位置与大小，与参考图像进行特征点匹配，筛选理想共面特征点，计算单应矩阵，识别目标点的当前位置；当满足参考图像更新条件时，将当前帧图像更新为参考图像，并筛选稳定的、符合仿射变换的、位于无穷远平面的理想共面特征点集，修正单应阵与目标点位置；最终根据方位角与像素对应关系表识别目标点对应航向。

Description

基于目标点识别的视觉航向识别方法

技术领域

本发明涉及基于目标点识别的视觉航向识别方法，属于视觉/惯性组合导航技术领域。

背景技术

导航系统在自主移动机器人完成复杂运动过程中发挥着重要作用。由视觉、惯性、GPS和类GPS系统、测距和方向识别等基本导航技术组合而成的组合导航系统已经得到了广泛的研究与发展。

视觉导航与惯性导航是典型的独立式导航系统，它们在没有外界条件的帮助下，尤其是当GPS不可用以及其他复杂情况下，仍可提供可靠的导航服务。即时定位与地图构建(SLAM)算法是目前最常应用于实现自主移动机器人的定位与导航的算法。基于大量的可匹配图像特征，视觉SLAM(VSLAM)算法成为一种实现完全自主移动机器人和无人飞行器的潜在的有效方法。VSLAM首先通过摄像头进行图像信息采集并进行压缩，然后将信息反馈到由神经网络和统计学方法构成的学习子系统中，再由该子系统将图像信息与机器人的实际位置融合计算，进而完成机器人的自主定位与导航功能。但由于计算量庞大和单目视觉的尺度模糊，VSLAM算法对机器人系统性能要求较高，其发展仍存在诸多瓶颈。

在为自主移动机器人设计导航系统时，高可靠性与低成本是必须遵守的同时又是互相矛盾的两项基本原则。使用高性能惯性元器件与处理器虽然可以保证较高的可靠性，但成本会随之大大增加；单独使用低性能惯性元器件或低成本视觉传感器可减低成本，但无法保证系统可靠性。

发明内容

本发明的目的是为了解决上述问题，提出一种利用低成本相机与低成本惯性元件的基于目标点识别的视觉航向解算方法，大大减少了计算量，同时在保证了较高的系统可靠性与导航精度的同时降低了系统成本，更能满足实际工程的需求。

本发明提出了一种基于特征的目标点识别算法，实现了在可匹配的不同图像间对任意位置目标点的识别与跟踪，进而通过由相机标定得到方位角与像素点对应关系表获得目标点对应的机器人实际航向，实现了非特征目标点的有效识别与可靠的航向检测。

包括以下几个步骤：

步骤1：在低成本线性相机未标定的情况下对相机进行简易静态标定，获得方位角与像素对应关系表；

步骤2：载入方位角与像素对应关系表，在第一帧图像中选择初始目标点，提取并描述第一与第二兴趣区域中的SURF特征点，并将此帧图像存储为第一幅参考图像；

步骤3：采集下一帧图像，调整第一与第二兴趣区域的位置与大小，提取并描述此帧图像第一兴趣区域中的SURF特征点并与参考图像进行匹配；筛选理想共面特征点，计算单应矩阵，识别目标点在当前图像中的位置；

步骤4：当满足更新参考图像的条件时，将当前帧图像更新为参考图像，并筛选稳定的、符合仿射变换的、位于无穷远平面的理想共面特征点集，修正目标点位置；

步骤5：根据方位角与像素对应关系表获得目标点对应航向角。重复步骤3至步骤5直至导航结束。

本发明的优点在于：

(1)本发明设计了一种快速的有效检测光轴与棋盘格标靶垂直关系的方法，该方法为低成本相机的简易静态标定的实现提供了基础；

(2)本发明基于两三角形面积之比的仿射不变性筛选共面特征点集，此方法提高了被筛选点集的准确性，进而使单应阵的计算与目标点的定位更加准确；

(3)本发明基于兴趣区域进行特征点匹配、筛选与单应阵的计算，此方法能有效的减少计算量，同时可保证单应阵的计算精度不受影响；

(4)本发明设计了参考图像自适应更新算法，以当前图像与参考图像的匹配确定目标点，对比相邻帧匹配方式能够较低误差累积；

(5)本发明通过挑选目标点跟踪过程中稳定复现的特征点进行理想共面特征点的筛选，有效地修正了单应阵解算误差。

附图说明

图1是本发明的方法流程图；

图2是线性相机成像原理图；

图3是棋盘格标靶在视场中的图像；

图4是被选择出的三角形示意图；

图5是实验过程图：其中(a)为起始目标点位置示意图，(b)为ROI₁(1)中匹配的SURF特征点，(c)为I(1)中筛选出的理想共面特征点示意图，(d)为I(6)中筛选出的理想共面特征点示意图，(e)为更换参考图像时三角形的选取示意图，(f)为I(20)中目标点识别结果。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明的一种基于特征的目标点识别算法，实现了在可匹配的不同图像间对任意位置目标点的识别与跟踪，进而通过由相机标定得到方位角与像素点对应关系表获得目标点对应的机器人实际航向，实现了非特征目标点的有效识别与可靠的航向检测。

包括以下几个步骤：

步骤1：对线性相机进行简易静态标定

以相机光轴为基准，相机视场中每一像素点都对应确定的方位角与俯仰角，为实现航向角即方位角的定量测量首先需要对相机进行标定，由于低成本相机的稳定性较差，无法用传统的标定方法对其进行有效的标定，故本发明对低成本线性相机采取简易静态标定。具体做法是：

1)置参照物棋盘格与相机光轴垂直

将棋盘格放置于距相机镜头一定距离D，并使棋盘格图像充满相机视场。通过距离图像边界最近的四对相邻的两角点间的距离关系保证棋盘格与相机光轴的垂直关系，计算公式为：

其中，a和b分别为距离图像上下边界最近的两对相邻的交点间距离，c和d分别为距离图像左右边界最近的两对相邻的交点间距离。

在调整过程中，不断在图像中打印出a、b、c和d的值以及垂直关系是否满足，当上述条件满足后，打印提醒光轴与棋盘格已垂直，调整完成。

2)获得方位角与像素点对应关系表

提取棋盘格角点后可得每一角点的像平面坐标及其世界坐标。根据双线性插值可求得任意像素的世界坐标(x^W,y^W,0)，其中x^W和y^W分别代表像素点在世界坐标系中X轴方向和Y轴方向上的坐标。

已知相机镜头焦距f，以光轴为基准，视场中任意像素点P'对应的方位角θ(P')的计算公式为：

其中，与分别为像素点P'与像平面坐标原点O在世界坐标系中的横坐标。

步骤2：在不同时刻采集的图像间识别目标点

对于低成本视觉/惯性组合导航系统，对理想航向对应目标点的识别与跟踪可以有效地减少由低精度陀螺引起的航向角解算误差，尤其是在自主移动机器人在二维平面上沿直线运动过程当中。然而，当目标点不具备可被直接提取图像特征时在不同时刻采集的图像中它将无法被直接定位与跟踪。为了解决这一问题，本发明利用了共面点的仿射变换不变性，通过在以目标点为中心，T_dis为半径的圆形区域内选取符合仿射变换的理想共面特征点集，并求取两图像此点集间的单应矩阵，进而根据单应阵更新目标点位置。具体方法如下：

1)利用距离阈值筛选出初始理想共面特征点集

已知机器人行进过程中在k₁时刻与k₂时刻采集的两幅图像I(k₁)和I(k₂)，I(k₁)中的目标点P'(k₁)位置已知。首先提取、描述并匹配两幅图中的特征点，通过距离阈值T_dis筛选初始理想共面特征点集具体计算方法如下：

其中，像素P点的坐标(x_P,y_P)为在以图像中心为原点、水平向右为X轴正方向、竖直向下为Y轴正方向的坐标系XOY中的坐标，为I(k₁)中第i个匹配的特征点，N_m为匹配成功的特征点总数。

2)利用MSAC算法去除匹配异常点对挑选理想平面

由于匹配算法根据特征点描述子间距离进行匹配，有可能误将两幅图中非同一特征点成功匹配，此外，由于只经阈值T_dis筛选难以保证被筛选出的特征点均位于同一平面，因此需利用MSAC算法将异常匹配点对去除并进一步挑选准确理想平面，得到理想共面特征点候选集实际使用中MSAC算法距离判定阈值为1.6像素。具体计算方法如下：

其中，特征点特征点N_d为中点的个数。

3)根据两三角形面积比的仿射不变性筛选理想共面特征点集

由于经阈值T_dis及MSAC筛选难以保证被筛选出的特征点均位于同一理想平面，因此根据两三角形的面积之比这一仿射不变量，在中进一步挑选符合仿射变换的理想共面特征点，具体方法如下：

首先，在中挑选三个点构成种子三角形ΔVMF，然后在其他点中不断挑选第四点与种子三角形一边构成检测三角形，如ΔTMF。为保证两三角形面积比对第四点的位置具有较高的敏感度，种子三角形的选取以及第四点的选取应符合以下条件：

其中，S_ΔTMF(k₁)表示三角形ΔTMF的面积，S_ΔVMF(k₁)表示三角形ΔVMF的面积，T_r为面积比阈值；C_ΔVMF(k₁)表示种子三角形ΔVMF的周长，T_c为周长最小阈值；θ₁、θ₂、θ₃为三角形ΔVMF的三个内角，为三角形ΔTMF的三个内角，T_a为内角最小阈值。实际应用中三个阈值可分别设为1.2、25像素、30度。

当图像I(k₁)中的两三角形确定后，根据I(k₁)和I(k₂)间的匹配关系，即可得到S_ΔTMF(k₂)与S_ΔVMF(k₂)，当两三角形之比满足下列条件时，即证明I(k₂)中所选第四点为符合仿射变换的并且与ΔVMF共面的点，将其加入理想共面特征点集

其中，T_m为最大面积比之差，实际应用中可设置为10^-5。

4)计算单应阵，更新目标点位置

目标点为理想航向所对应的无穷远点，根据成像原理，它与中的点共面，因此，可根据最小二乘法(Least Square Method)求得理想共面特征点集与间的单应阵H₁，进而以此计算目标点在I(k₂)中位置P'(k₂)，具体计算方法如下：

[P'(k₂)1]＝H₁[P'(k₁)1]

其中，N_MT为中点的个数。

步骤3：兴趣区域的选取与更新

为加速计算，特征点的选取与筛选只在两个选定的同心兴趣区域ROI₁和ROI₂内进行，其中ROI₁为矩形区域，ROI₂为原型区域，ROI₁的初始宽度为图像宽度的60％，初始宽度是ROI₂直径的两倍，ROI₂的初始直径为图像高度的30％。图像I(k+1)中的ROI₁(k+1)和ROI₂(k+1)均以预测目标点P_c(k+1)为中心，P_c(k+1)以及ROI₁(k+1)的宽度均根据陀螺仪的测量数据计算得到，具体计算方法如下：

其中，W_I为图像宽度，α为水平视场角，ω表示陀螺仪测得的角速率，Δt为连续两幅图像的采集时间间隔，W_R1(k+1)为ROI₁(k+1)的宽度，η_W(k+1)代表ROI₁(k+1)与图像宽度的比。当|ωΔt/α|≥0.5时，两幅图像中的将无特征点可匹配。

实际应用中，ROI₁(k+1)的高度可取为ROI₂(k+1)半径的4倍，ROI₂(k+1)的半径T_dis(k+1)的计算方法如下：

步骤4：参考帧自适应选取方法

由于在连续帧间进行目标点识别跟踪会产生累积误差，并且当目标点与相机间有物体干扰时，MSAC方法筛选后的点集可能并非在无穷远平面的稳定理想点集，二者均造成所求单应阵准确度降低，本发明针对以上问题提出如下解决方法：

1)为抑制累积误差，挑选可有效匹配的、目标点位置已知的图像作为参考图像I_r(k)，并且在当前图像与参考图像间进行目标点的识别与跟踪。当当前图像相对于参考图像产生较大变化，匹配特征点数目小于阈值时，I_r(k)需要更新为当前图像，更新方法如下：

其中，N(k+1)与N(k+v)分别代表I_r(k)与I(k+1)、I_r(k)与I(k+v)间的匹配特征点对数。

2)为修正目标点在新参考图像中的位置，在进行参考图像更新的同时，选取稳定的、符合仿射变换的、位于无穷远平面的理想共面特征点集与计算修正单应矩阵H₂，具体计算方法如下：

首先，挑选从至稳定出现的特征点集

然后，根据两三角形面积之比的仿射不变性与上文所述三角形挑选条件，在中筛选稳定的理想共面特征点集并根据匹配关系得到使用最小二乘法求得修正后的单应矩阵H₂，据此修正目标点在新参考图像中的位置。具体计算方法如下：

[P'(k+v)1]^t＝H₂[P'(k)1]^t

其中，N_SI为中点的个数。

步骤5：目标点对应航向识别方法

利用相机标定获得的方位角与像素对应关系表，根据目标点跟踪结果P'即可识别当前航向：

实施例：

本发明是一种基于目标点识别的视觉航向解算方法，该方法提出了一种基于特征的目标点识别算法，根据此算法和相机标定结果可准确地有效地解算出视觉航向。方法的实现流程如图1所示，具体步骤如下：

步骤1：根据如图2所示线性相机成像原理，利用棋盘格对低成本线性相机进行简易静态标定，获得方位角与像素对应关系表，如图3所示为棋盘格靶标在视场中的图像；

步骤2：载入方位角与像素对应关系表，获得第一帧图像I(1)，在I(1)中选择初始目标点，如图5(a)所示，提取并描述ROI₁(1)中的SURF特征点，将I(1)存储为第一幅参考图像I_r(k)；

步骤3：采集下一帧图像，调整ROI₁与ROI₂位置与大小，提取并描述ROI₁中的SURF特征点，与参考图像I_r(k)中特征点进行匹配，匹配成功的特征点在I_r(k)中位置如图5(b)所示；筛选理想共面特征点，计算单应阵，识别目标点在当前图像中的位置：

1)利用距离阈值筛选出初始理想共面特征点集。已知图像尺寸为640×480像素，T_dis设置为0.15×480像素

2)利用MSAC算法去除匹配异常点对挑选理想平面，距离判定阈值为1.6像素

3)利用两三角形面积比的仿射不变性筛选理想共面特征点集根据以下条件挑选三角形，理想的三角形选择结果如图4所示：

并且以10^-3作为面积比之差最大值筛选理想共面特征点，结果如图5(c)所示。

4)计算单应阵，更新目标点位置

[P'(k+v)1]＝H₁[P'(k)1]

步骤4：当满足更新参考图像的条件时，将当前采集的图像更新为参考图像，并修正目标点位置，稳定的理想共面匹配特征点的筛选如图5(e)所示

[P'(k+v)1]^t＝H₂[P'(k)1]^t

步骤5：已知f＝2cm，D＝26cm，根据方位角与像素对应关系表获得目标点对应航向角ψ(P')；

重复步骤2至步骤5直至导航结束。目标点识别在I(20)中的结果如图5(f)所示。

Claims (6)

1.一种基于目标点识别的视觉航向识别方法，包括以下几个步骤：

步骤1：对线性相机进行静态标定，获得方位角与像素对应关系表；

以相机光轴为基准，相机视场中每一像素点都对应确定的方位角与俯仰角，首先对线性相机进行静态标定，然后，获得方位角与像素对应关系表；

步骤2：载入方位角与像素对应关系表，在第一帧图像中选择初始目标点，提取并描述第一兴趣区域ROI₁与第二兴趣区域ROI₂中的SURF特征点，并将此帧图像存储为第一幅参考图像；

以目标点为中心，T_dis为半径的圆形区域内选取符合仿射变换的理想共面特征点集，并求取两图像此点集间的单应矩阵，根据单应阵更新目标点位置；

步骤3：采集下一帧图像，调整第一与第二兴趣区域的位置与大小，提取并描述此帧图像第一兴趣区域中的SURF特征点并与参考图像进行匹配,筛选理想共面特征点，计算单应矩阵，识别目标点在当前图像中的位置；

特征点的选取与筛选在两个选定的同心兴趣区域ROI₁和ROI₂内进行，其中ROI₁为矩形区域，ROI₂为原型区域；k+1时刻采集的图像I(k+1)中的ROI₁(k+1)和ROI₂(k+1)均以预测目标点P_c(k+1)为中心，P_c(k+1)以及ROI₁(k+1)的宽度均根据陀螺仪的测量数据计算得到，具体计算方法如下：

$P_c(k+1)=P^{\′}\left(k\right)+(\frac{W_I}{\mathrm{\α}}\mathrm{\ω}\mathrm{\Δ}t,0)$

$\left\{\begin{array}{c}{W}_{R_1}(k+1)={\mathrm{\η}}_W(k+1)W_I\\ {\mathrm{\η}}_W(k+1)=\left\{\begin{array}{c}0.6,|\mathrm{\ω}\mathrm{\Δ}t/\mathrm{\α}|\∈\[0,0.1)\\ 0.2|\mathrm{\ω}\mathrm{\Δ}t/\mathrm{\α}|,|\mathrm{\ω}\mathrm{\Δ}t/\mathrm{\α}|\∈\[0.1,0.3)\\ 1.0,|\mathrm{\ω}\mathrm{\Δ}t/\mathrm{\α}|\∈\[0.3,0.5)\end{array}\right.\end{array}\right.$

其中，W_I为图像宽度，α为水平视场角，ω表示陀螺仪测得的角速率，Δt为连续两幅图像的采集时间间隔，为ROI₁(k+1)的宽度，η_W(k+1)代表ROI₁(k+1)与图像宽度的比；当|ωΔt/α|≥0.5时，两幅图像中的将无特征点可匹配；

步骤4：当满足更新参考图像的条件时，将当前帧图像更新为参考图像，并筛选稳定的、符合仿射变换的、位于无穷远平面的理想共面特征点集，修正目标点位置；

步骤5：根据方位角与像素对应关系表获得目标点对应航向角，重复步骤3至步骤6直至导航结束；

利用相机标定获得的方位角θ(P)与像素P对应关系表，根据目标点跟踪结果P'，识别当前航向ψ(P')：

$\mathrm{\ψ}\left(P^{\′}\right)=\mathrm{\θ}\left(P^{\′}\right)=arctan\left(\frac{x_{P^{\′}}^W-x_O^W}{D+f}\right).$

2.根据权利要求1所述的一种基于目标点识别的视觉航向识别方法，所述的步骤1具体包括：

1)置参照物棋盘格与相机光轴垂直

将棋盘格放置于距相机镜头距离D，并使棋盘格图像充满相机视场，通过距离图像边界最近的四对相邻的两角点间的距离，保证棋盘格与相机光轴的垂直关系，计算公式为：

$\frac{|a-b|}{a}\≤0.05,\frac{|c-d|}{c}\≤0.05$

其中，a和b分别为距离图像上下边界最近的两对相邻的交点间距离，c和d分别为距离图像左右边界最近的两对相邻的交点间距离；

在调整过程中，不断在图像中打印出a、b、c和d的值以及垂直关系是否满足，当上述条件满足后，打印提醒光轴与棋盘格已垂直，调整完成；

2)获得方位角与像素点对应关系表

提取棋盘格角点，得到每一角点的像平面坐标及其世界坐标，根据双线性插值求得任意像素的世界坐标(x^W,y^W,0)，其中x^W和y^W分别代表像素点在世界坐标系中X轴方向和Y轴方向上的坐标；

已知相机镜头焦距f，以光轴为基准，视场中任意像素点P'对应的方位角θ(P')的计算公式为：

$\mathrm{\θ}\left(P^{\′}\right)=arctan\left(\frac{x_{P^{\′}}^W-x_O^W}{D+f}\right)$

其中，与分别为像素点P'与像平面坐标原点O在世界坐标系中的横坐标；

进而，得到方位角与像素点对应关系表。

3.根据权利要求1所述的一种基于目标点识别的视觉航向识别方法，所述的步骤2具体包括：

1)利用距离阈值筛选出初始理想共面特征点集

已知机器人行进过程中在k₁时刻与k₂时刻采集的两幅图像I(k₁)和I(k₂)，I(k₁)中的目标点P'(k₁)位置已知；首先提取、描述并匹配两幅图中的特征点，通过距离阈值T_dis筛选初始理想共面特征点集具体计算方法如下：

其中，像素P点的坐标(x_P,y_P)为在以图像中心为原点、水平向右为X轴正方向、竖直向下为Y轴正方向的坐标系XOY中的坐标，为I(k₁)中第i个匹配的特征点，N_m为匹配成功的特征点总数；

2)利用MSAC算法去除匹配异常点对挑选理想平面

利用MSAC算法将异常匹配点对去除，并进一步挑选准确理想平面，得到理想共面特征点候选集具体计算方法如下：

其中，特征点特征点N_d为中点的个数；

3)根据两三角形面积比的仿射不变性筛选理想共面特征点集

首先，在中挑选三个点构成种子三角形ΔVMF，然后在其他点中不断挑选第四点与种子三角形一边构成检测三角形ΔTMF，种子三角形的选取以及第四点的选取应符合以下条件：

其中，S_ΔTMF(k₁)表示三角形ΔTMF的面积，S_ΔVMF(k₁)表示三角形ΔVMF的面积，T_r为面积比阈值；C_ΔVMF(k₁)表示种子三角形ΔVMF的周长，T_c为周长最小阈值；θ₁、θ₂、θ₃为三角形ΔVMF的三个内角，为三角形ΔTMF的三个内角，T_a为内角最小阈值；

当图像I(k₁)中的两三角形确定后，根据I(k₁)和I(k₂)间的匹配关系，得到S_ΔTMF(k₂)与S_ΔVMF(k₂)，当两三角形之比满足下列条件时，即证明I(k₂)中所选第四点为符合仿射变换的并且与ΔVMF共面的点，将其加入理想共面特征点集

$\left|\frac{S_{\mathrm{\&Delta;}TMF}\left(k\right)}{S_{\mathrm{\&Delta;}VMF}\left(k\right)}-\frac{S_{\mathrm{\&Delta;}TMF}(k+v)}{S_{\mathrm{\&Delta;}VMF}(k+v)}\right|<T_m$

其中，T_m为最大面积比之差；

4)计算单应阵，更新目标点位置

根据最小二乘法求得理想共面特征点集与间的单应阵H₁，计算目标点在I(k₂)中位置P'(k₂)，具体计算方法如下：

$H_1=\left\{H_1|min\left(M=\underset{l=1}{\overset{N_{MT}}{\&Sigma;}}{\left({\left[\begin{array}{cc}{P}_l^{MT}\left(k_2\right)& 1\end{array}\right]}^t-H_1{\left[\begin{array}{cc}{P}_l^{MT}\left(k_1\right)& 1\end{array}\right]}^t\right)}^2\right)\right\}$

[P'(k₂) 1]＝H₁[P'(k₁) 1]

其中，N_MT为中点的个数。

4.根据权利要求1所述的一种基于目标点识别的视觉航向识别方法，所述的步骤3中，ROI₁的初始宽度为图像宽度的60％，初始宽度是ROI₂直径的两倍，ROI₂的初始直径为图像高度的30％。

5.根据权利要求1所述的一种基于目标点识别的视觉航向识别方法，所述的步骤3中，ROI₁(k+1)的高度可取为ROI₂(k+1)半径的4倍，ROI₂(k+1)的半径T_dis(k+1)的计算方法如下：

6.根据权利要求1所述的一种基于目标点识别的视觉航向识别方法，所述的步骤4具体包括：

具体的：

1)当当前图像相对于参考图像的匹配特征点数目小于阈值时，I_r(k)更新为当前图像，更新方法如下：

$I_r\left(k\right)=I(k+v),if\frac{N(k+1)}{N(k+v)}<0.5$

其中，N(k+1)与N(k+v)分别代表I_r(k)与I(k+1)、I_r(k)与I(k+v)间的匹配特征点对数；

2)采用理想共面特征点集与计算修正单应矩阵H₂，具体计算方法如下：

首先，挑选从至稳定出现的特征点集

然后，在中筛选稳定的理想共面特征点集并根据匹配关系得到使用最小二乘法求得修正后的单应矩阵H₂，据此修正目标点在新参考图像中的位置；具体计算方法如下：

$H_2=\left\{H_2|min\left(M=\underset{z=1}{\overset{N_{SI}}{\&Sigma;}}{\left({\left[\begin{array}{cc}{P}_z^{SI}\left(k+v\right)& 1\end{array}\right]}^t-H_2{\left[\begin{array}{cc}{P}_z^{SI}\left(k\right)& 1\end{array}\right]}^t\right)}^2\right)\right\}$

[P'(k+v) 1]^t＝H₂[P'(k) 1]^t

其中，N_SI为中点的个数。