[发明专利]基于ROS的室内智能轮椅高精定位自主导航方法及系统

权利要求书

1.一种基于ROS的室内智能轮椅高精定位自主导航方法，其特征在于：包括如下步骤，

步骤一、构建栅格地图并生成代价地图，在机器人操作系统ROS(Robot OperatingSystem)中以消息的形式发送，并被节点订阅，从而被定位及路径规划部分调用；

步骤二、利用超带宽UWB(Ultra Wide Band)定位技术结合基于信号飞行时间的定位算法TOF(Time of Flight)，与惯性测量单元IMU(Inertial Measurement Unit)融合搭建定位系统；定位系统至少包括三个UWB基站和一个UWB标签，IMU被绑定在UWB标签上，利用TOF求解出轮椅到各个基站的距离d₁，d₂，...，d_n，并通过IMU信息解算出标签的坐标(x^IMU，y^IMU)；

步骤三、对(x^IMU，y^IMU)与标签到各个基站的距离d₁，d₂，...，d_n进行强跟踪卡尔曼滤波，利用强跟踪卡尔曼滤波器，获得(x^IMU，y^IMU)的误差(δp_x，Sp_y)；将(x^IMU,y^IMU)减去误差值，即获得轮椅的准确位置(x，y)；即通过强跟踪卡尔曼滤波使得轮椅在运动状态发生剧烈变化时依然能有较高的定位精度；

步骤三具体实现步骤如下，

步骤3.1、初始化卡尔曼滤波参数，包括过程噪声误差协方差矩阵Q_k，观测向量Y_K，观测噪声误差协方差矩阵R_k，观测采样间隔T以及系统状态X_K；系统状态方程如下所示：

式中X_k与F如下所示：

x＝[δv_x,k δv_y,k δp_x,k δp_y，k]^T

观测方程如下所示：

Y_K＝h(X_K)+η_K

其中η_K为观测噪声，Y_K的值如下所示：

其中为IMU测量出的标签到第i个基站的距离，为UWB测量出的标签到第i个基站的距离；此外h(X_K)值为：

其中(x_i，y_i)为第i个基站的坐标；

步骤3.2、估计下一时刻系统状态估计量

步骤3.3、输入当前观测量，将观测量线性化，线性化后的观测方程如下：

Y_K＝H_kX_k

其中

步骤3.4、根据系统状态估计量计算残差：

步骤3.5、根据残差实时调整状态估计误差的协方差阵P_k+1|k：

P_k+1|k＝λ_k+1FP_k|kF^T+Q_k

其中λ_k+1为时变渐消因子：

式中λ₀计算如下：

式中S_k+1计算如下，其中ρ为遗忘因子，取ρ＝0.95：

通过实时调整状态预报误差的协方差阵，实现在卡尔曼滤波中加入强跟踪滤波的性能；强跟踪滤波在运动状态剧烈变化时，通过实时检测残差值实时调整系统状态估计量的误差的协方差阵P_k+1|k，使得系统在运动状态剧烈变化时具有更高的精度；

步骤3.6、输出P_k+1|k的增益阵K_k+1以及系统状态量的最优估计值X_k+1，并更新系统状态估计量的误差的协方差阵P_k+1|k+1：

K_k+1＝P_k+1|kH_k^T[H_kP_k+1|kH_k^T+R_k+1]^-1

P_k+1|k+1＝[I-K_k+1H_k]P_k+1|k

步骤3.7、从X_k+1提取出IMU误差的最优估计值(δp_x，δp_y)，将(x^IMU，y^IMU)减去(δp_x，δp_y)即为轮椅准确坐标；

步骤四：定位子系统的定位信息通过ROS在UWB节点以TF消息的形式发送，被move_base节点所订阅；

步骤五、导航子系统根据当前UWB高精定位数据和目标点信息在栅格地图上生成一条从当前位置到目标点的路径；在完成一次路径规划后，导航子系统接收新的定位信息，并根据当前所在位置生成新的通往目标点的路径；

步骤六：根据步骤一至步骤五实现基于ROS的室内智能轮椅高精定位自主导航。