[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\)的值似乎越小越好。