CN112637760A - 一种基于uwb的室内非视距快速定位方法 - Google Patents

Abstract

本发明公开了一种基于UWB的室内非视距快速定位方法，克服了室内非视距定位仍需改善问题。该发明含有步骤1、构造辅助节点的RSSI向量R1和R2,设置判定定位匹配的适应函数F、判别阈值ε以及误差角度E_β，设置累计旋转角度α的初始值为0；步骤2、根据适应函数F是否小于判别阈值ε来确定辅助节点的旋转，并记录旋转角度θ；步骤3、根据第一对辅助节点的节点N的旋转角度θ_N，更新RSSI向量R1和累计旋转角度α；步骤4、设两对辅助节点的交点坐标R为未知节点A的预测位置,通过误差角度E_β计算尾节点的模糊点。该技术使用辅助节点的位置更新，可快速高效的确定未知节点的位置。

Description

一种基于UWB的室内非视距快速定位方法

技术领域

本发明涉及通信技术领域，特别是涉及一种基于UWB的室内非视距快速定位方法。

背景技术

现有的定位技术主要有、超宽带定位技术、Wi-Fi指纹定位技术、蓝牙定位技术、基于惯性传感器的定位技术等，其中：

超宽带定位技术，使用很窄的脉冲信号，摒弃了传统通信所使用的的载波信号。该技术在室内节点之间视距的情况下可以达到厘米级的测量精度，并且其结构简单，传输速度快，近几年被逐渐重视。但该技术对硬件的性能要求很高，在非视距的情况下，由于信号受到遮挡，使得定位精度十分不稳定，定位结果容易出现较大的误差。

Wi-Fi指纹定位技术，一种非测距的定位技术，该技术通过设置室内采样点，然后将每个采样点的Wi-Fi信息进行记录，然后建立出离线的Wi-Fi指纹库。最后通过指纹匹配算法估算出当前的目标的位置。该方法的优点是成本较低，当时缺点也很明显，即定位前期需要大量的测量工作来建立指纹库，并且该算法对一些复杂的室内场景不适用容易被其他信号影响，定位精度也差强人意。

蓝牙定位技术，利用智能手机的蓝牙模块，借助在定位环境中部署的蓝牙基站进行定位。该技术定位精度最高可以达到亚米级，但是这种定位方法成本高，在复杂的环境中，稳定性不佳、受到噪声干扰后定位精度下降明显。

基于惯性传感器的定位技术，主要分为两种：一种是传统惯性传感器定位方法，另一种是行人航迹推算方法。这两种方法均需要惯性传感器的支持，通过采集到的惯性传感器的数据，然后通过数据计算法获取设备的位置信息。其中，传统惯性传感器定位方法利用牛顿运动定理，对加速度进行二次积分获得速度和位移，该方法的优点是不易受到外界环境的干扰，缺点是计算复杂度太高，不适合实时性较强的场景。同时由于惯性传感器本身的精度误差问题会对定位结果产生影响。并且随着时间的累积，累计误差会逐渐变大，使得定位精度逐渐下滑。行人航迹推算方法，是在已知起始点坐标的基础上，根据移动的位移和航向角来确定下一点的坐标，这种定位方法对加速度传感器的精度要求不高，但是，航向角却是陀螺仪积分得到的，陀螺仪本身由于硬件属性的原因，自身有漂移误差，转换到航向角就会产生很大的累计误差，并且该误差会随着时间的推移不断变大。使得定位结果不稳定。

超宽带定位技术、蓝牙定位技术等需要节点之间是视距的状态，但是在非视距情况下，传播信号的遮挡会对定位技术本身产生严重干扰。Wi-Fi指纹定位技术和基于惯性传感器的定位技术虽然一定程度上可以应用于非视距的定位，但是这些技术本身却有不同程度的缺陷，定位效果也不高。当前非视距定位方法主要有非视距误差补偿技术和定位增强技术。

非视距误差补偿技术，根据遮挡物材质的不同来确定不同的误差补偿。该技术优点是原理简单，但缺点是算法普适性不高，并且定位精度也不能保证。

定位增强技术，该技术将非视距误差和定位过程相结合，增强定位结果对测量值中的非视距误差的鲁棒性，从而减小非视距误差对定位结果的影响。该技术需要有较多的参考节点辅助定位，成本较大，并且定位精度不稳定。

现有的室内定位在非视距的环境下都不能达到理想的定位效果，并且对非视距误差的补偿是一种主流思路，但是该方法本身有很多的局限性，并且障碍物的材质不同也会导致最后定位结果的不稳定。所以设计一种即高效又具有普适性的室内非视距定位算法十分有必要。

发明内容

本发明克服了现有技术中室内非视距定位仍需改善的问题，提供一种在非视距情况下快速低耗定位室内节点的基于UWB的室内非视距快速定位方法。

本发明的技术解决方案是，提供一种具有以下步骤的基于UWB的室内非视距快速定位方法：含有以下步骤：

步骤1、根据已知节点M、N、P、Q接收到来自未知节点A的UWB的信号强度，构造辅助节点的RSSI向量R1和R2,设置判定定位匹配的适应函数F、判别阈值ε以及误差角度E_β，设置累计旋转角度α的初始值为0；

步骤2、根据适应函数F是否小于判别阈值ε来确定辅助节点的旋转，并记录旋转角度θ；

步骤3、根据第一对辅助节点的节点N的旋转角度θ_N，更新RSSI向量R1和累计旋转角度α；

步骤4、重复步骤2直到RSSI向量R1能使适应函数F满足判定阈值ε，且α∈[0,180]；

步骤5、重复步骤3和步骤4使第二对辅助节点的R2能使适应函数F满足判定阈值ε；

步骤6、设此时两对辅助节点的交点坐标为R,若∠RMN或∠RNM为非钝角，则R为未知节点A的预测位置,否则，进入步骤7；

步骤7、通过误差角度E_β计算尾节点的模糊点，再分别将模糊点和首节点连接并延长得到四个交点记为O₁、O₂、O₃、O₄,则未知节点A的预测位置即为四边形O₁O₂O₃O₄顶点坐标的均值。

优选地，所述步骤1中通过UWB获取已知节点和未知位置节点之间的RSSI并构建RSSI向量、判定定位匹配的适应函数F以及判别阈值ε和误差角度E_β的方法如下：

步骤1.1、未知节点A与辅助节点M、N、P、Q之间为完全非视距的情况,设置A、M、N、P、Q的坐标分别为A(x_A,y_A)，M(x_M,y_M)，N(x_N,y_N)，P(x_P,y_P)，Q(x_Q,y_Q),其中M、N、P、Q满足的坐标关系如下

其中节点A为室内环境下的未知位置节点，M、P为已知位置的固定辅助节点，记为首节点，节点N、Q是两个移动辅助节点，记为尾节点，节点A与节点M、N、P、Q之间均为非视距情况，节点M、N、P、Q之间为视距情况，利用UWB测得M、N、A的RSSI向量R₁＝(RSSI_AM,RSSI_AN)和P、Q、A的RSSI向量R₂＝(RSSI_AP,RSSI_AQ)；

步骤1.2、构造节点匹配适应函数

C_L为接收信号传播损耗的强度因子，由两节点之间的距离得到，判定阈值ε取0.001，累计旋转角度α的初始值设置为0，旋转方向均为逆时针，误差角度E_β设为0.1。

优选地，所述步骤2中判断辅助节点M、N是否满足适应函数F的方法如下：将R₁作为适应函数F的输入，判断是否满足判定阈值ε，即

若F(R₁)<ε且α∈[0,180]，则此时AM与AN的非视距误差影响相同，未知节点A在直线MN上，固定节点N并进入步骤4，否则进入步骤3。

优选地，所述步骤3将辅助节点N绕节点M逆时针旋转，旋转角度为θ_N，并更新RSSI向量R₁的方法如下：节点N绕节点M逆时针旋转θ_N到达N'的位置，此时利用UWB更新RSSI向量

更新累计旋转角度α＝α+θ_N。

优选地，所述步骤4中判断辅助节点P、Q是否满足适应函数F的方法如下：将R₂作为适应函数F的输入，判断是否满足判定阈值ε，即

若F(R₂)<ε且α∈[0,180]，则此时AP与AQ的非视距误差影响相同，未知节点A在直线PQ上，固定节点Q并进入步骤6，否则进入步骤5。

优选地，所述步骤5中将辅助节点Q绕节点P逆时针旋转，并更新RSSI向量R₂的方法如下：节点Q绕节点P逆时针旋转θ_Q到达Q'的位置，此时利用UWB更新距离向量

然后更新累计旋转角度α＝α+θ_Q。

优选地，所述步骤6中设置两对辅助节点的交点坐标R，若∠RMN或∠RNM为非钝角，则确定预测节点A^res的方法如下：此时M、N与P、Q均满足适应函数F，且∠RMN或∠RNM为非钝角，所以直线MN与直线PQ必交于一点，该点记为R，R即为未知位置节点A的预测位置，直线MN和直线PQ的解析式分别为：

则交点R的坐标为R(x_R,y_R),则未知节点A的预测未知A^res＝R＝(x_R,y_R)。

优选地，所述步骤7中通过误差角度E_β计算尾节点的模糊点，再分别将模糊点和首节点连接并延长得到四个交点记为O₁、O₂、O₃、O₄，则通过四边形O₁O₂O₃O₄顶点预测节点A^res的方法如下：

步骤7.1、若累计旋转角度α<90，且∠RNM为钝角，当误差角度E_β作用与尾节点为负时，尾节点N和Q的误差几何角度分别为

则N点和Q点的角度模糊点

和

为

当误差角度E_β作用与尾节点为正时，尾节点N和Q的误差几何角度分别为

则N点和Q点的角度模糊点

和

为

步骤7.2、若累计旋转角度α>90，且∠RMN为钝角，当误差角度E_β作用与尾节点为负时，尾节点N和Q的误差几何角度分别为

则N点和Q点的角度模糊点

和

为

当误差角度E_β作用与尾节点为正时，尾节点N和Q的误差几何角度分别为

则N点和Q点的角度模糊点

和

为

步骤7.3、作N₁和M、N₂和M、Q₁和P、Q₂和P的直线，设四条直线的交点分别为

则未知节点A的预测位置

为

与现有技术相比，本发明基于UWB的室内非视距快速定位方法具有以下优点：

1.本发明由于在室内非视距定位过程中，使用辅助节点的旋转移动更新RSSI向量，在适应函数确定辅助节点匹配后，可以有效消除非视距带来的定位影响。

2.本发明由于是一种基于UWB的室内非视距快速定位方法，不需要其他的限定条件。因此对非视距定位具有普适性，不需要对障碍物的材料、厚度、信号传播角度有要求。同时，对于UWB模块本身，也不需要UWB的天线方向为特殊方向，使用全向天线既可以完成。

3.本方法使用了辅助节点的位置更新，并且定位算法中旋转角范围较小，因此可以快速高效的确定未知节点的位置。

4.在非视距环境下，实现可靠高效的室内定位。

附图说明

图1是本发明的实现流程图；

图2是本发明的非视距定位方法中非钝角情况的示意图；

图3是本发明的非视距定位方法中的钝角情况的示意图。

具体实施方式

下面结合附图和具体实施方式对本发明基于UWB的室内非视距快速定位方法作进一步说明：如图(1)所示，本实施例中的技术原理是利用RSSI向量对非视距误差进行判定抵消。

非视距误差判定抵消包含三个方面：一是设计出误差适应函数、判定阈值和误差角度，二是固定两对辅助节点的首节点，移动两对辅助节点的尾节点并更新RSSI向量，三是将误差适应函数和RSSI向量进行匹配判定，如果判定匹配成功，则由两对辅助节点的位置推算出未知节点的位置。在误差适应函数和RSSI向量匹配的过程中，可以分为三个阶段：

第一阶段中，使用两对辅助节点，位置节点与两对辅助节点之间全部为完全非视距情况。将每对辅助节点的首节点固定在障碍物上，尾节点可以绕首节点任意旋转移动。第二阶段，此时，将两对辅助节点的RSSI向量代入适应函数中，若适应函数大于设定阈值，则旋转尾节点，并不断更新RSSI向量，重复阶段二。若小于设定阈值，则进入第三阶段。第三阶段，在第一对辅助节点匹配成功后，将其这对辅助节点都固定住，然后让第二对辅助节点执行阶段二。当两对辅助节点都和适应函数都匹配成功时，先得到两对辅助节点的延长线的交点。然后判断该交点与两组辅助节点的夹角关系。若其为锐角或直角关系，则该交点即为非视距情况下未知节点的位置所在；若其为钝角关系，则根据设定的误差角度求得各尾节点的两个模糊点，然后根据模糊点与首节点作直线可以得到一个四边形，则该四边形顶点坐标的均值即为未知节点的位置所在。

所述第一阶段：在所选择的室内非视距场景中，假设未知节点的位置为A(x_A,y_A)。第一对辅助节点的位置为M(x_M,y_M)，N(x_N,y_N)。第二对辅助节点的位置为P(x_P,y_P)，Q(x_Q,y_Q)。M、P为首节点，N、Q为尾节点，M、N、P、Q的位置都是可以获得的，而A的位置完全未知，M、N、P、Q可以接收来自A的UWB信号强度。但是由于非视距的影响，M、N、P、Q的RSSI会有较大差异，即RSSI_AM、RSSI_AN、RSSI_AP、RSSI_AQ都包含了不同的非视距误差。其中，M与P节点位置固定，而N可以绕M旋转，Q可以绕P旋转。

所述第二阶段，设置适应函数F，该函数可以判断当前是否可以通过M、N来确定未知节点A的位置。构造节点之间的RSSI向量R1＝(RSSI_AM,RSSI_AN)，然后判断阈值为ε，误差角度为E_β。累计旋转角度α的初始值为0。让节点N不断绕节点M旋转，旋转角度为θ_N。然后更新R1和累计旋转角度α＝α+θ_N并不断计算F(R1)。直到F(R1)<ε且α∈[0,180]时，说明辅助节点M、N已经匹配成功，进入第三阶段。

所述第三阶段，将已经匹配的辅助节点M、N固定。然后设置RSSI向量为R2＝(RSSI_AP,RSSI_AQ),让节点Q不断绕节点P旋转，旋转角度为θ_Q。然后更新R2和累计旋转角度α＝α+θ_Q，并不断计算F(R2)。直到F(R2)<ε且α∈[0,180]。此时，PQ组成的辅助节点也成功匹配。设MN和PQ延长线的交点为R。若∠RMP或∠RPM为直角或锐角三角形，则未知节点A的位置即为MN和PQ延长线的交点。若∠RMP或∠RPM为钝角三角形，根据设置的误差角度E_β分别计算将N和Q顺时针和逆时针旋转E_β的坐标，得到尾节点的模糊点N₁、N₂、Q₁、Q₂。再作N₁、N₂与M的直线，Q₁、Q₂与P的直线，得到四边形O₁O₂O₃O₄。则未知节点A的位置即为四边形O₁O₂O₃O₄顶点坐标的均值。

具体过程如下：步骤1，根据已知节点M、N、P、Q接收到来自未知节点A的UWB的信号强度，构造辅助节点的RSSI向量R1和R2。设置判定定位匹配的适应函数F、判别阈值ε以及误差角度E_β，设置累计旋转角度α的初始值。

1a)未知节点A与辅助节点M、N、P、Q之间为完全非视距的情况。设置A、M、N、P、Q的坐标分别为A(x_A,y_A)，M(x_M,y_M)，N(x_N,y_N)，P(x_P,y_P)，Q(x_Q,y_Q)。M、N、P、Q满足的坐标关系如下

其中节点A为室内环境下的未知位置节点。M、P为已知位置的固定辅助节点，记为首节点。节点N、Q是两个移动辅助节点，记为尾节点。节点A与节点M、N、P、Q之间均为非视距情况。节点M、N、P、Q之间为视距情况。利用UWB测得M、N、A的RSSI向量R₁＝(RSSI_AM,RSSI_AN)和P、Q、A的RSSI向量R₂＝(RSSI_AP,RSSI_AQ)；

1b)本实例中构造了节点匹配适应函数

C_L为接收信号传播损耗的强度因子，可由两节点之间的距离得到。判定阈值ε取0.001，累计旋转角度α的初始值设置为0，旋转方向均为逆时针。误差角度E_β设为0.1。

步骤2，判断辅助节点M、N是否满足适应函数F。

2a)本实例中固定节点M，利用UWB得到辅助节点M、N与未知节点A的RSSI值，得到RSSI向量R₁＝(RSSI_AM,RSSI_AN)。

2b)将R₁作为适应函数F的输入，判断是否满足判定阈值ε。即

若F(R₁)<ε且α∈[0,180]，则说明此时AM与AN的非视距误差影响相同，未知节点A在直线MN上，固定节点N并进入步骤4)，否则进入步骤3)。

步骤3，将辅助节点N绕节点M逆时针旋转，旋转角度为θ_N，并更新RSSI向量R₁。

3a)如图2，节点N绕节点M逆时针旋转了θ_N到达N'的位置。此时利用UWB更新RSSI向量

更新累计旋转角度α＝α+θ_N。

3b)重复步骤2中的2b)。

步骤4，利用UWB测得P、Q、A的RSSI向量R₂,并使用适应函数F判断是否匹配。

4a)本实例中固定节点P，利用UWB测得辅助节点P、Q与未知节点A的RSSI值，得到RSSI向量R₂＝(RSSI_AP,RSSI_AQ)。

4b)将R₂作为适应函数F的输入，判断是否满足判定阈值ε。即

若F(R₂v<ε且α∈[0,180]，则说明此时AP与AQ的非视距误差影响相同，未知节点A在直线PQ上，固定节点Q并进入步骤6)，否则进入步骤5)。

步骤5，将辅助节点Q绕节点P逆时针旋转，并更新RSSI向量R₂。

5a)如图2，节点Q绕节点P逆时针旋转了θ_Q到达Q'的位置。此时利用UWB更新RSSI向量

然后更新累计旋转角度α＝α+θ_Q。

5b)重复步骤4中的4b)。

步骤6，设两对辅助节点的交点坐标为R。若∠RMP或∠RPM为非钝角，则R即为未知节点A的预测位置A^res。

6a)此时M、N与P、Q均满足适应函数F，且∠RMP或∠RPM为非钝角，所以直线MN与直线PQ必交于一点。该点记为R，R即为未知位置节点A的预测位置。直线MN和直线PQ的解析式分别为：

设交点R的坐标为R(x_R,y_R)，则未知节点A的预测未知A^res＝R＝(x_R,y_R)。

6b)若M、N与P、Q均满足适应函数F，但∠RMP或∠RPM为钝角，则进入步骤7。

步骤7，通过误差角度E_β计算尾节点的模糊点，再分别将模糊点和首节点连接并延长得到四个交点记为O₁、O₂、O₃、O₄，根据O₁、O₂、O₃、O₄的位置确定预测节点A^res。

7a)若累计旋转角度α<90，且∠RPM为钝角，当误差角度E_β作用与尾节点为负时，尾节点N和Q的误差几何角度分别为

则N点和Q点的角度模糊点

和

为

当误差角度E_β作用与尾节点为正时，尾节点N和Q的误差几何角度分别为

则N点和Q点的角度模糊点

和

为

7b)若累计旋转角度α>90，且∠RMP为钝角

当误差角度E_β作用与尾节点为负时，尾节点N和Q的误差几何角度分别为

则N点和Q点的角度模糊点

和

为

当误差角度E_β作用与尾节点为正时，尾节点N和Q的误差几何角度分别为

则N点和Q点的角度模糊点

和

为

7c)作N₁和M、N₂和M、Q₁和P、Q₂和P的直线。设四条直线的交点分别为

则未知节点A的预测位置

为

Claims (8)

1.一种基于UWB的室内非视距快速定位方法，其特征在于：含有以下步骤：

步骤1、根据已知节点M、N、P、Q接收到来自未知节点A的UWB的信号强度，构造辅助节点的RSSI向量R1和R2,设置判定定位匹配的适应函数F、判别阈值ε以及误差角度E_β，设置累计旋转角度α的初始值为0；

步骤2、根据适应函数F是否小于判别阈值ε来确定辅助节点的旋转，并记录旋转角度θ；

步骤3、根据第一对辅助节点的节点N的旋转角度θ_N，更新RSSI向量R1和累计旋转角度α；

步骤4、重复步骤2直到RSSI向量R1能使适应函数F满足判定阈值ε，且α∈[0,180]；

步骤5、重复步骤3和步骤4使第二对辅助节点的R2能使适应函数F满足判定阈值ε；

步骤6、设此时两对辅助节点的交点坐标为R,若∠RMN或∠RNM为非钝角，则R为未知节点A的预测位置,否则，进入步骤7；

步骤7、通过误差角度E_β计算尾节点的模糊点，再分别将模糊点和首节点连接并延长得到四个交点记为O₁、O₂、O₃、O₄,则未知节点A的预测位置即为四边形O₁O₂O₃O₄顶点坐标的均值。

2.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤1中通过UWB获取已知节点和未知位置节点之间的RSSI并构建RSSI向量、判定定位匹配的适应函数F以及判别阈值ε和误差角度E_β的方法如下：

步骤1.1、未知节点A与辅助节点M、N、P、Q之间为完全非视距的情况,设置A、M、N、P、Q的坐标分别为A(x_A,y_A)，M(x_M,y_M)，N(x_N,y_N)，P(x_P,y_P)，Q(x_Q,y_Q),其中M、N、P、Q满足的坐标关系如下

其中节点A为室内环境下的未知位置节点，M、P为已知位置的固定辅助节点，记为首节点，节点N、Q是两个移动辅助节点，记为尾节点，节点A与节点M、N、P、Q之间均为非视距情况，节点M、N、P、Q之间为视距情况，利用UWB测得M、N、A的RSSI向量R₁＝(RSSI_AM,RSSI_AN)和P、Q、A的RSSI向量R₂＝(RSSI_AP,RSSI_AQ)；

步骤1.2、构造节点匹配适应函数

C_L为接收信号传播损耗的强度因子，由两节点之间的距离得到，判定阈值ε取0.001，累计旋转角度α的初始值设置为0，旋转方向均为逆时针，误差角度E_β设为0.1。

3.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤2中判断辅助节点M、N是否满足适应函数F的方法如下：将R₁作为适应函数F的输入，判断是否满足判定阈值ε，即

若F(R₁)<ε且α∈[0,180]，则此时AM与AN的非视距误差影响相同，未知节点A在直线MN上，固定节点N并进入步骤4，否则进入步骤3。

4.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤3将辅助节点N绕节点M逆时针旋转，旋转角度为θ_N，并更新RSSI向量R₁的方法如下：节点N绕节点M逆时针旋转θ_N到达N'的位置，此时利用UWB更新RSSI向量

更新累计旋转角度α＝α+θ_N。

5.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤4中判断辅助节点P、Q是否满足适应函数F的方法如下：将R₂作为适应函数F的输入，判断是否满足判定阈值ε，即

若F(R₂)<ε且α∈[0,180]，则此时AP与AQ的非视距误差影响相同，未知节点A在直线PQ上，固定节点Q并进入步骤6，否则进入步骤5。

6.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤5中将辅助节点Q绕节点P逆时针旋转，并更新RSSI向量R₂的方法如下：节点Q绕节点P逆时针旋转θ_Q到达Q'的位置，此时利用UWB更新距离向量

然后更新累计旋转角度α＝α+θ_Q。

7.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤6中设置两对辅助节点的交点坐标R，若∠RMN或∠RNM为非钝角，则确定预测节点A^res的方法如下：此时M、N与P、Q均满足适应函数F，且∠RMN或∠RNM为非钝角，所以直线MN与直线PQ必交于一点，该点记为R，R即为未知位置节点A的预测位置，直线MN和直线PQ的解析式分别为：

则交点R的坐标为R(x_R,y_R),则未知节点A的预测未知A^res＝R＝(x_R,y_R)。

8.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤7中通过误差角度E_β计算尾节点的模糊点，再分别将模糊点和首节点连接并延长得到四个交点记为O₁、O₂、O₃、O₄，则通过四边形O₁O₂O₃O₄顶点预测节点A^res的方法如下：

步骤7.1、若累计旋转角度α<90，且∠RNM为钝角，当误差角度E_β作用与尾节点为负时，尾节点N和Q的误差几何角度分别为

则N点和Q点的角度模糊点

和

为

当误差角度E_β作用与尾节点为正时，尾节点N和Q的误差几何角度分别为

则N点和Q点的角度模糊点

和

为

步骤7.2、若累计旋转角度α>90，且∠RMN为钝角，当误差角度E_β作用与尾节点为负时，尾节点N和Q的误差几何角度分别为

则N点和Q点的角度模糊点

和

为

当误差角度E_β作用与尾节点为正时，尾节点N和Q的误差几何角度分别为

则N点和Q点的角度模糊点

和

为

步骤7.3、作N₁和M、N₂和M、Q₁和P、Q₂和P的直线，设四条直线的交点分别为

则未知节点A的预测位置

为

Images