CN112595320A - 基于ros的室内智能轮椅高精定位自主导航方法及系统 - Google Patents

基于ros的室内智能轮椅高精定位自主导航方法及系统 Download PDF

Info

Publication number
CN112595320A
CN112595320A CN202011332916.9A CN202011332916A CN112595320A CN 112595320 A CN112595320 A CN 112595320A CN 202011332916 A CN202011332916 A CN 202011332916A CN 112595320 A CN112595320 A CN 112595320A
Authority
CN
China
Prior art keywords
imu
wheelchair
positioning
ros
indoor
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202011332916.9A
Other languages
English (en)
Other versions
CN112595320B (zh
Inventor
金晓明
廖天睿
吴向东
汤璧钧
王俊奎
赵林惠
戴亚平
廣田薰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Beijing Union University
Original Assignee
Beijing Institute of Technology BIT
Beijing Union University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT, Beijing Union University filed Critical Beijing Institute of Technology BIT
Priority to CN202011332916.9A priority Critical patent/CN112595320B/zh
Publication of CN112595320A publication Critical patent/CN112595320A/zh
Application granted granted Critical
Publication of CN112595320B publication Critical patent/CN112595320B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开的基于ROS的室内智能轮椅高精定位自主导航方法及系统，属于室内移动轮椅导航技术领域。本发明提供一种基于ROS的室内智能轮椅高精定位自主导航方法和一种基于ROS的室内智能轮椅高精定位自主导航系统。本发明基于机器人操作系统ROS平台开发实现，包括室内融合定位子系统和智能导航子系统。所述ROS为UWB室内定位子系统和智能导航子系统的实施平台，并用于提供通讯机制、工具软件包以及库函数。本发明不仅能够提高室内智能轮椅定位精度和定位导航系统实时性，还能够利用强跟踪卡尔曼滤波器，使得轮椅在运动状态发生剧烈变化时依然能有较高的定位精度，并且室内智能轮椅高精定位自主导航系统具有可拓展性，方便用户根据个人需求进行拓展。

Description

基于ROS的室内智能轮椅高精定位自主导航方法及系统
技术领域
本发明属于室内移动轮椅导航技术领域，具体涉及基于ROS的室内智能轮椅高精定位自主导航方法及系统。
背景技术
随着当今社会生活环境智能化程度的提高，老人和残障人士等弱势群体对生活质量的要求也越来越高，智能轮椅的出现改善了他们的生活质量和生活自由度。当下社会老龄化问题严重，智能轮椅是提高老人生活质量的主要手段，同样对于残障人士来讲，智能轮椅能够帮助其进行一定范围的自由活动。智能轮椅关键技术是自主定位导航技术，要求能够精确反映轮椅当前位置，并规划出一条从当前位置到达目标点的安全快捷的路径。然而，目前现有的智能轮椅主要通过人为控制或者根据传感器信息半自主控制，并不能完全实现智能自主导航，特别是在室内封闭复杂环境中，由于接收不到GPS信号，无法做到室内精准定位，而常见的室内定位方法又不能满足智能轮椅的精度要求。
发明内容
针对现有技术室内智能轮椅导航技术存在的上述技术问题，本发明的目的是提供一种基于ROS的室内智能轮椅高精定位自主导航方法和一种基于ROS的室内智能轮椅高精定位自主导航系统，不仅能够提高室内智能轮椅定位精度和定位导航系统实时性，还能够利用强跟踪卡尔曼滤波器，使得轮椅在运动状态发生剧烈变化时依然能有较高的定位精度，并且室内智能轮椅高精定位自主导航系统具有可拓展性，方便用户根据个人需求进行拓展。
本发明目的是通过下述技术方案实现的：
本发明公开的一种基于ROS的室内智能轮椅高精定位自主导航方法，所述ROS为机器人操作系统(Robot Operating System，ROS)，利用超带宽UWB(Ultra Wide Band)定位技术结合基于信号飞行时间的定位算法TOF(Time of Flight)，与惯性测量单元IMU(Inertial Measurement Unit)融合搭建室内融合定位子系统；在解算出标签到基站的距离以及IMU惯导解算位置(xIMU,yIMU)，对(xIMU,yIMU)与标签到各个基站的距离进行强跟踪卡尔曼滤波，获得(xIMU,yIMU)的误差，将(xIMU,yIMU)减去误差值，即获得轮椅的准确位置(x,y)；在ROS系统中将高精定位信息通过UWB节点以坐标系变换TF(Coodinate FramesTransforms)消息的形式发布；在ROS系统中的节点中通过订阅UWB高精定位数据和目标点信息在栅格地图上生成一条从当前位置到目标点的安全路径。智能导航子系统接收新的定位信息，并根据当前所在位置生成新的通往目标点的路径，即基于ROS的室内智能轮椅高精定位自主导航。
本发明公开的一种基于ROS的室内智能轮椅高精定位自主导航方法，包括如下步骤：
步骤一、构建栅格地图并生成代价地图，在机器人操作系统ROS(Robot OperatingSystem)中以消息的形式发送，并被节点订阅，从而被定位及路径规划部分调用。
为了提高基于ROS的室内智能轮椅高精定位自主导航系统兼容性，作为优选，步骤一中，在机器人操作系统ROS中以map消息的形式发送，并被move_base节点订阅。
步骤二、利用超带宽UWB(Ultra Wide Band)定位技术结合基于信号飞行时间的定位算法TOF(Time of Flight)，与惯性测量单元IMU(Inertial Measurement Unit)融合搭建定位系统。定位系统至少包括三个UWB基站和一个UWB标签，IMU被绑定在UWB标签上，利用TOF求解出轮椅到各个基站的距离d1,d2,…,dn，并通过IMU信息解算出标签的坐标(xIMU,yIMU)；
在保证定位精度的前提下，从节约成本的角度出发，作为优选，UWB基站数量为三个，三个UWB基站呈三角形式安装，在步骤二中，利用TOF求解出轮椅到各个基站的距离d1,d2,d3，并通过IMU信息解算出标签的坐标(xIMU,yIMU)。
步骤三、对(xIMU,yIMU)与标签到各个基站的距离d1,d2,…,dn进行强跟踪卡尔曼滤波，利用强跟踪卡尔曼滤波器，获得(xIMU,yIMU)的误差(δpx,δpy)。将(xIMU,yIMU)减去误差值，即获得轮椅的准确位置(x,y)。即通过强跟踪卡尔曼滤波使得轮椅在运动状态发生剧烈变化时依然能有较高的定位精度。
步骤三具体实现步骤如下：
步骤3.1、初始化卡尔曼滤波参数，包括过程噪声误差协方差矩阵Qk，观测向量YK，观测噪声误差协方差矩阵Rk，观测采样间隔T以及系统状态XK。系统状态方程如下所示：
Figure BDA0002791696150000021
式中Xk与F如下所示：
x＝[δvx,k δvy,k δpx,k δpy,k]T
Figure BDA0002791696150000022
观测方程如下所示：
YK＝h(XK)+ηK
其中ηK为观测噪声，YK的值如下所示：
Figure BDA0002791696150000031
其中
Figure BDA0002791696150000032
为IMU测量出的标签到第i个基站的距离，
Figure BDA0002791696150000033
为UWB测量出的标签到第i个基站的距离；此外h(XK)值为：
Figure BDA0002791696150000034
其中(xi,yi)为第i个基站的坐标。
步骤3.2、估计下一时刻系统状态估计量
Figure BDA0002791696150000035
Figure BDA0002791696150000036
步骤3.3、输入当前观测量，将观测量线性化，线性化后的观测方程如下：
YK＝HkXk
其中
Figure BDA0002791696150000037
步骤3.4、根据系统状态估计量计算残差：
Figure BDA0002791696150000038
步骤3.5、根据残差实时调整状态估计误差的协方差阵Pk+1|k：
Pk+1|k＝λk+1FPk|kFT+Qk
其中λk+1为时变渐消因子：
Figure BDA0002791696150000039
式中λ0计算如下：
Figure BDA00027916961500000310
式中Sk+1计算如下，其中ρ为遗忘因子，取ρ＝0.95：
Figure BDA00027916961500000311
通过实时调整状态预报误差的协方差阵，实现在卡尔曼滤波中加入强跟踪滤波的性能。强跟踪滤波在运动状态剧烈变化时，通过实时检测残差值实时调整系统状态估计量
Figure BDA00027916961500000312
的误差的协方差阵Pk+1|k，使得系统在运动状态剧烈变化时具有更高的精度。
步骤3.6、输出Pk+1|k的增益阵Kk+1以及系统状态量的最优估计值Xk+1，并更新系统状态估计量的误差的协方差阵Pk+1|k+1：
Kk+1＝Pk+1|kHk T[HkPk+1|kHk T+Rk+1]-1
Figure BDA0002791696150000041
Pk+1|k+1＝[I-Kk+1Hk]Pk+1|k
步骤3.7、从Xk+1提取出IMU误差的最优估计值(δpx,δpy)，将(xIMU,yIMU)减去(δpx,δpy)即为轮椅准确坐标。
作为优选，步骤三中、对(xIMU,yIMU)与d0,d1,d2进行强跟踪卡尔曼滤波，获得(xIMU,yIMU)的误差。将(xIMU,yIMU)减去误差值，即可获得轮椅的准确位置(x,y)；
步骤四：定位子系统的定位信息通过ROS在UWB节点以TF消息的形式发送，被move_base节点所订阅。
步骤五、导航子系统根据当前UWB高精定位数据和目标点信息在栅格地图上生成一条从当前位置到目标点的路径。在完成一次路径规划后，导航子系统接收新的定位信息，并根据当前所在位置生成新的通往目标点的路径。
步骤六：根据步骤一至步骤五实现基于ROS的室内智能轮椅高精定位自主导航。
本发明公开的一种基于ROS的室内智能轮椅高精定位自主导航系统，基于机器人操作系统ROS(Robot Operating System)平台开发实现，包括室内融合定位子系统和智能导航子系统。
所述ROS为UWB室内定位子系统和智能导航子系统的实施平台，并用于提供通讯机制、工具软件包以及库函数。
所述室内融合定位子系统包括至少三个设置在室内的定位基站和一个安装在智能轮椅上的定位标签，还包括传感器信息解算模块与强跟踪卡尔曼滤波模块。
所述智能导航子系统包括代价地图生成模块和全局路径规划模块。
代价地图生成模块用于根据栅格地图与轮椅的体积信息生成代价地图，在机器人操作系统ROS(Robot Operating System)中以消息的形式发送，并被节点订阅，从而被定位及路径规划部分调用。
传感器信息解算模块用于从传感器中读取到IMU传感器信息，解算出轮椅的坐标(xIMU,yIMU)，并从传感器中读取UWB基站到各个标签的距离d0,d1…dn，将坐标(xIMU,uIMU)与距离d0,d1…dn发布至强跟踪卡尔曼滤波模块。
强跟踪卡尔曼滤波模块用于通过强跟踪卡尔曼滤波使得轮椅在运动状态发生剧烈变化时依然能有较高的定位精度。将轮椅的坐标(xIMU,yIMU)与距离d0,d1…dn作为观测量进行强跟踪卡尔曼滤波，获得利用IMU解算出的轮椅坐标(xIMU,yIMU)的准确误差(ΔxIMU,ΔyIMU)。将坐标(xIMU,yIMU)减去(ΔxIMU,ΔyIMU)后，即获得准确坐标(x,y)，并将准确坐标(x,y)发布至全局路径规划模块。
全局路径规划模块用于接收准确坐标(x,y)、目标点坐标以及代价地图信息，生成一条从当前位置到目标点的路径。在完成一次路径规划后，全局路径规划模块接收新的定位信息，并根据当前所在位置生成新的通往目标点的路径。
有益效果：
1、目前现有的智能轮椅主要通过人为控制或者根据传感器信息半自主控制，并不能完全实现智能自主导航，特别是在室内封闭复杂环境中，由于接收不到GPS信号，无法做到室内精准定位，而常见的室内定位方法又不能满足智能轮椅的精度要求。本发明公开的基于ROS的室内智能轮椅高精定位自主导航方法及系统，无需接收GPS信号，利用超带宽UWB(Ultra Wide Band)定位技术结合基于信号飞行时间的定位算法TOF(Time of Flight)，与惯性测量单元IMU(Inertial Measurement Unit)融合搭建定位系统；在解算出标签到基站的距离以及IMU惯导解算位置(xIMU,yIMU)，对(xIMU,yIMU)与标签到各个基站的距离进行强跟踪卡尔曼滤波，获得(xIMU,yIMU)的误差，将(xIMU,yIMU)减去误差值，即获得轮椅的准确位置(x,y)；在ROS系统中将高精定位信息通过UWB节点以坐标系变换TF(Coodinate FramesTransforms)消息的形式发布；在ROS系统中的节点中通过订阅UWB高精定位数据和目标点信息在栅格地图上生成一条从当前位置到目标点的安全路径。导航子系统接收新的定位信息，并根据当前所在位置生成新的通往目标点的路径，即基于ROS的室内智能轮椅高精定位自主导航。
2、本发明公开的基于ROS的室内智能轮椅高精定位自主导航方法及系统，利用强跟踪卡尔曼滤波器，使得轮椅在运动状态发生剧烈变化时依然能有较高的定位精度。
3、本发明公开的基于ROS的室内智能轮椅高精定位自主导航方法及系统，UWB结合ROS的信息传输使得系统的定位和路径规划能够同时进行，加快系统工作的周期，使系统的实时性更强。
4、本发明公开的基于ROS的室内智能轮椅高精定位自主导航方法及系统，本系统具有插件化、模块化和接口化的特性，方便拓展用户自定义需求，具有很大的拓展空间。
附图说明
图1是本发明的基于ROS的室内智能轮椅高精定位自主导航方法流程图；
图2是本发明的一种基于ROS的室内智能轮椅高精定位自主导航系统模块图；
图3是本发明实例中基于ROS的室内智能轮椅高精定位自主导航方法流程图；
图4是本发明实例中系统生成的代价地图；
图5是本发明实例中定位系统示意图；
图6是本发明实例中定位系统结构图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面结合实施方式和附图，对本发明作进一步地详细描述，具体流程参见图3。
如图1所示，本实施例公开的基于ROS的室内智能轮椅高精定位自主导航方法，具体实现步骤如下：
步骤一、根据室内环境建立栅格地图，并在栅格地图的基础上生成代价地图，在ROS中以map消息的形式发送，并被move_base节点订阅，从而能够被定位及路径规划部分调用；
其中，代价地图为利用栅格地图根据轮椅外接圆半径生成的地图，如图4所示。
步骤二、利用UWB定位技术结合TOF算法搭建定位系统。定位系统包括三个UWB基站和一个UWB标签，IMU传感器被置于UWB标签上，利用TOF求解出轮椅到三个基站的距离d0，d1，d2，并通过IMU信息解算出标签的坐标(xIMU，yIMU)；
坐标(xIMU，yIMU)利用上一次采样时轮椅速度与坐标以及本次采样的IMU信息得到，由于IMU传感器测量的信息仅包含加速度accx|k、accy|k与角加速度accan|k，故需首先对速度vx|k、vy|k与角速度van|k进行推算，推算过程如下所示。
vx|k＝vx|k-1+accx|k-1T
vy|k＝vy|k-1+accy|k-1T
van|k＝van|k-1+accan|k-1T
利用上述信息，即能够推算出当前面向角度ank与坐标
Figure BDA0002791696150000061
ank＝ank-1+van|k-1T
Figure BDA0002791696150000062
Figure BDA0002791696150000063
基于UWB的定位系统如图5所示，其中A0、A1、A2为三个UWB基站，T0为UWB标签，IMU传感器同样置于UWB标签内，固定在智能轮椅上，并与固定于智能轮椅的个人计算机(Personal Computer，PC)相连接。
步骤三、在解算出距离d0,d1,d2和坐标(xIMU,yIMU)后，对于解出的数据进行强跟踪卡尔曼滤波，即可获得利用IMU解算出的轮椅坐标(xIMU,yIMU)的准确误差(ΔxIMU,ΔyIMU)。将坐标(xIMU,yIMU)减去(ΔxIMU,ΔyIMU)后，即可获得准确坐标(x,y)，具体步骤如下所示：
步骤3.1、初始化卡尔曼滤波参数，包括过程噪声误差协方差矩阵Qk，观测向量YK，观测噪声误差协方差矩阵Rk，观测采样间隔T以及系统状态XK。系统状态方程如下所示：
Figure BDA0002791696150000071
式中Xk与F如下所示：
x＝[δvx，k δvy，k δpx，k δpy，k]T
Figure BDA0002791696150000072
观测方程如下所示：
YK＝h(xK)+ηK
其中ηK为观测噪声，YK的值如下所示：
Figure BDA0002791696150000073
其中
Figure BDA0002791696150000074
为IMU测量出的标签到第i个基站的距离，
Figure BDA0002791696150000075
为UWB测量出的标签到第i个基站的距离；此外h(XK)值为：
Figure BDA0002791696150000076
其中(xi,yi)为第i个基站的坐标。
在获得状态方程与观测方程后，需要初始化过程噪声误差协方差矩阵Qk与观测噪声误差协方差矩阵Rk，根据经验，初始化Qk与Rk为：
Qk＝diag(0.5,0.5,0.25,0.25)
Rk＝diag(1,1,1)
步骤3.2、估计下一时刻系统状态估计量
Figure BDA0002791696150000077
Figure BDA0002791696150000078
步骤3.3、输入当前观测量，将观测量线性化，线性化后的观测方程如下：
YK＝Hkxk
其中
Figure BDA0002791696150000079
步骤3.4、根据系统状态估计量计算残差：
Figure BDA00027916961500000710
步骤3.5、根据残差实时调整状态估计误差的协方差阵Pk+1|k：
Pk+1|k＝λk+1FPk|kFT+Qk
其中λk+1为时变渐消因子：
Figure BDA0002791696150000081
式中λ0计算如下：
Figure BDA0002791696150000082
式中Sk+1计算如下，其中ρ为遗忘因子，取ρ＝0.95：
Figure BDA0002791696150000083
步骤3.6、输出Pk+1|k的增益阵Kk+1以及系统状态量的最优估计值Xk+1，并更新系统状态估计量的误差的协方差阵Pk+1|k+1：
Kk+1＝Pk+1|kHk T[HkPk+1|kHk T+Rk+1]-1
Figure BDA0002791696150000084
Pk+1|k+1＝[I-Kk+1Hk]Pk+1|k
步骤3.7、从Xk+1提取出IMU误差的最优估计值(δpx，δpy)，将(xIMU，yIMU)减去(δpx，δpy)即为轮椅准确坐标。
步骤四、系统的定位信息通过ROS系统在UWB节点以TF消息的形式发送，被move_base节点所订阅；
步骤五、根据当前的位置和目标点信息在栅格地图上生成一条从当前位置到目标点的路径。在完成一次路径规划后，系统将会接收新的定位信息，并根据当前所在位置生成新的通往目标点的路径。
如图2所示，本实施例公开的一种基于ROS的室内智能轮椅高精定位自主导航系统，基于机器人操作系统ROS(Robot Operating System)平台开发实现，包括室内融合定位子系统和智能导航子系统。
所述ROS为UWB室内定位子系统和智能导航子系统的实施平台，并用于提供通讯机制、工具软件包以及库函数。
所述室内融合定位子系统包括至少三个设置在室内的定位基站和一个安装在智能轮椅上的定位标签，还包括传感器信息解算模块与强跟踪卡尔曼滤波模块。
所述智能导航子系统包括代价地图生成模块和全局路径规划模块。
智能导航子系统通过订阅UWB室内定位子系统提供的TF消息(高精定位信息)、map消息和在人机交互界面得到的目标点信息，利用路径规划算法(如A*算法)规划出一条从起点到目标点的无碰撞路径，以指导智能轮椅安全到达目标点。
代价地图生成模块用于根据栅格地图与轮椅的体积信息生成代价地图，在机器人操作系统ROS(Robot Operating System)中以消息的形式发送，并被节点订阅，从而被定位及路径规划部分调用。
传感器信息解算模块用于从传感器中读取到IMU传感器信息，解算出轮椅的坐标(xIMU,yIMU)，并从传感器中读取UWB基站到各个标签的距离d0,d1…d2，将坐标(xIMU,yIMU)与距离d0,d1…d2发布至强跟踪卡尔曼滤波模块。
强跟踪卡尔曼滤波模块用于通过强跟踪卡尔曼滤波使得轮椅在运动状态发生剧烈变化时依然能有较高的定位精度。将轮椅的坐标(xIMU,yIMU)与距离d0,d1…d2作为观测量进行强跟踪卡尔曼滤波，获得利用IMU解算出的轮椅坐标(xIMU,yIMU)的准确误差(ΔxIMU,ΔyIMU)。将坐标(xIMU,yIMU)减去(ΔxIMU,ΔyIMU)后，即获得准确坐标(x,y)，并将准确坐标(x,y)发布至全局路径规划模块。
全局路径规划模块用于接收准确坐标(x,y)、目标点坐标以及代价地图信息，生成一条从当前位置到目标点的路径。在完成一次路径规划后，全局路径规划模块接收新的定位信息，并根据当前所在位置生成新的通往目标点的路径。
以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种基于ROS的室内智能轮椅高精定位自主导航方法，其特征在于：包括如下步骤，
步骤一、构建栅格地图并生成代价地图，在机器人操作系统ROS(Robot OperatingSystem)中以消息的形式发送，并被节点订阅，从而被定位及路径规划部分调用；
步骤二、利用超带宽UWB(Ultra Wide Band)定位技术结合基于信号飞行时间的定位算法TOF(Time of Flight)，与惯性测量单元IMU(Inertial Measurement Unit)融合搭建定位系统；定位系统至少包括三个UWB基站和一个UWB标签，IMU被绑定在UWB标签上，利用TOF求解出轮椅到各个基站的距离d1，d2，...，dn，并通过IMU信息解算出标签的坐标(xIMU，yIMU)；
步骤三、对(xIMU，yIMU)与标签到各个基站的距离d1，d2，...，dn进行强跟踪卡尔曼滤波，利用强跟踪卡尔曼滤波器，获得(xIMU，yIMU)的误差(δpx，δpy)；将(xIMU，yIMU)减去误差值，即获得轮椅的准确位置(x，y)；即通过强跟踪卡尔曼滤波使得轮椅在运动状态发生剧烈变化时依然能有较高的定位精度；
步骤四：定位子系统的定位信息通过ROS在UWB节点以TF消息的形式发送，被move_base节点所订阅；
步骤五、导航子系统根据当前UWB高精定位数据和目标点信息在栅格地图上生成一条从当前位置到目标点的路径；在完成一次路径规划后，导航子系统接收新的定位信息，并根据当前所在位置生成新的通往目标点的路径；
步骤六：根据步骤一至步骤五实现基于ROS的室内智能轮椅高精定位自主导航。
2.如权利要求1所述的一种基于ROS的室内智能轮椅高精定位自主导航方法，其特征在于：为了提高基于ROS的室内智能轮椅高精定位自主导航系统兼容性，步骤一中，在机器人操作系统ROS中以map消息的形式发送，并被move_base节点订阅。
3.如权利要求2所述的一种基于ROS的室内智能轮椅高精定位自主导航方法，其特征在于：在保证定位精度的前提下，从节约成本的角度出发，UWB基站数量为三个，三个UWB基站呈三角形式安装，在步骤二中，利用TOF求解出轮椅到各个基站的距离d1，d2，d3，并通过IMU信息解算出标签的坐标(xIMU，yIMU)。
4.如权利要求3所述的一种基于ROS的室内智能轮椅高精定位自主导航方法，其特征在于：步骤三具体实现步骤如下，
步骤3.1、初始化卡尔曼滤波参数，包括过程噪声误差协方差矩阵Qk，观测向量YK，观测噪声误差协方差矩阵Rk，观测采样间隔T以及系统状态XK；系统状态方程如下所示：
Figure FDA0002791696140000011
式中Xk与F如下所示：
x＝[δvx，k δvy，k δpx，k δpy，k]T
Figure FDA0002791696140000021
观测方程如下所示：
YK＝h(XK)+ηK
其中ηK为观测噪声，YK的值如下所示：
Figure FDA0002791696140000022
其中
Figure FDA0002791696140000023
为IMU测量出的标签到第i个基站的距离，
Figure FDA0002791696140000024
为UWB测量出的标签到第i个基站的距离；此外h(XK)值为：
Figure FDA0002791696140000025
其中(xi，yi)为第i个基站的坐标；
步骤3.2、估计下一时刻系统状态估计量
Figure FDA0002791696140000026
Figure FDA0002791696140000027
步骤3.3、输入当前观测量，将观测量线性化，线性化后的观测方程如下：
YK＝HkXk
其中
Figure FDA0002791696140000028
步骤3.4、根据系统状态估计量计算残差：
Figure FDA0002791696140000029
步骤3.5、根据残差实时调整状态估计误差的协方差阵Pk+1|k：
Pk+1|k＝λk+1FPk|kFT+Qk
其中λk+1为时变渐消因子：
Figure FDA00027916961400000210
式中λ0计算如下：
Figure FDA00027916961400000211
式中Sk+1计算如下，其中ρ为遗忘因子，取ρ＝0.95：
Figure FDA0002791696140000031
通过实时调整状态预报误差的协方差阵，实现在卡尔曼滤波中加入强跟踪滤波的性能；强跟踪滤波在运动状态剧烈变化时，通过实时检测残差值实时调整系统状态估计量
Figure FDA0002791696140000032
的误差的协方差阵Pk+1|k，使得系统在运动状态剧烈变化时具有更高的精度；
步骤3.6、输出Pk+1|k的增益阵Kk+1以及系统状态量的最优估计值Xk+1，并更新系统状态估计量的误差的协方差阵Pk+1|k+1：
Kk+1＝Pk+1|kHk T[HkPk+1|kHk T+Rk+1]-1
Figure FDA0002791696140000033
Pk+1|k+1＝[I-Kk+1Hk]Pk+1|k
步骤3.7、从Xk+1提取出IMU误差的最优估计值(δpx，δpy)，将(xIMU，yIMU)减去(δpx，δpy)即为轮椅准确坐标。
5.如权利要求4所述的一种基于ROS的室内智能轮椅高精定位自主导航方法，其特征在于：步骤三中、对(xIMU，yIMU)与d0，d1，d2进行强跟踪卡尔曼滤波，获得(xIMU，yIMU)的误差；将(xIMU，yIMU)减去误差值，即可获得轮椅的准确位置(x，y)。
6.一种基于ROS的室内智能轮椅高精定位自主导航系统，其特征在于：基于机器人操作系统ROS(Robot Operating System)平台开发实现，包括室内融合定位子系统和智能导航子系统；
所述ROS为UWB室内定位子系统和智能导航子系统的实施平台，并用于提供通讯机制、工具软件包以及库函数；
所述室内融合定位子系统包括至少三个设置在室内的定位基站和一个安装在智能轮椅上的定位标签，还包括传感器信息解算模块与强跟踪卡尔曼滤波模块；
所述智能导航子系统包括代价地图生成模块和全局路径规划模块；
代价地图生成模块用于根据栅格地图与轮椅的体积信息生成代价地图，在机器人操作系统ROS(Robot Operating System)中以消息的形式发送，并被节点订阅，从而被定位及路径规划部分调用；
传感器信息解算模块用于从传感器中读取到IMU传感器信息，解算出轮椅的坐标(xIMU，yIMU)，并从传感器中读取UWB基站到各个标签的距离d0，d1...dn，将坐标(xIMU，yIMU)与距离d0，d1...dn发布至强跟踪卡尔曼滤波模块；
强跟踪卡尔曼滤波模块用于通过强跟踪卡尔曼滤波使得轮椅在运动状态发生剧烈变化时依然能有较高的定位精度；将轮椅的坐标(xIMU，yIMU)与距离d0，d1...dn作为观测量进行强跟踪卡尔曼滤波，获得利用IMU解算出的轮椅坐标(xIMU，yIMU)的准确误差(ΔxIMU，ΔyIMU)；将坐标(xIMU，yIMU)减去(ΔxIMU，ΔyIMU)后，即获得准确坐标(x，y)，并将准确坐标(x，y)发布至全局路径规划模块；
全局路径规划模块用于接收准确坐标(x，y)、目标点坐标以及代价地图信息，生成一条从当前位置到目标点的路径；在完成一次路径规划后，全局路径规划模块接收新的定位信息，并根据当前所在位置生成新的通往目标点的路径。
CN202011332916.9A 2020-11-23 2020-11-23 基于ros的室内智能轮椅高精定位自主导航方法及系统 Active CN112595320B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011332916.9A CN112595320B (zh) 2020-11-23 2020-11-23 基于ros的室内智能轮椅高精定位自主导航方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011332916.9A CN112595320B (zh) 2020-11-23 2020-11-23 基于ros的室内智能轮椅高精定位自主导航方法及系统

Publications (2)

Publication Number Publication Date
CN112595320A true CN112595320A (zh) 2021-04-02
CN112595320B CN112595320B (zh) 2023-06-30

Family

ID=75183717

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011332916.9A Active CN112595320B (zh) 2020-11-23 2020-11-23 基于ros的室内智能轮椅高精定位自主导航方法及系统

Country Status (1)

Country Link
CN (1) CN112595320B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113115214A (zh) * 2021-06-16 2021-07-13 北京奇岱松科技有限公司 基于不可翻转定位标签的室内人体朝向识别系统
CN115307636A (zh) * 2022-07-13 2022-11-08 西北工业大学 一种智能轮椅自主导航方法

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103033186A (zh) * 2012-12-30 2013-04-10 东南大学 一种用于水下滑翔器的高精度组合导航定位方法
CN107192387A (zh) * 2017-05-23 2017-09-22 北京理工大学 一种基于无迹卡尔曼滤波的组合定位方法
CN108279003A (zh) * 2018-02-01 2018-07-13 福州大学 一种基于适用于变电站使用的无人机高精度定位巡检系统
CN108801269A (zh) * 2018-08-29 2018-11-13 山东大学 一种室内云机器人导航系统及方法
CN108981720A (zh) * 2018-10-25 2018-12-11 哈尔滨工业大学 Uwb与惯导融合的室内导航系统
CN109813319A (zh) * 2019-03-07 2019-05-28 山东大学 一种基于slam建图的开环优化方法及系统
CN110244715A (zh) * 2019-05-23 2019-09-17 西安理工大学 一种基于超宽带技术的多移动机器人高精度协同跟踪方法
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统
CN110532636A (zh) * 2019-08-05 2019-12-03 东南大学 一种面向多场景的智能驾驶自主车道保持性能检测方法
CN110954097A (zh) * 2019-12-13 2020-04-03 陕西瑞特测控技术有限公司 一种用于机器人组合的导航定位方法
GB202013641D0 (en) * 2020-08-31 2020-10-14 Continental Automotive Gmbh Unmanned moving objects and method of localization

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103033186A (zh) * 2012-12-30 2013-04-10 东南大学 一种用于水下滑翔器的高精度组合导航定位方法
CN107192387A (zh) * 2017-05-23 2017-09-22 北京理工大学 一种基于无迹卡尔曼滤波的组合定位方法
CN108279003A (zh) * 2018-02-01 2018-07-13 福州大学 一种基于适用于变电站使用的无人机高精度定位巡检系统
CN108801269A (zh) * 2018-08-29 2018-11-13 山东大学 一种室内云机器人导航系统及方法
CN108981720A (zh) * 2018-10-25 2018-12-11 哈尔滨工业大学 Uwb与惯导融合的室内导航系统
CN109813319A (zh) * 2019-03-07 2019-05-28 山东大学 一种基于slam建图的开环优化方法及系统
CN110244715A (zh) * 2019-05-23 2019-09-17 西安理工大学 一种基于超宽带技术的多移动机器人高精度协同跟踪方法
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统
CN110532636A (zh) * 2019-08-05 2019-12-03 东南大学 一种面向多场景的智能驾驶自主车道保持性能检测方法
CN110954097A (zh) * 2019-12-13 2020-04-03 陕西瑞特测控技术有限公司 一种用于机器人组合的导航定位方法
GB202013641D0 (en) * 2020-08-31 2020-10-14 Continental Automotive Gmbh Unmanned moving objects and method of localization

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
YOU ET AL.: "Data Fusion of UWB and IMU Based on Unscented Kalman Filter for Indoor Localization of Quadrotor UAV", IEEE ACCESS, vol. 8, pages 64971 - 64981, XP011783685, DOI: 10.1109/ACCESS.2020.2985053 *
周恭谦,等: "基于时频差的高阶强跟踪UKF算法", 系统工程与电子技术, vol. 40, no. 10, pages 2180 - 2187 *
徐元 等: "面向室内行人的 Ｒange-only UWB /INS 紧组合导航方法", 仪器仪表学报, vol. 37, no. 9, pages 2115 - 2121 *
徐元 等: "面向室内行人的Range-only UWB/INS紧组合导航方法", 仪器仪表学报, vol. 37, no. 9, pages 2115 - 2121 *
曹宇: "基于UWB的ROS机器人室内定位系统设计", 现代计算机, no. 17, pages 97 - 100 *
王古超: "基于 ROS 的全向移动机器人系统设计与研究", 中国优秀硕士学位论文全文数据库信息科技辑, no. 2020, pages 63 - 65 *
王古超: "基于ROS的全向移动机器人系统设计与研究", 中国优秀硕士学位论文全文数据库信息科技辑, no. 2020, pages 63 - 65 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113115214A (zh) * 2021-06-16 2021-07-13 北京奇岱松科技有限公司 基于不可翻转定位标签的室内人体朝向识别系统
CN115307636A (zh) * 2022-07-13 2022-11-08 西北工业大学 一种智能轮椅自主导航方法
CN115307636B (zh) * 2022-07-13 2024-04-30 西北工业大学 一种智能轮椅自主导航方法

Also Published As

Publication number Publication date
CN112595320B (zh) 2023-06-30

Similar Documents

Publication Publication Date Title
CN109459040B (zh) 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法
Fan et al. Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB
CN110244715B (zh) 一种基于超宽带技术的多移动机器人高精度协同跟踪方法
CN112595320B (zh) 基于ros的室内智能轮椅高精定位自主导航方法及系统
CN106225790A (zh) 一种无人车定位精度的确定方法及装置
CN113189613B (zh) 一种基于粒子滤波的机器人定位方法
Xiao et al. Residual attention network-based confidence estimation algorithm for non-holonomic constraint in GNSS/INS integrated navigation system
CN109781120A (zh) 一种基于同步定位构图的车辆组合定位方法
CN114111802B (zh) 一种行人航迹推算辅助uwb的定位方法
CN112797988B (zh) 基于神经网络的未知机动航天器轨道确定方法
CN206095257U (zh) 智能巡视机器人组合导航系统
CN117119586A (zh) 一种基于uwb和imu室内定位融合算法
Dou et al. A Lidar-assisted Self-localization Technology for Indoor Wireless Sensor Networks
CN114111840B (zh) 一种基于组合导航的dvl误差参数在线标定方法
Ding et al. Refined On-Manifold IMU Preintegration Theory for Factor Graph Optimization Based on Equivalent Rotation Vector
Liu et al. An UWB/PDR fusion algorithm based on improved square root unscented Kalman filter
CN112683263B (zh) 一种基于改进模型的uwb/imu/odom多传感器数据融合移动机器人定位方法
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone
Yuan et al. Research on indoor positioning and navigation method of AGV based on multi-sensor fusion
CN103322999A (zh) 一种适用于多艇导航的保留历史状态的信息滤波算法
CN117289294B (zh) 一种基于多分辨率贝叶斯网格的融合定位方法
CN116202534B (zh) 一种隧道内定位方法、装置、设备及存储介质
CN110749879B (zh) 一种基于多观测者测速信息的分布式目标跟踪方法
CN117031473B (zh) 一种水下目标协同轨迹跟踪方法
CN100359336C (zh) 基于小波变换的双平台纯角度机动目标融合跟踪方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant