CN113324544B - 一种基于图优化的uwb/imu的室内移动机器人协同定位方法 - Google Patents

一种基于图优化的uwb/imu的室内移动机器人协同定位方法 Download PDF

Info

Publication number
CN113324544B
CN113324544B CN202110421274.8A CN202110421274A CN113324544B CN 113324544 B CN113324544 B CN 113324544B CN 202110421274 A CN202110421274 A CN 202110421274A CN 113324544 B CN113324544 B CN 113324544B
Authority
CN
China
Prior art keywords
robot
uwb
imu
data
pose
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.)
Active
Application number
CN202110421274.8A
Other languages
English (en)
Other versions
CN113324544A (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 University of Chemical Technology
Original Assignee
Beijing University of Chemical Technology
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 University of Chemical Technology filed Critical Beijing University of Chemical Technology
Priority to CN202110421274.8A priority Critical patent/CN113324544B/zh
Publication of CN113324544A publication Critical patent/CN113324544A/zh
Application granted granted Critical
Publication of CN113324544B publication Critical patent/CN113324544B/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
    • G01S5/10—Position of receiver fixed by co-ordinating a plurality of position lines defined by path-difference measurements, e.g. omega or decca systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于图优化的UWB/IMU的室内移动机器人定位方法，包括机器人运动控制系统，机器人通讯系统，UWB系统，IMU惯性测量单元，所述UWB系统包括2个及以上标签与IMU，4个及以上基站，所述标签将接收基站所发送的信号，通过TOF的方法测得标签与基站之间的距离。所述标签与IMU皆搭设于机器人。本发明利用UWB系统，IMU惯性测量单元，通过IMU数据初步校验UWB数据，判断数据是否受到NLOS影响，使用柯西鲁棒核函数，列文伯格‑马夸尔特算法优化机器人当前时刻的位姿。大大提高了使用UWB在非视距情况下，进行机器人定位时机器人定位位姿不准的问题。得到在视距情况下比单一UWB定位更加精确的定位效果。

Description

一种基于图优化的UWB/IMU的室内移动机器人协同定位方法
技术领域
本发明涉及一种室内移动机器人定位方法属于机器人领域，属于无线传感器网络的定位技术领域。
背景技术
超宽带(UWB)定位技术其本质上属于无线传感器网络的定位技术，在室内定位中，UWB定位技术因其定位精度较高，设备价格低廉，抗干扰能力强等特点而得到广泛应用，目前UWB定位技术主要采用TDOA测距算法，RSSI测距算法等各种算法进行定位距离估算，但TDOA，RSSI测距算法误差较大，且在机器人运动过程中，UWB无法评估机器人自身姿态，本发明利用UWB系统，IMU惯性测量单元，通过IMU数据初步校验UWB数据，判断数据是否受到NLOS影响，使用柯西鲁棒核函数，列文伯格-马夸尔特算法优化机器人当前时刻的位姿。大大提高了使用UWB在非视距情况下，进行机器人定位时机器人定位位姿不准的问题。得到在视距情况下比单一UWB定位更加精确的定位效果
发明内容
为了解决上述问题，本发明提出了一种基于图优化的UWB/IMU的室内移动机器人定位方法。
为了实现上述技术目的，本发明的技术方案为：
一种基于图优化的UWB/IMU的室内移动机器人协同定位方法，实现该方法的系统包括机器人运动控制系统、机器人通讯系统、UWB系统和IMU惯性测量单元；所述UWB系统包括2个及以上标签与IMU，4个及以上基站，所述标签将接收基站所发送的信号，通过TOF的方法测得标签与基站之间的距离。所述标签与IMU皆搭设于机器人。本发明利用UWB系统，IMU惯性测量单元，通过IMU数据初步校验UWB数据，判断数据是否受到NLOS影响，使用柯西鲁棒核函数，列文伯格-马夸尔特算法优化机器人当前时刻的位姿。大大提高了使用UWB在非视距情况下，进行机器人定位时机器人定位位姿不准的问题。得到在视距情况下比单一UWB定位更加精确的定位效果。
包括以下步骤：
(1)建立机器人UWB/IMU的测量模型；
(2)初步校验UWB数据，判断数据是否受到NLOS影响。
(3)构建机器人位姿优化图，推导误差函数，将机器人位置优化问题转化为位姿图优化问题。
(4)使用柯西鲁棒核函数，列文伯格-马夸尔特算法，优化机器人当前时刻的位姿；
在步骤(1)中，所述UWB测量模型如下：
Figure BDA0003026494090000021
其中L1，L2，L3....Ln为各基站到机器人的距离，(x1,y1,z`1),(x2,y2,z2),(x3,y3,z3).....(xn,yn,zn)表示为基站坐标，(xu,yu,zu)为机器人位置；
在步骤(1)中，所述IMU测量模型如下：
Figure BDA0003026494090000022
Figure BDA0003026494090000023
为机器人在j,i时刻的位置，
Figure BDA0003026494090000024
为机器人j.i时刻在世界坐标系下的速度，gw为世界坐标系下，机器人的重力加速度。Δt为两次采样时间间隔。
Figure BDA0003026494090000025
为imu预积分量。
Figure BDA0003026494090000026
为imu加速度计，陀螺仪的整体偏移量，
Figure BDA0003026494090000027
为整体偏移使用随机游走模型的参数变量。a,w为使用中值滤波法计算得到的机器人当前时刻线加速度与角速度。
步骤(2)中，初步校验UWB数据，判断数据是否受到NLOS影响。在k到k+1时刻有：
Figure BDA0003026494090000031
其中xk,xk-1为机器人在k，k-1时刻的位姿，因此可令
Figure BDA0003026494090000032
对rk进行检验，若
Figure BDA0003026494090000033
则可认为UWB出现了异常测量值。
丢弃该测量值，记录UWB中有效测量的数量。
若此时有效数量m＜2，则该时刻的UWB帧无法作为定位使用，利用IMU对该时刻进行保持。
若m＝2，由于室内环境中，地面相对平坦，可将z轴坐标看做一个常数，可令ZK＝ZK-1。
步骤(3)中，依照步骤2中的测量模型，构建误差函数
Figure BDA0003026494090000034
Figure BDA0003026494090000035
其中ei,eu,eui分别为imu测量残差，uwb测量残差，以及imu到UWB坐标转换残差
步骤(4)中使用了柯西鲁棒核函数，列文伯格-马夸尔特算法，进行非线性优化。
柯西鲁棒核函数为：
Figure BDA0003026494090000036
其中，s为误差函数，c为控制参数，设置为1.5，使用该函数更新
Figure BDA0003026494090000037
线性化
Figure BDA0003026494090000038
可得：
Figure BDA0003026494090000039
其中，J为
Figure BDA00030264940900000310
为的雅可比矩阵，
Figure BDA00030264940900000311
为待优化的机器人状态量，使用步骤(2)中得到的机器人位姿预估值为初值，
使用列文伯格-马夸尔特算法修正方程式为：
Figure BDA0003026494090000041
其中H＝JTJ，b＝JTf，通过启发式地调整λ来改善算法在局部极值附近的特性。
对
Figure BDA0003026494090000042
解该方程，即可得到优化后的机器人位姿。
采用上述技术方案带来的有益效果：
本发明采用了使用了IMU对UWB数据进行检验，采用柯西鲁棒核函数进行优化，显著的改善了UWB受到NLOS影响导致定位数据不准确的情况
本发明使用了列文伯格-马夸尔特算法优化机器人位姿，进一步提升了系统定位精度。
本发明不仅能够显著的提高UWB/IMU融合定位的定位精度，而且适用于其他的多机系统，具有广泛的应用前景。
附图说明
图1是本发明算法流程框图；
图2是本发明的UWB/IMU，机器人布局图；
图3是机器人在场景中运动的位姿图；
具体实施方式
以下将结合附图，对本发明的技术方案进行详细说明。
本发明设计了一种基于图优化的UWB/IMU的室内移动机器人协同定位方法，如图1所示，基本步骤如下：
(1)建立传感器观测模型
(2)初步校验UWB数据，判断数据是否受到NLOS影响。
(3)构建机器人位姿优化图，构建误差函数，将机器人位置优化问题转化为位姿图优化问题。
(4)使用柯西鲁棒核函数，列文伯格-马夸尔特算法，优化机器人当前时刻的位姿。
在本实施例中，采用如下优选方案实现步骤1：
在(1)步骤中，所述UWB测量模型如下：
Figure BDA0003026494090000051
其中L1，L2，L3....Ln为各基站到机器人的距离，(x1,y1,z`1),(x2,y2,z2),(x3,y3,z3).....(xn,yn,zn)表示为基站坐标，(xu,yu,zu)为机器人位置
按照权利要求1所述的一种基于图优化的UWB/IMU的室内移动机器人定位方法其特征在于，在(1)步骤中，所述IMU测量模型如下：
Figure BDA0003026494090000052
Figure BDA0003026494090000053
为机器人在j,i时刻的位置，
Figure BDA0003026494090000054
为机器人j.i时刻在世界坐标系下的速度，gw为世界坐标系下，机器人的重力加速度。Δt为两次采样时间间隔。
Figure BDA0003026494090000055
为imu预积分量。
Figure BDA0003026494090000056
为imu加速度计，陀螺仪的整体偏移量，
Figure BDA0003026494090000057
为整体偏移使用随机游走模型的参数变量。a,w为使用中值滤波法计算得到的机器人当前时刻线加速度与角速度。
在本实施例中，采用如下优选方案实现步骤2：
初步校验UWB数据，判断数据是否受到NLOS影响。在k到k+1时刻有：
Figure BDA0003026494090000058
其中xk,xk-1为机器人在k，k-1时刻的位姿，因此可令
Figure BDA0003026494090000061
对rk进行检验，若
Figure BDA0003026494090000062
则可认为UWB出现了异常测量值。
丢弃该测量值，记录UWB中有效测量的数量。
若此时有效数量m＜2，则该时刻的UWB帧无法作为定位使用，利用IMU对该时刻进行保持。
若m＝2，由于室内环境中，地面相对平坦，可将z轴坐标看做一个常数，可令ZK＝ZK-1。
在本实施例中，采用如下优选方案实现步骤(3)，依照步骤2中的测量模型，构建误差函数
Figure BDA0003026494090000063
Figure BDA0003026494090000064
其中ei,eu,eui分别为imu测量残差，uwb测量残差，以及imu到UWB坐标转换残差在本实施例中，采用如下优选方案实现步骤(4)：使用了柯西鲁棒核函数，列文伯格-马夸尔特算法，进行非线性优化。
柯西鲁棒核函数为：
Figure BDA0003026494090000065
其中，s为误差函数，c为控制参数，设置为1.5，使用该函数更新
Figure BDA0003026494090000066
线性化
Figure BDA0003026494090000067
可得：
Figure BDA0003026494090000068
其中，J为
Figure BDA0003026494090000069
为的雅可比矩阵，
Figure BDA00030264940900000610
为待优化的机器人状态量，使用步骤(2)中得到的机器人位姿预估值为初值，
使用列文伯格-马夸尔特算法修正方程式为：
Figure BDA00030264940900000611
其中H＝JTJ，b＝JTf，通过启发式地调整λ来改善算法在局部极值附近的特性。
对
Figure BDA0003026494090000071
解该方程，即可得到优化后的机器人位姿。

Claims (1)

1.一种基于图优化的UWB/IMU的室内移动机器人定位方法，包括以下步骤：
(1)建立机器人UWB/IMU的测量模型；
(2)初步校验UWB数据，判断数据是否受到NLOS影响；
(3)构建机器人位姿优化图，构建误差函数，将机器人位置优化问题转化为位姿图优化问题；
(4)使用柯西鲁棒核函数，列文伯格-马夸尔特算法，优化机器人当前时刻的位姿；
在(1)步骤中，机器人UWB测量模型如下：
Figure FDA0004035948520000011
其中L1，L2，L3....Ln为各基站到机器人的距离，
(x1,y1,z1),(x2,y2,z2),(x3,y3,z3).....(xn,yn,zn)表示为基站坐标，(xu,yu,zu)为机器人位置；
在(1)步骤中，机器人IMU测量模型如下：
Figure FDA0004035948520000012
Figure FDA0004035948520000013
为机器人在j,i时刻的位置，
Figure FDA0004035948520000014
为机器人j，i时刻在世界坐标系下的速度，gw为世界坐标系下机器人的重力加速度；Δt为两次采样时间间隔；
Figure FDA0004035948520000021
为IMU预积分量；
Figure FDA0004035948520000022
为IMU加速度计，陀螺仪的整体偏移量，
Figure FDA0004035948520000023
为整体偏移使用随机游走模型的参数变量；a,w为使用中值滤波法计算得到的机器人当前时刻线加速度与角速度；
在步骤(2)中，初步校验UWB数据，判断数据是否受到NLOS影响；在k到k+1时刻有：
Figure FDA0004035948520000024
其中xk,xk-1为机器人在k，k-1时刻的位姿，因此令
Figure FDA0004035948520000025
对rk进行检验，若
Figure FDA0004035948520000026
则认为UWB出现了异常测量值；
丢弃该测量值，记录UWB中有效测量的数量；
若此时有效数量m＜2，则该时刻的UWB帧无法作为定位使用，利用IMU对该时刻进行保持；
若m＝2，由于室内环境中，地面相对平坦，可将z轴坐标看做一个常数，可令ZK＝ZK-1；
其特征在于步骤(3) 中构建误差函数
Figure FDA0004035948520000027
Figure FDA0004035948520000028
其中ei,eu,eui分别为IMU测量残差，UWB测量残差，以及IMU到UWB坐标转换残差；
步骤(4)中使用了柯西鲁棒核函数，列文伯格-马夸尔特算法，进行非线性优化；
使用柯西鲁棒核函数，列文伯格-马夸尔特算法，进行非线性优化；
柯西鲁棒核函数为：
Figure FDA0004035948520000029
其中，s为误差函数，c为控制参数，设置为1.5，使用该函数更新
Figure FDA00040359485200000210
线性化
Figure FDA00040359485200000211
可得：
Figure FDA0004035948520000031
其中，J为
Figure FDA0004035948520000032
的雅可比矩阵，
Figure FDA0004035948520000033
为待优化的机器人状态量，使用步骤(2)中得到的机器人位姿预估值为初值，
使用列文伯格-马夸尔特算法修正方程式为：
Figure FDA0004035948520000034
其中H＝JTJ，b＝JTf，通过启发式地调整λ来改善算法在局部极值附近的特性；对
Figure FDA0004035948520000035
解该列文伯格-马夸尔特算法修正方程式，即可得到优化后的机器人位姿。
CN202110421274.8A 2021-04-19 2021-04-19 一种基于图优化的uwb/imu的室内移动机器人协同定位方法 Active CN113324544B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110421274.8A CN113324544B (zh) 2021-04-19 2021-04-19 一种基于图优化的uwb/imu的室内移动机器人协同定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110421274.8A CN113324544B (zh) 2021-04-19 2021-04-19 一种基于图优化的uwb/imu的室内移动机器人协同定位方法

Publications (2)

Publication Number Publication Date
CN113324544A CN113324544A (zh) 2021-08-31
CN113324544B true CN113324544B (zh) 2023-02-28

Family

ID=77414871

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110421274.8A Active CN113324544B (zh) 2021-04-19 2021-04-19 一种基于图优化的uwb/imu的室内移动机器人协同定位方法

Country Status (1)

Country Link
CN (1) CN113324544B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114554392B (zh) * 2022-02-25 2023-05-16 新基线(江苏)科技有限公司 一种基于uwb与imu融合的多机器人协同定位方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108810133A (zh) * 2018-06-08 2018-11-13 深圳勇艺达机器人有限公司 一种基于uwb和tdoa算法的智能机器人定位方法及定位系统
CN109828510A (zh) * 2019-03-13 2019-05-31 桂林电子科技大学 一种基于超宽带和惯性导航技术的agv定位系统及定位方法
CN109883428A (zh) * 2019-03-27 2019-06-14 成都电科慧安科技有限公司 一种融合惯导、地磁和WiFi信息的高精度定位方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN111852456A (zh) * 2020-07-29 2020-10-30 中国矿业大学 基于因子图的鲁棒uwb井下锚杆钻孔定位方法
CN112525197A (zh) * 2020-11-23 2021-03-19 中国科学院空天信息创新研究院 基于图优化算法超宽带惯性导航融合位姿估计方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108810133A (zh) * 2018-06-08 2018-11-13 深圳勇艺达机器人有限公司 一种基于uwb和tdoa算法的智能机器人定位方法及定位系统
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109828510A (zh) * 2019-03-13 2019-05-31 桂林电子科技大学 一种基于超宽带和惯性导航技术的agv定位系统及定位方法
CN109883428A (zh) * 2019-03-27 2019-06-14 成都电科慧安科技有限公司 一种融合惯导、地磁和WiFi信息的高精度定位方法
CN111852456A (zh) * 2020-07-29 2020-10-30 中国矿业大学 基于因子图的鲁棒uwb井下锚杆钻孔定位方法
CN112525197A (zh) * 2020-11-23 2021-03-19 中国科学院空天信息创新研究院 基于图优化算法超宽带惯性导航融合位姿估计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
UWB和IMU技术融合的室内定位算法研究;王嘉欣等;《单片机与嵌入式系统应用》;20200801(第08期);全文 *
因子图发展及其在定位与导航的应用技术;周雅婧等;《全球定位系统》;20200215(第01期);全文 *

Also Published As

Publication number Publication date
CN113324544A (zh) 2021-08-31

Similar Documents

Publication Publication Date Title
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN105589064B (zh) Wlan位置指纹数据库快速建立和动态更新系统及方法
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
US10295365B2 (en) State estimation for aerial vehicles using multi-sensor fusion
CN110487267B (zh) 一种基于vio＆uwb松组合的无人机导航系统及方法
CN110764506B (zh) 移动机器人的航向角融合方法、装置和移动机器人
CN107490378B (zh) 一种基于mpu6050与智能手机的室内定位与导航的方法
CN109141427B (zh) 在非视距环境下基于距离和角度概率模型的ekf定位方法
CN107421537B (zh) 基于惯性传感器刚体网格的物体运动姿态感知方法和系统
CN109855621A (zh) 一种基于uwb与sins的组合室内行人导航系统及方法
CN113311411A (zh) 一种用于移动机器人的激光雷达点云运动畸变校正方法
CN113124856A (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN110617795B (zh) 一种利用智能终端的传感器实现室外高程测量的方法
CN111091587A (zh) 一种基于视觉标志物的低成本动作捕捉方法
CN111983660A (zh) Gnss拒止环境下四旋翼无人机定位系统和方法
CN112461224B (zh) 一种基于已知姿态角的磁力计标定方法
CN107941211A (zh) 基于二阶级联的多元素融合定位方法、装置及电子设备
CN111982102B (zh) 一种复杂环境下基于bp-ekf的uwb-imu定位方法
KR101576424B1 (ko) 실내 측위를 위한 지자기 센서 자동 보정 방법
CN104880201A (zh) Mems陀螺自动标定方法
CN114623823B (zh) 一种融合里程计的uwb多模态定位系统、方法及装置
CN108387236A (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN113324544B (zh) 一种基于图优化的uwb/imu的室内移动机器人协同定位方法
CN110849364B (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