一种基于图优化的UWB/IMU的室内移动机器人协同定位方法

文档序号:26489305发布日期:2021-08-31 17:44阅读:435来源:国知局
一种基于图优化的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测量模型如下:

其中l1,l2,l3....ln为各基站到机器人的距离,(x1,y1,z`1),(x2,y2,z2),(x3,y3,z3).....(xn,yn,zn)表示为基站坐标,(xu,yu,zu)为机器人位置;

在步骤(1)中,所述imu测量模型如下:

为机器人在j,i时刻的位置,为机器人j.i时刻在世界坐标系下的速度,gw为世界坐标系下,机器人的重力加速度。δt为两次采样时间间隔。为imu预积分量。为imu加速度计,陀螺仪的整体偏移量,为整体偏移使用随机游走模型的参数变量。a,w为使用中值滤波法计算得到的机器人当前时刻线加速度与角速度。

步骤(2)中,初步校验uwb数据,判断数据是否受到nlos影响。在k到k+1时刻有:

其中xk,xk-1为机器人在k,k-1时刻的位姿,因此可令

对rk进行检验,若则可认为uwb出现了异常测量值。

丢弃该测量值,记录uwb中有效测量的数量。

若此时有效数量m<2,则该时刻的uwb帧无法作为定位使用,利用imu对该时刻进行保持。

若m=2,由于室内环境中,地面相对平坦,可将z轴坐标看做一个常数,可令zk=zk-1。

步骤(3)中,依照步骤2中的测量模型,构建误差函数

其中ei,eu,eui分别为imu测量残差,uwb测量残差,以及imu到uwb坐标转换残差

步骤(4)中使用了柯西鲁棒核函数,列文伯格-马夸尔特算法,进行非线性优化。

柯西鲁棒核函数为:

其中,s为误差函数,c为控制参数,设置为1.5,使用该函数更新线性化可得:

其中,j为为的雅可比矩阵,为待优化的机器人状态量,使用步骤(2)中得到的机器人位姿预估值为初值,

使用列文伯格-马夸尔特算法修正方程式为:

其中h=jtj,b=jtf,通过启发式地调整λ来改善算法在局部极值附近的特性。

解该方程,即可得到优化后的机器人位姿。

采用上述技术方案带来的有益效果:

本发明采用了使用了imu对uwb数据进行检验,采用柯西鲁棒核函数进行优化,显著的改善了uwb受到nlos影响导致定位数据不准确的情况

本发明使用了列文伯格-马夸尔特算法优化机器人位姿,进一步提升了系统定位精度。

本发明不仅能够显著的提高uwb/imu融合定位的定位精度,而且适用于其他的多机系统,具有广泛的应用前景。

附图说明

图1是本发明算法流程框图;

图2是本发明的uwb/imu,机器人布局图;

图3是机器人在场景中运动的位姿图;

具体实施方式

以下将结合附图,对本发明的技术方案进行详细说明。

本发明设计了一种基于图优化的uwb/imu的室内移动机器人协同定位方法,如图1所示,基本步骤如下:

(1)建立传感器观测模型

(2)初步校验uwb数据,判断数据是否受到nlos影响。

(3)构建机器人位姿优化图,构建误差函数,将机器人位置优化问题转化为位姿图优化问题。

(4)使用柯西鲁棒核函数,列文伯格-马夸尔特算法,优化机器人当前时刻的位姿。

在本实施例中,采用如下优选方案实现步骤1:

在(1)步骤中,所述uwb测量模型如下:

其中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测量模型如下:

为机器人在j,i时刻的位置,为机器人j.i时刻在世界坐标系下的速度,gw为世界坐标系下,机器人的重力加速度。δt为两次采样时间间隔。为imu预积分量。为imu加速度计,陀螺仪的整体偏移量,为整体偏移使用随机游走模型的参数变量。a,w为使用中值滤波法计算得到的机器人当前时刻线加速度与角速度。

在本实施例中,采用如下优选方案实现步骤2:

初步校验uwb数据,判断数据是否受到nlos影响。在k到k+1时刻有:

其中xk,xk-1为机器人在k,k-1时刻的位姿,因此可令

对rk进行检验,若则可认为uwb出现了异常测量值。

丢弃该测量值,记录uwb中有效测量的数量。

若此时有效数量m<2,则该时刻的uwb帧无法作为定位使用,利用imu对该时刻进行保持。

若m=2,由于室内环境中,地面相对平坦,可将z轴坐标看做一个常数,可令zk=zk-1。

在本实施例中,采用如下优选方案实现步骤(3),依照步骤2中的测量模型,构建误差函数

其中ei,eu,eui分别为imu测量残差,uwb测量残差,以及imu到uwb坐标转换残差在本实施例中,采用如下优选方案实现步骤(4):使用了柯西鲁棒核函数,列文伯格-马夸尔特算法,进行非线性优化。

柯西鲁棒核函数为:

其中,s为误差函数,c为控制参数,设置为1.5,使用该函数更新线性化可得:

其中,j为为的雅可比矩阵,为待优化的机器人状态量,使用步骤(2)中得到的机器人位姿预估值为初值,

使用列文伯格-马夸尔特算法修正方程式为:

其中h=jtj,b=jtf,通过启发式地调整λ来改善算法在局部极值附近的特性。

解该方程,即可得到优化后的机器人位姿。

当前第1页1 2 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1