机器人地面行走PD算法代码分析 - Lua 实现

{"title":"分析下列PD算法代码\nfunction run(P,D)\n\tdata2={}\n\tPD=0\n\terror=0\n\tfor i=1,5,1 do\n\t\tres,data=sim.readVisionSensor(floorSensorHandles[i])\n\t\tif(res>=0)then\n\t\t\tdata2[i]=data[11]\n\t\tend\n\tend\n\tif(data2[4]<0.73) then\n\t\tdata2[3]=0\n\tend\n\tif(data2[2]<0.73) then\n\t\tdata2[1]=0\n\tend\n\tif(data2[2]<data2[4]) then\n\t\tif(data2[3]<data2[1]) then\n\t\t\terror=math.abs(data2[5]-0.095)+math.abs(data2[3]-0.85)+math.abs(data2[2]-0.85)\n\t\telse\n\t\t\terror=math.abs(data2[5]-0.095)+math.abs(data2[1]-0.85)+math.abs(data2[2]-0.85)\n\t\tend\n\telse\n\t\tif(data2[3]<data2[1]) then\n\t\t\terror=math.abs(data2[5]-0.095)+math.abs(data2[3]-0.85)+math.abs(data2[4]-0.85)\n\t\telse\n\t\t\terror=math.abs(data2[5]-0.095)+math.abs(data2[1]-0.85)+math.abs(data2[4]-0.85)\n\t\tend\n\tend\n\tPD=Perror+D(error-previousError)\n\tpreviousError=error\n\tpreviousdata=data2\n\t\nif error<0.2 then\n\t\trightV=speed\n\t\tleftV=speed\n\t\tpreviousError=0\n\telseif data2[3]<0.75 or data2[4]<0.75 then\n\t\trightV=speed+PDspeed\n\t\tleftV=speed-PDspeed\n\telseif data2[1]<0.75 or data2[2]<0.75 then\n\t\trightV=speed-PDspeed\n\t\tleftV=speed+PDspeed\n\tend\n\tsim.setJointTargetVelocity(leftmotor,leftV)\n\tsim.setJointTargetVelocity(rightmotor,rightV)\nend\n该代码是一个PD算法的实现，用于控制机器人在地面上行走。下面是代码的分析：\n\n1. 首先定义了一个函数run，接受两个参数P和D，即比例项和微分项的系数。\n\n2. 初始化了一个空的表data2，PD和error变量。\n\n3. 使用一个循环从1到5，读取地面传感器的数据，并将第11个数据存储到data2表中。\n\n4. 如果data2[4]小于0.73，则将data2[3]设置为0。\n\n5. 如果data2[2]小于0.73，则将data2[1]设置为0。\n\n6. 如果data2[2]小于data2[4]，则进行以下判断：\n\t- 如果data2[3]小于data2[1]，则计算error的值为math.abs(data2[5]-0.095)+math.abs(data2[3]-0.85)+math.abs(data2[2]-0.85)。\n\t- 否则，计算error的值为math.abs(data2[5]-0.095)+math.abs(data2[1]-0.85)+math.abs(data2[2]-0.85)。\n\n7. 如果data2[2]不小于data2[4]，则进行以下判断：\n\t- 如果data2[3]小于data2[1]，则计算error的值为math.abs(data2[5]-0.095)+math.abs(data2[3]-0.85)+math.abs(data2[4]-0.85)。\n\t- 否则，计算error的值为math.abs(data2[5]-0.095)+math.abs(data2[1]-0.85)+math.abs(data2[4]-0.85)。\n\n8. 计算PD的值为P乘以error加上D乘以error和previousError的差。然后更新previousError为当前的error，更新previousdata为当前的data2。\n\n9. 如果error小于0.2，则将leftV和rightV都设置为speed，并将previousError设置为0。\n\n10. 如果data2[3]小于0.75或data2[4]小于0.75，则将leftV设置为speed加上PD乘以speed，rightV设置为speed减去PD乘以speed。\n\n11. 如果data2[1]小于0.75或data2[2]小于0.75，则将leftV设置为speed减去PD乘以speed，rightV设置为speed加上PD乘以speed。\n\n12. 使用sim.setJointTargetVelocity函数设置左右电机的目标速度为leftV和rightV。