[ROB_1] PID控制基础和应用实例

PID控制基础和应用实例

理论基础

E(error)

P(proportional)

I(integral)

D(derivative)

\[u(t) = K_pe(t) + Ki\int{e(t)dt} + K_d\frac{de(t)}{dt} \]

调节过程就是先调节\(K_p\)、再调节\(K_i\)，最后调节\(K_d\)

感觉这个动图很好直观(Wekipidia yyds!)

算法

找到一个不错的轮子PID算法(python描述)， 源码给了非常详细的注释，这里用伪代码给出其核心实现：

1. 创建PID_Object，设置目标值setpoint，并初始化各个参数
2. 在每次call这个实例时：
   last_input = 上一次call这个实例时的_input
   dt = 两次call这个实例的时间差
   error = setpoint - input
   P = K_P * error
   I += K_I * error * dt
   D = (input - last_input)/dt
   last_input = input
   return P + I + D (with limitation)

其中，dt可以在call时传入，或者默认通过python.time计算获得

这个轮子写的较为简单巧妙，但个人感觉存在一个明显的问题：D的部分仅取决于两次call前后的input值，这可能导致算法不够稳健，一个改进方案即记录input的历史，使得求导时结果更加稳定。

应用

在task3中自动控制小乌龟的force时直接借用了上述轮子，经历了长时间的调参，最后效果还是不错的，将核心实现部分总结如下：

# 设置基准值
from simple_pid import PID
position_history_x = []
position_history_y = []
curFoodInfo = None
pid_x = None
pid_y = None
velocity_limitation = 1
I = 3
D = 0.001
P = 8
dt = 0.1

# 核心方法getDirection
def getDirection(self):
    global curFoodInfo, pid_x, pid_y
    x = self.position[0]
    y = self.position[1]
    foodList = rospy.get_param("food")
    speed_limit_x = 0 if abs(self.velocity[0])<velocity_limitation else 100 if self.velocity[0]>0 else -100
    speed_limit_y = 0 if abs(self.velocity[1])<velocity_limitation else 100 if self.velocity[1]>0 else -100
    
    #  sort the foodList with key=distance(turtle_position, foodPosition) `SFF`
    for foodName, foodInfo in sorted(foodList.items(), key=lambda a: math.hypot(x-a[1]['x'], y-a[1]['y'])):
        if foodInfo['isEaten']:
            continue
        if curFoodInfo == foodInfo:
            break
        curFoodInfo = foodInfo
        pid_x = PID(P, D, I, setpoint=curFoodInfo['x'])
        pid_y = PID(P, D, I, setpoint=curFoodInfo['y'])
        break
    if not pid_x:  # no food, set target point in the middle of the table.
        pid_x = PID(P, D, I, setpoint=3)
        pid_y = PID(P, D, I, setpoint=3)

    force_x = pid_x(x, dt=dt)
    force_y = pid_y(y, dt=dt)
    return [force_x - speed_limit_x, force_y - speed_limit_y]

首先，将foodList按照与乌龟距离从小到大排序，从而保证最近食物优先的算法实现

其次，在找到第一个没有被吃掉的食物时，设定pid_x, pid_y这两个PID的实例，设置setpoint分别为食物的x坐标和y坐标。

通过分支控制， 保证在当前食物curFood未被吃掉之前，pid_x, pid_y保持不变，每次执行getDirection方法时，判断当前食物是否和循环后产生的目标食物一致，若一致，则直接返回PID控制接口的返回值；否则，即当前目标食物被吃掉后，转换新的目标，重新生成pid_x和pid_y实例。

算法中比较丑陋地加入了正方形井的速度限制，也没考虑增加"使得小乌龟尽量别靠近边界"的变量

调参过程相当耗时，好长时间乌龟都是一直围着食物打转，最后发现在此场景下，\(K_D\)的值似乎越小越好。