1.一种基于无线传感器网络的管道内检测机器人跟踪定位方法，其特征在于，该方法具体包括以下步骤：S1：根据动力学模型得到管道内检测机器人的位置X′和速度V′，并进行离散化处理，作为卡尔曼滤波算法的预测模型；

S2：根据测量模型，判断管道内检测机器人的位置，具体为：由中继器节点(Repeater node，RN)向外发射无线电波信号，若管道内检测机器人上的移动传感器节点(Sensor node，SN)接收到信号，则进入高置信度区(High confidence zone，HCZ)，反之则进入低置信度区(Low confidence zone，LCZ)；在高置信度区时，由惯性测量单元(Inertial measurement unit，IMU)测得速度V，利用接收信号强度(Received signal strength，RSS)测得位置X；在低置信度区时，由惯性测量单元(Inertial measurement unit，IMU)测得速度V；

S3：在高置信度区由X,V,X′,V′得到管道内检测机器人的位置和速度；在低置信度区由V,X′,V′得到预估的管道内检测机器人位置和速度，然后利用RTS平滑算法优化位置和速度信息；

S4：通过预测模型输出管道内检测机器人的整个行程的位置和速度信息。

2.根据权利要求1所述的管道内检测机器人跟踪定位方法，其特征在于，该方法中，采用基站、RN和SN三级通信定位，其中SN搭载在地下管道内检测机器人中，基站与RN位于地面，基站与RN通信，RN与SN直接通信；整个管道区域划分成RN信号覆盖的区域和未覆盖的区域；将信号覆盖区域定为HCZ；未覆盖的区域定为LCZ。

3.根据权利要求1所述的管道内检测机器人跟踪定位方法，其特征在于，步骤S1中，构建管道内检测机器人的动力学模型，具体包括：由管道内检测机器人的运动受力得到下式物理模型：其中， 和 分别表示管道内检测机器人在t时刻的速度和加速度，ξ是粘性摩擦系数，m表示机器人质量，u(t)是施加的动力，w(t)表示高斯随机白噪声；

T

其中， U(t)＝u(t)，W(t)＝[0,w(t)] ；

假设估计过程和测量过程都有相同的时间间隙Δt，K用于表示时间点t＝KΔt，将公式(2)改成卡尔曼滤波的形式，得到：XK＝FKXK‑1+GKUK‑1+LKWK                     (3)FK＝exp(AΔt)≈(I+AΔt)      (4)‑1

GK＝(exp(AΔt)‑I)A B≈BΔt    (5)‑1

LK＝(exp(AΔt)‑I)A ≈IΔt     (6)其中，XK、UK‑1、WK分别表示离散后的 U(t)、W(t)；I表示单位矩阵。

4.根据权利要求1所述的管道内检测机器人跟踪定位方法，其特征在于，步骤S2中，构r r建的测量模型为：确定RSS与传播路径的关系：y (t)＝r[x(t)]，y (t)表示t时刻管道内检测机器人SN处的RSS，r[x(t)]是SN和RN的距离；采用对数长程衰减模型，有：r

y[dB]＝alog(ds)+b‑ωΩ[dB]    (7)其中，alog(ds)+b表示平均RSS，由现场测量得出；ωΩ表示均值为零，方差为Ω的噪声；

管道内检测机器人速度测量由IMU的数据得到：

v

y(t)＝v(t)+γΓ      (8)

v

其中，y (t)表示测量速度，v(t)表示实际速度；γΓ是均值为0，方差为Γ的噪声；令则测量向量变为：Y(t)＝h[x(t)，v(t)]+V(t)    (9)C

其中，V(t)是均值为零，协方差为R的噪声。

5.根据权利要求3所述的管道内检测机器人跟踪定位方法，其特征在于，步骤S2中，高置信度区的定位算法具体包括以下步骤：S201：初始化卡尔曼滤波：

其中，假设检测机器人在HCZ1区域Ka时刻出发， 表示假设的初始时刻的位置和速度向量， 表示假设的初始时刻的协方差矩阵；

S202：对位置和速度矢量进行预测：

其中， 表示由上一时刻预测得到的当前时刻位置和速度向量， 表示上一时刻的迭代更新的位置和速度向量值，PK|K‑1表示由上一时刻预测的当前时刻的状态协方差矩阵，PK‑1|K‑1表示上一时刻的迭代更新的协方差矩阵， 表示FK的转置， 表示随机噪声；

S203：迭代更新位置和速度：

PK|K＝(I‑KKHK)PK|K‑1      (18)其中， 是测量残差，YK表示当前测量的RSS和速度向量，h(·)是式(9)中关于预测的位置和速度向量的函数值，SK是残余协方差，KK是卡尔曼增益，表示了测量残差 相对于先前估计 的相对重要性， PK|K分别表示 与YK结合生成精确的估计位置和速度向量和协方差估计，HK是对 线性化处理后的矩阵。

6.根据权利要求3所述的管道内检测机器人跟踪定位方法，其特征在于，步骤S2中，低置信度区定位算法具体包括以下步骤：从HCZ1进入LCZ时采用扩展卡尔曼滤波器算法计算，估计SN通过LCZ的时间间隙(Kb，Kc)内的速度和位置，称为前向通道；后向通道是在得到前向通道处理的结果后计算SN从HCZ1移动到HCZ2的时间间隔(Km，Kn)内的位置和速度；

S211：基于卡尔曼滤波算法计算前向通道：

其中， 和 用于初始化位置和速度估计值，由上述的高置信度区定位算法计算得到；

按照上式初始化卡尔曼滤波器，然后迭代计算到KC；

PK|K＝(I‑KKHK)PK|K‑1       (27)其中， 表示由上一时刻预测的位置和速度向量， 表示上一时刻迭代更新的位置和速度向量，PK|K‑1表示由上一时刻预测的状态协方差矩阵，PK‑1|K‑1表示上一时刻迭代更新的协方差矩阵， 表示FK的转置， 是随机噪声； 是测量残差，YK表示当前测量的RSS和速度向量，hv是测量的速度，SK是残余协方差，KK是卡尔曼增益，表示了测量残差 相对于先前估计 的相对重要性，Γ表示均值为0，方差为Γ的噪声， PK|K分别表示与YK结合生成精确的位置和速度向量和协方差估计，HK是对 线性化处理后的矩阵；

S212：基于前向通道结果利用RTS平滑算法计算得到后向通道：其中，k＝kn‑1,kn‑2,...,km， 和 作为初始估计值，由高置信度区算法得到，CK表示平滑增益， 是CK的转置，PK+1|Kn表示上一时刻迭代更新的协方差矩阵；按照上式初始化并迭代计算时间间隔(Km,Kn)内的位置和速度，Km和Kn的选择依据精度和数据处理能力进行折中。