Scribd
Upload
295 views

EE4308: Project 2-Autonomous Hector Navigation and Control: John Tan Victor Tay

The document describes the objectives and methodology for autonomous navigation and control of a UAV named Hector. The objectives are for Hector to hover above 5 meters and chase after an AGV while maintaining altitude. Hector must return to base and land after the AGV reaches its end goal. The methodology involves using hector_guider.py to determine state and command movement, hector_move.py to introduce a PID controller for position control, and an EKF for state estimation integrating GPS and IMU measurements. Key aspects of the EKF include prediction of state based on IMU readings and kinematics model, and correction of state using GPS and magnetic compass measurements.

Uploaded by

jzhong_7
Copyright
© © All Rights Reserved
Available Formats
Download as PPTX, PDF, TXT or read online on Scribd
295 views

EE4308: Project 2-Autonomous Hector Navigation and Control: John Tan Victor Tay

The document describes the objectives and methodology for autonomous navigation and control of a UAV named Hector. The objectives are for Hector to hover above 5 meters and chase after an AGV while maintaining altitude. Hector must return to base and land after the AGV reaches its end goal. The methodology involves using hector_guider.py to determine state and command movement, hector_move.py to introduce a PID controller for position control, and an EKF for state estimation integrating GPS and IMU measurements. Key aspects of the EKF include prediction of state based on IMU readings and kinematics model, and correction of state using GPS and magnetic compass measurements.

Uploaded by

jzhong_7
Copyright
© © All Rights Reserved
Available Formats
Download as PPTX, PDF, TXT or read online on Scribd
You are on page 1/ 18

EE4308: Project

2- Autonomous
Hector
Navigation and
Control
John Tan
Victor Tay
Hector Guider 01 02 Hector Move

Hector Flight
Requirements 03 04 Hector Motion
Modelling with EKF
Objective
● UAV (Hector) have to hover higher than 5 meters
● Hector have to chase after the AGV (Turtle)
● Cycle Hector movement between Turtle and End Goal while Turtle is moving to End
Goal
● Hector must return to Base and Land after Turtle together with Hector has reached End
Goal
Hector Guider

Overview of how
hector_guider.py
● Determine UAV State
● Command high-level
overall movement
Hector Move

Overview of how
hector_move.py
● Introduce PID controller
● Position Controller
● Better flight performance
Hector Move
err Routine
(Error Calculation)

err_x = cos(Current Yaw Angle)*(Target x - Current x) - sin(Current Yaw Angle)*(Target x - Current x)


err_y = cos(Current Yaw Angle)*(Target y - Current y) + sin(Current Yaw Angle)*(Target y - Current y)
err_z = Target z - Current z
err_o = (atan2(Target y - Current y, Target x - Current x) - Current Yaw Angle + PI) % TWOPI - PI
Hector Move
vel Routine
(PID Controller)
# Integrator Term
# Differentator Term
i_x = i_x + err_x
d_x = err_x - err_x_prev
i_y = i_y + err_y
d_y = err_y - err_y_prev
i_z = i_z + err_z
d_z = err_z - err_z_prev
i_o = i_o + err_o
d_o = err_o - err_o_prev

# PID using errors from world frame


vel_x = kp_x * err_x + ki_x * i_x + kd_x * d_x
vel_y = kp_y * err_y + ki_y * i_x + kd_y * d_y
vel_z = kp_z * err_z + ki_z * i_x + kd_z * d_z
vel_o = kp_o * err_o + ki_o * i_x + kd_o * d_o
Hector Move
Publish Message

# Convert back to UAV Frame


vel_x = cos(Current Yaw Angle)*vel_x + sin(Current Yaw Angle)*vel_x
vel_y = cos(Current Yaw Angle)*vel_y - sin(Current Yaw Angle)*vel_y

# -- Teleop (convert to robot frame before teleop) ---


cmd_lin.x = vel_x # convert to rbt frame
cmd_lin.y = vel_y # convert to rbt frame
cmd_lin.z = vel_z # z in rbt_frame and world_frame are parallel
cmd_ang.z = 0.6283 # angular
pub_cmd.publish(cmd_vel)
Hector Flight Requirements
● Maximum horizontal speed is ● Constant yaw rate of 36
capped at 2 meters per second degree per second
● Maximum vertical ascend speed ● Straight horizontal flight is
is 1 meter per second achievable during yaw
● Maximum vertical descend
speed is 0.5 meters per second
cmd_ang.z = 0.6283 # angular

● Take off to above 5m, and


maintain that altitude while
flying.
Hector Motion Modelling with EKF: Overview

Correction produces a
weighted average of the
Initialization mean and variance based
on the uncertainty of the
measurement and the
Measurement is made from prediction
GPS and Magnetic Compass

Kalman Gain: K
Mean: X_pred Prediction Correction Mean: X_cor
Variance: P_pred
Variance: P

Predict state (t) based on Time advances from t-1 to t:


IMU readings (t -1) and Corrected mean and variance
kinematics model. passed to next iteration
Hector Motion Modelling with EKF: INTUITION

Correction produces a weighted average of the mean and


variance based on the uncertainty of the measurement and
the prediction
Kalman Gain:
KK = ΣK|K-1 * HKT * Inv( HK * ΣK|K-1 * HKT + VK * RK * VKT)

Uncertainty
Uncertainty in
in Measuremen
Prediction Corrected Mean: t
XK|K = XK|K-1 + kK * ( YK - hd * XK|K-1 )

When there is higher uncertainty


in measurement: kK decreases.
Then a higher weight is given to
the prediction XK|K-1
Hector Motion Modelling with EKF: Converting
Measurements into useful Data

GPS to world coordinates (Equirectangular


Projection)
Function Equirectangular(rbt_gps):

𝜆0 = Initial longitude
Transforms spherical
φ0 = Initial latitude
𝜆 = rbt_gps[1] #current latitude coordinates into planar
φ = rbt_gps[0] #current longitude coordinates
x = RAD_EQUATOR * (𝜆 - 𝜆0) * cos(φ0)
y = RAD_EQUATOR * (φ - φ0)
z = rbt_gps[2] #current altitude

Return x,y,z

EndFunction
Hector Motion Modelling with EKF: Converting
Measurements into useful Data

IMU to world coordinates

Function IMUToWorld(rbt_imu, yaw): Rotation Matrix from IMU


to World
ax = (rbt_imu[0]) * cos(yaw) - (rbt_imu[0]) * sin(yaw)
ay = (rbt_imu[1]) * cos(yaw) + (rbt_imu[1]) * sin(yaw)
az = rbt_imu[2] - 9.81

yaw_rate = rbt_imu[3]

imu_world = [ax, ay, az, yaw_rate]

Return imu_world

EndFuncton
Hector Motion Modelling with EKF: Initialization

State variables X (2x1 matrix) Covariance Q (1x1 matrix) Covariance P (1x1 matrix)

#Initial values rx0, ry0, rz0 #IMU #Initial covariances are 0


and ro0 P_x = array([[0,0], [0,0]])
Q_x_imu = array([[0.1225]])
X_x = array([[rx0], [0.]]) Q_y_imu = array([[0.1225]]) P_y = array([[0,0], [0,0]])
X_y = array([[ry0], [0.]]) Q_z_imu = array([[0.09]]) P_z = array([[0,0], [0,0]])
X_z = array([[rz0], [0.]]) Q_o_imu = array([[0.0025]]) P_o = array([[0,0], [0,0]])
X_o = array([[ro0], [0.]])
#GPS
V = array([[1]]) Q_x_gps = array([[0.04]])
Q_y_gps = array([[0.04]])
Q_z_gps = array([[0.04]])
H = array([[1., 0.]])
#Magnetic compass
Q_compass = array([[0.0001]])
Hector Motion Modelling with EKF: Prediction
PREDICTION: X, Y, Z and O

WHILE ros and msg_guider are still running:


#Start of EKF Prediction step
dt = time elapsed since start of loop
W_k = array([ [0.5*dt^2], [dt] ]) #2x1 column vector
F_k = array([[1, dt], [0., 1]])
#Predicted Mean Predicted Mean:
X_x_pred = F * X_x + W * Q_x_imu
X_y_pred = F * X_y + W * Q_y_imu XK|K-1 = fd(XK-1|K-1, uK-1 , 𝜙)
X_z_pred = F * X_z + W * Q_z_imu
X_o_pred = F * X_o + W * Q_o_imu
#Predicted Covariance Predicted Covariance:
P_x_pred = F * P_x * transpose(F) + W * Q_x * transpose(W)
P_y_pred = F * P_y * transpose(F) + W * Q_y * transpose(W) ΣK|K-1 = FK * ΣK-1|K-1 * FKT + WK * ΣK-1|K-1 * WK-1
P_z_pred = F * P_z * transpose(F) + W * Q_z * transpose(W)
P_o_pred = F * P_o * transpose(F) + W * Q_z * transpose(W)
...
Hector Motion Modelling with EKF: Correction after
measurement
Correcting O
IF there is measurement from compass:
#Retrieve measurement R_o
R_o = gps_world[0]

#Obtain Kalman Gain K_o Kalman Gain:


K_o = P_o_pred * transpose(H) * (H * P_o_pred *
transpose(H) + V * R_o * transpose(V)) KK = ΣK|K-1 * HKT * Inv( HK * ΣK|K-1 * HKT + VK * RK * VKT)

#Obtain Corrected mean X_o Corrected Mean:


y_o = H*X_x + V
X_o = X_o_pred + K_o * (y_o - X_o_pred) XK|K = XK|K-1 + kK * ( YK - hd * XK|K-1 )

#Obtain Corrected Covariance cov_o Corrected Covariance:


P_o = P_o_pred - K_o * H * P_o_pred
ΣK|K = ΣK|K-1 - KK * hK * ΣK|K-1
ENDIF
Hector Motion Modelling with EKF: Correction after
measurement
Correcting X, Y and Z

IF there is measurement from GPS:


#Obtain measurements from gps
R_x = gps_world[0]
…
#Kalman gain Kalman Gain:
K_x = P_x_pred * transpose(H) * (H * P_x_pred *
KK = ΣK|K-1 * HKT * Inv( HK * ΣK|K-1 * HKT + VK * RK * VKT)
transpose(H) + V * R_x * transpose(V))
…
#Corrected mean Corrected Mean:
X_x = X_x_pred + K_x * (y_x - X_x_pred)
… XK|K = XK|K-1 + kK * ( YK - hd * XK|K-1 )

#Corrected covariance Corrected Covariance:


P_x = P_x_pred - K_x * H * P_x_pred
... ΣK|K = ΣK|K-1 - KK * hK * ΣK|K-1
* Y and Z omitted for brevity
ENDIF
Thank You

You might also like