[发明专利]基于无线传感器网络的管道内检测机器人跟踪定位方法

权利要求书

1.一种基于无线传感器网络的管道内检测机器人跟踪定位方法，其特征在于，该方法具体包括以下步骤：

S1：根据动力学模型得到管道内检测机器人的位置X′和速度V′，并进行离散化处理，作为卡尔曼滤波算法的预测模型；

S2：根据测量模型，判断管道内检测机器人的位置，具体为：由中继器节点(Repeaternode，RN)向外发射无线电波信号，若管道内检测机器人上的移动传感器节点(Sensornode，SN)接收到信号，则进入高置信度区(High confidence zone，HCZ)，反之则进入低置信度区(Low confidence zone，LCZ)；在高置信度区时，由惯性测量单元(Inertialmeasurement 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)表示高斯随机白噪声；

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

假设估计过程和测量过程都有相同的时间间隙Δt，K用于表示时间点t＝KΔt，将公式(2)改成卡尔曼滤波的形式，得到：

X_K＝F_KX_K-1+G_KU_K-1+L_KW_K (3)

F_K＝exp(AΔt)≈(I+AΔt) (4)

G_K＝(exp(AΔt)-I)A^-1B≈BΔt (5)

L_K＝(exp(AΔt)-I)A^-1≈IΔt (6)

其中，X_K、U_K-1、W_K分别表示离散后的U(t)、W(t)；I表示单位矩阵。