双磁条线跟踪控制

1问题 

同学反馈小车跟随磁力线，双轮差速小车，左右侧各有2个磁条传感器和各1条磁条线，需要控制小车跟随磁条线轨迹。

2 方法

（1）普通小车可能没有速度反馈，则不考虑转弯半径，仅考虑一个控制关系。

（2）左右轮子的车速转变需要调整，因为两个电机相对安装，直线、转向的转向是不同的。

根据上述问题，查kimi 给出算法

结合补充信息（小车左右两侧各有两个磁条传感器，磁条线在两个传感器中间时直线行驶），以下是修正后的控制算法：

控制算法

1. 传感器信号处理：

   • 每个磁条传感器输出的信号强度与磁条距离成反比。

   • 左侧两个传感器分别为 left_sensor1 和 left_sensor2，右侧两个传感器分别为 right_sensor1 和 right_sensor2。

   • 对每个传感器的信号进行滤波和校准，确保信号的稳定性和准确性。

2. 偏差计算：

   • 计算左侧两个传感器的平均信号强度：

     plaintext复制

     left_avg = (left_sensor1 + left_sensor2) / 2

   • 计算右侧两个传感器的平均信号强度：

     plaintext复制

     right_avg = (right_sensor1 + right_sensor2) / 2

   • 根据左右侧的平均信号强度，计算偏差值 error：

     plaintext复制

     error = (left_avg - right_avg) / (left_avg + right_avg)

     • error 的范围为 [-1, 1]，表示小车偏离磁条线的程度：

       • error = 0：磁条线在两个传感器中间，小车直线行驶。

       • error > 0：小车向右偏离磁条线。

       • error < 0：小车向左偏离磁条线。

3. PID控制：

   • 使用PID控制器对偏差进行处理，生成控制信号：

     plaintext复制

     control_signal = Kp * error + Ki * integral_error + Kd * derivative_error

     • Kp：比例系数，直接影响对偏差的响应速度。

     • Ki：积分系数，用于消除稳态误差。

     • Kd：微分系数，用于抑制偏差变化率，减少振荡。

4. 速度调节：

   • 根据控制信号，调整左右轮的速度：

     plaintext复制

     left_speed = base_speed + control_signal
     right_speed = base_speed - control_signal

     • base_speed：小车的基础速度。

     • control_signal：PID控制器的输出值，用于调整速度差。

5. 特殊情况处理：

   • 如果所有传感器的信号强度都低于某个阈值（例如，未检测到磁条线），小车应降低速度并进行搜索：

     plaintext复制

     if (left_avg < threshold && right_avg < threshold):
         left_speed = search_speed
         right_speed = search_speed

6. 闭环控制：

   • 实时采集传感器信号，计算偏差，更新控制信号，并调整车轮速度，形成闭环控制。

实现步骤

1. 初始化：

   • 设置传感器和电机驱动。

   • 对每个传感器进行校准，确定其阈值。

   • 初始化PID控制器参数（Kp, Ki, Kd）。

2. 主循环：

   • 采集左右两侧传感器的信号强度。

   • 计算左右侧的平均信号强度。

   • 计算偏差值 error。

   • 使用PID控制器计算控制信号。

   • 调整左右轮速度。

   • 重复上述步骤，实现实时跟踪。

// 初始化
calibrate_sensors()
set_pid_parameters(Kp, Ki, Kd)
base_speed = 100  // 基础速度
threshold = 10    // 信号强度阈值
search_speed = 50 // 搜索结果速度

// 主循环
while True:
    // 采集传感器信号
    left_sensor1 = read_sensor("left1")
    left_sensor2 = read_sensor("left2")
    right_sensor1 = read_sensor("right1")
    right_sensor2 = read_sensor("right2")

    // 计算平均信号强度
    left_avg = (left_sensor1 + left_sensor2) / 2
    right_avg = (right_sensor1 + right_sensor2) / 2

    // 判断是否检测到磁条线
    if left_avg < threshold && right_avg < threshold:
        // 未检测到磁条线，进行搜索
        left_speed = search_speed
        right_speed = search_speed
    else:
        // 计算偏差
        error = (left_avg - right_avg) / (left_avg + right_avg)

        // PID控制
        control_signal = pid_control(error)

        // 调整速度
        left_speed = base_speed + control_signal
        right_speed = base_speed - control_signal

    // 设置车轮速度
    set_motor_speed("left", left_speed)
    set_motor_speed("right", right_speed)

左右转动：可以看 之前的帖子。MATLAB+Arduino控制小车直行+转向_arduino 驱动轮子-优快云博客

3 现场情况

同学到课堂现场来测试和调试，效果不错。但还需要改进。

（1）PID 分段控制问题，如设定死区，和pid分段，确保车辆平滑；

（2）速度控制策略：直线段、斜线 的车辆中心速度要有差异。

（3）多传感器融合判断问题。

如果结合IMU，IMU主要用于航向角，或者用于角速度w的控制； 则可用横向偏差（四个sensor差值）控制一个期望角度w1， 然后利用IMU反馈实现W1的精确控制。 则需要内外、外环2个PID。

具体实施待测试。