亚博microros小车-原生ubuntu支持系列：14雷达跟踪与雷达守卫

背景知识

激光雷达的数据格式参见：

亚博microros小车-原生ubuntu支持系列：13 激光雷达避障-CSDN博客

本节体验雷达跟踪跟守卫

PID控制

从百度百科摘一段介绍

比例积分微分控制（proportional-integral-derivative control），简称PID控制，是最早发展起来的控制策略之一，由于其算法简单、鲁棒性好和可靠性高，被广泛应用于工业过程控制，仍有90%左右的控制回路具有PID结构。

PID控制器各校正环节的作用如下：

比例环节：即时成比例地反应控制系统的偏差信号e(t)，偏差一旦产生，控制器立即产生控制作用以减小误差。当偏差e=0时，控制作用也为0。因此，比例控制是基于偏差进行调节的，即有差调节。

积分环节：能对误差进行记忆，主要用于消除静差，提高系统的无差度，积分作用的强弱取决于积分时间常数Ti，Ti越大，积分作用越弱，反之则越强。

微分环节：能反映偏差信号的变化趋势(变化速率)，并能在偏差信号值变得太大之前，在系统中引入一个有效的早期修正信号，从而加快系统的动作速度，减小调节时间。

从时间的角度讲，比例作用是针对系统当前误差进行控制，积分作用则针对系统误差的历史，而微分作用则反映了系统误差的变化趋势，这三者的组合是“过去、现在、未来”的完美结合

 对应的代码实现，common.py

#!/usr/bin/env python
# coding:utf-8
#import rospy
import rclpy
from geometry_msgs.msg import Twist
from std_msgs.msg import Boolclass SinglePID:def __init__(self, P=0.1, I=0.0, D=0.1):self.Kp = Pself.Ki = Iself.Kd = Dprint("init_pid: ", P, I, D)self.pid_reset()def Set_pid(self, P, I, D):self.Kp = Pself.Ki = Iself.Kd = Dprint("set_pid: ", P, I, D)self.pid_reset()def pid_compute(self, target, current):self.error = target - current#计算误差self.intergral += self.error#计算积分self.derivative = self.error - self.prevError#计算微分result = self.Kp * self.error + self.Ki * self.intergral + self.Kd * self.derivative#PID输出self.prevError = self.errorreturn resultdef pid_reset(self):self.error = 0self.intergral = 0self.derivative = 0self.prevError = 0

雷达跟踪

小车连接上代理，运行程序，小车上的雷达扫描设定范围内的最近的一个物体，并且根据设定的跟踪距离，调试自身的速度，与该物体保持一定的距离。通过动态参数调节器可以调整雷达检测的范围和避障检测的距离等参数。

src/yahboomcar_laser/yahboomcar_laser/目录下新建文件laser_Tracker.py，代码如下：

#ros lib
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
import os
import sys
#commom lib
import math
import numpy as np
import time
from time import sleep
from yahboomcar_laser.common import *
print ("improt done")
RAD2DEG = 180 / math.piclass laserTracker(Node):def __init__(self,name):super().__init__(name)#create a subself.sub_laser = self.create_subscription(LaserScan,"/scan",self.registerScan,1)self.sub_JoyState = self.create_subscription(Bool,'/JoyState', self.JoyStateCallback,1)#create a pubself.pub_vel = self.create_publisher(Twist,'/cmd_vel',1)#declareparam 声明参数self.declare_parameter("priorityAngle",10.0)#优先检测雷达角度self.priorityAngle = self.get_parameter('priorityAngle').get_parameter_value().double_valueself.declare_parameter("LaserAngle",15.0)self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_valueself.declare_parameter("ResponseDist",0.55)self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_valueself.declare_parameter("Switch",False)self.Switch = self.get_parameter('Switch').get_parameter_value().bool_valueself.Right_warning = 0self.Left_warning = 0self.front_warning = 0self.Joy_active = Falseself.ros_ctrl = SinglePID()self.Moving = Falseself.lin_pid = SinglePID(2.0, 0.0, 2.0)self.ang_pid = SinglePID(3.0, 0.0, 5.0)self.timer = self.create_timer(0.01,self.on_timer)def on_timer(self):self.Switch = self.get_parameter('Switch').get_parameter_value().bool_valueself.priorityAngle = self.get_parameter('priorityAngle').get_parameter_value().double_valueself.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_valueself.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_valuedef JoyStateCallback(self, msg):if not isinstance(msg, Bool): returnself.Joy_active = msg.datadef exit_pro(self):cmd1 = "ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "cmd2 = '''"{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"'''cmd = cmd1 +cmd2os.system(cmd)#雷达回调def registerScan(self, scan_data):if not isinstance(scan_data, LaserScan): returnranges = np.array(scan_data.ranges)offset = 0.5frontDistList = []frontDistIDList = []minDistList = []minDistIDList = []for i in range(len(ranges)):angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEGif angle > 180: angle = angle - 360if abs(angle) < self.priorityAngle:if 0 < ranges[i] < (self.ResponseDist + offset):frontDistIDList.append(angle)frontDistList.append(ranges[i])elif abs(angle) < self.LaserAngle and ranges[i] > 0:minDistList.append(ranges[i])minDistIDList.append(angle)if len(frontDistIDList) != 0: #找到最近的一个物体minDistID            minDist = min(frontDistList)minDistID = frontDistIDList[frontDistList.index(minDist)]else:minDist = min(minDistList)minDistID = minDistIDList[minDistList.index(minDist)]if self.Joy_active or self.Switch == True: #手柄或者开关打开，则停止移动if self.Moving == True:self.pub_vel.publish(Twist())self.Moving = not self.Movingreturnself.Moving = Truevelocity = Twist()print("minDist: ",minDist)#根据需要跟踪的物体，计算角速度和线速度，然后发布速度数据if abs(minDist - self.ResponseDist) < 0.1: minDist = self.ResponseDistvelocity.linear.x = -self.lin_pid.pid_compute(self.ResponseDist, minDist)ang_pid_compute = self.ang_pid.pid_compute(minDistID/48, 0)if minDistID > 0: velocity.angular.z = ang_pid_computeelse: velocity.angular.z = ang_pid_computevelocity.angular.z = ang_pid_computeif abs(ang_pid_compute) < 0.1: velocity.angular.z = 0.0self.pub_vel.publish(velocity)def main():rclpy.init()laser_tracker = laserTracker("laser_Tracker")print ("start it")try:rclpy.spin(laser_tracker)except KeyboardInterrupt:passfinally:laser_tracker.exit_pro()laser_tracker.destroy_node()rclpy.shutdown()

构建后运行

bohu@bohu-TM1701:~/yahboomcar/yahboomcar_ws$ ros2 run yahboomcar_laser laser_Tracker 
improt done
init_pid:  0.1 0.0 0.1
init_pid:  2.0 0.0 2.0
init_pid:  3.0 0.0 5.0
start it
minDist:  2.104
minDist:  2.1
minDist:  2.092
minDist:  2.092
minDist:  2.033
minDist:  1.955
minDist:  1.857
minDist:  1.76
minDist:  1.662
minDist:  1.662
minDist:  1.567
minDist:  1.461
minDist:  1.369
minDist:  1.273
minDist:  1.273
minDist:  1.078
minDist:  1.078
minDist:  0.975
minDist:  0.881
minDist:  0.881
minDist:  0.802
minDist:  0.718
minDist:  0.654
minDist:  0.615
minDist:  0.601
minDist:  0.601
minDist:  0.603

程序启动后，会寻找雷达扫描范围内最近的物体，并且与它保持设定的距离。缓慢移动被跟踪的物体，小车会跟踪物体移动。可以通过动态参数调节器去设置一些参数，终端输入，

ros2 run rqt_reconfigure rqt_reconfigure

以上参数说明：

• priorityAngle：雷达优先检测的角度

• LaserAngle：雷达检测角度

• ResponseDist：跟踪的距离

• Switch：玩法开关

要是小车没反应。关机重启下，跟踪移动要缓慢些，快了丢失了。

雷达守卫

小车连接上代理，运行程序，小车上的雷达扫描设定范围内的最近的一个物体，并且会通过自转跟踪该物体，如果该物体靠近雷达小于设定的距离，则小车上的蜂鸣器会响以示警告。通过动态参数调节器可以调整雷达检测的范围和避障检测的距离等参数。

在目录下src/yahboomcar_laser/yahboomcar_laser/新建文件laser_Warning.py，代码如下：

#ros lib
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Bool,UInt16
#commom lib
import os
import sys
import math
import numpy as np
import time
from time import sleep
from yahboomcar_laser.common import *
print ("improt done")
RAD2DEG = 180 / math.piclass laserWarning(Node):def __init__(self,name):super().__init__(name)#create a subself.sub_laser = self.create_subscription(LaserScan,"/scan",self.registerScan,1)self.sub_JoyState = self.create_subscription(Bool,'/JoyState', self.JoyStateCallback,1)self.sub_JoyState = self.create_subscription(Bool,'/JoyState', self.JoyStateCallback,1)#create a pubself.pub_vel = self.create_publisher(Twist,'/cmd_vel',1)#控制小车self.pub_Buzzer = self.create_publisher(UInt16,'/beep',1)#蜂鸣器#declareparamself.declare_parameter("LaserAngle",10.0)self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_valueself.declare_parameter("ResponseDist",0.3)self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_valueself.declare_parameter("Switch",False)self.Switch = self.get_parameter('Switch').get_parameter_value().bool_valueself.Right_warning = 0self.Left_warning = 0self.front_warning = 0self.Joy_active = Falseself.ros_ctrl = SinglePID()self.ang_pid = SinglePID(3.0, 0.0, 5.0)self.Buzzer_state = Falseself.Moving = Falseself.timer = self.create_timer(0.01,self.on_timer)def on_timer(self):self.Switch = self.get_parameter('Switch').get_parameter_value().bool_valueself.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_valueself.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_valuedef JoyStateCallback(self, msg):if not isinstance(msg, Bool): returnself.Joy_active = msg.datadef exit_pro(self):cmd1 = "ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "cmd2 = '''"{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"'''cmd = cmd1 +cmd2os.system(cmd)cmd3 = "ros2 topic pub --once /beep std_msgs/msg/UInt16 "cmd4 = '''"data: 0"'''cmd = cmd1 +cmd2os.system(cmd)#雷达回调函数def registerScan(self, scan_data):if not isinstance(scan_data, LaserScan): returnif self.Joy_active  or self.Switch == True:if self.Moving == True:print("stop")self.pub_vel.publish(Twist())self.Moving = not self.Movingreturnself.Moving = Trueranges = np.array(scan_data.ranges)minDistList = []minDistIDList = []for i in range(len(ranges)):angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEGif angle > 180: angle = angle - 360#查找雷达检索范围内最近的物体if  abs(angle) < self.LaserAngle and ranges[i] > 0:minDistList.append(ranges[i])minDistIDList.append(angle)if len(minDistList) != 0:minDist = min(minDistList)minDistID = minDistIDList[minDistList.index(minDist)]velocity = Twist()#根据与跟踪物体的位置偏差，计算角速度，使得小车车头对准该物体angle_pid_compute = self.ang_pid.pid_compute(minDistID/48, 0)if abs(angle_pid_compute) < 0.1:velocity.angular.z = 0.0else:velocity.angular.z = angle_pid_computeself.pub_vel.publish(velocity)print("minDist: ",minDist)print("minDistID: ",minDistID)#判断最小物体的雷达检测的距离是否小于设定的范围，随后做出判断是否需要让蜂鸣器响if minDist <= self.ResponseDist:print("---------------")b = UInt16()b.data = 1self.pub_Buzzer.publish(b)else:print("no obstacles@")b = UInt16()b.data = 0self.pub_Buzzer.publish(UInt16())def main():rclpy.init()laser_warn = laserWarning("laser_Warnning")print ("start it")try:rclpy.spin(laser_warn)except KeyboardInterrupt:passfinally:laser_warn.exit_pro()laser_warn.destroy_node()rclpy.shutdown()

 构建后运行：

ros2 run yahboomcar_laser laser_Warning        #雷达警卫

日志如下：

no obstacles@
minDist:  0.352
minDistID:  9.999993519233985
no obstacles@
minDist:  0.317
minDistID:  9.999993519233985
no obstacles@
minDist:  0.289
minDistID:  9.999993519233985
---------------
minDist:  0.289
minDistID:  9.999993519233985
---------------
minDist:  0.281
minDistID:  9.999993519233985
---------------
minDist:  0.279
minDistID:  9.999993519233985
---------------
minDist:  0.278
minDistID:  7.999993534726719
---------------

程序启动后，会寻找雷达扫描范围内最近的物体，缓慢移动该物体，小车会通过自转去跟踪该物体。快了也会丢失，太小了识别不佳，我找了个鞋盒子测试。

如上所示，没有超过设定范围则打印【no obstacles@】，出现了障碍物则会打印【---------------】并且蜂鸣器会响。可以通过动态参数调节器去设置一些参数，终端输入，

ros2 run rqt_reconfigure rqt_reconfigure

以上参数说明：

• LaserAngle：雷达检测角度

• ResponseDist：跟踪的距离

• Switch：玩法开关

最后看一下节点通信图

以上。