ROS-基于简单势场算法编写的线型编队领航

 前言

        大创项目也快结束了，记录一下，免得以后都不记得自己还做过ROS了，有关领航者编队控制的事情，有时间再更新吧......

        前段时间，在古月居学习了一个利用雷达结点跟随最近目标点的程序。与此同时，项目基于领航者-跟随者的编队控制避障项目也遇到了瓶颈，正在尝试仿真，因此在琢磨之余以古月的代码为底板写了一套线型编队跟随，效果还可以，比较稳定（不受网络质量、ROS通信的影响），最重要的是非常简单！虽然存在很明显的缺陷，但非常适合入门ROS的一个小程序。先贴出之前看到的那个文章：

        ROS下空环境中用激光雷达做跟随

        翻阅本文章的内容，首先需要学习ROS相关概念，对python有一定的基础。且本文不再对机器人的激光雷达进行介绍说明。这里附一个介绍激光雷达数据的，没看要先看，不然可能会看不懂激光数据处理那部分的代码：

        激光雷达数据分析

控制原理

        在一个线型编队中，为了保持编队阵型，一辆车与它前后车辆的相对关系可以有如下图的三种情况（这里拿相对复杂的中间车说明，头车与尾车更简单一些）：

• 当前车离自己远了，会给小车一个向前的引力，吸引小车前进，保持距离。线速度与到前车的距离distance有关，角速度与到前车的y轴距离有关（这里需要用到TF的相关知识）
• 当前车离自己太近了，为了避免碰撞，会给小车一个向后的斥力，排斥小车迫使它先后退。它的线速度、角速度关系与第一种情况相同。
• 当后车离自己太近了，为了避免碰撞，会给小车一个向前的斥力，排斥小车迫使它向前走。它的线速度、与第一种情况相同，但角速度与第一种情况相反。（右手比划比划就明白了）

         根据这个思路，我们就可以写出一套有关小车运动控制的程序。在小车上，距离的判断用激光雷达扫描完成，而引力与斥力则体现在小车的运动速度、角速度上。

应用与解析

使用方法

        我的实验方案是一台阿克曼头车后面配合两台差速，下面给出的是中间车的代码，头车启用底盘+键盘结点引领全队，跟车启用底盘+雷达+此代码即可。（有关雷达驱动怎么写，里程计驱动怎么写就不细说了，网上资源很丰富）

#!/usr/bin/env python
#-*-coding:UTF-8 -*-

import rospy
from geometry_msgs.msg import Twist
from rospy.exceptions import ROSInterruptException
from sensor_msgs.msg import LaserScan
import math


#capture lidar data
def capturer():
    ldata=rospy.wait_for_message("/scan",LaserScan,timeout=None)         # catch lidar datas
    return ldata


#publish the cmd to the car
def cmd_pub(data):
    p_gain_vel = 1.15
    p_gain_ang = 2.5
    vel_msg = Twist()

    pub = rospy.Publisher('/cmd_vel', Twist,queue_size=10)
    x_pillar,y_pillar = position_cal(data)
    print(x_pillar,y_pillar)
#repel      
    if x_pillar<0.325 and x_pillar > 0:
        vel_msg.linear.x = 0.01/x_pillar
        vel_msg.angular.z = (y_pillar * p_gain_ang)
# atraction
    elif x_pillar < -0.30 and x_pillar > -0.35:
        if x_pillar <=-0.30 and x_pillar > -0.31:
            vel_msg.linear.x = -(x_pillar *p_gain_vel * 0.1)
            vel_msg.angular.z = -(y_pillar * p_gain_ang)
        elif x_pillar <= -0.31 and x_pillar > -0.32:
            vel_msg.linear.x = -(x_pillar *p_gain_vel * 0.2)
            vel_msg.angular.z = -(y_pillar * p_gain_ang)
        elif x_pillar <= -0.32 and x_pillar > -0.33:
            vel_msg.linear.x = -(x_pillar *p_gain_vel * 0.3)
            vel_msg.angular.z = -(y_pillar * p_gain_ang)
        elif x_pillar <= -0.33 and x_pillar > -0.34:
            vel_msg.linear.x = -(x_pillar *p_gain_vel * 0.4)
            vel_msg.angular.z = -(y_pillar * p_gain_ang)
        elif x_pillar <= -0.34 and x_pillar > -0.35:
            vel_msg.linear.x = -(x_pillar * p_gain_vel*0.5)
            vel_msg.angular.z = -(y_pillar * p_gain_ang)
#backward_repel
    elif x_pillar < 0 and x_pillar > -0.2:
        vel_msg.linear.x = 0.01/x_pillar
        vel_msg.angular.z = (y_pillar * p_gain_ang)
    else:
         vel_msg.linear.x = 0
         vel_msg.angular.z = 0
    print(vel_msg.linear.x,vel_msg.angular.z)

    pub.publish(vel_msg)


#calculate the position of the car & item
def position_cal(data):
    smallest_distance = 200
    arr_size =int(math.floor((data.angle_max-data.angle_min)/data.angle_increment))
    for i in range(arr_size):
        if data.ranges[i] < smallest_distance:
            smallest_distance = data.ranges[i]
            alpha_pillar = (data.angle_min + (i * data.angle_increment))
    x = smallest_distance * math.cos(alpha_pillar)
    y = smallest_distance * math.sin(alpha_pillar)
    return x,y 


def main():
    rospy.init_node('follower')
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        lidar_data=capturer()
        cmd_pub(lidar_data)
        rate.sleep


if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        print('Error or Interuption happen')
        pass

模块引入        

        首先引入需要的一些模块，这里用到了rospy（个人不会cpp，只能改成python的），发布速度信息的Twist，激光雷达数据的Laserscan，还有一些数学公式math。

import rospy
from geometry_msgs.msg import Twist
from rospy.exceptions import ROSInterruptException
from sensor_msgs.msg import LaserScan
import math

雷达结点

        定义一个函数capturer用来接收并返回雷达的相关数据。wait_for_message是一个对于稳定输出源很好用的接收函数，它不需要回调函数，是一个直接赋值的功能。至于为什么好用这里不做详细赘述，感兴趣的小伙伴们可以自行查询。

#capture lidar data
def capturer():
    ldata=rospy.wait_for_message("/scan",LaserScan,timeout=None)         # catch lidar datas
    return ldata

 位置计算

        根据得到的激光雷达数据，实时的计算此时距离机器人的最近点，推算出它的距离，进而推算出x和y坐标。（可以观察x,y的取值，并思考为啥——它是以目标点为原点建立的tf）这里要用到激光雷达数据的相关知识。

#calculate the position of the car & item
def position_cal(data):
    smallest_distance = 200
    arr_size =int(math.floor((data.angle_max-data.angle_min)/data.angle_increment))
    for i in range(arr_size):
        if data.ranges[i] < smallest_distance:
            smallest_distance = data.ranges[i]
            alpha_pillar = (data.angle_min + (i * data.angle_increment))
    x = smallest_distance * math.cos(alpha_pillar)
    y = smallest_distance * math.sin(alpha_pillar)
    return x,y

 引力斥力函数（速度发布器）

        说好听点，叫引力斥力函数，其实就是一个速度的发布器。根据对x，y（理论上应该是distance和θ）判定来给机器人赋予不同的值，来调制小车的位姿以及姿态。都是大量的ifelse语句，逐个分析即可，趋势与前面原理所述一致。

#publish the cmd to the car
def cmd_pub(data):
    p_gain_vel = 1.15
    p_gain_ang = 2.5
    vel_msg = Twist()

    pub = rospy.Publisher('/cmd_vel', Twist,queue_size=10)
    x_pillar,y_pillar = position_cal(data)
    print(x_pillar,y_pillar)
#repel      
    if x_pillar<0.325 and x_pillar > 0:
        vel_msg.linear.x = 0.01/x_pillar
        vel_msg.angular.z = (y_pillar * p_gain_ang)
# atraction
    elif x_pillar < -0.30 and x_pillar > -0.35:
        if x_pillar <=-0.30 and x_pillar > -0.31:
            vel_msg.linear.x = -(x_pillar *p_gain_vel * 0.1)
            vel_msg.angular.z = -(y_pillar * p_gain_ang)
        elif x_pillar <= -0.31 and x_pillar > -0.32:
            vel_msg.linear.x = -(x_pillar *p_gain_vel * 0.2)
            vel_msg.angular.z = -(y_pillar * p_gain_ang)
        elif x_pillar <= -0.32 and x_pillar > -0.33:
            vel_msg.linear.x = -(x_pillar *p_gain_vel * 0.3)
            vel_msg.angular.z = -(y_pillar * p_gain_ang)
        elif x_pillar <= -0.33 and x_pillar > -0.34:
            vel_msg.linear.x = -(x_pillar *p_gain_vel * 0.4)
            vel_msg.angular.z = -(y_pillar * p_gain_ang)
        elif x_pillar <= -0.34 and x_pillar > -0.35:
            vel_msg.linear.x = -(x_pillar * p_gain_vel*0.5)
            vel_msg.angular.z = -(y_pillar * p_gain_ang)
#backward_repel
    elif x_pillar < 0 and x_pillar > -0.2:
        vel_msg.linear.x = 0.01/x_pillar
        vel_msg.angular.z = (y_pillar * p_gain_ang)
    else:
         vel_msg.linear.x = 0
         vel_msg.angular.z = 0
    print(vel_msg.linear.x,vel_msg.angular.z)

    pub.publish(vel_msg)

主函数

        最后，大功告成，在主函数里创建结点，定义发布频率，调用前面的函数，就可以愉快的调参数了。

def main():
    rospy.init_node('follower')
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        lidar_data=capturer()
        cmd_pub(lidar_data)
        rate.sleep

效果如下：

编队领航

---

 改进的思路

        整体代码在大致框架写完后需要考虑几个问题：

        1.如何保证中间车辆的平稳行驶，也就是避免小车由于距离的变化进而在车队中间像弹簧一样的跳动？

                设置合理的速度系数，一个是为了让它的速度变化更加贴合编队运动，一个也是为了让中间车更倾向于被某一种力引领。也就是说引力和斥力的设置范围应该不同，且有一个力更占主动性。

        2.如何尽可能消除在编队转弯时，由于雷达判定所带来的编队混乱问题？

                可以限定y的扫描范围，减小因为拐弯带来的错误点。

        3.如何选取合适的引力、斥力范围，以及线速度、角速度系数？

                线速度、角速度过大，会造成机器人的摇摆、抽搐。线速度、角速度过小，机器人不能及时跟随前车的方向变化。

一个没有做的想法

        对于这个编队，虽然很好玩，但它毕竟就直接建立在雷达数据之上。太简单了，因此也没法再做更多的事情。拿y进行角速度的判断实际上是不合理的，只能说趋势上是一致的，但做不到精准的控制。

        一种想法是根据相对变化趋势来进行机器人的调制，也就是通过毕竟上一个点与当前点的变化趋势来进行算法编写。

        另一个想法就是在算法中拿到机器人的位姿（odom）然后利用提供的机器人状态信息来增强编队控制的准确性。