视觉小车寻迹

PID算法

PID（比例-积分-微分）算法是一种常用的闭环控制算法，广泛应用于工业控制系统中。PID控制器通过三种基本控制作用（比例P、积分I和微分D）的组合，对系统进行控制以达到期望的输出。现在，我们来详细解释这三种控制作用和PID算法的工作原理。

比例控制（P）

• 原理：比例控制部分是系统偏差（设定点与实际输出值之间的差值）的线性响应。其作用是根据偏差的大小来调整控制量，偏差越大，控制作用越强。

• 特点：比例控制可以快速减少系统的偏差，但存在一个固有的问题，即无法完全消除偏差，会有一个稳态误差。

积分控制（I）

• 原理：积分控制部分是对偏差随时间积分的响应。它的作用是消除长期积累的偏差，从而消除稳态误差。

• 特点：积分作用能够确保系统输出最终达到设定点，但其响应速度较慢，并可能引起超调和系统的振荡。

微分控制（D）

• 原理: 微分控制部分是对偏差变化率的响应。其目的是预测偏差的未来趋势，加以提前调整，从而减少系统的超调和振荡。

• 特点：微分作用有助于提高系统的稳定性和响应速度，但在存在噪声的系统中可能造成控制作用的频繁变化。

PID 控制器的数学表达式

PID 控制器的输出通常可以表示为：

其中：

控制器的调整

• PID 控制器的性能极大地依赖于  这三个参数的设定。正确的参数设定可以使系统快速稳定地达到目标值，而参数设定不当可能会导致控制系统的性能不稳定，甚至发散。
• 调参方法：理论分析、经验法则、软件仿真、自动调参工具等。

坐标转换，不用坐标转换，摄像头检测

将乒乓球在图像中的位置转换成小车的实际移动坐标。这需要根据摄像头的位置、角度和焦距等参数，将图像坐标转换成实际环境中的坐标。

1.算出最近的小球
 

两个输入：

• 对于转向，偏差可以是乒乓球中心和摄像头视野中心之间的水平像素距离差。
• 对于速度，偏差可以是乒乓球大小的目标值（例如理想捡球距离处的像素大小）和当前检测到的大小之差。

路径规划

衡量标椎：走最短的路，捡最多的球

初始判断：选择初始方向，小球权重最大的方向（数量多，并且整体距离近）

最佳方向判断方式：旋转90°，判断一圈四个方向上，哪个方向上的小球权重最大

小球权重计算方式：当前窗口内的所有小球的距离的平均值

决策判断：距离最近

        先做一次目标检测得到当前检测到的小球的vector< Point2i >信息（坐标和个数）,然后再针对这些信息来进行数学上的运算以得到我们所需要的决策信息，最后再加入串口通信的代码将我们的信息发送给电控即可。

没有小球：环形扩散，转向和前进 分开做决策

遇到墙：（自动避障？）

receive_msg中有球的数量ball_num，根据此来判断是否检测到小球.

给电机发的距离，不是小车实际动的距离，减去管道距离

判断红外是否收到小球

小车的初始模式mode0：记下当前窗口小球的数量，向右旋转90°，记下当前窗口小球的数量。判断是否旋转一周？否：继续旋转，是：把当前小车方向改为小球数量最多的方向。然后判断是否识别到小球？是：进入mode1捡球模式：调用一个函数receive_msg来获取最近的小球的前进距离和方向（best_distance，best_angle），这两个量已知，可以直接获取，再写一个函数来发送这两个数据给电机，控制小车运动。之后继续判断是否识别到小球？否：进入mode2原地旋转模式，判断向右旋转45°后是否检测到小球？是：进入mode1捡球模式，否：判断是否旋转一周？否：继续向右旋转45°后是否检测到小球？是（旋转一周）：进入mode3环形扩散模式。环形扩散模式已有，调用spiral_movement即可。

没有小球也要发数据，不发的话，程序默认要写0

测转角的算法

        首先，它通过处理由YOLO检测算法返回的边界框信息，然后计算这个目标的中点坐标(target_point.x, target_point.y)。这个中点坐标是相对于图像中心的偏移量，其中图像中心偏移量被计算为(0,0)点（屏幕分辨率为640 x 480，代码中将目标点的x坐标直接跟320做差，y坐标跟480做差，目的是将图像中心定位为坐标系原点）。

        然后，计算从原点（图像的中心）到中点的直线距离（即斜边长度）以及这个直线与图像水平轴（即x轴的正方向）的夹角angle，这里的angle 是使用反正弦函数asin计算得到的，具体反映了目标点相对于图像水平中心线的垂直方向的偏移角。最后将角度从弧度转为度，通过这个角度可以用来调整车辆的朝向，以使车辆转向并朝向目标点。主要代码如下：

 target_x = (x1 + x2) / 2 - 320  # 图像中心为原点的x坐标
 target_y = 480 - (y1 + y2) / 2  # 图像中心为原点的y坐标

def calculate_rotation_angle(x, y):
    hypotenuse = math.hypot(x, y)  # 计算斜边长度
    if hypotenuse == 0:  # 避免除以零的情况
        return 0
    angle = math.asin(x / hypotenuse)  # 用反正弦(arc sin)计算角度（以弧度为单位）
    angle1 = angle  # 调整角度方向
    angle2 = angle1 * 180 / math.pi  # 将角度从弧度转换为度数
    return angle2

动态窗口法（Dynamic Window Approach, DWA）

RRT*（Rapidly-exploring Random Trees Star）

Proximal Policy Optimization (PPO)强化学习算法

PPO算法因其稳定性和高效性而备受青睐，它采用了一种更新策略，旨在避免更新步骤太大而破坏学习进程。它适用于复杂的环境中，诸如动态避障和路径规划问题。

运动范围

避障

import RPi.GPIO as GPIO  # 用于控制树莓派上的GPIO引脚
import time  # 用于控制时间，如延时
from picamera.array import PiRGBArray  # 用于抓取摄像头数据为numpy数组
from picamera import PiCamera  # 控制树莓派摄像头

from move import CarMove  # 导入控制小车移动的自定义模块
from ultrasound import CarUltrasound  # 导入超声波距离检测的自定义模块
from infrared import CarInfrared  # 导入红外线探测器的自定义模块
from camera import CarCamera  # 导入摄像头控制的自定义模块

GPIO.setwarnings(False)  # 禁止GPIO警告信息
GPIO.setmode(GPIO.BCM)  # 设置GPIO的编码方式为BCM模式

class Car(CarMove, CarUltrasound, CarInfrared, CarCamera):
    def __init__(self):
        CarMove.__init__(self)  # 初始化移动控制模块
        CarUltrasound.__init__(self)  # 初始化超声波模块
        CarInfrared.__init__(self)  # 初始化红外线模块
        CarCamera.__init__(self)  # 初始化摄像头模块
    
    def AllStop(self):  # 定义停止所有活动的函数
        CarMove.MotorStop(self)  # 停止运动
        CarCamera.CameraCleanup(self)  # 清理摄像头资源
        GPIO.cleanup()  # 清理GPIO资源

if __name__ == '__main__':
    try:
        car = Car()  # 创建小车对象
        start_time = None  # 初始化计时器

        while True:
            # 感知环境
            dist_mov_ave = car.DistMeasureMovingAverage()  # 获取超声波传感器的移动平均距离
            print('Distance', dist_mov_ave)

            [left_measure, right_measure] = car.InfraredMeasure()  # 获取两侧红外线传感器的状态

            # 决策，根据感知到的信息来控制小车的运动
            if (start_time is None) or (time.time() - start_time >  0.5):  # 只有当一定时间间隔内没有做决定时
                start_time = None  # 重置计时器
                # 根据红外线传感器的情况来决定小车的运动方向
                if left_measure == 0 and right_measure == 1:
                    print("Going right")
                    car.right(80)
                elif left_measure == 1 and right_measure == 0:
                    print("Going left")
                    car.left(80)
                elif left_measure == 0 and right_measure == 0:
                    print("Going back")
                    car.back(50)
                else:
                    if dist_mov_ave < 20:  # 如果前方距离过近，则执行转弯
                        car.left(80)
                        print("Going left")
                        start_time = time.time()
                    elif dist_mov_ave < 100:  # 如果前方有一定距离，则视情况前行
                        car.forward(dist_mov_ave/2 + 40)
                    else:  # 如果前方距离充足则全速前进
                        car.forward(90)
            else:
                pass

    except KeyboardInterrupt:  # 当用户中断程序（如按Ctrl+C）时，执行清理操作
        print("Measurement stopped by User")
        car.AllStop()