当前位置: 首页 > article >正文

亚博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:玩法开关

最后看一下节点通信图

以上。


http://www.kler.cn/a/522430.html

相关文章:

  • Cannot resolve symbol ‘XXX‘ Maven 依赖问题的解决过程
  • go gin配置air
  • JVM--类加载器
  • git困扰的问题
  • 星火大模型接入及文本生成HTTP流式、非流式接口(JAVA)
  • 汇编基础语法及其示例
  • CAN波特率匹配
  • OPPO自研DataFlow架构与实践
  • RHEL封闭环境部署zabbix
  • 【数据资产】数据资产管理概述
  • Workerman和Swoole有什么区别
  • Verilog中if语句和case语句综合出的电路区别
  • RabbitMQ 多种安装模式
  • 【信息系统项目管理师-选择真题】2013下半年综合知识答案和详解
  • 基于Springboot + vue实现的洗衣店订单管理系统
  • 2025年01月27日Github流行趋势
  • MySQL 日志:undo log、redo log、binlog 概述
  • java基础——专题一 《面向对象之前需要掌握的知识》
  • 一文大白话讲清楚webpack基本使用——18——HappyPack
  • react页面定时器调用一组多个接口,如果接口请求返回令牌失效,清除定时器不再触发这一组请求
  • 【浏览器 - Chrome调试模式,如何输出浏览器中的更多信息】
  • 如何根据壁纸主题选择合适的主色调?
  • 对海康威视工业相机进行取图
  • 产业园管理系统提升企业综合管理效率与智能化水平的成功案例分析
  • 若依路由配置教程
  • 图像处理篇---图像压缩格式编码格式