本发明公开了一种基于UWB的室内非视距快速定位方法，克服了室内非视距定位仍需改善问题。该发明含有步骤1、构造辅助节点的RSSI向量R1和R2,设置判定定位匹配的适应函数F、判别阈值ε以及误差角度Eβ，设置累计旋转角度α的初始值为0；步骤2、根据适应函数F是否小于判别阈值ε来确定辅助节点的旋转，并记录旋转角度θ；步骤3、根据第一对辅助节点的节点N的旋转角度θN，更新RSSI向量R1和累计旋转角度α；步骤4、设两对辅助节点的交点坐标R为未知节点A的预测位置,通过误差角度Eβ计算尾节点的模糊点。该技术使用辅助节点的位置更新，可快速高效的确定未知节点的位置。
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β计算尾节点的模糊点，再分别将模糊点和首节点连接并延长得到四个交点记为O1、O2、O3、O4，则未知节点A的预测位置即为四边形O1O2O3O4顶点坐标的均值；
所述步骤7中通过误差角度Eβ计算尾节点的模糊点，再分别将模糊点和首节点连接并延res
长得到四个交点记为O1、O2、O3、O4，则通过四边形01O2O3O4顶点预测节点A 的方法如下：步骤7.1、若累计旋转角度α＜90，且∠RNM为钝角，当误差角度Eβ作用与尾节点为负时，尾节点N和Q的误差几何角度分别为
当误差角度Eβ作用与尾节点为正时，尾节点N和Q的误差几何角度分别为则N点和Q点的角度模糊点 和 为
步骤7.2、若累计旋转角度α＞90，且∠RMN为钝角，当误差角度Eβ作用与尾节点为负时，尾节点N和Q的误差几何角度分别为
当误差角度Eβ作用与尾节点为正时，尾节点N和Q的误差几何角度分别为则N点和Q点的角度模糊点 和 为
步骤7.3、作N1和M、N2和M、Q1和P、Q2和P的直线，设四条直线的交点分别为则未知节点A的预测位置 为
2.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤1中通过UWB获取已知节点和未知位置节点之间的RSSI并构建RSSI向量、判定定位匹配的适应函数F以及判别阈值ε和误差角度Eβ的方法如下：步骤1.1、未知节点A与辅助节点M、N、P、Q之间为完全非视距的情况，设置A、M、N、P、Q的坐标分别为A(xA，yA)，M(xM，yM)，N(xN，yN)，P(xP，yP)，Q(xQ，yQ)，其中M、N、P、Q满足的坐标关系如下
其中节点A为室内环境下的未知位置节点，M、P为已知位置的固定辅助节点，记为首节点，节点N、Q是两个移动辅助节点，记为尾节点，节点A与节点M、N、P、Q之间均为非视距情况，节点M、N、P、Q之间为视距情况，利用UWB测得M、N、A的RSSI向量R1＝(RSSIAM，RSSIAN)和P、Q、A的RSSI向量R2＝(RSSIAp，RSSIAQ)；
步骤1.2、构造节点匹配适应函数 CL为接收信号传播损耗的强度因子，由两节点之间的距离得到，判定阈值ε取0.001，累计旋转角度α的初始值设置为0，旋转方向均为逆时针，误差角度Eβ设为0.1。
3.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤2中判断辅助节点M、N是否满足适应函数F的方法如下：将R1作为适应函数F的输入，判断是否满足判定阈值ε，即 若F(R1)＜ε且α∈[0，180]，则此时AM与AN的非视距误差影响相同，未知节点A在直线MN上，固定节点N并进入步骤4，否则进入步骤3。
4.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤3将辅助节点N绕节点M逆时针旋转，旋转角度为θN，并更新RSSI向量R1的方法如下：节点N绕节点M逆时针旋转θN到达N′的位置，此时利用UWB更新RSSI向量 更新累计旋转角度α＝α+θN。
5.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤4中判断辅助节点P、Q是否满足适应函数F的方法如下：将R2作为适应函数F的输入，判断是否满足判定阈值ε，即 若F(R2)＜ε且α∈[0，180]，则此时AP与AQ的非视距误差影响相同，未知节点A在直线PQ上，固定节点Q并进入步骤6，否则进入步骤
6.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤5中将辅助节点Q绕节点P逆时针旋转，并更新RSSI向量R2的方法如下：节点Q绕节点P逆时针旋转θQ到达Q′的位置，此时利用UWB更新距离向量 然后更新累计旋转角度α＝α+θQ。
7.根据权利要求1所述的基于UWB的室内非视距快速定位方法，其特征在于：所述步骤6res
中设置两对辅助节点的交点坐标R，若∠RMN或∠RNM为非钝角，则确定预测节点A 的方法如下：此时M、N与P、Q均满足适应函数F，且∠RMN或∠RNM为非钝角，所以直线MN与直线PQ必交于一点，该点记为R，R即为未知位置节点A的预测位置，直线MN和直线PQ的解析式分别为：res
则交点R的坐标为R(xR，yR)，则未知节点A的预测未知A ＝R＝(xR，yR)。
一种基于UWB的室内非视距快速定位方法
技术领域
[0001] 本发明涉及通信技术领域，特别是涉及一种基于UWB的室内非视距快速定位方法。
背景技术
[0002] 现有的定位技术主要有、超宽带定位技术、Wi‑Fi指纹定位技术、蓝牙定位技术、基于惯性传感器的定位技术等，其中：
[0003] 超宽带定位技术，使用很窄的脉冲信号，摒弃了传统通信所使用的载波信号。该技术在室内节点之间视距的情况下可以达到厘米级的测量精度，并且其结构简单，传输速度
快，近几年被逐渐重视。但该技术对硬件的性能要求很高，在非视距的情况下，由于信号受
到遮挡，使得定位精度十分不稳定，定位结果容易出现较大的误差。
[0004] Wi‑Fi指纹定位技术，一种非测距的定位技术，该技术通过设置室内采样点，然后将每个采样点的 Wi‑Fi信息进行记录，然后建立出离线的Wi‑Fi指纹库。最后通过指纹匹配
算法估算出当前的目标的位置。该方法的优点是成本较低，当时缺点也很明显，即定位前期
需要大量的测量工作来建立指纹库，并且该算法对一些复杂的室内场景不适用容易被其他
信号影响，定位精度也差强人意。
[0005] 蓝牙定位技术，利用智能手机的蓝牙模块，借助在定位环境中部署的蓝牙基站进行定位。该技术定位精度最高可以达到亚米级，但是这种定位方法成本高，在复杂的环境
中，稳定性不佳、受到噪声干扰后定位精度下降明显。
[0006] 基于惯性传感器的定位技术，主要分为两种：一种是传统惯性传感器定位方法，另一种是行人航迹推算方法。这两种方法均需要惯性传感器的支持，通过采集到的惯性传感
器的数据，然后通过数据计算法获取设备的位置信息。其中，传统惯性传感器定位方法利用
牛顿运动定理，对加速度进行二次积分获得速度和位移，该方法的优点是不易受到外界环
境的干扰，缺点是计算复杂度太高，不适合实时性较强的场景。同时由于惯性传感器本身的
精度误差问题会对定位结果产生影响。并且随着时间的累积，累计误差会逐渐变大，使得定
位精度逐渐下滑。行人航迹推算方法，是在已知起始点坐标的基础上，根据移动的位移和航
向角来确定下一点的坐标，这种定位方法对加速度传感器的精度要求不高，但是，航向角却
是陀螺仪积分得到的，陀螺仪本身由于硬件属性的原因，自身有漂移误差，转换到航向角就
会产生很大的累计误差，并且该误差会随着时间的推移不断变大。使得定位结果不稳定。
[0007] 超宽带定位技术、蓝牙定位技术等需要节点之间是视距的状态，但是在非视距情况下，传播信号的遮挡会对定位技术本身产生严重干扰。Wi‑Fi指纹定位技术和基于惯性传
感器的定位技术虽然一定程度上可以应用于非视距的定位，但是这些技术本身却有不同程
度的缺陷，定位效果也不高。当前非视距定位方法主要有非视距误差补偿技术和定位增强
技术。
[0008] 非视距误差补偿技术，根据遮挡物材质的不同来确定不同的误差补偿。该技术优点是原理简单，但缺点是算法普适性不高，并且定位精度也不能保证。
[0009] 定位增强技术，该技术将非视距误差和定位过程相结合，增强定位结果对测量值中的非视距误差的鲁棒性，从而减小非视距误差对定位结果的影响。该技术需要有较多的
参考节点辅助定位，成本较大，并且定位精度不稳定。
[0010] 现有的室内定位在非视距的环境下都不能达到理想的定位效果，并且对非视距误差的补偿是一种主流思路，但是该方法本身有很多的局限性，并且障碍物的材质不同也会
导致最后定位结果的不稳定。所以设计一种即高效又具有普适性的室内非视距定位算法十
分有必要。
发明内容
[0011] 本发明克服了现有技术中室内非视距定位仍需改善的问题，提供一种在非视距情况下快速低耗定位室内节点的基于UWB的室内非视距快速定位方法。
[0012] 本发明的技术解决方案是，提供一种具有以下步骤的基于UWB的室内非视距快速定位方法：含有以下步骤：
[0013] 步骤1、根据已知节点M、N、P、Q接收到来自未知节点A的UWB的信号强度，构造辅助节点的 RSSI向量R1和R2,设置判定定位匹配的适应函数F、判别阈值ε以及误差角度Eβ，设
置累计旋转角度α的初始值为0；
[0014] 步骤2、根据适应函数F是否小于判别阈值ε来确定辅助节点的旋转，并记录旋转角度θ；
[0015] 步骤3、根据第一对辅助节点的节点N的旋转角度θN，更新RSSI向量R1和累计旋转角度α；
[0016] 步骤4、重复步骤2直到RSSI向量R1能使适应函数F满足判定阈值ε，且α∈[0,180]；
[0017] 步骤5、重复步骤3和步骤4使第二对辅助节点的R2能使适应函数F满足判定阈值ε；
[0018] 步骤6、设此时两对辅助节点的交点坐标为R,若∠RMN或∠RNM为非钝角，则R为未知节点A的预测位置,否则，进入步骤7；
[0019] 步骤7、通过误差角度Eβ计算尾节点的模糊点，再分别将模糊点和首节点连接并延长得到四个交点记为O1、O2、O3、O4,则未知节点A的预测位置即为四边形O1O2O3O4顶点坐标的
均值。
[0020] 优选地，所述步骤1中通过UWB获取已知节点和未知位置节点之间的RSSI并构建RSSI向量、判定定位匹配的适应函数F以及判别阈值ε和误差角度Eβ的方法如下：
[0021] 步骤1.1、未知节点A与辅助节点M、N、P、Q之间为完全非视距的情况,设置A、M、N、P、Q的坐标分别为A(xA,yA)，M(xM,yM)，N(xN,yN)，P(xP,yP)，Q(xQ,yQ),其中M、N、P、Q满足的坐标
关系如下
[0022]
[0023] 其中节点A为室内环境下的未知位置节点，M、P为已知位置的固定辅助节点，记为首节点，节点 N、Q是两个移动辅助节点，记为尾节点，节点A与节点M、N、P、Q之间均为非视距
情况，节点M、N、 P、Q之间为视距情况，利用UWB测得M、N、A的RSSI向量R1＝(RSSIAM,RSSIAN)和
P、Q、A的RSSI 向量R2＝(RSSIAP,RSSIAQ)；
[0024] 步骤1.2、构造节点匹配适应函数 CL为接收信号传播损耗的强度因子，由两节点之间的距离得到，判定阈值ε取0.001，累计旋转角度α的初始值设置
为0，旋转方向均为逆时针，误差角度Eβ设为0.1。
[0025] 优选地，所述步骤2中判断辅助节点M、N是否满足适应函数F的方法如下：将R1作为适应函数F 的输入，判断是否满足判定阈值ε，即 若F(R1)<
ε且α∈[0,180]，则此时AM与AN的非视距误差影响相同，未知节点A在直线MN上，固定节点N
并进入步骤4，否则进入步骤3。
[0026] 优选地，所述步骤3将辅助节点N绕节点M逆时针旋转，旋转角度为θN，并更新RSSI向量R1的方法如下：节点N绕节点M逆时针旋转θN到达N'的位置，此时利用UWB更新RSSI向量
更新累计旋转角度α＝α+θN。
[0027] 优选地，所述步骤4中判断辅助节点P、Q是否满足适应函数F的方法如下：将R2作为适应函数F 的输入，判断是否满足判定阈值ε，即 若F(R2)<
ε且α∈[0,180]，则此时AP与AQ的非视距误差影响相同，未知节点A在直线PQ上，固定节点Q
并进入步骤6，否则进入步骤5。
[0028] 优选地，所述步骤5中将辅助节点Q绕节点P逆时针旋转，并更新RSSI向量R2的方法如下：节点Q绕节点P逆时针旋转θQ到达Q'的位置，此时利用UWB更新距离向量
然后更新累计旋转角度α＝α+θQ。
[0029] 优选地，所述步骤6中设置两对辅助节点的交点坐标R，若∠RMN或∠RNM为非钝角，res
则确定预测节点A 的方法如下：此时M、N与P、Q均满足适应函数F，且∠RMN或∠RNM为非钝
角，所以直线MN 与直线PQ必交于一点，该点记为R，R即为未知位置节点A的预测位置，直线
MN和直线PQ的解析式分别为：
[0030]
[0031] 则交点R的坐标为R(xR,yR),则未知节点A的预测未知Ares＝R＝(xR,yR)。
[0032] 优选地，所述步骤7中通过误差角度Eβ计算尾节点的模糊点，再分别将模糊点和首res
节点连接并延长得到四个交点记为O1、O2、O3、O4，则通过四边形O1O2O3O4顶点预测节点A 的
方法如下：
[0033] 步骤7.1、若累计旋转角度α<90，且∠RNM为钝角，当误差角度Eβ作用与尾节点为负时，尾节点N和Q的误差几何角度分别为
[0034]
[0035] 则N点和Q点的角度模糊点 和 为
[0036]
[0037] 当误差角度Eβ作用与尾节点为正时，尾节点N和Q的误差几何角度分别为
[0038]
[0039] 则N点和Q点的角度模糊点 和 为
[0040]
[0041] 步骤7.2、若累计旋转角度α>90，且∠RMN为钝角，当误差角度Eβ作用与尾节点为负时，尾节点N和Q的误差几何角度分别为
[0042]
[0043] 则N点和Q点的角度模糊点 和 为
[0044]
[0045] 当误差角度Eβ作用与尾节点为正时，尾节点N和Q的误差几何角度分别为
[0046]
[0047] 则N点和Q点的角度模糊点 和 为
[0048]
[0049] 步骤7.3、作N1和M、N2和M、Q1和P、Q2和P的直线，设四条直线的交点分别为则未知节点A的预测位置 为
[0050]
[0051] 与现有技术相比，本发明基于UWB的室内非视距快速定位方法具有以下优点：
[0052] 1.本发明由于在室内非视距定位过程中，使用辅助节点的旋转移动更新RSSI向量，在适应函数确定辅助节点匹配后，可以有效消除非视距带来的定位影响。
[0053] 2.本发明由于是一种基于UWB的室内非视距快速定位方法，不需要其他的限定条件。因此对非视距定位具有普适性，不需要对障碍物的材料、厚度、信号传播角度有要求。同
时，对于UWB模块本身，也不需要UWB的天线方向为特殊方向，使用全向天线既可以完成。
[0054] 3.本方法使用了辅助节点的位置更新，并且定位算法中旋转角范围较小，因此可以快速高效的确定未知节点的位置。
[0055] 4.在非视距环境下，实现可靠高效的室内定位。
附图说明
[0056] 图1是本发明的实现流程图；
[0057] 图2是本发明的非视距定位方法中非钝角情况的示意图；
[0058] 图3是本发明的非视距定位方法中的钝角情况的示意图。
具体实施方式
[0059] 下面结合附图和具体实施方式对本发明基于UWB的室内非视距快速定位方法作进一步说明：如图(1) 所示，本实施例中的技术原理是利用RSSI向量对非视距误差进行判定
抵消。
[0060] 非视距误差判定抵消包含三个方面：一是设计出误差适应函数、判定阈值和误差角度，二是固定两对辅助节点的首节点，移动两对辅助节点的尾节点并更新RSSI向量，三是
将误差适应函数和RSSI向量进行匹配判定，如果判定匹配成功，则由两对辅助节点的位置
推算出未知节点的位置。在误差适应函数和RSSI 向量匹配的过程中，可以分为三个阶段：
[0061] 第一阶段中，使用两对辅助节点，位置节点与两对辅助节点之间全部为完全非视距情况。将每对辅助节点的首节点固定在障碍物上，尾节点可以绕首节点任意旋转移动。第
二阶段，此时，将两对辅助节点的RSSI向量代入适应函数中，若适应函数大于设定阈值，则
旋转尾节点，并不断更新RSSI向量，重复阶段二。若小于设定阈值，则进入第三阶段。第三阶
段，在第一对辅助节点匹配成功后，将其这对辅助节点都固定住，然后让第二对辅助节点执
行阶段二。当两对辅助节点都和适应函数都匹配成功时，先得到两对辅助节点的延长线的
交点。然后判断该交点与两组辅助节点的夹角关系。若其为锐角或直角关系，则该交点即为
非视距情况下未知节点的位置所在；若其为钝角关系，则根据设定的误差角度求得各尾节
点的两个模糊点，然后根据模糊点与首节点作直线可以得到一个四边形，则该四边形顶点
坐标的均值即为未知节点的位置所在。
[0062] 所述第一阶段：在所选择的室内非视距场景中，假设未知节点的位置为A(xA,yA)。第一对辅助节点的位置为M(xM,yM)，N(xN,yN)。第二对辅助节点的位置为P(xP,yP)，Q(xQ,yQ)。
M、P为首节点，N、Q为尾节点，M、N、P、Q的位置都是可以获得的，而A的位置完全未知，M、N、P、Q
可以接收来自A的UWB信号强度。但是由于非视距的影响，M、N、P、Q的RSSI会有较大差异，即
RSSIAM、RSSIAN、RSSIAP、RSSIAQ都包含了不同的非视距误差。其中，M与P节点位置固定，而N可
以绕M旋转，Q可以绕P旋转。
[0063] 所述第二阶段，设置适应函数F，该函数可以判断当前是否可以通过M、N来确定未知节点A的位置。构造节点之间的RSSI向量R1＝(RSSIAM,RSSIAN)，然后判断阈值为ε，误差角
度为Eβ。累计旋转角度α的初始值为0。让节点N不断绕节点M旋转，旋转角度为θN。然后更新R1
和累计旋转角度α＝α+θN并不断计算F(R1)。直到F(R1)<ε且α∈[0,180]时，说明辅助节点M、
N已经匹配成功，进入第三阶段。
[0064] 所述第三阶段，将已经匹配的辅助节点M、N固定。然后设置RSSI向量为R2＝(RSSIAP,RSSIAQ),让节点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β的坐标，得到尾节点的模糊点N1、 N2、Q1、Q2。再作N1、N2与M的
直线，Q1、Q2与P的直线，得到四边形O1O2O3O4。则未知节点A的位置即为四边形O1O2O3O4顶点坐
标的均值。
[0065] 具体过程如下：步骤1，根据已知节点M、N、P、Q接收到来自未知节点A的UWB的信号强度，构造辅助节点的RSSI向量R1和R2。设置判定定位匹配的适应函数F、判别阈值ε以及误
差角度Eβ，设置累计旋转角度α的初始值。
[0066] 1a)未知节点A与辅助节点M、N、P、Q之间为完全非视距的情况。设置A、M、N、P、Q的坐标分别为A(xA,yA)，M(xM,yM)，N(xN,yN)，P(xP,yP)，Q(xQ,yQ)。M、N、P、Q满足的坐标关系如下
[0067]
[0068] 其中节点A为室内环境下的未知位置节点。M、P为已知位置的固定辅助节点，记为首节点。节点N、 Q是两个移动辅助节点，记为尾节点。节点A与节点M、N、P、Q之间均为非视距
情况。节点M、N、P、Q 之间为视距情况。利用UWB测得M、N、A的RSSI向量R1＝(RSSIAM,RSSIAN)和
P、Q、A的RSSI向量R2＝ (RSSIAP,RSSIAQ)；
[0069] 1b)本实例中构造了节点匹配适应函数 CL为接收信号传播损耗的强度因子，可由两节点之间的距离得到。判定阈值ε取0.001，累计旋转角度α的初始
值设置为0，旋转方向均为逆时针。误差角度Eβ设为0.1。
[0070] 步骤2，判断辅助节点M、N是否满足适应函数F。
[0071] 2a)本实例中固定节点M，利用UWB得到辅助节点M、N与未知节点A的RSSI值，得到RSSI向量R1＝ (RSSIAM,RSSIAN)。
[0072] 2b)将R1作为适应函数F的输入，判断是否满足判定阈值ε。即
[0073] 若F(R1)<ε且α∈[0,180]，则说明此时AM与AN的非视距误差影响相同，未知节点A在直线MN上，固定节点N并进入步骤4)，否则进入步骤3)。
[0074] 步骤3，将辅助节点N绕节点M逆时针旋转，旋转角度为θN，并更新RSSI向量R1。
[0075] 3a)如图2，节点N绕节点M逆时针旋转了θN到达N'的位置。此时利用UWB更新RSSI向量 更新累计旋转角度α＝α+θN。
[0076] 3b)重复步骤2中的2b)。
[0077] 步骤4，利用UWB测得P、Q、A的RSSI向量R2,并使用适应函数F判断是否匹配。
[0078] 4a)本实例中固定节点P，利用UWB测得辅助节点P、Q与未知节点A的RSSI值，得到RSSI向量R2＝ (RSSIAP,RSSIAQ)。
[0079] 4b)将R2作为适应函数F的输入，判断是否满足判定阈值ε。即
[0080] 若F(R2v<ε且α∈[0,180]，则说明此时AP与AQ的非视距误差影响相同，未知节点A在直线PQ上，固定节点Q并进入步骤6)，否则进入步骤5)。
[0081] 步骤5，将辅助节点Q绕节点P逆时针旋转，并更新RSSI向量R2。
[0082] 5a)如图2，节点Q绕节点P逆时针旋转了θQ到达Q'的位置。此时利用UWB更新RSSI向量 然后更新累计旋转角度α＝α+θQ。
[0083] 5b)重复步骤4中的4b)。
[0084] 步骤6，设两对辅助节点的交点坐标为R。若∠RMP或∠RPM为非钝角，则R即为未知res
节点A的预测位置A 。
[0085] 6a)此时M、N与P、Q均满足适应函数F，且∠RMP或∠RPM为非钝角，所以直线MN与直线PQ必交于一点。该点记为R，R即为未知位置节点A的预测位置。直线MN和直线PQ的解析式
分别为：
[0086]
[0087] 设交点R的坐标为R(xR,yR)，则未知节点A的预测未知Ares＝R＝(xR,yR)。
[0088] 6b)若M、N与P、Q均满足适应函数F，但∠RMP或∠RPM为钝角，则进入步骤7。
[0089] 步骤7，通过误差角度Eβ计算尾节点的模糊点，再分别将模糊点和首节点连接并延res
长得到四个交点记为O1、O2、O3、O4，根据O1、O2、O3、O4的位置确定预测节点A 。
[0090] 7a)若累计旋转角度α<90，且∠RPM为钝角，当误差角度Eβ作用与尾节点为负时，尾节点N和Q的误差几何角度分别为
[0091]
[0092] 则N点和Q点的角度模糊点 和 为
[0093]
[0094] 当误差角度Eβ作用与尾节点为正时，尾节点N和Q的误差几何角度分别为
[0095]
[0096] 则N点和Q点的角度模糊点 和 为
[0097]
[0098] 7b)若累计旋转角度α>90，且∠RMP为钝角
[0099] 当误差角度Eβ作用与尾节点为负时，尾节点N和Q的误差几何角度分别为
[0100]
[0101] 则N点和Q点的角度模糊点 和 为
[0102]
[0103] 当误差角度Eβ作用与尾节点为正时，尾节点N和Q的误差几何角度分别为
[0104]
[0105] 则N点和Q点的角度模糊点 和 为
[0106]
[0107] 7c)作N1和M、N2和M、Q1和P、Q2和P的直线。设四条直线的交点分别为
[0108] 则未知节点A的预测位置 为
