亚博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 Bool
class SinglePID:
def __init__(self, P=0.1, I=0.0, D=0.1):
self.Kp = P
self.Ki = I
self.Kd = D
print("init_pid: ", P, I, D)
self.pid_reset()
def Set_pid(self, P, I, D):
self.Kp = P
self.Ki = I
self.Kd = D
print("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.error
return result
def pid_reset(self):
self.error = 0
self.intergral = 0
self.derivative = 0
self.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.pi
class laserTracker(Node):
def __init__(self,name):
super().__init__(name)
#create a sub
self.sub_laser = self.create_subscription(LaserScan,"/scan",self.registerScan,1)
self.sub_JoyState = self.create_subscription(Bool,'/JoyState', self.JoyStateCallback,1)
#create a pub
self.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_value
self.declare_parameter("LaserAngle",15.0)
self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
self.declare_parameter("ResponseDist",0.55)
self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
self.declare_parameter("Switch",False)
self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
self.Right_warning = 0
self.Left_warning = 0
self.front_warning = 0
self.Joy_active = False
self.ros_ctrl = SinglePID()
self.Moving = False
self.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_value
self.priorityAngle = self.get_parameter('priorityAngle').get_parameter_value().double_value
self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
def JoyStateCallback(self, msg):
if not isinstance(msg, Bool): return
self.Joy_active = msg.data
def 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 +cmd2
os.system(cmd)
#雷达回调
def registerScan(self, scan_data):
if not isinstance(scan_data, LaserScan): return
ranges = np.array(scan_data.ranges)
offset = 0.5
frontDistList = []
frontDistIDList = []
minDistList = []
minDistIDList = []
for i in range(len(ranges)):
angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
if angle > 180: angle = angle - 360
if 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.Moving
return
self.Moving = True
velocity = Twist()
print("minDist: ",minDist)
#根据需要跟踪的物体,计算角速度和线速度,然后发布速度数据
if abs(minDist - self.ResponseDist) < 0.1: minDist = self.ResponseDist
velocity.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_compute
else: velocity.angular.z = ang_pid_compute
velocity.angular.z = ang_pid_compute
if abs(ang_pid_compute) < 0.1: velocity.angular.z = 0.0
self.pub_vel.publish(velocity)
def main():
rclpy.init()
laser_tracker = laserTracker("laser_Tracker")
print ("start it")
try:
rclpy.spin(laser_tracker)
except KeyboardInterrupt:
pass
finally:
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.pi
class laserWarning(Node):
def __init__(self,name):
super().__init__(name)
#create a sub
self.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 pub
self.pub_vel = self.create_publisher(Twist,'/cmd_vel',1)#控制小车
self.pub_Buzzer = self.create_publisher(UInt16,'/beep',1)#蜂鸣器
#declareparam
self.declare_parameter("LaserAngle",10.0)
self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
self.declare_parameter("ResponseDist",0.3)
self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
self.declare_parameter("Switch",False)
self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
self.Right_warning = 0
self.Left_warning = 0
self.front_warning = 0
self.Joy_active = False
self.ros_ctrl = SinglePID()
self.ang_pid = SinglePID(3.0, 0.0, 5.0)
self.Buzzer_state = False
self.Moving = False
self.timer = self.create_timer(0.01,self.on_timer)
def on_timer(self):
self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
def JoyStateCallback(self, msg):
if not isinstance(msg, Bool): return
self.Joy_active = msg.data
def 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 +cmd2
os.system(cmd)
cmd3 = "ros2 topic pub --once /beep std_msgs/msg/UInt16 "
cmd4 = '''"data: 0"'''
cmd = cmd1 +cmd2
os.system(cmd)
#雷达回调函数
def registerScan(self, scan_data):
if not isinstance(scan_data, LaserScan): return
if self.Joy_active or self.Switch == True:
if self.Moving == True:
print("stop")
self.pub_vel.publish(Twist())
self.Moving = not self.Moving
return
self.Moving = True
ranges = np.array(scan_data.ranges)
minDistList = []
minDistIDList = []
for i in range(len(ranges)):
angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
if 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.0
else:
velocity.angular.z = angle_pid_compute
self.pub_vel.publish(velocity)
print("minDist: ",minDist)
print("minDistID: ",minDistID)
#判断最小物体的雷达检测的距离是否小于设定的范围,随后做出判断是否需要让蜂鸣器响
if minDist <= self.ResponseDist:
print("---------------")
b = UInt16()
b.data = 1
self.pub_Buzzer.publish(b)
else:
print("no obstacles@")
b = UInt16()
b.data = 0
self.pub_Buzzer.publish(UInt16())
def main():
rclpy.init()
laser_warn = laserWarning("laser_Warnning")
print ("start it")
try:
rclpy.spin(laser_warn)
except KeyboardInterrupt:
pass
finally:
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:玩法开关
最后看一下节点通信图
以上。