Pioneer_p3dx 小车路径规划与控制算法实现

function run(P,D)
    data2={}
    PD=0
    error=0
    for i=1,5,1 do
        res,data=sim.readVisionSensor(floorSensorHandles[i])
        if(res>=0)then
            data2[i]=data[11]
        end
    end
    if(data2[4]<0.73) then
        data2[3]=0
    end
    if(data2[2]<0.73) then
        data2[1]=0
    end
    if(data2[2]<data2[4]) then
        if(data2[3]<data2[1]) then
            error=math.abs(data2[5]-0.095)+math.abs(data2[3]-0.85)+math.abs(data2[2]-0.85)
        else
            error=math.abs(data2[5]-0.095)+math.abs(data2[1]-0.85)+math.abs(data2[2]-0.85)
        end
    else
        if(data2[3]<data2[1]) then
            error=math.abs(data2[5]-0.095)+math.abs(data2[3]-0.85)+math.abs(data2[4]-0.85)
        else
            error=math.abs(data2[5]-0.095)+math.abs(data2[1]-0.85)+math.abs(data2[4]-0.85)
        end
    end
    PD=P*error+D*(error-previousError)
    previousError=error
    previousdata=data2
    
    if error<0.2 then
        rightV=speed
        leftV=speed
        previousError=0
    elseif data2[3]<0.75 or data2[4]<0.75 then
        rightV=speed+PD*speed
        leftV=speed-PD*speed
    elseif data2[1]<0.75 or data2[2]<0.75 then
        rightV=speed-PD*speed
        leftV=speed+PD*speed
    end
    sim.setJointTargetVelocity(leftmotor,leftV)
    sim.setJointTargetVelocity(rightmotor,rightV)
end

function sysCall_threadmain()
    leftmotor=sim.getObjectHandle('Pioneer_p3dx_leftMotor')
    rightmotor=sim.getObjectHandle('Pioneer_p3dx_rightMotor')
    car=sim.getObjectHandle('P1')
    floorSensorHandles={-1,-1,-1,-1,-1}
    floorSensorHandles[1]=sim.getObjectHandle('l')
    floorSensorHandles[2]=sim.getObjectHandle('m')
    floorSensorHandles[3]=sim.getObjectHandle('r')
    floorSensorHandles[4]=sim.getObjectHandle('m0')
    floorSensorHandles[5]=sim.getObjectHandle('m2')
    postion1={-0.09,-1.37}
    ccc={-1,-1,-1}
    aaa=-1
    bbb=-1
    speed=-5
    back=-1
    previousError=0
    previousdata={}
    flag={0,0,0,0,0,0,0,0}
    stop={{-0.1,0.099},{-0.731,0.098},{-1.302,0.104},{-1.469,1.263},{-0.782,1.299},{0.238,1.296},{0.822,1.284},{0.6,0.131}}
    sim.setIntegerSignal('occupy2',1)
    sim.setJointTargetVelocity(leftmotor,0)
    sim.setJointTargetVelocity(rightmotor,0)
    sim.setIntegerSignal('arrive2',1)
    sim.waitForSignal('finish2')
    sim.clearIntegerSignal('finish2')
    while true do
        if(sim.getIntegerSignal('occupy3')==nil) then
            break
        end
    end
    sim.clearIntegerSignal('occupy2')
    flag={0,1,0,0,0,0,0,0}
    while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do
        position=sim.getObjectPosition(car,-1)
        if (math.abs(position[1]-stop[1][1])<0.05)and(math.abs(position[2]-stop[1][2])<0.05) and (flag[1]==0) then
            sim.setIntegerSignal('occupy1',1)
            sim.setJointTargetVelocity(leftmotor,0)
            sim.setJointTargetVelocity(rightmotor,0)
            sim.wait(0.8)
            sim.setIntegerSignal('arrive1',1)
            sim.waitForSignal('finish1')
            sim.clearIntegerSignal('finish1')
            while true do
                if(sim.getIntegerSignal('occupy2')==nil) then
                    break
                end
            end
            sim.clearIntegerSignal('occupy1')
            flag={1,0,0,0,0,0,0,0}
        end
        if (math.abs(position[1]-stop[2][1])<0.05)and(math.abs(position[2]-stop[2][2])<0.05) and (flag[2]==0) then
            sim.setIntegerSignal('occupy2',1)
            sim.setJointTargetVelocity(leftmotor,0)
            sim.setJointTargetVelocity(rightmotor,0)
            sim.wait(0.8)
            sim.setIntegerSignal('arrive2',1)
            sim.waitForSignal('finish2')
            sim.clearIntegerSignal('finish2')
            while true do
                if(sim.getIntegerSignal('occupy3')==nil) then
                    break
                end
            end
            sim.clearIntegerSignal('occupy2')
            flag={0,1,0,0,0,0,0,0}
        end
        if (math.abs(position[1]-stop[5][1])<0.05)and(math.abs(position[2]-stop[5][2])<0.05) and (flag[5]==0) then
            sim.setIntegerSignal('occupy5',1)
            sim.setJointTargetVelocity(leftmotor,0)
            sim.setJointTargetVelocity(rightmotor,0)
            sim.wait(0.8)
            sim.setIntegerSignal('arrive5',1)
            sim.waitForSignal('finish5')
            sim.clearIntegerSignal('finish5')
            while true do
                if(sim.getIntegerSignal('occupy6')==nil) then
                    break
                end
            end
            sim.clearIntegerSignal('occupy5')
            flag={0,0,0,0,1,0,0,0}
        end
        if (math.abs(position[1]-stop[6][1])<0.05)and(math.abs(position[2]-stop[6][2])<0.05) and (flag[6]==0) then
            sim.setIntegerSignal('occupy6',1)
            sim.setJointTargetVelocity(leftmotor,0)
            sim.setJointTargetVelocity(rightmotor,0)
            sim.wait(0.8)
            sim.setIntegerSignal('arrive6',1)
            sim.waitForSignal('finish6')
            sim.clearIntegerSignal('finish6')
            while true do
                if(sim.getIntegerSignal('occupy7')==nil) then
                    break
                end
            end
            sim.clearIntegerSignal('occupy6')
            flag={0,0,0,0,0,1,0,0}
        end
        if (math.abs(position[1]-stop[3][1])<0.05)and(math.abs(position[2]-stop[3][2])<0.05) and (flag[3]==0) then
            sim.setIntegerSignal('occupy3',1)
            if(sim.getIntegerSignal('occupy4')==nil) then
                sim.clearIntegerSignal('occupy3')
            else
                sim.setJointTargetVelocity(leftmotor,0)
                sim.setJointTargetVelocity(rightmotor,0)
                while true do
                    if(sim.getIntegerSignal('occupy4')==nil) then
                        break
                    end
                end
                sim.clearIntegerSignal('occupy3')
            end
            flag={0,0,1,0,0,0,0,0}
        end
        if (math.abs(position[1]-stop[4][1])<0.05)and(math.abs(position[2]-stop[4][2])<0.05) and (flag[4]==0) then
            sim.setIntegerSignal('occupy4',1)
            if(sim.getIntegerSignal('occupy5')==nil) then
                sim.clearIntegerSignal('occupy4')
            else
                sim.setJointTargetVelocity(leftmotor,0)
                sim.setJointTargetVelocity(rightmotor,0)
                while true do
                    if(sim.getIntegerSignal('occupy5')==nil) then
                        break
                    end
                end
                sim.clearIntegerSignal('occupy4')
            end
            flag={0,0,0,1,0,0,0,0}
        end
        if (math.abs(position[1]-stop[7][1])<0.05)and(math.abs(position[2]-stop[7][2])<0.05) and (flag[7]==0) then
            sim.setIntegerSignal('occupy7',1)
            if(sim.getIntegerSignal('occupy8')==nil) then
                sim.clearIntegerSignal('occupy7')
            else
                sim.setJointTargetVelocity(leftmotor,0)
                sim.setJointTargetVelocity(rightmotor,0)
                while true do
                    if(sim.getIntegerSignal('occupy8')==nil) then
                        break
                    end
                end
                sim.clearIntegerSignal('occupy7')
            end
            flag={0,0,0,0,0,0,1,0}
        end
        if (math.abs(position[1]-stop[8][1])<0.05)and(math.abs(position[2]-stop[8][2])<0.05) and (flag[8]==0) then
            sim.setIntegerSignal('occupy8',1)
            if(sim.getIntegerSignal('occupy1')==nil) then
                sim.clearIntegerSignal('occupy8')
            else
                sim.setJointTargetVelocity(leftmotor,0)
                sim.setJointTargetVelocity(rightmotor,0)
                while true do
                    if(sim.getIntegerSignal('occupy1')==nil) then
                        break
                    end
                end
                sim.clearIntegerSignal('occupy8')
            end
            flag={0,0,0,0,0,0,0,1}
        end
        run(0.22,0.06)
    end
end

逐句分析内容：

1. 定义函数 run(P,D)，该函数接受两个参数 P 和 D，表示控制器的比例系数和微分系数。
2. 初始化变量 data2、PD 和 error。
3. 通过循环读取地面传感器的数据，并将其存储在 data2 中。
4. 根据传感器数据对 data2 进行调整。
5. 根据 data2 中的数据计算出误差值 error。
6. 根据误差值和比例系数、微分系数计算出 PD 值。
7. 根据 error 值和一些条件，计算出左右轮的速度。
8. 设置左右轮的速度。
9. 定义函数 sysCall_threadmain()，该函数为主线程函数。
10. 获取左右轮的句柄和小车的句柄。
11. 获取地面传感器的句柄。
12. 初始化变量 postion1、ccc、aaa、bbb、speed、back、previousError、previousdata 和 flag。
13. 设置停靠点的坐标。
14. 设置信号量 occupy2 为 1，表示小车已经到达第二个停靠点。
15. 停止左右轮的运动。
16. 等待信号量 arrive2。
17. 清除信号量 finish2。
18. 当模拟器未停止时，执行以下操作：
19. 获取小车的位置。
20. 如果小车到达了第一个停靠点，并且 flag[1] 为 0，则设置信号量 occupy1 为 1，表示小车已经到达第一个停靠点。
21. 停止左右轮的运动。
22. 等待一段时间。
23. 设置信号量 arrive1。
24. 等待信号量 finish1。
25. 清除信号量 finish1。
26. 如果 occupy2 信号量不存在，则跳出循环。
27. 清除信号量 occupy1。
28. 设置 flag 中的相应元素为 1，表示小车已经到达了第一个停靠点。
29. 如果小车到达了第二个停靠点，并且 flag[2] 为 0，则设置信号量 occupy2 为 1，表示小车已经到达第二个停靠点。
30. 停止左右轮的运动。
31. 等待一段时间。
32. 设置信号量 arrive2。
33. 等待信号量 finish2。
34. 清除信号量 finish2。
35. 如果 occupy3 信号量不存在，则跳出循环。
36. 清除信号量 occupy2。
37. 设置 flag 中的相应元素为 1，表示小车已经到达了第二个停靠点。
38. 如果小车到达了第五个停靠点，并且 flag[5] 为 0，则设置信号量 occupy5 为 1，表示小车已经到达第五个停靠点。
39. 停止左右轮的运动。
40. 等待一段时间。
41. 设置信号量 arrive5。
42. 等待信号量 finish5。
43. 清除信号量 finish5。
44. 如果 occupy6 信号量不存在，则跳出循环。
45. 清除信号量 occupy5。
46. 设置 flag 中的相应元素为 1，表示小车已经到达了第五个停靠点。
47. 如果小车到达了第六个停靠点，并且 flag[6] 为 0，则设置信号量 occupy6 为 1，表示小车已经到达第六个停靠点。
48. 停止左右轮的运动。
49. 等待一段时间。
50. 设置信号量 arrive6。
51. 等待信号量 finish6。
52. 清除信号量 finish6。
53. 如果 occupy7 信号量不存在，则跳出循环。
54. 清除信号量 occupy6。
55. 设置 flag 中的相应元素为 1，表示小车已经到达了第六个停靠点。
56. 如果小车到达了第三个停靠点，并且 flag[3] 为 0，则设置信号量 occupy3 为 1，表示小车已经到达第三个停靠点。
57. 如果 occupy4 信号量不存在，则清除信号量 occupy3。
58. 否则，停止左右轮的运动，并等待信号量 occupy4 清除。
59. 清除信号量 occupy3。
60. 设置 flag 中的相应元素为 1，表示小车已经到达了第三个停靠点。
61. 如果小车到达了第四个停靠点，并且 flag[4] 为 0，则设置信号量 occupy4 为 1，表示小车已经到达第四个停靠点。
62. 如果 occupy5 信号量不存在，则清除信号量 occupy4。
63. 否则，停止左右轮的运动，并等待信号量 occupy5 清除。
64. 清除信号量 occupy4。
65. 设置 flag 中的相应元素为 1，表示小车已经到达了第四个停靠点。
66. 如果小车到达了第七个停靠点，并且 flag[7] 为 0，则设置信号量 occupy7 为 1，表示小车已经到达第七个停靠点。
67. 如果 occupy8 信号量不存在，则清除信号量 occupy7。
68. 否则，停止左右轮的运动，并等待信号量 occupy8 清除。
69. 清除信号量 occupy7。
70. 设置 flag 中的相应元素为 1，表示小车已经到达了第七个停靠点。
71. 如果小车到达了第八个停靠点，并且 flag[8] 为 0，则设置信号量 occupy8 为 1，表示小车已经到达第八个停靠点。
72. 如果 occupy1 信号量不存在，则清除信号量 occupy8。
73. 否则，停止左右轮的运动，并等待信号量 occupy1 清除