一种基于前项反馈修正的井下动目标定位方法

暂无

一种基于前项反馈修正的井下动目标定位方法\n技术领域\n[0001] 本发明涉及一种井下目标定位的方法，特别是一种基于前项反馈修正的井下动目标定位方法。\n背景技术\n[0002] 煤矿井下环境恶劣，地质条件复杂，使得无线通信的多径效应明显、信号衰落快，给基于无线通信手段井下的目标定位带来了很大困难。提高井下定位系统的精度，对于井下人员的调度管理、安全生产和灾后营救有着重要的意义。\n[0003] 井下定位常用的定位算法有基于信号强度(RSSI)、到达角度(DOA/AOA)、到达时间(TOA)、到达时间差(TDOA)等方式。其中，基于信号强度的定位方法主要利用基站发射无线信号的信号强度值，计算移动节点和基站间的距离，不需要额外的硬件支持容易实现，设备廉价，但井下通信环境变化明显，导致路径衰减因子值随环境随机变化，造成测距误差较大。\n[0004] 基于到达角度的定位方法主要利用基站的波达方向估计判断识别信号来角方向，由基站坐标和角度建立移动节点的直线方程，进而通过两个或多个基站的直线交点进行定位。该方法结构简单，但定位效果易受信号波达方向估计影响，而井下信号通常会产生反射、散射、衍射等非视距路径，形成多种相干信源，从而导致DOA定位精度下降。\n[0005] 基于到达时间的定位方法首先通过估计电磁波从移动节点到基站的传播延时，然后乘以电磁波的传播速度，得到移动节点距离基站的距离。随后建立以基站为圆心，估计距离为半径的圆。当同时收到三个基站的信号的时候，即可得到三个这样的圆，这三个圆的交点，，即为移动节点坐标。这种方法受到时间同步精度的影响，估计距离往往偏差较大，定位精度不太高。\n[0006] 基于到达时间差的定位方法通过移动节点发送的无线信号到达不同基站的时间差，根据无线信号的传播速度可以计算出移动节点与基站之间的距离，或是移动节点发送两种不同的信号到达基站，根据两种信号不同的速度计算出移动节点的位置。虽然它的定位精度比基于到达时间的定位方法高，但是需要多个基站的TOA估计信息，有些距离基站距离节点较远，这就容易受到多径和非视距因素的影响，使得TDOA测距误差偏大，进而导致定位误差变大。\n[0007] 当前，多数矿井已经建设了井下定位系统，重新搭建不仅要投入大量的人力、物力，而且由于井下环境恶劣，可操作性较差，所以现有井下定位系统不适合大范围改变，最好在原有系统的基础上尽可能提高定位精度。\n发明内容\n[0008] 本发明的目的是要提供一种基于前项反馈修正的井下动目标定位方法，实现在不改变已有定位系统的原有算法基础上提高定位精度，以增强系统的实用性和易用性。\n[0009] 本发明的目的是这样实现的：该目标定位方法是根据井下平面图和巷道规划特点，首先建立平面坐标系，将各段不同的巷道标记为不同的区域，并将各个区域的方向角或者圆心点及半径存入信息数据库；然后对动目标执行区域识别算法，判断动目标是否在巷道区域范围内，若不在巷道区域范围内，则根据前项反馈信息，采用位置修正算法对坐标进行修正；若在巷道区域范围内，则执行位置判别算法，判断动目标是否满足移动长度条件，如果满足，则直接输出结果，否则根据前项反馈信息，采用位置修正算法对坐标进行修正；\n最后将动目标信息存入位置信息数据库中，作为下一位置判别修正的前项信息；具体方法是：\n[0010] (1)将井下定位系统加入时间戳；基站每隔时间Δt对动目标定位一次，并得到动目标的位置坐标；\n[0011] (2)利用井下平面图和巷道规划特点，将各段不同的巷道标记为不同的区域，并将各个区域的方向角或者圆心点及半径录入信息数据库；\n[0012] (3)对动目标坐标执行区域识别算法，判断动目标是否在巷道区域范围内，若不在巷道区域范围内，则根据前项反馈信息，采用位置修正算法对坐标进行修正；\n[0013] (4)若在巷道区域范围内，则读取所在巷道区域信息并执行位置判别算法，判断动目标是否满足移动长度条件，如果满足，则直接输出结果，否则根据前项反馈信息，采用位置修正算法对坐标进行修正；\n[0014] (5)将动目标坐标存入位置信息数据库中，作为下一位置判别修正的前项信息。\n[0015] 所述的位置判别算法，具体方法是：\n[0016] (1)已知当前时刻基站对动目标的定位坐标(xpi ypi)，用testPoint(i)表示；当前时刻判别修正后的坐标点(xi yi)，用positionPoint(i)表示；从位置信息数据库中读取动目标前两次位置坐标(xi-1 yi-1)、(xi-2 yi-2)，分别用positionPoint(i-1)、positionPoint(i-2)表示；\n[0017] (2)从信息数据库判断坐标是在直巷道中还是曲线巷道中，如果在直巷道中，读取巷道正方向角θ；如果在曲线巷道中，读取巷道圆弧所在圆心坐标(a b)和半径r；\n[0018] (3)井下巷道中动目标的作业习惯，将动目标在短时间内看作是匀速运动的，即将每次定位时间间隔Δt时间内动目标看作是匀速运动的，又因动目标运动速度有一定的范围，根据经验值可设置动目标运动过程中的最小速度vmin，最大速度vmax，默认速度vdef，vdef在动目标运动开始后第一个位置坐标中，作为前项信息使用；\n[0019] 根据经验速度求动目标在Δt内移动的距离s的取值范围[smin smax]，可计算positionPoint(i-1)、testPoint(i)两坐标点间的移动距离di：\n[0020]\n[0021] positionPoint(i-1)与testPoint(i)点间直线与x轴的夹角θ'(范围[0 2π])：\n[0022]\n[0023] 接下来，判断是否满足移动长度条件：\n[0024] ①直巷道中\n[0025] 判断动目标在Δt时间内的移动长度是否符合实际，即判断di是否在[smin smax]范围内，如果满足条件，动目标坐标不需要修正，将testPoint(i)赋值给positionPoint(i)，直接输出，否则利用前项反馈信息对定位坐标进行修正；\n[0026] 在判定过程中，因不确定动目标移动方向，会出现两个区域范围，所以要根据前项positionPoint(i-1)和testPoint(i)信息，确定最终positionPoint(i)点所在区域；\n[0027] 已知直巷道中的正方向角θ，判断testPoint(i)更趋向于A、B哪个区域，即判断θ与θ'之间的关系：\n[0028] 当 时，从positionPoint(i-1)到testPoint(i)的直线趋向于沿巷\n道负方向运动，即判断满足条件的点在阴影A区域；\n[0029] 当 时，从positionPoint(i-1)到testPoint(i)直线趋向于沿巷道\n正方向运动，即判断满足条件的点在阴影B区域；\n[0030] ②弯曲巷道中\n[0031] 已知巷道半径r，前后两坐标间的移动距离di，Δt时间内移动的最小、最大弧长smin，smax，可求两定位点之间的最小、最大距离dmin，dmax，判断di是否在[dmin dmax]范围内，如果满足条件，动目标坐标不需要修正，将testPoint(i)赋值给positionPoint(i)，直接输出，否则利用前项反馈信息对定位坐标进行修正；\n[0032] 在判定过程中，因不确定动目标移动方向，会出现两个区域范围，所以要根据前项positionPoint(i-1)和testPoint(i)信息，确定最终positionPoint(i)点所在区域；\n[0033] 判断testPoint(i)更趋向于A、B哪个区域，即判断θ'与x轴之间的关系：\n[0034] 当 时，从positionPoint(i-1)到testPoint(i)的直线趋向于沿\n巷道负方向运动，即判断满足条件的点在阴影A区域；\n[0035] 否则，从positionPoint(i-1)到testPoint(i)的直线趋向于沿巷道正方向运动，即判断满足条件的点在阴影B区域。\n[0036] 所述的位置修正算法，具体方法是：\n[0037] (1)求动目标在positionPoint(i-1)和positionPoint(i)两点间Δt时间内的移动距离\n[0038] 井下巷道中动目标的作业习惯，将动目标在短时间内看作是匀速运动的，即positionPoint(i-1)，positionPoint(i-2)两点间在Δt时间内动目标是匀速运动的，若两次速度相同，则positionPoint(i-1)和positionPoint(i)两点间在Δt时间内动目标移动距离s'i与positionPoint(i-1)和positionPoint(i-2)两点间的移动距离是相同的，即s'i为：\n[0039]\n[0040] (2)求positionPoint(i)的坐标\n[0041] 已知动目标沿巷道运动，巷道方向即为动目标移动的理想方向，即取巷道方向为positionPoint(i-1)和positionPoint(i)之间的移动方向，又由位置判别算法可知，positionPoint(i-1)到testPoint(i)点趋向于沿巷道正(负)方向运动，可求\npositionPoint(i)的坐标；\n[0042] ①直巷道中\n[0043] 已知positionPoint(i-1)到testPoint(i)点趋向于沿巷道正(负)方向运动，取巷道正(负)方向角θ(π-θ)，求positionPoint(i)的坐标(xi yi)：\n[0044] 如果沿巷道正方向角运动：\n[0045]\n[0046] 如果沿巷道负方向角运动：\n[0047]\n[0048] ②弯曲巷道中\n[0049] 已知巷道所在圆心坐标(a b)，半径r，令positionPoint(i-1)坐标点与y轴正方向夹角为\n[0050] 由数学公式可计算两坐标点弧线间的距离d'i：\n[0051]\n[0052] 有已知条件，可知positionPoint(i-1)坐标点与圆心点间斜率为：\n[0053]\n[0054] 即可求positionPoint(i-1)坐标点与y轴正方向夹角为\n[0055] 如果沿巷道正方向角运动，positionPoint(i)坐标(xi yi)为：\n[0056]\n[0057] 如果沿巷道负方向角运动，positionPoint(i)坐标(xi yi)为：\n[0058]\n[0059] (xi yi)即为判别修正后positionPoint(i)点坐标，并将所求坐标存入位置信息数据库中，作为下一位置判别修正的前项信息。\n[0060] 有益效果，由于采用了上述方案，本发明对动目标执行区域识别算法，判断动目标是否在巷道区域范围内，若不在巷道区域范围内，则根据前项反馈信息，采用位置修正算法对坐标进行修正；若在巷道区域范围内，判断所在巷道是直巷道还是弯曲巷道，并读取相应的方向角或者圆心点及半径，执行位置判别算法，根据巷道分类，判断动目标是否满足移动长度条件，如果满足，则直接输出结果，否则根据前项反馈信息，采用位置修正算法对坐标进行修正。最后将动目标信息存入位置信息数据库中，作为下一位置判别修正的前项信息。\n其方法简单、使用灵活、成本低。\n[0061] 优点：(1)在原有定位系统不变的基础上，充分利用井下巷道规划和动目标自身运动特征，对定位误差较大、超出给定估计范围的坐标点进行前向反馈修正，达到提高定位精度的目的；\n[0062] (2)本方法可用于井下各种不同的定位系统及定位方法，如基于Zigbee、Wifi、UWB等的定位系统，基于RSSI、TOA、TDOA等的定位方法，适用范围广、可移植性强。\n附图说明\n[0063] 图1为本发明井下巷道区域分配图。\n[0064] 图2为本发明直巷道中位置判别所属范围图。\n[0065] 图3为本发明弯曲巷道中位置判别所属范围图。\n[0066] 图4为本发明直巷道中位置求解图。\n[0067] 图5为本发明弯曲巷道中位置求解图。\n[0068] 图6为本发明基于前项反馈修正的井下动目标定位方法流程图。\n具体实施方式\n[0069] 下面结合附图对本发明的一个实施例作进一步的描述：\n[0070] 本发明的基于前项反馈修正的井下动目标定位方法根据井下平面图和巷道规划特点，首先建立平面坐标系，将各段不同的巷道标记为不同的区域，并将各个区域的方向角或者圆心点及半径存入信息数据库；然后对动目标执行区域识别算法，判断动目标是否在巷道区域范围内，若不在巷道区域范围内，则根据前项反馈信息，采用位置修正算法对坐标进行修正；若在巷道区域范围内，则执行位置判别算法，判断动目标是否满足移动长度条件，如果满足，则直接输出结果，否则根据前项反馈信息，采用位置修正算法对坐标进行修正；最后将动目标信息存入位置信息数据库中，作为下一位置判别修正的前项信息；具体方法是：\n[0071] (1)将井下定位系统加入时间戳；基站每隔时间Δt对动目标定位一次，并得到动目标的位置坐标；\n[0072] (2)利用井下平面图和巷道规划特点，将各段不同的巷道标记为不同的区域，并将各个区域的方向角或者圆心点及半径录入信息数据库；\n[0073] (3)对动目标坐标执行区域识别算法，判断动目标是否在巷道区域范围内，若不在巷道区域范围内，则根据前项反馈信息，采用位置修正算法对坐标进行修正；所述的位置修正算法，具体方法是：\n[0074] a.井下巷道中动目标的作业习惯，可将动目标在短时间内看作是匀速运动的，即positionPoint(i-1)，positionPoint(i-2)两点间在Δt时间内动目标是匀速运动的，若两次速度相同，则positionPoint(i-1)和positionPoint(i)两点间在Δt时间内动目标移动距离s′i与positionPoint(i-1)和positionPoint(i-2)两点间的移动距离是相同的，即s′i为：\n[0075]\n[0076] b.已知动目标沿巷道运动，巷道方向即为动目标移动的理想方向，即可取巷道方向为positionPoint(i-1)和positionPoint(i)之间的移动方向，又由位置判别算法可知，positionPoint(i-1)到testPoint(i)点趋向于沿巷道正(负)方向运动，可求\npositionPoint(i)的坐标；\n[0077] ①直巷道中\n[0078] 已知positionPoint(i-1)到testPoint(i)点趋向于沿巷道正(负)方向运动，取巷道正(负)方向角θ(π-θ)，求positionPoint(i)的坐标(xi yi)：\n[0079] 如果沿巷道正方向角运动：\n[0080]\n[0081] 如果沿巷道负方向角运动：\n[0082]\n[0083] ②弯曲巷道中\n[0084] 已知巷道所在圆心坐标(a b)，半径r，令positionPoint(i-1)坐标点与y轴正方向夹角为\n[0085] 由数学公式可计算两坐标点弧线间的距离d'i：\n[0086]\n[0087] 有已知条件，可知positionPoint(i-1)坐标点与圆心点间斜率为：\n[0088]\n[0089] 即可求positionPoint(i-1)坐标点与y轴正方向夹角为\n[0090] 如果沿巷道正方向角运动，positionPoint(i)坐标(xi yi)为：\n[0091]\n[0092] 如果沿巷道负方向角运动，positionPoint(i)坐标(xi yi)为：\n[0093]\n[0094] (xi yi)即为判别修正后positionPoint(i)点坐标，并将所求坐标存入位置信息数据库中，作为下一位置判别修正的前项信息。\n[0095] (4)若在巷道区域范围内，则读取所在巷道区域信息并执行位置判别算法，判断动目标是否满足移动长度条件，如果满足，直接输出结果，否则根据前项反馈信息，采用位置修正算法对坐标进行修正；所述的位置判别算法，具体方法是：\n[0096] a.已知当前时刻基站对动目标的定位坐标(xpi ypi)，用testPoint(i)表示；当前时刻判别修正后的坐标点(xi yi)，用positionPoint(i)表示；从位置信息数据库中读取动目标前两次位置坐标(xi-1 yi-1)、(xi-2 yi-2)，分别用positionPoint(i-1)、positionPoint(i-2)表示；\n[0097] b.从信息数据库判断坐标是在直巷道中还是曲线巷道中，如果在直巷道中，读取巷道正方向角θ；如果在曲线巷道中，读取巷道圆弧所在圆心坐标(a b)和半径r；\n[0098] c.井下巷道中动目标的作业习惯，将动目标在短时间内看作是匀速运动的，即将每次定位时间间隔Δt时间内动目标看作是匀速运动的，又因动目标运动速度有一定的范围，根据经验值可设置动目标运动过程中的最小速度vmin，最大速度vmax，默认速度vdef，vdef在动目标运动开始后第一个位置坐标中，作为前项信息使用；\n[0099] 根据经验速度可求动目标在Δt内移动的距离s的取值范围[smin smax]，可计算positionPoint(i-1)、testPoint(i)两坐标点间的移动距离di：\n[0100]\n[0101] positionPoint(i-1)与testPoint(i)点间直线与x轴的夹角θ'(范围[0 2π])：\n[0102]\n[0103] 接下来，判断是否满足移动长度条件：\n[0104] ①直巷道中\n[0105] 判断动目标在Δt时间内的移动长度是否符合实际，即判断di是否在[smin smax]范围内，如果满足条件，动目标坐标不需要修正，将testPoint(i)赋值给positionPoint(i)，直接输出，否则利用前项反馈信息对定位坐标进行修正；\n[0106] 在判定过程中，因不确定动目标移动方向，会出现两个区域范围，所以要根据前项positionPoint(i-1)和testPoint(i)信息，确定最终positionPoint(i)点所在区域；\n[0107] 已知直巷道中的正方向角θ，判断testPoint(i)更趋向于A、B哪个区域，即判断θ与θ'之间的关系：\n[0108] 当 时，从positionPoint(i-1)到testPoint(i)的直线趋向于沿巷\n道负方向运动，即判断满足条件的点在阴影A区域；\n[0109] 当 时，从positionPoint(i-1)到testPoint(i)直线趋向于沿巷道\n正方向运动，即判断满足条件的点在阴影B区域；\n[0110] ②弯曲巷道中\n[0111] 已知巷道半径r，前后两坐标间的移动距离di，Δt时间内移动的最小、最大弧长smin，smax，可求两定位点之间的最小、最大距离dmin，dmax。判断di是否在[dmin dmax]范围内，如果满足条件，动目标坐标不需要修正，将testPoint(i)赋值给positionPoint(i)，直接输出，否则利用前项反馈信息对定位坐标进行修正；\n[0112] 在判定过程中，因不确定动目标移动方向，会出现两个区域范围，所以要根据前项positionPoint(i-1)和testPoint(i)信息，确定最终positionPoint(i)点所在区域；\n[0113] 判断testPoint(i)更趋向于A、B哪个区域，即判断θ'与x轴之间的关系：\n[0114] 当 时，从positionPoint(i-1)到testPoint(i)的直线趋向于沿\n巷道负方向运动，即判断满足条件的点在阴影A区域；\n[0115] 否则，从positionPoint(i-1)到testPoint(i)的直线趋向于沿巷道正方向运动，即判断满足条件的点在阴影B区域。\n[0116] (5)将动目标坐标存入位置信息数据库中，作为下一位置判别修正的前项信息。\n[0117] 实施例1、\n[0118] 以井下人员定位为例：井下人员携带标识卡，与基站之间进行相互通信，并使用定位算法对人员进行初始定位。然后利用前项反馈信息，判别、修正井下动目标位置。\n[0119] 1、建立信息数据库\n[0120] (1)在井下巷道规划、施工后，可以得到井下巷道分布平面图及巷道的信息(根据施工可知巷道长度、宽度、位置，直巷道中可知巷道方向，弯曲巷道中根据曲线巷道标定方法，井下曲线为圆曲线，可知每段曲线巷道的圆心点、半径)；\n[0121] (2)根据井下平面图和巷道规划特点，建立平面坐标系，并依据巷道类别，直巷道方向，弯曲巷道圆心、半径的不同，将巷道分为不同的区域并作以标记。\n[0122] (3)将各段巷道区域边界信息录入信息数据库；\n[0123] (4)直巷道中，将巷道正方向角θ(范围[0 2π])，即巷道与平面图直角坐标系x轴正方向之间的夹角，录入信息数据库；\n[0124] (5)弯曲巷道中，将巷道圆心点坐标(a b)、半径r录入信息数据库。\n[0125] 区域划分图形说明如图1所示：\n[0126] 2、区域识别算法\n[0127] (1)将井下定位系统加入时间戳。基站每隔时间Δt对井下人员定位一次，并得到当前时刻基站对井下人员定位坐标点(xpi ypi)，用testPoint(i)表示，当前时刻判别修正后的坐标点用positionPoint(i)表示。\n[0128] (2)使用信息数据库对井下人员的位置坐标进行区域识别，判断井下人员是否在区域范围内(即是否在巷道中)，若不在区域范围内，则根据前项反馈信息，采用位置修正算法对坐标进行修正；若在区域范围内，则读取该区域数据库信息(直巷道中的正方向角θ，弯曲巷道中的圆心点坐标(a b)、半径r)并执行位置判别算法。\n[0129] 3、位置判别算法\n[0130] 井下巷道中井下人员的作业习惯，可将井下人员在短时间内看作是匀速运动的，即将每次定位时间间隔Δt时间内井下人员看作是匀速运动的，又由井下人员运动速度有一定的范围，根据经验值可设置井下人员运动过程中的最小速度vmin，最大速度vmax，默认速度vdef，vdef在井下人员运动开始后第一个位置坐标中，作为前项信息使用。\n[0131] 根据经验速度可求井下人员在Δt内移动的距离s的取值范围[smin smax]，即[0132]\n[0133] 从位置信息数据库中读取井下人员前一位置坐标点positionPoint(i-1)，不妨表示为(xi-1 yi-1)，可计算positionPoint(i-1)、testPoint(i)两坐标点间的移动距离di：\n[0134]\n[0135] positionPoint(i-1)到testPoint(i)点间直线与x轴的夹角θ'(范围[0 2π])：\n[0136]\n[0137] 接下来，判断是否满足移动长度条件：\n[0138] ①直巷道中\n[0139] 判断井下人员在Δt时间内的移动长度是否符合实际，即判断di是否在[smin smax]范围内，如果满足条件，井下人员坐标不需要修正，将testPoint(i)赋值给positionPoint(i)，直接输出，否则根据前项反馈信息，采用位置修正算法对坐标进行修正；如图2所示，图中两个圆分别是以positionPoint(i-1)为圆心，smin,smax长度为半径画圆，巷道为长带状，因此两圆与巷道的交集即为positionPoint(i-1)的估计位置。又因为在区域识别中排除了巷道外的点并知道井下人员运动的方向，可初步判断满足条件的点在阴影A、B区域。\n[0140] 已知直巷道中的正方向角θ，判断testPoint(i)更趋向于A、B哪个区域，即判断θ与θ'之间的关系：\n[0141] 当 时，从positionPo  int(i-1)  positionPoint(i-1)到\ntestPoint(i)的直线趋向于沿巷道负方向运动，即判断满足条件的点在阴影A区域。\n[0142] 当 时，从positionPoint(i-1) positionPoint(i-1)到testPoint\n(i)直线趋向于沿巷道正方向运动，即判断满足条件的点在阴影B区域。\n[0143] ②弯曲巷道中\n[0144] 已知巷道半径r和由公式(1)所求的最小、最大弧长smin，smax，求两定位点之间的最小、最大距离dmin，dmax。\n[0145] 根据弧长公式：\n[0146] s＝r·α    (4)\n[0147] 可求圆心角α，其中α为弧度角。\n[0148] 根据三角公式，可求得弧线间的距离d，即：\n[0149]\n[0150] 联立(4)式和(5)式，可求得dmin、dmax为：\n[0151]\n[0152] 由公式(2)可计算前后两坐标间的移动距离di，判断di是否在[dmin dmax]范围内，如果满足条件，井下人员坐标不需要修正，将testPoint(i)赋值给positionPoint(i)，直接输出，否则根据前项反馈信息，采用位置修正算法对坐标进行修正；如图3所示，可初步判断满足条件的点在阴影A、B区域。接着判断testPoint(i)更趋向于A、B哪个区域，即判断θ'与x轴之间的关系：\n[0153] 当 时，从positionPoint(i-1)到testPoint(i)的直线趋向于沿\n巷道负方向运动，即判断满足条件的点在阴影A区域。\n[0154] 否则，从positionPoint(i-1)到testPoint(i)的直线趋向于沿巷道正方向运动，即判断满足条件的点在阴影B区域。\n[0155] 4、位置修正\n[0156] (1)求井下人员在positionPoint(i-1)和positionPoint(i)两点间Δt时间内的移动距离\n[0157] 从位置信息数据库中读取前两次定位坐标点positionPoint(i-1)，\npositionPoint(i-2)的坐标(xi-1 yi-1)，(xi-2 yi-2)，可把短时间Δt内，positionPoint(i-\n1)，positionPoint(i-2)两点间井下人员看作是匀速运动的，若两次速度相同，则positionPoint(i-1)和positionPoint(i)两点间在Δt时间内井下人员移动距离s'i与positionPoint(i-1)，positionPoint(i-2)两点间的移动距离是相同的，即s'i为：\n[0158]\n[0159] (2)求positionPoint(i)的坐标\n[0160] 已知井下人员沿巷道运动，巷道方向即为井下人员移动的理想方向，即可取巷道方向为positionPoint(i-1)和positionPoint(i)之间的移动方向，又由位置判别算法可知，positionPoint(i-1)到testPoint(i)点趋向于沿巷道正(负)方向运动，testPoint(i)在如图2、图3所示的阴影B(A)内，可求positionPoint(i)的坐标。\n[0161] ①直巷道中\n[0162] 已知positionPoint(i-1)到testPoint(i)点趋向于沿巷道正(负)方向运动，取巷道正(负)方向角θ(π-θ)，求positionPoint(i)的坐标(xi yi)(如图4所示)：\n[0163] 如果沿巷道正方向角运动：\n[0164]\n[0165] 如果沿巷道负方向角运动：\n[0166]\n[0167] ②弯曲巷道中\n[0168] 已知巷道所在圆心坐标(a b)，半径r，令positionPoint(i-1)与y轴正方向夹角为[0169] 由公式(7)可知Δt时间井下人员移动距离s'i，根据公式(4)可求得圆心角α：\n[0170]\n[0171] 由图5可知positionPoint(i-1)坐标点与圆心点间斜率为：\n[0172]\n[0173] 即可求positionPoint(i-1)坐标点与y轴正方向夹角为\n[0174] 如果沿巷道正方向角运动，positionPoint(i)坐标(xi yi)为：\n[0175]\n[0176] 如果沿巷道负方向角运动，positionPoint(i)坐标(xi yi)为：\n[0177]\n[0178] (xi yi)即为判别修正后positionPoint(i)点坐标，并将所求坐标存入位置信息数据库中，作为下一位置判别修正的前项信息。整个定位方法过程可用图6表示。