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

背景知识

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

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

本节体验雷达跟踪跟守卫

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):