CHƢƠNG 4 KẾT HỢP GPS VÀ INS
4.2 Áp dụng bộ lọc Kalman tích hợp INS và GPS
Sử dụng bộ lọc Kalman trong việc tích hợp tín hiệu INS và GPS đƣợc thực hiện qua các bƣớc sau:
Bƣớc 1: Ƣớc tính vị trí hiện tại đựa trên phƣơng pháp Dead Reckoning.
̂ = ̂ + B ̇ [ ̇ ] = [ ] [ ̇ ] + [ ] ̇ = * ̇ ̇ + + [ ] ̇ = * ̇ ̇ ̇ +
Cập nhật thời gian (“Dự đoán”)
𝑥̂𝑘 𝐴𝑥̂𝑘 ⬚ 𝐵𝑢𝑘 ⬚ 𝑃𝑘 𝐴𝑃𝑘 ⬚ 𝐴𝑇 𝑄
(1) Dự đoán trạng thái
(2) Dự đoán hiệp phƣơng sai
Measurement Update (“Correct”)
𝐾𝑘⬚ 𝑃𝑘𝐻⬚𝑇(𝐻𝑃𝑘 𝐻⬚𝑇 𝑅)
𝑥̂𝑘⬚ 𝑥̂𝑘 𝐾𝑘⬚(𝑧𝑘 𝐻𝑥̂𝑘) 𝑃𝑘⬚ (𝐼 𝐾𝑘⬚𝐻)𝑃𝑘
(1) Tính Kalman gain (2) Cập nhật dự đóán với zk (3) Cập nhật hiệp phƣơng sai lỗi
43 = * ( ̇ ̇ ) ̇ + Thực thi: Vn = Vn – biasX; posX = lastPosX + Vn*dt Ve = Ve – biasY;
posY = lastPosY + Ve*dt
Bƣớc 2: Ƣớc tính ma trận hiệp phƣơng sai lỗi dựa vào ma trận hiệp phƣơng sai lỗi
trƣớc đó. =F + [ ] = [ ] [ ] [ ] +[ ̇ ] = [ ] [ ]+[ ̇ ] =[ ( ) ] + [ ̇ ] =[ ( ) ̇ ] Thực thi:
44
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1]; P[1][1] += Q_gyroBias * dt;
Bƣớc 3: tính tốn sai khác giữa tọa độ vừa ƣớc đoán và tọa độ do GPS trả về.
̃ = ̂ = [ ] [ ̇ ] = Thực thi: yX = newPosX – posX; yY = newPosX – posX;
Bƣớc 4: Tính tốn giá trị phƣơng sai mới.
=H + R =[ ] [ ] [ ]+ R = + R = + var(v) Thực thi: S = P[0][0] + R_pos; Bƣớc 5: Tính tốn hệ số Kalman.
45 = [ ] =[ ] [ ] =[ ] = [ ] Thực thi: K[0] = P[0][0] / S; K[1] = P[1][0] / S;
Bƣớc 6: Cập nhật trạng thái hậu nghiệm.
̂ = ̂ + ̃ [ ̇ ] = [ ̇ ] + [ ] ̃ =[ ̇ ] +[ ̃ ̃] Thực thi:
posX = posX + K[0] * yX; biasX = biasX + K[1] * yX; posY = posY + K[0] * yY; biasY = biasY + K[1] * yY;
46 =( I H) [ ] = ([ ] [ ] [ ]) [ ] =([ ] [ ] ) [ ] =[ ] [ ] Thực thi: P[0][0] -= K[0] * P[0][0]; P[0][1] -= K[0] * P[0][1]; P[1][0] -= K[1] * P[0][0]; P[1][1] -= K[1] * P[0][1];
47
4.3 Xây dựng thuật tốn chƣơng trình
Hình 4.2: Giải thuật chƣơng trình.
Theo đó, dữ liệu GPS sẽ đƣợc cập nhật với tần số 5Hz. Dữ liệu cảm biến đƣợc cập nhật với tần số 250Hz.
48
4.4. Kết quả mô phỏng dẫn đƣờng GPS và INS
Kết quả mô phỏng quỹ thực hiện quỹ đạo bay ở chƣơng 2 khi kết hợp INS và GPS đƣợc trình bày ở chƣơng 5.
Kết quả mơ phỏng khi mất tín hiệu GPS và khơng có tín hiệu INS hỗ trợ
49
50
CHƢƠNG 5 : BỘ ĐIỀU KHIỂN DẪN ĐƢỜNG 5.1 Tổng quan bộ điều khiển 5.1 Tổng quan bộ điều khiển
Đối với yêu cầu kịch bản tham gia PCCC của thiết bị bay đƣợc xây dựng trong chƣơng 2, bộ điều khiển của thiết bị bay phải điều khiển đƣợc các yếu tố góc nghiêng, hƣớng, độ cao, vị trí, vận tốc bay.
Hình 5.1: Thơng số điều khiển
Các dữ liệu về gia tốc, vận tốc góc, hƣớng, độ cao, vị trí do các cảm biến accelerometer, gyro, từ trƣờng, khí áp và GPS thu thập sẽ đƣợc chuyển đổi thành các dữ liệu về góc nghiêng, hƣớng, độ cao, vị trí và vận tốc di chuyển theo phƣơng ngang của thiết bị bay.
Bộ điều khiển vị trí thực chất là điều khiển góc nghiêng và hƣớng đồng thời để di chuyển thiết bị bay từ vị trí hiện tại tới vị trí mong muốn. Khi đã biết đƣợc tọa độ GPS hiện tại và tọa độ GPS mong muốn ta có thể tính tốn hƣớng và khoảng cách theo các cơng thức sau
Cơng thức tính khoảng cách giữa 2 điểm khi biết tọa độ GPS a = sin²(Δφ/2) + cos φ1 ⋅ cos φ2 ⋅ sin²(Δλ/2)
c = 2 ⋅ atan2( √a, √(1−a) ) d = R ⋅ c
51
Cơng thức tính hƣớng giữa 2 điểm khi biết tọa độ GPS
θ = atan2( sin Δλ ⋅ cos φ2 , cos φ1 ⋅ sin φ2 − sin φ1 ⋅ cos φ2 ⋅ cos Δλ ) Trong đó φ là vĩ đỗ ( đơn vị 0
), λ là kinh độ ( đơn vị 0
), R là bán kính trái đất (lấy bằng 6,371km);
Từ giá trị khoảng cách giữa hai điểm ta có thể tính tốn giá trị vận tốc của phƣơng tiện bay. Nhƣ vậy bài tốn điều khiển vị trí đƣợc quy về bài tốn điều khiển hƣớng và vận tốc. Đối với thiết bị bay kiểu Multicopter để thay đổi vận tốc trong quá trình di chuyển thực chất là thay đổi góc nghiêng . Khi góc nghiêng có giá trị tuyệt đối càng lớn thì vận tốc di chuyển càng cao, khi góc nghiêng có giá trị tuyệt đối càng nhỏ thì vận tốc di chuyển càng nhỏ. Nếu nhƣ lý tƣởng góc nghiêng bằng 0 thì thiết bị bay sẽ không di chuyển. Nhƣ vậy điều khiển vận tốc của thiết bị bay thực chất là điều khiển góc nghiêng của thiết bị bay
Bộ điều khiển đƣợc xây dựng để điều khiển thiết bị bay thuộc bộ điều khiển PID. PID là bộ điều khiển vi tích phân tỉ lệ (PID) với cơ chế phản hồi vòng điều khiển tổng quát đƣợc sử dụng rộng rãi trong các hệ thống điều khiển công nghiệp – bộ điều khiển PID đƣợc sử dụng phổ biến nhất trong số các bộ điều khiển phản hồi. Một bộ điều khiển PID tính tốn một giá trị “sai số” là hiệu số giữa giá trị đo thông số biến đổi và giá trị đặt mong muốn. Bộ điều khiển sẽ thực hiện giảm tối đa sai số bằng cách điều chỉnh giá trị điều khiển đầu vào. Trong trƣờng hợp khơng có kiến thức cơ bản về q trình, bộ điều khiển PID là bộ điều khiển tốt nhất. Tuy nhiên, để đạt đƣợc kết quả tốt nhất, các thơng số PID sử dụng trong tính tốn phải điều chỉnh theo tính chất của hệ thống-trong khi kiểu điều khiển là giống nhau, các thông số phải phụ thuộc vào đặc thù của hệ thống.
52
Hình 5.2: Các bộ điều khiển cho phƣơng tiện bay
Nhƣ vậy sau quá trình biến đổi các dữ liệu đầu vào, cần xây dựng 3 bộ điều khiển PID tƣơng ứng điều khiển các góc nghiêng pitch và roll ( điều khiển góc pitch và góc roll hồn tồn giống nhau), góc hƣớng yaw, độ cao
5.2 Bộ điều khiển PID một lớp
Bộ điều khiển PID cơ bản bao gồm 3 thơng số riêng biệt, do đó đơi khi nó còn đƣợc gọi là điều khiển ba khâu: các giá trị tỉ lệ, tích phân và đạo hàm, viết tắt là P, I, và D
Trạng thái khởi tạo
e(t) 𝐾𝑝 𝑒(𝑡) 𝐾𝑝 𝑒(𝑡)𝑑𝜏 𝑡 0 𝐾𝐷 𝑑 𝑑𝑡𝑒(𝑡) Thực thi
Tín hiệu hồi tiếp
53
Giá trị tỉ lệ xác định tác động của sai số hiện tại, giá trị tích phân xác định tác động của tổng các sai số quá khứ, và giá trị vi phân xác định tác động của tốc độ biến đổi sai số. Tổng chập của ba tác động này dùng để điều chỉnh q trình thơng qua một phần tử điều khiển nhƣ vị trí của van điều khiển hay bộ nguồn của phần tử gia nhiệt. Nhờ vậy, những giá trị này có thể làm sáng tỏ về quan hệ thời gian: P phụ thuộc vào sai số hiện tại, I phụ thuộc vào tích lũy các sai số quá khứ, và D dự đoán các sai số tƣơng lai, dựa vào tốc độ thay đổi hiện tại.
Nhƣ trong sơ đồ tổng quan bộ điều khiển PID thì ta thấy có một đầu vào là Setpoint đây chính là trạng thái mà ta mong muốn thiết bị bay của ta duy trì, nhƣ vậy đầu vào ở đây không đơn thuần là dữ liệu sau lọc nữa mà ta phải đƣa cả dữ liệu điều khiển vào ln tại vị trí đầu vào này để điều khiển phƣơng tiện bay. Với những đầu vào nhƣ trên thì đầu ra của bộ điều khiển sẽ đƣợc tính tốn theo cơng thức dƣới đây:
( ) ∫ ( ) + ( ) Trong đó e(t)=Setpoint – Input(t).
Với mỗi thông số cần điều khiển ta lại cần phải có một bộ điều khiển PID riêng biệt, trong nội dung của đề tài nhóm nghiên đã thực hiện triển khai bốn bộ điều khiển PID để điều khiển ba góc Euler (góc roll, góc pitch, góc yaw – hƣớng), độ cao và vị trí. Các dữ liệu lấy ra từ cảm biến sẽ đƣợc đi vào bộ điều khiển PID sau đó đầu ra của bộ PID sẽ đƣợc đi qua một công đoạn xử lý nữa trƣớc khi đƣa dữ liệu ra động cơ mà nhóm tạm gọi công đoạn xử lý đó là hàm truyền đạt của bộ điều khiển PID
54
Hình 5.4: Bộ điều khiển PID một lớp với thiết bị bay
Trong quá trình điều khiển, các góc Pitch, Roll, Yaw và độ cao ban đầu sẽ đƣợc tính tốn trƣớc dựa trên quỹ đạo bay.
Xác định các hệ số điều khiển
Để xác định đƣợc bộ hệ số phù hợp nhất, bộ điều khiển PID đƣợc mô phỏng trên phần mềm Matlab sau hiệu chỉnh lại trong thực tế.
Kết quả mô phỏng trên Matlab của bộ điều khiển PID 1 lớp với góc Roll có = 2.5, = 0.05, = 0.8
55
Hình 5.5 : Đồ thị ứng với mức ga thấp nhất ( mức ga đủ để thiết bị bay nhấc bắt đầu cất cánh)
Hình 5.6: Đồ thị ứng với mức ga trung bình
56
Sau khi tìm ra đƣợc bộ hệ số thích hợp, bộ điều khiển PID đƣợc thử nghiệm lại trên thực tế. Để kiểm tra đƣợc đáp ứng của bộ điều khiển đối với góc Roll, trục Pitch của mơ hình thiết bị bay đƣợc cố định, chỉ có trục Roll có thể chuyển động.
Hình 5.8: Cơ cấu kiểm tra PID đối với một trục
Điểm mong muốn (Setpoint) sẽ đƣợc đặt ở giá trị 0, sử dụng ngoại lực tác động đẩy thiết bị bay ra khỏi Setpoint nhiều lần với cƣờng độ và tốc độ khác nhau để kiểm tra đáp ứng của bộ điều khiển.
Kết quả triển khai bộ điều khiển PID 1 lớp với góc Roll trong thực tế
57
Throttle : Kênh ga của thiết bị bay
Aileron : Tƣơng ứng với Setpoint. Giá trị kênh Elevator bằng 1518 ứng với giá trị Setpoint = 0
Kết quả cho thấy mặc dù bộ điều khiển đã đáp ứng đƣợc khả năng giữ ổn định quanh setpoint nhƣng khi bị đẩy lệch khỏi setpoint thì khả năng kéo về của bộ điều khiển vẫn chậm. Đối với thiết bị bay cần nhanh và chính xác thì bộ điều khiển PID 1 lớp vẫn chƣa đáp ứng đủ yêu cầu. Bộ điều khiển PID 1 lớp chỉ phù hợp điều khiển độ cao và hƣớng, là những tham số khơng cần tới vận tốc thay đổi. Để có thể vừa giữ trạng thái ổn định tốt và vừa chuyển động với tốc độ cao trong quá trình tham gia cơng tác PCCC thì bộ điều khiển PID đơn thuần cho góc nghiêng chƣa đáp ứng đủ nhu cầu, bộ điều khiển PID 2 lớp đã đƣợc triển khai để giải quyết các vấn đề trên.
58
5.3 Bộ điều khiển PID hai lớp 5.3.1 Thuật toán điều khiển 5.3.1 Thuật toán điều khiển
Setpoint giới hạn +/-350 giới hạn +/-350 K-ps Góc đo đƣợc Sai số góc u(t) Sai số vận tốc góc e(t) K-pr K-ir K-dr giới hạn +/-2500/s QUAD Vận tốc góc đo đƣợc
Hình 5.10: Thuật tốn điều khiển PID hai lớp Đầu ra của bộ điều khiển PID đƣợc tính nhƣ sau Đầu ra của bộ điều khiển PID đƣợc tính nhƣ sau
( ) ∫ ( ) + ( ) e(t) = ( ) – Input(t/s).
u(t) = Setpoint – Input(t).
Trong đó Setpoint là góc mong muốn, Setpoint(v) là vận tốc góc mong muốn.
Khối PID (lớp 1) cho đầu ra nhƣ một tín hiệu điều khiển tốc độ góc tƣơng ứng của nó dựa vào sai số giữa góc điều khiển mong muốn (Roll, Pitch) và góc hiện tại của thiết bị bay. Khi sai số góc nghiêng thực tế và góc điều khiển mong muốn càng lớn thì đầu ra cho giá trị càng lớn, nó sẽ là tham chiếu điều khiển tốc độ góc tƣơng ứng. Tốc độ góc quay tƣơng ứng sẽ đƣợc tăng lên, kéo góc nghiêng về vị trí mong muốn nhanh hơn.
59
5.3.1 Mơ phỏng bộ điều khiển PID hai lớp a. Tổng quan khối hệ thống điều khiển a. Tổng quan khối hệ thống điều khiển
Hình 5.11: Chi tiết các thành phần trong khối điều khiển
Đầu vào khối điều khiển :
Tín hiệu điều khiển từ khối điều khiển (từ 1000÷2000).
Các giá trị trạng thái của multicopter trong q trình chạy mơ phỏng + Gia tốc dài gắn thân của multicopter: ax, ay, az.
+ Vận tốc góc gắn thân của multicopter p, q ,r
Các giá trị mơi trƣờng gồm nhận đƣợc trong q trình mơ phỏng động học: +Temp (nhiệt độ)
+ Press (áp suất)
Bảng 4: Tín hiệu đầu vào khối điều khiển
Đầu vào Mô tả Khoảng giá trị Đơn vị
Ch1 Điều khiển góc roll - Aileron 1000-2000 [ ]
Ch2 Điều khiên góc pitch - Elevator 1000-2000 [ ]
Ch3 Điều khiển góc yaw - Rudder 1000-2000 [ ]
Ch4 Điều khiển ga chung - Throttle 1000-2000 [ ]
60
Ch6 Không sử dụng 1000-2000 [ ]
p Tốc độ góc theo trục X -249÷249 Deg/s
q Tốc độ góc theo trục Y -249÷249 Deg/s
r Tốc độ góc theo trục Z -249÷249 Deg/s
ax Gia tốc dài theo trục X -19.62÷19.62 g(**)
ay Gia tốc dài theo trục Y -19.62÷19.62 g
az Gia tốc dài theo trục Z -19.62÷19.62 g
Press Áp suất khí quyển N/m^2
temp Nhiệt độ môi trƣờng Độ C
Đầu ra:
Xung PWM (thực chất là giá trị đại diện cho độ rộng của xung PWM) điều khiển động cơ (8 tín hiệu PWM với máy bay x8 hoặc 4 tín hiệu PWM với máy bay x4). Giá trị độ rộng xung PWM tƣơng ứng nằm trong khoảng từ 1000÷2000 (ứng với chu kì là 2000).
Bảng 5: Tín hiệu đầu ra motor
Đầu ra Mơ tả Khoảng giá trị Đơn vị
m1 Độ rộng xung PWM điều khiển motor1 1000-2000 [ ] m2 Độ rộng xung PWM điều khiển motor2 1000-2000 [ ] m3 Độ rộng xung PWM điều khiển motor3 1000-2000 [ ] m4 Độ rộng xung PWM điều khiển motor4 1000-2000 [ ] m5 Độ rộng xung PWM điều khiển motor5(nếu có) 1000-2000 [ ] m6 Độ rộng xung PWM điều khiển motor6(nếu có) 1000-2000 [ ] m7 Độ rộng xung PWM điều khiển motor7(nếu có) 1000-2000 [ ] m8 Độ rộng xung PWM điều khiển motor8(nếu có) 1000-2000 [ ]
61
b. Khối chuẩn hóa góc pitch và góc roll
Hình 5.12: Khối cảm biến tổng hợp và khối chuẩn hóa góc pitch và roll trong
Chức năng:
Khối xử lý cảm biến sử dụng bộ lọc Kalman để đƣa ra giá trị góc pitch và góc roll. Mơ phỏng q trình lọc dữ liệu từ cảm biến với các yếu tố sát với thực tế bao gồm nhiễu của cảm biến gia tốc góc(accelerometer noise, kí hiệu Q_angle), nhiễu cảm biến tốc độ góc (gyroscope noise, kí hiệu Q_gyroBias).
Đầu vào
Tốc độ góc p, q, r (trong hệ quy chiếu gắn thân) lấy từ cảm biến Gyro trong khối động học (Dynamic). Đơn vị là radian/s.
Gia tốc dài ax, ay, az (trong hệ quy chiếu gắn thân) lấy từ cảm biến gia tốc góc accelerometer trong khối động học (Dynamic) . Đơn vị là g (1g = 9.8m/s^2).
Bảng 6: Các tín hiệu đầu vào khối cảm biến
Đầu vào Mô tả Khoảng giá trị Đơn vị
p Tốc độ góc theo trục X -249÷249 Deg/s
q Tốc độ góc theo trục Y -249÷249 Deg/s
62
ax Gia tốc dài theo trục X -19.62÷19.62 g(**)
ay Gia tốc dài theo trục Y -19.62÷19.62 g
az Gia tốc dài theo trục Z -19.62÷19.62 g
Đầu ra
Góc Roll, Pitch, Yaw (Các góc Euler, đơn vị là Deg). Bảng 7: Các tín hiệu đầu ra khối cảm biến
Đầu ra Mô tả Khoảng giá trị Đơn vị
Roll_angle Góc Roll (1 trong 3 góc Euler) -90÷90 Độ Pitch_angle Góc Pitch (1 trong 3 góc Euler) -90÷90 Độ Yaw_angle Góc Yaw (1 trong 3 góc Euler) -180÷180 Độ
c. Khối chuẩn hóa tốc độ góc p, q, r
Hình 5.13: Khối chuẩn hóa tốc độ góc p, q, r
Chức năng:
Chuẩn hóa tín hiệu tốc độ góc về khoảng giá trị -1÷1. Do tốc độ góc nhận đƣợc từ cảm biến nên giá trị tốc độ góc bị giới hạn bởi khả năng của cảm biến (Gyro). Trong mơ hình sử dụng, giới hạn tốc độ góc đo đƣợc của cảm biến là -
63
249÷249 độ/s (xem thơng số trong khối IMU thuộc khối Dynamic) nên khối này có tác dụng chia tốc độ góc cho 250 (xấp xỉ 249) để chuẩn hóa.
Đầu vào:
Tốc độ góc p q r (với hệ quy chiếu gắn thân). Đơn vị là Deg/s.
p q r là tốc độ góc đo đƣợc từ khối mô phỏng IMU trong khối Dynamic model.
Bảng 8: Các tín hiệu đầu vào khối chuẩn hóa tốc độ góc
Đầu vào Mơ tả Khoảng giá trị Đơn vị