PIC Vietnam - Thắc mắc về điều khiển DC servo bằng giải thuật PID

Em đang tìm hiểu giải thuật PID điều khiển động cơ DC servo, em đã viết chương trình điều khiển, làm mạch thử nghiệm. Sau khi test thì em thấy có hiện tượng như sau:
Khi nhập vị trí mong muốn vào bộ điều khiển, động cơ phản hồi tương đối nhanh để quay tới vị trí mong muốn, gần tới vị trí mong muốn thì tốc độ giảm dần và dao động nhỏ trước khi đạt được vị trí mong muốn. Sau khi tới vị trí mong muốn một thời gian chừng 30s động cơ bắt đầu giao động nhỏ, tốc độ giao động tăng nhanh theo thời gian. Sau 1 phút thì động cơ giao động rung bần bật. Kì lạ hơn, nếu ngay sau khi động cơ đạt tới vị trí mong muốn mà dùng tay quay trục động cơ (đặt tải trọng nhỏ lên trục) thì ngay lập tức động cơ bắt đầu giao động.
Em thử tăng tần số PWM từ 1kHz lên 40 kHz thì thấy thời gian ổn định cửa động cơ ở vị trí mong muốn lâu hơn.
Em muốn bộ điều khiển làm việc ổn định nhưng không hiểu vì sao có hiện tượng này. Các thầy và các bạn giải thích giúp em hiện tượng này và nếu có thể thì nêu giúp em cả biện pháp khắc phục.

Thông số động cơ em sử dụng như sau:
công suất 60 W
điện áp 24VDC
Encoder 1000 pulses
IC công suất: L298N
Các thông số bộ PID em sử dụng như sau:
PWM freq: 40kHz
Thời gian lấy mẫu: 25 ms
Kp=1.55
Kd=1
Ki=0.00001
Tần số làm việc của vi điều khiển: 40 MHz

Thank you.