基于MATLAB+PID算法实现小车巡线功能

只看该作者 · 2023-2-28 17:50

否则，新的路成为最近的路，原先最近的路成为第二近的路，需要对err2和direct进行更新：

elseif err < 0

        err2 = err;

        direct = -1;

else

        err2 = err;

        direct = 1;

end

只看该作者 · 2023-2-28 17:50

当然，最后还要对err进行更新：

复制

err = dis;

只看该作者 · 2023-2-28 17:50

再或者，新路是当前第二近的路，则直接用新路的误差进行err2和direct的更新：

复制

elseif abs(dis) < abs(err2)

        err2 = dis;

        if err2 < 0

                direct = -1;

        else

                direct = 1;

        end                   

end

只看该作者 · 2023-2-28 17:52

如果当前行没有找到路，则考虑画面的上一行，对distance进行递减。但是我们不希望对太远的地方的路进行判断，导致过远的路影响正常判断，因此到画面的上半部分则直接退出循环，不予考虑：

复制

distance = distance - 1;

if distance <= 320

        break

end

只看该作者 · 2023-2-28 17:52

如此一来，就得到了当前的误差err与下次急转弯的预期转弯方向direct，并进行返回。

只看该作者 · 2023-2-28 17:53

2.3 PD算法控制小车速度
控制速度时，我们只考虑比例和微分部分。因为路径状况不稳定，不同的位置有不同的状态，因此积分控制可能会导致错误。

只看该作者 · 2023-2-28 17:53

如果direct_tmp为0，说明其不带有方向性，不更新实际的方向direct：

复制

if direct_tmp ~= 0

        direct = direct_tmp;

只看该作者 · 2023-2-28 17:53

接下来就是PD算法的主体实现部分了。设定速度的基础值为1.2，比例和微分控制常量分别为0.02和0.001，计算速度的校正值：delta_v = kp * new_err + kd * (new_err - err);。考虑到误差值的正负表示转弯方向的左右，因此校正值的正负也是表示方向左右，也就是左右轮的速度差。得到矫正值后，左右轮速度分别为v + delta_v和v - delta_v。

只看该作者 · 2023-2-28 17:53

复制

v = 1.2;

kp = 0.02;

kd = 0.001;

% PD算法计算速度差

delta_v = kp * new_err + kd * (new_err - err);

err = new_err;

% 更新速度

vrep.simxSetJointTargetVelocity(clientID, motorLeft, v + delta_v, vrep.simx_opmode_oneshot);

vrep.simxSetJointTargetVelocity(clientID, motorRight, v - delta_v, vrep.simx_opmode_oneshot);

只看该作者 · 2023-2-28 17:54

如此一来，小车就能正常依据画面状况来控制左右轮子的速度，从而实现寻路了。

只看该作者 · 2023-2-28 17:54

对急转弯的单独讨论
实现上述算法后，在急转弯时小车并不能正确运行。这是因为依据我们的算法，急转弯的尽头小车依旧会找到路从而直走而不会转弯，从而冲出赛道，到达画面中看不到路径的地方。这样一来，就应该进行原地转弯找回原来的路。原来的路的方向就是我们之前的参数direct指定的方向

只看该作者 · 2023-2-28 17:54

画面中首次出现路时，往往在画面的最边缘，这时恢复原来的算法可能导致转向速度异常。因此我们考虑重复获取画面与转向，直到画面中心的最下面出现路，可以沿直线走位为止。

只看该作者 · 2023-2-28 17:54

具体实现代码如下：

复制

if min(min(img(:,:))) > 20

        % 进行旋转

        vrep.simxSetJointTargetVelocity(clientID, motorLeft, direct, vrep.simx_opmode_oneshot);

        vrep.simxSetJointTargetVelocity(clientID, motorRight, -direct, vrep.simx_opmode_oneshot);

        % 旋转到画面的中间出现出现路为止

        while img(480,320) > 20

        code = 1;

                while code

                        [code, size, img] = vrep.simxGetVisionSensorImage2(clientID, camera, 1, vrep.simx_opmode_oneshot);

                end

        end

        % 更新误差

        err = 0;

        new_err = 0;