[发明专利]一种基于卡尔曼滤波的超宽带室内定位系统及方法

权利要求书

1.一种基于卡尔曼滤波的超宽带室内定位系统，其特征在于，包括：DW1000测距模块、上位机、无线透明传输模块、OLED屏幕；所述上位机与DW1000测距模块、OLED屏幕通过SPI实现通信；

所述DW1000测距模块用于超宽带测距；

所述无线透明传输模块用于将DW1000测距模块测得的距离数据传输至上位机；

所述上位机用于接收串口数据、计算坐标点、对坐标位置进行卡尔曼滤波，进行图像绘制；

所述OLED屏幕用于根据指示灯的亮、灭、闪烁判断上电情况以及信号连通情况。

2.根据权利要求1所述的基于卡尔曼滤波的超宽带室内定位系统，其特征在于，所述DW1000测距模块采用对称双边双向进行测距。

3.一种基于卡尔曼滤波的超宽带室内定位方法，其特征在于，包括如下步骤：

步骤S1：配置系统初始状态；

步骤S2：DW1000测距模块进行超宽带测距，得到距离数据，并进行误差调整后传至上位机转化为位置坐标；

步骤S3：将上述位置坐标作为状态量进行卡尔曼滤波，并进行图像绘制。

4.根据权利要求3所述的基于卡尔曼滤波的超宽带室内定位方法，其特征在于，所述步骤S1的具体步骤如下：

步骤S101：通过串口将三个模块分别设置为Anchor1、Anchor2、Anchor3基站模式，其地址分别为0x01，0x02，0x03；将另一个模块设置为Tag标签模式，向标签发送命令T0，Tag标签的地址为0x00；

步骤S102：设置基站坐标分别为Anchor1(0，0)，Anchor2(2，0)，Anchor3(0，2.7)，以Anchor1-Anchor2为X轴，Anchor1-Anchor3为Y轴。

5.根据权利要求3所述的基于卡尔曼滤波的超宽带室内定位方法，其特征在于，所述步骤S2的具体步骤如下：

步骤S201：DW1000测距模块进行超宽带测距，得到距离数据，并进行误差调整，误差调整方程为：当distance<2.0m时，distance＝distance+0.1×(2.2-distance)，为了减小数据的波动，令distance＝0.3×distance+0.7×distance_pre，再将得到的平均数据带入误差调整方程中得到最终距离值；

步骤S202：将最终标签距离三个基站的距离数据通过无线透明传输模块传输至电脑串口，在上位机接收数据，数据发送的格式为：1：distance1，2：distance2，3：distance3，其中1：、2：、3：分别表示编号为1、2、3的基站，distance1、distance2、distance3分别表示实际测得的三个距离，“，”作为分隔符便于分割；

步骤S203：上位机接收到上述步骤所述格式的字符串后按照特征符号进行分割，将三个距离结果分别赋值于d1，d2，d3，通过公式计算出Tag坐标(x，y)，其中，x_i,y_i(i＝1,2,3)分别表示三个基站Anchor1，Anchor2，Anchor3的坐标值，x₁＝0,y₁＝0,x₂＝2,y₂＝0,x₃＝0,y₃＝2.7。

6.根据权利要求3所述的基于卡尔曼滤波的超宽带室内定位方法，其特征在于，所述步骤S3的具体步骤如下：

步骤S301：卡尔曼的初始化参数为：

状态向量x_p_k,y_p_k分别为当前位置的横纵坐标值；

状态转移方程

其中，A为状态转移矩阵，在此w_k为k时刻的系统噪声，是服从N(0,Q)的高斯噪声，其协方差即预测当前的状态值为上一时刻的状态值加上高斯噪声；

观测方程

其中，x_m_k′,y_m_k′分别为根据预测值得到的横纵坐标，H为观测矩阵，在此v_k为k时刻的测量噪声，是服从N(0,R)的高斯噪声，其协方差为

误差协方差矩阵为

卡尔曼初始估计结果s_e_k＝第一次预测值s_p_k＝第一次测量值s_m_k；

步骤S302：将上位机计算得到的Tag坐标(x，y)分别赋值给s_m_k和s_p_k，即可得到：

实际测量值其中，x_m_k,y_m_k分别为DW1000测距模块测量得到的横纵坐标；

卡尔曼预测方程为：

其中，s_e_k为卡尔曼初始估计结果，A为状态转移矩阵，在此

误差协方差的预测方程更新为：

其中，A为状态转移矩阵，A^T为状态转移矩阵的转置，P为误差协方差矩阵；Q为协方差，

卡尔曼增益更新为：

其中，H为观测矩阵，H^T为观测矩阵的转置，R为协方差，

计算k时刻的状态最优估计值为：

其中，s_p_k为状态向量，s_m_k为实际测量值，H为观测矩阵，

最优误差协方差的估计值为：

其中，I为单位矩阵，H为观测矩阵，K为卡尔曼增益；

步骤S303：获取当前时间，分别动态绘制t-x，t-y，x-y的原始坐标数据以及卡尔曼估计坐标数据，进行效果对比。