PID控制基礎和應用例項

阿新 • • 發佈：2021-07-18

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描述)，原始碼給了非常詳細的註釋，這裡用虛擬碼給出其核心實現：

建立PID_Object，設定目標值setpoint，並初始化各個引數

在每次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$的值似乎越小越好。