5.1激光雷达跟随（冰达机器人）

5.1激光雷达跟随功能开发

5.1.1激光雷达跟随功能需求分析

在完成激光雷达跟踪之前，我们先来拆解一下功能。要实现跟随，首先需要确定跟随的目标，在这个例程中，我们使机器人根据离它最近的物体。周围物体的距离可以通过激光雷达测量结果得到，但是激光雷达是360°的，在跟随的过程中很容易被周围的物体带“跑偏”.例如我们想要让机器人跟随人走过一段走廊，在跟随的过程中有可能走廊的墙体会比人距离更近，机器人会试图以墙体作为跟随目标。针对这种情况我们可以从激光雷达的数据中找出一定角度范围内的最近物体，如图10-2-1所示。

在检测到需要跟随的目标后，根据目标距离机器人的距离和角度信息，计算机器人的运动速度，使机器人朝目标运动。

机器人跟随也不能无限制的接近跟随的物体，否则就撞上了，可以设置一个安全的跟随距离，当小于跟随距离后，机器人不再继续运动靠近跟随目标。

通过查看激光雷达消息类型的内容，如图5-1-2所示，ranges数组中存放的是距离信息，所以距离信息可以通过查找数组中最小值获取。数组元素和角度的对应关系可以通过angle_min和angle_increment计算得到。例如ranges中第113个元素为最小值，那么最小值所对应的角度就是angle_min+angle_increment*113。但是这个角度是相对于雷达的坐标系而言，机器人的坐标系和雷达坐标系可能还有旋转关系，我们需要对这个旋转关系做变换。NanoRobot上的激光雷达和机器人之间的坐标有着绕Z轴旋转180°的变换关系，我们需要对这个变换关系做处理。

5.1.2编写代码实现激光雷达跟随功

理解清楚设计目标后就可以设计代码了，笔者编写的代码是lidar_follow.py，代码如下

#!/usr/bin/python
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
import numpy as np
 
class LidarTracker:
    def __init__(self):
        rospy.init_node('lidar_follow', anonymous=False)
        self.scanSubscriber = rospy.Subscriber('/scan', LaserScan, self.scancallback)
        self.cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
 
        self.followDistance = rospy.get_param('~followDistance',0.5)
        self.minAngle = rospy.get_param('~minAngle',-0.5)
        self.maxAngle = rospy.get_param('~maxAngle',0.5)
        self.deltaDist = rospy.get_param('~deltaDist',0.2)
        self.winSize = rospy.get_param('~winSize',2)
        self.lidarInstallAngle = rospy.get_param('~lidarInstallAngle',0)
        self.max_velocity = 0.3
        self.min_velocity = 0.05
        rospy.spin()
 
    def scancallback(self,scan_data):
 
if __name__ == '__main__':
    try:
       LidarTracker()
    except rospy.ROSInterruptException:
        pass

这段代码中，定的LidarTracker类中，定义了一个cmd_vel话题发布器cmd_pub和scan话题订阅器scanSubscriber，话题订阅器的回调函数为scancallback。并设置一系列参数用于指定跟随角度范围，跟随最近距离，滤波窗口大小，雷达安装角度等。跟随动作的实现是在scancallback函数中，代码如下

def scancallback(self,scan_data):
    # make scan data as a array
    ranges = np.array(scan_data.ranges)
    # arrange data index ascending order
    rangesIndex = np.argsort(ranges)
    tempMinDistance = float("inf")
    for i in rangesIndex:
        tempMinDistance = ranges[i]
        tempMinDistanceAngle = scan_data.angle_min + i*scan_data.angle_increment
        tempMinDistanceAngle += self.lidarInstallAngle
        if tempMinDistanceAngle > 3.14159:
            tempMinDistanceAngle -= 3.14159*2
        elif tempMinDistanceAngle < -3.14159:
            tempMinDistanceAngle += 3.14159*2
        else:
            pass
 
        windowIndex = np.clip([i-self.winSize, i+self.winSize+1],0,len(ranges))
        window = ranges[windowIndex[0]:windowIndex[1]]
        # filt senser noise point
        with np.errstate(invalid='ignore'):
            if(np.any(abs(window - tempMinDistance) < self.deltaDist)):
                if tempMinDistanceAngle > self.minAngle and tempMinDistanceAngle < self.maxAngle:
                    print (tempMinDistance,tempMinDistanceAngle)
                    break
                else:
                    pass
            else:
                pass
    #catches no scan, no minimum found, minimum is actually inf
    if tempMinDistance > scan_data.range_max:
        rospy.logwarn('laser no object found')
    else:
        twist = Twist()
        if tempMinDistance > self.followDistance:
            twist.linear.x = tempMinDistance - self.followDistance
            #set a velocity threshold control
            if twist.linear.x < self.min_velocity:
                twist.linear.x = self.min_velocity
            elif twist.linear.x > self.max_velocity:
                twist.linear.x = self.max_velocity
            else:
                pass
        else:
            twist.linear.x = 0.0
        if abs(tempMinDistanceAngle) > 0.05:
            twist.angular.z = tempMinDistanceAngle*3
        self.cmd_pub.publish(twist)

在这段代码中，先将雷达消息中的ranges元素转换为一个numpy的数组ranges方便后续处理，然后获取数组的长度rangesIndex，将最小值设置为无穷大“inf”。

接下来就从ranges数组中搜索最小距离值tempMinDistance，计算出最小距离值相对雷达的角度tempMinDistanceAngle，再根据雷达-底盘之间的安装角度关系对角度值做变换，转换为相对机器人底盘的角度。

为了防止获取到的最小值是由雷达传感器噪声产生的，可以对最小值和数组中临近的值做比较，如果最小值没有通过滤波器，则丢弃数据，继续寻找新的最小值。如果通过了滤波器，则判断最近距离点是否在设定的跟随角度范围内，如果不在，则丢弃数据，继续寻找新的最小值。如果在则跳出搜索最小值的循环。

在结束搜索最小值的循环后，对最小值做判断，如果是有效的最小值，则开始计算机器人运动速度。如果最小值大于跟随距离，则根据当前到目标的距离计算机器人沿X轴运动的线速度。如果最小值小于跟随距离，则将X轴线速度设为0。Z轴的角速度是根据跟随目标点和机器人的角度tempMinDistanceAngle值计算。计算完成后通过cmd_pub话题发布器发布话题。

现在可以测试一下效果，启动机器人底盘、雷达节点，或者启动仿真器环境，然后运行雷达跟随节点。

rosrun bingda_application lidar_follow.py lidarInstallAngle:=3.14159

需要注意的是，NanoRobot硬件上雷达和底盘直接由180°的旋转关系，仿真器中的机器人模型是不存在的，所以使用机器人硬件运行例程时，需要将lidarInstallAngle参数设置为3.14159，使用仿真器则不需要设置，使用默认值0即可。

使用机器人硬件可以使用人站在机器人前方，通过运动来检验跟随效果。在仿真环境中可以通过添加或者移动仿真场景中的物体位置来检验跟随效果。例如笔者通过移动仿真场景中桌子的位置，使机器人使用跟随桌腿运动，如图所示。

移动仿真场景中物体位置验证跟随效果