基于制造技术

本发明专利技术公开了一种基于

【技术实现步骤摘要】
基于IMU和里程计融合和雷达定位算法


[0001]本专利技术涉及一种基于
IMU
和里程计融合，并根据雷达进行定位的算法，属于
Ros
的
slam
算法领域
。

技术介绍

[0002]现有的机器人小车里程计信息，主要根据电机获取到的角速度以及线速度以及时间差来计算出里程计的四元数信息，雷达的常规定位方法使用的是
amcl
定位
。
[0003]里程计的主要参数是
odom_trans.transform.translation
的
xyz
参数，根据速度以及短时间的
Δ
t
来计算出的
x
和
y
，以及常规里程计使用的一些协方差参数
。
[0004]Slam
定位方法一般使用的是
amcl
定位
。amcl
定位用于
2D
移动机器人的概率定位系统，它实现了自适应
(
或
KLD
采样
)
蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置
。
[0005]鉴于此，本文特提出了一种基于
IMU
和里程计融合方法
。
利用雷达来处理点云信息，来进行
slam
定位的算法
。

技术实现思路

[0006](
一
)
解决的技术问题
[0007]针对现有技术的不足，本专利技术提供了基于
IMU
和里程计融合和雷达定位算法
。
由于小车底盘的速度误差较大，并且计算的
Δ
t
同时也存在误差，因此里程计准确性不高
。
同时
amcl
的定位方式存在着较大的误差以及需要调节很多的参数，针对小车里程计的底盘信息，同时根据外部的
IMU
传感器，来获取小车的俯仰角，偏航角和横滚角，根据三轴的角度来对于底盘的里程计信息进行数据优化
。
小车的
amcl
导航的方法根据使用的新里程计数据以及
16
线雷达来进行点云信息的匹配工作，根据更加准确的里程计来确保
amcl
定位的大方向
。
[0008](
二
)
技术方案
[0009]为实现上述目的，本专利技术提供如下技术方案：基于
IMU
和里程计融合和雷达定位算法，包括以下步骤：
[0010]S1
：欧拉角和里程计融合；
[0011]首先根据常规的运动学结算获得小车在
x
轴和
y
轴上的线速度，其次根据
IMU
所提供的欧拉角来计算获得所需要的旋转角度；
[0012]再通过
IMU
提供实时欧拉角并融合新里程计；
[0013]实时发布新的里程计；
[0014]S2
：
amcl
定位导航；
[0015]根据雷达提供实时点云，来实现
slam
定位
、
导航；
[0016]实时发布小车角速度线速度
。
[0017]优选的，所述
S1
中，根据
Δ
t
来得到里程计的
odom_
trans.transform.translation.x
和
odom_trans.transform.translation.y
，加入
IMU
所提供的欧拉角，加入到里程计的
createQuaternionMsgFromYaw
中间，以实时发布新的里程；
[0018]其中，
vth
＝
imu_msg.angular_velocity.z
；
[0019]odom_trans.transform.translation.x
＝
x+(vx*cos(th)
‑
vy*sin(th))*dt
；
[0020]odom_trans.transform.translation.y
＝
y+(vx*sin(th)+vy*cos(th))*dt
；
[0021]odom_trans.transform.rotation
＝
tf::createQuaternionMsgFromYaw(th)。
[0022]优选的，所述
S2
中，首先导入已经融合的里程计数据和雷达的点云数据，根据现有参数进行
amcl
定位
。
[0023]优选的，所述
S2
具体包括：
[0024]坐标系导入：
[0025]导入已知坐标系：里程计坐标系，地图坐标系，实时点云数据坐标系
(
机器人坐标系
)
分别为
odom_frame_id
，
base_frame_id
，
global_frame_id。
[0026]优选的，所述
S2
还包括：推算小车的粒子云和绑架时加入自适应变种：
[0027]使用
M
个粒子的集合表示置信度，初始置信度由先验分布随机产生的
M
个这样的粒子得到，使用运动模型采样，以当前置信度为起点使用粒子，使用测量模型以确定粒子的重要性权值通过增加粒子总数
M
能提高定位的近似精度；
[0028]当机器人遭遇绑架问题的时候，会随机注入粒子，由于增加粒子，因此需要解决两个问题，首先是每次算法迭代中应该增加多少粒子，其次是从那种分布产生这些粒子可以通过监控传感器测量的概率来评估增加粒子来解决第一个问题
。
[0029]优选的，所述精度解决公式如下：
[0030]p(z
t
|z1：
t
‑1，
u1：
t
，
m)。
[0031]优选的，所述
S2
中还需要将其与平均测量概率联系起来，在粒子滤波中这个数量的近似容易根据重要性因子获取，其重要性权重是这个概率的随机估计
。
[0032]优选的，所述概率的平均值公式为：
[0033][0034](
三
)
有益效果
[0035]与现有技术相比，本专利技术提供了一种基于
IMU
和里程计融合和雷达定位算法，具备以下有益效果：
[0036]该基于
IMU
和里程计融合和雷达定位算法，通过本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
基于
IMU
和里程计融合和雷达定位算法，其特征在于，包括以下步骤：
S1
：欧拉角和里程计融合；首先根据常规的运动学结算获得小车在
x
轴和
y
轴上的线速度，其次根据
IMU
所提供的欧拉角来计算获得所需要的旋转角度；再通过
IMU
提供实时欧拉角并融合新里程计；实时发布新的里程计；
S2
：
amcl
定位导航；根据雷达提供实时点云，来实现
slam
定位
、
导航；实时发布小车角速度线速度
。2.
根据权利要求1所述的基于
IMU
和里程计融合和雷达定位算法，其特征在于：所述
S1
中，根据
Δ
t
来得到里程计的
odom_trans.transform.translation.x
和
odom_trans.transform.translation.y
，加入
IMU
所提供的欧拉角，加入到里程计的
createQuaternionMsgFromYaw
中间，以实时发布新的里程；其中，
vth
＝
imu_msg.angular_velocity.z
；
odom_trans.transform.translation.x
＝
x+(vx*cos(th)
‑
vy*sin(th))*dt
；
odom_trans.transform.translation.y
＝
y+(vx*sin(th)+vy*cos(th))*dt
；
odom_trans.transform.rotation
＝
tf::createQuaternionMsgFromYaw(th)。3.
...

【专利技术属性】
技术研发人员：雷凌，朱恩东，
申请(专利权)人：南京北新智能科技有限公司，
类型：发明
国别省市：