3.tf在多机器人编队中的代码实现

0x00 内容简介

在上一篇文章中，我们讲了tf在多机器人编队中的应用，简单来说就是编队中的机器人利用tf向world坐标系发送自己的坐标，这样处在world坐标系中的机器人就可以通过tf来建立自己的局部坐标系，获取两个机器人之间的相对位姿。这一篇文章我们介绍一下组成编队的核心代码及在我们三轮小车上的演示视频。三轮小车使用三个全向轮，具备全向移动功能。配置激光雷达，实现slam建图，自动导航。配备高性能锂电池，可以长时间工作，整体平台易扩展。小车前进后退移动速度 0.36m/s，横向移动速度 0.31m/s，转圈 2.8rad/s。

0x01 tf发布代码

首先是tf发布的整体代码：

#!/usr/bin/env python
#coding=utf-8

import rospy
import tf
import nav_msgs.msg

def handle_robot_pose(msg, robotname):
    br = tf.TransformBroadcaster() 
    br.sendTransform((msg.pose.pose.position.x, msg.pose.pose.position.y, 0), 
                     (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w), 
                     rospy.Time.now(), 
                     '%s' % robotname,  
                     "world")

if __name__ == '__main__':
    rospy.init_node('robot_tf_broadcaster')
    robotname = rospy.get_param('~robot')    
    rospy.Subscriber('/%s/odom_ekf' % robotname, 
                     nav_msgs.msg.Odometry,  
                     handle_robot_pose,      
                     robotname)              
    rospy.spin()

注册一个名为"robot_tf_broadcaster"的节点。

rospy.init_node('robot_tf_broadcaster')

这个节点订阅话题"robotX/odom_ekf"，这个话题的作用是发布自己的位姿信息。当有信息过来时，再交给"handle_robot_pose"函数去处理。

rospy.Subscriber('/%s/odom_ekf' % robotname, 
                     nav_msgs.msg.Odometry,  
                     handle_robot_pose,      
                     robotname)

"handle_robot_pose"函数接收到来自robotX的pose信息时，建立从world到robotX的变换，也即是robotX向world广播自己的坐标。

br = tf.TransformBroadcaster()
br.sendTransform((msg.pose.pose.position.x, msg.pose.pose.position.y, 0), 
                     (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w), 
                     rospy.Time.now(), 
                     '%s' % robotname,  
                     "world")

当编队中有多台机器人小车时，每一台都须向world坐标系发布自己的坐标，虚拟机器人需要向领航者机器人发布固定不变的坐标。如果我们的编队系统中只有两台机器人，那么上述代码完成之后，此时tf-tree如下图所示：

要实现上图中的tf-tree，首先需要配置ros中的主从机，将robot1当做master，然后建立各自机器人的命名空间，robot1和robot2向world发送tf变换之后，启动整个系统，tf会自动建立起如上图所示的tf-tree。

0x02 创建tf监听

当我们的小车向world坐标系发布自己的坐标后，就可以通过tf来计算各个小车之间的坐标关系，具体代码如下：

while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/robot2', '/wingman1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

"lookupTransform"函数返回以robot2为原点的wingman1的坐标，包括位置和方向。有了robot2和wingman1的位姿关系，我们就可以设定控制器使跟随者和领航者编队保持问题转化为领航者对虚拟机器人的跟随问题。

0x03 效果展示

前面小车robot1为领航者，后面小车robot2为跟随者，在领航者旋转、前进时跟随者执行相同的动作来保持队形。目前视频中存在领航者运动的时候，跟随者反应慢的问题，问题的可能解决办法有提高跟随者的控制频率，找到合适的控制跟随者的控制器。

演示视频

0x04 实验的总结与思考

通过上一篇文章分析tf的应用，编写代码，成功控制两台小车组成简单编队，但接下来要解决的问题还有很多，对几个问题给出一些个人的想法，欢迎大家在评论区留言讨论：

如何设计合适的控制器使编队保持稳定：使用PID控制机器人保持稳定的距离和方向；通过运动学模型分析，构建误差系统，设计控制器。
如何切换编队队形：调整虚拟机器人相对于领航者的位姿来切换队形。在切换队形过程中也要寻找合适的算法防止队形内机器人碰撞。
编队被人为破坏如何恢复：目前，人为破坏编队会使得破坏基于里程计的定位，这时需要借助其他传感器比如雷达视觉等对机器人进行重定位来恢复编队。
如何增加或减少编队机器人数量：动态增加或减少虚拟机器人的个数，使得跟随者机器人有虚拟机器人可跟踪。当放入新的跟随者机器人同样需要向world发布自己的tf，来获取和虚拟机器人的坐标变换。
编队如何避障：借助雷达等传感器检测障碍物，不仅领航者机器人要避障，跟随者跟随的同时检测到障碍物也要避障，或者领航者检测到障碍物的信息共享给跟随者来识别障碍物，再寻找合适的算法来避障。或者利用虚拟结构法与领航跟随法相结合，将队形看成一个刚体，对机器人实现分布式控制，使得整体避障，再切换回领航跟随法。
编队如何导航：领航者根据算法规划的路径导航到目标点，跟随者跟随，但要注意编队避障。
如何切换领航者：当领航者出现故障，指定一跟随者成为领航者，并保持跟随者原队形继续工作，需要成为领航者的跟随者发布固定的虚拟机器人坐标，使其他的跟随者跟随新的领航者。

0x05 参考资料

[1].Writing a tf broadcaster (Python)[OL].http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29.

[2].一类非完整移动机器人编队控制方法[J] 张瑞雷,李胜,陈庆伟.控制与决策.2013 (11).

[3].陈传均. 多机器人领航—跟随型编队控制研究[D].杭州电子科技大学,2015.

[4].ROS中的tf(transform)的理解,你追我小乌龟的深入剖析[OL].https://blog.csdn.net/qq_24371789/article/details/99673470.

0x06 问题反馈

大家在按照教程操作过程中有任何问题，可以关注ROS小课堂的官方微信公众号，在公众号中给我发消息反馈问题即可，我基本上每天都会处理公众号中的留言！当然，如果你要是顺便给ROS小课堂打个赏，我也会感激不尽的，打赏30块还会邀请进ROS小课堂的微信群与更多志同道合的小伙伴一起学习和交流！

本文原创，作者：adam_zhuo，其版权均为ROS小课堂所有。
如需转载，请注明出处：https://www.corvin.cn/2525.html

{{userData.name}}