码迷,mamicode.com
首页 > 其他好文 > 详细

第四天:SLAM智能小车DIY乐趣-小车控制ROS驱动包开发

时间:2018-09-19 21:53:10      阅读:5788      评论:0      收藏:0      [点我收藏+]

标签:位置   span   The   sla   智能   ima   print   串口   slam   

小车控制ROS驱动包开发

####写在前面####

对于做纯SLAM算法和应用功能开发的朋友来说,其实不需要搞明白小车的底层硬件原理和软件实现等繁琐的细节,只需要通过上层API调用来实现小车的控制等操作就可以了。因此,本文开始讲解用于提供上层API调用的ROS驱动包,本文主要内容:

1) 通信协议简介(stm32与ROS驱动包通信)

2) ROS接口(topic订阅与发布)

3) ROS驱动包程序解析

4) 基于航迹推演的里程计解算

####正文####

1.通信协议简介(stm32与ROS驱动包通信)

    通信协议都基于串口通信,通信数据帧由串口通信的单个字节组合而成,stm32的串口2由“小车控制-ROS驱动包”驱动,stm32的串口1由“小车debug-ROS驱动包”驱动,两个串口都使用115200的波特率。下面分别介绍两个驱动包的通信协议:

stm32与小车控制ROS驱动包通信:

技术分享图片

技术分享图片

不难发现,其实“小车控制-ROS驱动包”发送和接收数据遵循同样的协议。

stm32与小车debug-ROS驱动包通信:

 技术分享图片 

技术分享图片

stm32发送过来的调试信息时纯字符串流,可以通过printf()直接来查看;

小车debug-ROS驱动发送给stm32的kp、ki、kd是一个扩大10000倍后的整数,除以10000后使用。

2. ROS接口(topic订阅与发布)

ROS接口为上层应用提供API调用,便于SLAM算法和应用功能开发。

订阅topic:

/cmd_vel  (geometry_msgs::Twist)

发布topic:

/enc_l    (msgs::Int16)

/enc_r    (msgs::Int16)

/odom    (nav_msgs::Odometry)

/tf        (odom->base_footprint)

3.ROS驱动包程序解析

了解了前面的通信协议和ROS接口后,接下来就来看看ROS程序是如何实现的。首先程序订阅/cmd_vel作为用户的控制输入,将控制输入的速度信息转换为通信协议中规定的格式,然后通过串口下发给stm32,实现对小车的运动控制。

下图是订阅/cmd_vel的回调函数:

技术分享图片

下图是串口下发函数:

技术分享图片

知道了怎么控制小车运动,接下来介绍获取小车反馈回来的码盘信息,并将反馈回来的码盘信息进行里程计解算。

如下图是串口读取和里程计解算函数:

技术分享图片

如下图是里程计和相关topic发布:

 技术分享图片

4.基于航迹推演的里程计解算

 首先通过下面的一张图来了解一下里程计解算的大致原理,下图包含两个坐标分别对应图A、图B,请看图:

技术分享图片

在图A中,设定机器人初始位置与坐标系原点重合,机器人的正前方为坐标系x轴正方向,机器人方向角定义为由坐标系x轴正方向逆时针方向(即图中theta轴所示)。利用极限的思想,机器人在很短的时间delta_t内,由A位置运动到B位置,机器人绕p转动一个微小的角度,其中delta_d_l与delta_d_r分别为左轮和右轮的位移量,可以通过编码反馈值计算得;delta_theta是方向角变化量,由等腰三角形的关系可以得出delta_theta与图中各角的关系;L是两轮之间的轴距。有了这个理论基础,下面接着介绍如何解算出里程计。

在图B中,主要说明了如何通过迭代来计算机器人在坐标系中的姿态,从而得到我们需要的里程计。通过图A中介绍的原理,不难得出delta_d、delta_theta、delta_x、delta_y各变化量,然后通过过迭代的方法便可以求出里程计中的机器人位置(x,y,theta)和机器人的速度(v,w)。

有了里程计解算的理论基础后,接下来谈谈具体程序实现,如下图:

 技术分享图片

程序的输入是在采样周期内得到的左右轮编码值(delta_encode_left和delta_encode_right),这个编码值可以通过速度系数speed_ratio便可以换算成实际的位移量,速度系数speed_ratio可以通过后续的标定得到;两个轮子的轴距wheel_distance也是通过后续的标定得到,采样周期encode_sampling_time在程序中是一个可选的参数。有了delta_encode_left、delta_encode_right的值,便可以推算出delta_d、delta_theta,继而可以推算出delta_x、delta_y,继续往下使用迭代可以计算出最终的里程计值position_x、position_y、oriention、velocity_linear、velocity_angular。最后通过ROS将里程计发布出去就ok了。

第四天:SLAM智能小车DIY乐趣-小车控制ROS驱动包开发

标签:位置   span   The   sla   智能   ima   print   串口   slam   

原文地址:https://www.cnblogs.com/hiram-zhang/p/9676725.html

(0)
(0)
   
举报
评论 一句话评论(0
0条  
登录后才能评论!
© 2014 mamicode.com 版权所有 京ICP备13008772号-2
迷上了代码!