3.2.2.1 Quá trình ước lượng
Bộ lọc Kalman đề cập đến bài toán tổng quát nhằm ước lượng trạng thái của một quá trình được mô hình hóa một cách rời rạc theo thời gian bằng một phương trình ngẫu nhiên tuyến tính như sau:
(3.1)
Với giá trị đo đạc là :
Trong đó wk và vk là hai vector biến ngẫu nhiên đại diện cho nhiễu hệ thống và nhiễu đo đạc. Hai biến ngẫu nhiên này độc lập với nhau và được giả sử là tuân theo phân bố Gaussian với giá trị trung bình bằng 0 và ma trận hiệp biến (covariance matrix) lần lượt là Q và R.
p(w) ~ N(0, Q) (3.3)
p(v) ~ N(0, R) (3.4)
Trong thực tế, ma trận Q và R có thể thay đổi theo thời gian hoặc theo phép đo, tuy nhiên các ma trận này được giả định là hằng số.
Ma trận có kích thước là ma trận liên kết của trạng thái trước, bước thứ – và trạng thái hiện tại bước thứ . Ma trận có kích thước là ma trận liên kết giữa ngõ vào tùy biến với trạng thái . Ma trận có kích thước , trong phương trình phép đo (3.2) liên kết trạng thái với kết quả đo đạc . Chú ý rằng các ma trận Q, R, A, H có thể thay đổi theo thời gian (từng bước ), nhưng ở đây chúng được giả sử là không đổi [5].
3.2.2.2 Giải thuật lọc Kalman rời rạc
Bộ lọc Kalman ước lượng tiến trình bằng cách sử dụng hình thức kiểm soát phản hồi: bộ lọc ước lượng trạng thái của tiến trình tại vài thời điểm và sau đó thu nhận sự phản hồi bằng các giá trị đo thực tế (có nhiễu). Như vậy, phương trình của bộ lọc Kalman chia thành hai nhóm: phương trình cập nhật thời gian và phương trình cập nhật giá trị đo. Phương trình cập nhật thời gian chịu trách nhiệm dự đoán trước (theo thời gian) sử dụng giá trị hiện tại và hiệp biến số ước lượng để dự đoán ước lượng tiền nghiệm cho thời điểm kế tiếp. Phương trình cập nhật giá trị đo chịu trách nhiệm đối với sự phản hồi nghĩa là kết hợp giá trị mới với ước lượng tiền nghiệm nhằm hiệu chỉnh ước lượng hậu nghiệm.
Phương trình cập nhật thời gian có thể được xem như là phương trình dự đoán, trong khi phương trình cập nhật phép đo có thể xem như phương trình hiệu chỉnh. Thật vậy, thuật toán ước lượng cuối cùng rất giống với thuật toán dự đoán, hiệu chỉnh nhằm giải quyết các vấn đề số như trong Hình 3.2 [5].
Hình 3.2 Quy trình của bộ lọc Kalman
Các công thức tương ứng với quá trình cập nhật thời gian được cho trong công thức (3.5) và (3.6). Công thức tương ứng với quá trình cập nhật phép đo được cho trong công thức (3.7), (3.8) và (3.9). ̂ ̂ (3.5) (3.6) (3.7) ̂ ̂ ̂ (3.8) (3.9)
Việc đầu tiên trong quá trình cập nhật giá trị đo là tính giá trị độ lợi Kalman, . Bước kế tiếp là đo giá trị thực của quá trình và sau đó tìm ra giá trị hậu nghiệm ước lượng bằng cách kết hợp với kết quả đo như trong công thức (3.8). Bước cuối cùng là tìm hiệp biến số hậu nghiệm bằng công thức (3.9).
Sau mỗi bước cập nhật thời gian và cập nhật phép đo, quá trình được lặp lại với ước lượng hậu nghiệm trước đó để dự đoán một ước lượng tiền nghiệm mới. Quá trình lặp lại tự nhiên này là một đặc điểm khá thu hút của bộ lọc Kalman làm cho việc xây dựng bộ lọc Kalman dễ thực hiện hơn so với bộ lọc Wiener vốn được thiết kế để hoạt động trực tiếp trên tất cả các dữ liệu ước lượng. Thay vì vậy, bộ lọc Kalman tham chiếu các giá trị ước lượng hiện tại với tất cả các phép đo trước đó. Hình 3.3 cho thấy toàn bộ hoạt động của bộ lọc [6].
Hình 3.3 Quy trình hoàn chỉnh của bộ lọc Kalman
3.3 Xây dựng bộ lọc Kalman áp dụng cho cảm biến IMU
Cảm biến IMU gồm hai cảm biến tích hợp là cảm biến gia tốc và cảm biến con quay hồi chuyển. Bộ lọc Kalman được sử dụng để kết hợp các giá trị từ cảm biến gia tốc và cảm biến con quay hồi chuyển để cho ra các góc điều khiển chính là: x, y, và z.
Bộ lọc Kalman được xây dựng theo các bước như sau: Đầu tiên xây dựng mảng của trạng thái trước đó:
̂ (3.10)
Đó là trạng thái trước đó ước tính dựa trên trạng thái trước đó và dự toán của các trạng thái trước khi xảy ra.
Tiếp theo là một trạng thái tiên nghiệm:
̂ (3.11)
Một tiên nghiệm có nghĩa là ước tính của ma trận trạng thái tại thời điểm hiện tại k dựa trên trạng thái trước đó của hệ thống và dự toán của các trạng thái trước khi xảy ra.
̂ (3.12)
̂ ước tính của trạng thái tại thời điểm k và bao gồm cả lúc k ở hiện tại. Vấn đề trạng thái hệ thống chính bị ẩn nên chỉ có thể được quan sát thông qua quan sát. Điều này cũng được gọi là một mô hình Hidden Markov. Điều này có nghĩa rằng trạng thái sẽ được dựa trên trạng thái tại thời điểm k và tất cả các trạng thái trước đó .
Chiếc mũ trên các hệ số dùng để ước tính các trạng thái. Vì vậy trạng thái được ký hiệu:
Trạng thái của hệ thống tại thời điểm k được cho bởi:
(3.13)
Mà là ma trận trạng thái đó được cho bởi :
̂ [ ̇ ] (3.14)
Có thể thấy đầu ra của bộ lọc sẽ là góc mà trạng thái dựa trên các phép đo từ gia tốc và con quay hồi chuyển. Trạng thái là là các con quay hồi chuyển đã trôi dạt . Điều này có nghĩa rằng có thể tính toán bằng cách trừ trạng thái từ các phép đo con quay hồi chuyển.
Tiếp theo là ma trận , đó là mô hình chuyển trạng thái được áp dụng cho tình trạng trước đó.
Trong trường hợp này được định nghĩa là:
A = [
] (3.15)
Tiếp theo là kiểm soát đầu vào , trong trường hợp này là đo con quay hồi chuyển trong độ mỗi giây (° / s ) tại thời điểm k , điều này cũng được gọi là tốc độ. Viết lại phương trình trạng thái như sau:
̇ (3.16)
Tiếp theo là ma trận B. Được gọi là mô hình kiểm soát đầu vào, được định nghĩa là:
là nhiễu quá trình đó là phân phối Gaussian với hệ số trung bình bằng không với hiệp phương sai đến thời điểm k.
(3.18)
là quá trình nhiễu phương sai ma trận và trong trường hợp này ma trận hiệp phương sai dự toán trạng thái của gia tốc.
Ma trận cuối cùng được xác định như sau: [
̇] (3.19)
Có thể thấy ma trận hiệp biến phụ thuộc vào thời gian hiện tại k, do sự thay đổi của gia tốc.
Giá trị đo lường :
Sự đo lường của trạng thái thực sự. Quan sát được cho bởi:
(3.20)
H được gọi là ma trận quan sát và được sử dụng để lập bản đồ không gian trạng thái thực sự vào không gian quan sát được. Khi đo là chỉ số đo từ các gia tốc, H được cho bởi:
[ ] (3.21)
Tiếng ồn của phép đo phân bố Gaussian cũng như với trung bình bằng không và là phương sai :
(3.22)
Nhưng đây không phải là một ma trận nhiễu đo lường và chỉ bằng phương sai của đo lường, kể từ khi hiệp biến của cùng một biến bằng phương sai.
[ ] (3.23)
Nhiễu trong đo lường là như nhau và không phụ thuộc vào thời gian k:
Phương trình bộ lọc Kalman:
Dự đoán:
Hai phương trình đầu tiên để dự đoán trạng thái hiện tại và ma trận hiệp phương sai lỗi tại thời gian k. Đầu tiên các bộ lọc sẽ cố ước tính trạng thái hiện tại dựa trên tất cả các trạng thái trước và dùng đo con quay hồi chuyển :
̇ (3.25)
Đó cũng là lý do tại sao được gọi là kiểm soát đầu vào, vì là một đầu vào thêm để ước tính trạng thái tại thời điểm hiện tại k đựa trên trạng thái trước đó .
Tiếp theo là phương trình ước tính tiên hiệp phương sai lỗi ma trận dựa vào ma trận hiệp phương sai lỗi trước đó:
(3.26)
Ma trận này được sử dụng để ước tính bao nhiêu giá trị thay đổi. Phương sai lỗi sẽ tăng khi giá trị cập nhật lần cuối dự toán của trạng thái.
Ma trận hiệp phương sai lỗi trong trường hợp này là một ma trận 2 × 2: [
] (3.27)
Cập nhật giá trị mới:
Tính toán giá trị và tiên nghiệm :
̃ ̂ (3.28)
Ma trận H được sử dụng để lập bản đồ trạng thái vào không gian quan sát là đo lường từ các gia tốc.
̃ [̃] (3.29)
Tiếp theo là tính toán phương sai:
( ) (3.30)
sẽ để dự đoán, đo lường dựa trên một tiên ma trận hiệp biến lỗi và ma trận hiệp biến đo lường. Mô hình quan sát H được sử dụng để lập bản đồ tiên ma trận hiệp phương sai lỗi vào mô hình quan sát được.
trong trường hợp này không phải là một ma trận và chỉ được viết như sau:
Bước tiếp theo là tính toán để tăng Kalman.
( ) (3.32)
Lưu ý rằng nếu không biết trạng thái khi khởi động, có thể thiết lập các ma trận hiệp phương sai lỗi như sau:
[ ] (3.33)
Để tăng độ lợi Kalman là một ma trận 2 x 1:
[ ] (3.34)
Bây giờ có thể cập nhật một hậu ước tính của trạng thái hiện tại:
̂ ̂ ̃ (3.35)
Cuối cùng là cập nhật các ma trận hiệp phương sai lỗi hậu: Nơi được gọi là ma trận đơn vị và được xác định là:
(3.36)
I được gọi là ma trận đơn vị và được xác định là :
I= [ ] (3.37)
Thực hiện biến đổi các phương trình của bộ lọc: Cập nhật thời gian (dự đoán)
Từ phương trình (3.25) suy ra:
̇ [ ̇ ] = [ ] [ ̇ ] + [ ] ̇k [ ̇ ̇ ] + [ ] ̇k = [ ̇ ̇ ̇ ] = [ ̇ ̇ ̇ ]
Vậy góc ước đoán tại bằng góc ước đoán trước đó + .
[ ] = [ ] [ ] [ ] [ ̇] [ ] [ ] [ ̇ ] [ ] [ ̇] [ ̇ ] [ ̇ ] Cập nhật giá trị đo (hiệu chỉnh)
Biến đổi phương trình (3.28) được:
̃ ̂ = [ ] [ ] [ ] Từ phương trình (3.30): ( ) = [ ] [ ] [ ]
Biến đổi phương trình (3.32):
( ) [ ] [ ] [ ] [ ]
[
]
Khai triển phương trình (3.35) như sau: ̂ ̂ ̃ [ ̇ ] [ ̇ ] [ ] ̃ [ ̇ ] [ ̃ ̃]
Cuối cùng là khai triển phương trình ma trận hiệp phương sai lỗi hậu (3.36): [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ] [ ]
3.4 Giải thuật điều khiển PID
PID là viết tắt của cụm từ (Proportional Integral Derivative) là giải thuật được dùng nhiều trong các ứng dụng điều khiển tự động với yêu cầu chính xác, nhanh và ổn định. Bộ điều khiển PID bao gồm 3 thông số riêng biệt và được gọi là điều khiển ba khâu: tỷ lệ, tích phân và đạo hàm, viết tắt là: P, I và D.
Hàm truyền của bộ điều khiển PID như sau:
(3.38)
Trong đó:
: Độ lợi khâu tỷ lệ
: Độ lợi khâu tích phân : Độ lợi khâu vi phân
Cấu trúc bộ điều khiển PID
Hình 3.4 Hệ thống điều khiển vòng kín
Trong đó:
Controller: Bộ điều khiển cung cấp tín hiệu điều khiển cho đối tượng điều khiển.
Plant: Đối tượng điều khiển.
Biến e là thành phần sai lệch giữa giá trị mong muốn (R) và giá trị ngõ ra thực tế (Y). Tín hiệu sai lệch (e) sẽ gửi tới bộ điều khiển PID, và bộ điều khiển tính toán cả thành phần tích phân lẫn vi phân của (e). Tín hiệu ra (u) của bộ điều khiển bằng:
∫ (3.39)
Lúc này đối tượng điều khiển có tín hiệu vào là (u) và tín hiệu ra là (Y). (Y) được hồi tiếp về bằng các cảm biến để tiếp tục tính sai lệch (e). Sau đó bộ điều khiển tiếp tục lặp lại như trên.
Đặc tính của bộ điều khiển PID
Thành phần tỉ lệ (KP) có tác dụng làm tăng tốc độ đáp ứng của hệ và làm giảm, chứ không triệt tiêu sai số xác lập của hệ (steady-state error).
Thành phần tích phân (KI) có tác dụng triệt tiêu sai số xác lập nhưng có thể làm giảm tốc độ đáp ứng của hệ.
Thành phần vi phân (KD) làm tăng độ ổn định hệ thống, giảm độ vọt lố và cải thiện tốc độ đáp ứng của hệ.
Ảnh hưởng của các thành phần KP, KI, KD đối với hệ kín được tóm tắt trong bảng sau:
Bảng 3.1 Ảnh hưởng của các thành phần KP, KI, KD đối với hệ kín Thông số Thời gian đáp ứng Độ vọt lố Thời gian xác lập Sai số xác lập
KP Giảm Tăng Ít thay đổi Giảm
KI Giảm Tăng Tăng Triệt tiêu
KD Ít thay đổi Giảm Giảm Ít thay đổi
3.5 Xây dựng giải thuật PID điều khiển robot
Để tìm hệ thức PID rời rạc, xét đồ thị sau:
Hình 3.5 Đồ thị biểu diễn hệ số PID theo thời gian
Đường biểu diễn điểm mong muốn. Đường biểu diễn điểm thực tế.
Đường là đồ thị rời rạc hóa của vận tốc động cơ. ∆t là thời gian lấy mẫu.
Thành phần tích phân ∫ ∑ (3.40) Thành phần vi phân [ ] (3.41)
Do đó khi lấy gần đúng ∆t = є >0 thì: [ ] (3.42) Tóm lại: [ ( ) ] ∑ (3.43) Đặt: (3.44) ∑ (3.45)
Trong công thức (3.44) trên thời gian lấy mẫu ∆t rất nhỏ, nên bỏ qua ∆t. Khi tìm các hệ số Kp, Ki, Kd đã bao gồm cả ∆t.
Khi đó công thức lại được viết lại như sau:
(3.46)
Điều khiện biên: u(0) = duty > 0.
Tìm hệ số PID bằng phương pháp thực nghiệm:
Cho các hệ số Kp, Ki, Kd bằng không.
Tăng Kp từ nhỏ đến lớn cho đến khi ngõ ra đáp ứng với sự rung động là dao động ổn định.
Giảm Kp xuống một nữa giá trị phản ứng ở trên.
Tăng Ki từ nhỏ đến lớn, quan sát đáp ứng của hệ thống khi thông số thay đổi. Tăng cho đến khi dao động ở mức cho phép.
Cuối cùng là tăng Kd cho đến khi hệ thống đạt tối ưu. Tìm hệ số PID bằng phương pháp Ziegler-Nichols
Độ lợi KI và KD lúc đầu được gán bằng 0 và độ lợi P được tăng cho đến khi P tiến tới độ lợi tới hạn Ku ở đầu ra của vòng điều khiển bắt đầu dao động. Ku và thời gian dao động Pu được dùng để gán độ lợi như sau:
Bảng 3.2 Phương pháp Ziegler–Nichols Dạng điều khiển KP KI KD P 0.50Ku PI 0.45Ku 1.2 KP/Pu PID 0.60Ku 2 KP/Pu KP.Pu/8
3.6 Giải thuật điều khiển robot cân bằng bám theo vị trí ban đầu
(b) Robot tại vị trí ban đầu
(c) Robot bị trôi qua bên phải (a) Robot bị trôi qua
bên trái
Hình 3.6 Robot cân bằng bám theo vị trí ban đầu
Dựa vào giá trị góc nghiêng thu được từ cảm biến qua bộ lọc Kalman giải thuật PID sẽ giữ cho robot cân bằng nhưng do lực hấp dẫn của trái đất hay lực tác động từ bên ngoài làm cho robot bị ngã về một hướng nào đó. Để giữ cân bằng thì robot sẽ di chuyển ra khỏi vị trí ban đầu. Để có thể điều khiển robot cân bằng dao động quanh vị trí ban đầu thì giá trị sai lệch về vị trí hiện tại so với vị trí ban đầu được tính toán.
Bắt đầu
Lấy dữ liệu góc nghiêng trên robot từ cảm biến
Tính toán sai số giữa góc nghiêng khi robot cân bằng và góc nghiêng hiện tại trên robot
Điều khiển chiều quay và tốc độ động cơ để robot cân
bằng Điều khiển robot di
chuyển về vị trí ban đầu
Tính toán sai lệch vị trí hiện tại so với vị trí ban đầu
Sai
Đúng Giá trị sai lệch
vị trí = 0 ?
Chương 4
KẾT QUẢ THỰC NGHIỆM
4.1 Độ chính xác của cảm biến
Để có thể điều khiển robot giữ cân bằng thì thông tin về góc nghiêng của robot đóng vai trò quan trong nhất. Giá trị góc nghiêng được cung cấp bởi cảm biến IMU tích hợp gồm có cảm biến gia tốc và cảm biến con quay hồi chuyển. Hai cảm biến này là dạng cảm biến vi cơ điện tử nên rất nhạy với rung động. Giá trị góc thu trực tiếp từ cảm biến có dao động rất lớn và phải được xử lý lọc nhiễu trước khi đưa vào giải thuật cân bằng.
Hình 4.1 trình bày kết quả so sánh giá trị góc nghiêng theo trục y thu trực tiếp từ cảm biến IMU (đường màu xanh lá), giá trị góc nghiêng theo trục y của bộ lọc