基于IMU和里程计融合和雷达定位算法的制作方法

本发明涉及一种基于imu和里程计融合，并根据雷达进行定位的算法，属于ros的slam算法领域。

背景技术：

1、现有的机器人小车里程计信息，主要根据电机获取到的角速度以及线速度以及时间差来计算出里程计的四元数信息，雷达的常规定位方法使用的是amcl定位。

2、里程计的主要参数是odom_trans.transform.translation的xyz参数，根据速度以及短时间的δt来计算出的x和y，以及常规里程计使用的一些协方差参数。

3、slam定位方法一般使用的是amcl定位。amcl定位用于2d移动机器人的概率定位系统，它实现了自适应(或kld采样)蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。

4、鉴于此，本文特提出了一种基于imu和里程计融合方法。利用雷达来处理点云信息，来进行slam定位的算法。

技术实现思路

1、(一)解决的技术问题

2、针对现有技术的不足，本发明提供了基于imu和里程计融合和雷达定位算法。由于小车底盘的速度误差较大，并且计算的δt同时也存在误差，因此里程计准确性不高。同时amcl的定位方式存在着较大的误差以及需要调节很多的参数，针对小车里程计的底盘信息，同时根据外部的imu传感器，来获取小车的俯仰角，偏航角和横滚角，根据三轴的角度来对于底盘的里程计信息进行数据优化。小车的amcl导航的方法根据使用的新里程计数据以及16线雷达来进行点云信息的匹配工作，根据更加准确的里程计来确保amcl定位的大方向。

3、(二)技术方案

4、为实现上述目的，本发明提供如下技术方案：基于imu和里程计融合和雷达定位算法，包括以下步骤：

5、s1：欧拉角和里程计融合；

6、首先根据常规的运动学结算获得小车在x轴和y轴上的线速度，其次根据imu所提供的欧拉角来计算获得所需要的旋转角度；

7、再通过imu提供实时欧拉角并融合新里程计；

8、实时发布新的里程计；

9、s2：amcl定位导航；

10、根据雷达提供实时点云，来实现slam定位、导航；

11、实时发布小车角速度线速度。

12、优选的，所述s1中，根据δt来得到里程计的odom_trans.transform.translation.x和odom_trans.transform.translation.y，加入imu所提供的欧拉角，加入到里程计的createquaternionmsgfromyaw中间，以实时发布新的里程；

13、其中，vth＝imu_msg.angular_velocity.z；

14、odom_trans.transform.translation.x＝x+(vx*cos(th)-vy*sin(th))*dt；

15、odom_trans.transform.translation.y＝y+(vx*sin(th)+vy*cos(th))*dt；

16、odom_trans.transform.rotation＝tf::createquaternionmsgfromyaw(th)。

17、优选的，所述s2中，首先导入已经融合的里程计数据和雷达的点云数据，根据现有参数进行amcl定位。

18、优选的，所述s2具体包括：

19、坐标系导入：

20、导入已知坐标系：里程计坐标系，地图坐标系，实时点云数据坐标系(机器人坐标系)分别为odom_frame_id，base_frame_id，global_frame_id。

21、优选的，所述s2还包括：推算小车的粒子云和绑架时加入自适应变种：

22、使用m个粒子的集合表示置信度，初始置信度由先验分布随机产生的m个这样的粒子得到，使用运动模型采样，以当前置信度为起点使用粒子，使用测量模型以确定粒子的重要性权值通过增加粒子总数m能提高定位的近似精度；

23、当机器人遭遇绑架问题的时候，会随机注入粒子，由于增加粒子，因此需要解决两个问题，首先是每次算法迭代中应该增加多少粒子，其次是从那种分布产生这些粒子可以通过监控传感器测量的概率来评估增加粒子来解决第一个问题。

24、优选的，所述精度解决公式如下：

25、p(zt|z1：t-1，u1：t，m)。

26、优选的，所述s2中还需要将其与平均测量概率联系起来，在粒子滤波中这个数量的近似容易根据重要性因子获取，其重要性权重是这个概率的随机估计。

27、优选的，所述概率的平均值公式为：

28、

29、(三)有益效果

30、与现有技术相比，本发明提供了一种基于imu和里程计融合和雷达定位算法，具备以下有益效果：

31、该基于imu和里程计融合和雷达定位算法，通过采用imu实时采集欧拉角的数据信息，硬件开销少，并且相比现有的里程计计算方法，可以通过硬件更加提升准确性；且实时提供更准确的里程计，给slam的定位导航方法提供更加准确的定位。

技术特征：

1.基于imu和里程计融合和雷达定位算法，其特征在于，包括以下步骤：

2.根据权利要求1所述的基于imu和里程计融合和雷达定位算法，其特征在于：所述s1中，根据δt来得到里程计的odom_trans.transform.translation.x和odom_trans.transform.translation.y，加入imu所提供的欧拉角，加入到里程计的createquaternionmsgfromyaw中间，以实时发布新的里程；

3.根据权利要求1所述的基于imu和里程计融合和雷达定位算法，其特征在于：所述s2中，首先导入已经融合的里程计数据和雷达的点云数据，根据现有参数进行amcl定位。

4.根据权利要求3所述的基于imu和里程计融合和雷达定位算法，其特征在于：所述s2具体包括：

5.根据权利要求1所述的基于imu和里程计融合和雷达定位算法，其特征在于：所述s2还包括：推算小车的粒子云和绑架时加入自适应变种：

6.根据权利要求5所述的基于imu和里程计融合和雷达定位算法，其特征在于：所述精度解决公式如下：

7.根据权利要求1所述的基于imu和里程计融合和雷达定位算法，其特征在于：所述s2中还需要将其与平均测量概率联系起来，在粒子滤波中这个数量的近似容易根据重要性因子获取，其重要性权重是这个概率的随机估计。

8.根据权利要求7所述的基于imu和里程计融合和雷达定位算法，其特征在于：所述概率的平均值公式为：

技术总结
本发明公开了一种基于IMU和里程计融合和雷达定位算法，包括以下步骤：S1：欧拉角和里程计融合；首先根据常规的运动学结算获得小车在x轴和y轴上的线速度，其次根据IMU所提供的欧拉角来计算获得所需要的旋转角度；再通过IMU提供实时欧拉角并融合新里程计；实时发布新的里程计；S2：amcl定位导航；根据雷达提供实时点云，来实现slam定位、导航；实时发布小车角速度线速度。本发明通过采用IMU实时采集欧拉角的数据信息，硬件开销少，并且相比现有的里程计计算方法，可以通过硬件更加提升准确性；且实时提供更准确的里程计，给slam的定位导航方法提供更加准确的定位。

技术研发人员：雷凌,朱恩东
受保护的技术使用者：南京北新智能科技有限公司
技术研发日：
技术公布日：2024/1/15