循迹小车

循迹小车¶

循迹小车制作主要是让小车实现根据黑线的路径进行循迹的功能.

智能小车可以分成三个部分——传感器部分、控制器部分、执行器部分

控制器部分：接收传感器部分传递过来的信号，并根据事前写入的决策系统（软件程序），来决定机器人对外部信号的反应，将控制信号发给执行器部分。

执行器部分：驱动机器人做出各种行为，包括发出各种信号（点亮发光二极管、发出声音）的部分，并且可以根据控制器部分的信号调整自己的状态。对机器人小车来说，最基本的就是轮子。

传感器部分：机器人用来读取各种外部信号的传感器，以及控制机器人行动的各种开关。

系统构成-系统框图

PID控制¶

https://baijiahao.baidu.com/s?id=1687928966424595013&wfr=spider&for=pc

对自动驾驶来说，控制就是指使用方向盘、油门和刹车，将车驾驶到期望的位置。我们开车在十字路口或拐弯时候，可以凭着直觉和经验来决定拐弯的角度大小，加速的时机，以及是否需要刹车等。我们需要将这种直觉教给计算机。

我们经常把控制算法称之为控制器，PID控制器就是最常见、最基础的控制器之一。在介绍PID控制器之前，我们先了解关于控制的大的分类，根据是否有反馈可以分为开环控制和闭环控制。

开环控制¶

开环控制，全称开环控制系统（Open Loop Control System），又称为无反馈系统。即系统的输入可以影响输出，但是 输入不受输出影响 的系统。输入到输出的信号是单向传递的。

以下为生活中开环系统的一些例子：

控制系统	输入量	控制器	受控对象	输出量
风扇调速	设置档位	控制电路	风扇电机	风扇转速
红外感应门	人体热辐射	控制电路	自动门电机	门打开
洗衣机	设置洗衣时间	控制电路	水阀&电机	洁净衣服
电饭煲	电源开关	控制电路	锅底加热器	锅内温度
水箱注水	水龙头开关	进水阀门	水流	水箱水位

闭环控制¶

闭环控制，全称闭环控制系统（Closed Loop Control System）：系统的输出可以通过 反馈回路 对输入造成影响，进而影响控制过程的系统。

常见闭环控制系统场景

车道线校正
车速控制
四轴飞行器高度控制
变频空调温控
锅炉温控

位置式PID¶

e(k): 用户设定的值（目标值） - 控制对象的当前的状态值

比例P : e(k)

积分I : ∑e(i) 误差的累加

微分D : e(k) - e(k-1) 这次误差-上次误差

增量式PID¶

比例P : e(k)-e(k-1) 这次误差-上次误差

积分I : e(i) 误差

微分D : e(k) - 2e(k-1)+e(k-2) 这次误差-2*上次误差+上上次误差

注意:结果只是增量,需要和前面的增量进行累加才能使用

差速运行¶

左转:右轮速度快,左轮速度慢

右转:左轮速度快,右轮速度慢

注意:

pid计算的结果是两个轮子的差速调整值

循迹小车实现¶

from genki.car import linetrack, motor
import ohos
import os
import socket

high_speed = 64
low_speed = 59
speed = high_speed

kp = 0.8
ki = 0.001
kd = 0.35

# 上一次误差
last_error = 0
# 上上次误差
prev_error = 0

# pid计算的结果
delta_u = 0.0
# 增量
delta_t = 0.0


def get_pos(val):
    acc = 0
    count = 0
    for i in range(0, 7, 1):
        if ((val >> i) & 0x01) == 1:
            count += 1
            if i < 3:
                acc += 16 * (3 - i)
            elif i == 3:
                acc += 0
            else:
                acc += -16 * (i - 3)

    if count > 0:
        return acc / count
    return 0


def track_task(arg):
    global speed
    print('start task')
    enable = linetrack.isEnable()
    print(enable)
    global delta_t, delta_u, prev_error, last_error

    while enable:
        state = linetrack.getState()
        # 重置
        delta_t = 0.0
        if state == 0xffff:
            motor.run(0, 0)
            last_error = 0
            prev_error = 0
            delta_t = 0
            delta_u = 0
            os.sleep(0.1)
            continue

        if state != 0x00:
            err = get_pos(state)
        else:
            err = last_error

        if abs(err) > 20:
            speed = low_speed
        else:
            speed = high_speed
        delta_t = kp * (err - last_error)
        delta_t += ki * err
        delta_t += kd * (err - 2 * last_error + prev_error)
        delta_u += delta_t

        prev_error = last_error
        last_error = err

        motor.run(int(speed + delta_u), int(speed - delta_u))
        os.sleep(0.02)


if __name__ == '__main__':
    print('app start')
    ohos.run(track_task, 0)