Upload
vuthien
View
219
Download
5
Embed Size (px)
Citation preview
AR Drone Setup with ROS and Sensor Data Fusion using AR Drone’s
Accelerometer and Gyroscope
Welcome
Lab 5
Dr. Ahmad Kamal Nasir
25.02.2015 Dr. Ahmad Kamal Nasir 1
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Today’s Objectives
• Introduction to AR-Drone – Hardware
– Communication
• AR-Drone Interface with ROS– ROS driver nodes
– Teleop Keyboard/Joystick
• ROS with Quadrotor Gazebo Model– Orientation estimation
– Setting up pose estimation node based on EKF
25.02.2015 Dr. Ahmad Kamal Nasir 2
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Quad-rotor
25.02.2015 Dr. Ahmad Kamal Nasir 3
• Quad-rotors were introduced 14 years before helicopters but due to control problems were not able to make the way.
• French company parrot SA Introduced $300 device at International Consumer Electric Show in Las Vegas 2010-12
• Expanded poly propylene body, 380grams(outdoor), 420grams(indoor)
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Flight Basics
• Unstable system, therefore, require feedback for stability
25.02.2015 Dr. Ahmad Kamal Nasir 4
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Flight Control
25.02.2015 Dr. Ahmad Kamal Nasir 5
Yaw (Rotate)CW/CCW
ThrottleUp/Down
RollLeft/Right
PitchForward/Backward
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
AR-Drone Hardware
• CPU (ARM Cortex A8,OMAP 3630) @ 1GHz, GPU (PowerVR SGX530) @ 800 MHz,1GB RAM, 128MB ROM
• 2 Webcams• 1 WiFi• 1 Ultrasonic• 1 Barometer• 1 9DOF IMU• 1 USB Port (GPS and LTE Modem)• 4 brushless motors @ 28500 RPM, 14.5W,
1:8.75 Gear Ratio, with control board (ATMEGA8L)
• Up to 5m/sec, 13 mins of continuous flight• 1000mAh, 11.1V LiPo batteries (Discharge
capacity 15C, 80grams) voltage decreases from full charge (12.5 Volts) to low charge (9 Volts)
25.02.2015 Dr. Ahmad Kamal Nasir 6
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Onboard Processing Power
• Busy Box based GNU/Linux distribution with 2.6.27 Kernel• It is possible to cross-compile an application for the ARM processor and run it
directly on the AR-Drone control board. • In this case, one can access the drone cameras and onboard sensors directly
without a delay caused by the wireless data transfer. Thus, one can achieve faster control loops and experiment with a low level control of the drone.
• The AR-Drone runs Linux on-board. The AR-Drone is running a telnet as well as an FTP daemon . The Telnet daemon will allow login as root (no password e.g telnet 192.168.1.1).
• The cameras are exposed as standard video4linux2 devices (/dev/video0 and /dev/video1)
• The navigation board, which handles accelerometer, gyrometer, and sonar sensors, is exposed as a serial port (/dev/ttyPA2) according /dev/ttyPA0 for USB serial port and /dev/PA1 for motor controllers
• DroneGames, which took place over the weekend in San Francisco, tasked programmers with hacking the UAVs in the most interesting and creative ways possible. { echo "reboot"; sleep 1 } | telnet 192.168.1.1
25.02.2015 Dr. Ahmad Kamal Nasir 7
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
• Vertical camera 63fov @60fps, 240p for horizontal speed measurements. Front cam 93fov@30fps,720p.
• Ultrasound sensors has maximum range of 6m. Barometric sensor (±10pa)for higher altitudes. It determines the vertical displacement of the vehicle. Vertical scene depth in the image.
• The received video can be from either of the two cameras or a picture-in-picture video with one of the camera images superposed on the top left corner of the other one.
AR-Drone Vision System
25.02.2015 Dr. Ahmad Kamal Nasir 8
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
AR-Drone Vision System (Cont.)
• AR-Drone can run a simple analysis of the images from the frontal camera and search for a specially designed tags in the images.
• In the case the tags are detected, the navdatacontains estimates of their positions.
25.02.2015 Dr. Ahmad Kamal Nasir 9
TAG
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
On-board Velocity Sensor - Vision
• To achieve a stable hovering and position control, the AR-Drone estimates its horizontal velocity using its vertical camera.
• Two different algorithms are used to estimate the horizontal velocity. – One tracks local interest points (FAST corners) over different frames
and calculates the velocity from the displacement of these points. It provides a more accurate estimate of the velocity and is used when the vehicle’s speed is low and there is enough texture in the picture.
– The second algorithm estimates the horizontal speed by computing the optical flow on pyramidal images. It is the default algorithm during flight. It is less precise but more robust since it does not rely on highly textured or high-contrast scenes.
• The AR-Drone uses inertial information from its IMU for estimating the state of the vehicle. It fuses the IMU data with information from the vision algorithms and an aerodynamics model to estimate the velocity of the vehicle
25.02.2015 Dr. Ahmad Kamal Nasir 10
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
AR-Drone Navigation System
• Navigation board contains 3axis accelerometer(±50mg), 2axis gyro(2000deg/sec), precise yaw gyro(XB-3500CV, drift 12deg/min dynamic, 4deg/min static). Data is processed at 200Hz.3 Axis magnetometer(±6deg).
• The navigation board uses a 16bits dsPIC24h micro-controller running at 40 MHz, and serves as an interface with the sensors. These sensors are a 3-axis accelerometers, a 2-axis gyroscope, a 1-axis vertical gyroscope, and a ultrasonic Sensors (200Hz). The PIC micro-controller handles the ultrasonic transmitter, 25Hz
25.02.2015 Dr. Ahmad Kamal Nasir 11
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
On-board Control Algorithm
25.02.2015 Dr. Ahmad Kamal Nasir 12
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Communication Ports• WIFI network with ESSID:ardrone_xxx. 192.168.1.1. clients request IP from DHCP server.
• AR-Drone sends two types of streams
– Controlling and configuring the drone is done by sending AT commands on UDP port 5556.
– navdata, are sent by the drone to its client on UDP port 5554. 15HZ in demo mode and 200Hz in full(debug)
• A video stream is sent by the AR-Drone to the client device on port 5555
• A fourth communication channel, called control port, can be established on TCP port 5559 to transfer critical data
25.02.2015 Dr. Ahmad Kamal Nasir 13
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Communication Protocol
25.02.2015 Dr. Ahmad Kamal Nasir 14
• AT commands are text strings sent to the drone to control its actions.
• AT*PCMD=<sequence>,<enable>,<pitch>,<roll>,<gaz>,<yaw>
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Android GUI
25.02.2015 Dr. Ahmad Kamal Nasir 15
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
AR-Drone with ROS
• Install ardrone_autonomypackages found at– sudo apt-get install ros-indigo-
ardrone_autonomy
• Use the following command to launch the quadrotor ROS driver, make sure wireless connection between AR-Drone and Computer is already established– rosrun ardrone_autonomy
ardrone_driver_realtime_navdata:=False _navdata_demo:=0
25.02.2015 Dr. Ahmad Kamal Nasir 16
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
/navdata ROS Topic• Use ardrone/navdata topic to acquire sensor information
such as orientation, linear and angular velocity
25.02.2015 Dr. Ahmad Kamal Nasir 17
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Plot real-time data
• Use ardrone/imu topic to acquire raw IMU sensor information, use following command to view a live plot– rqt_plot /imu/linear_acceleration/x:y:z
25.02.2015 Dr. Ahmad Kamal Nasir 18
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
AR-Drone Camera
• To view the live camera stream rosrun img_view img_view/img_view:=/ardrone/front/image_raw
25.02.2015 Dr. Ahmad Kamal Nasir 19
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
AR-Drone teleop
• AR-Drone can be controlled either by using a joystick or by a keyboard. In both cases geometry_msgs/Twist message must be published to cmd_vel topic.
– -linear.x: move backward, +linear.x: move forward
– -linear.y: move right, +linear.y: move left
– -linear.z: move down, +linear.z: move up
– -angular.z: turn left, +angular.z: turn right
• Value range: -1.0 to +1.0
25.02.2015 Dr. Ahmad Kamal Nasir 20
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
AR-Drone teleop (Cont.)
• rostopic pub -1 std_msgs/Empty /ardrone/takeoff
• rostopic pub -1 std_msgs/Empty /ardrone/land
• rostopic pub -1 std_msgs/Empty /ardrone/reset
• Download (LMS) and run following node for controlling quadrotor using keyboard– roslaunch ardrone_tutorials
keyboard_controller.launch
• To navigate the AR-Drone using joypad– roslaunch ardrone_tutorials
joystick_controller.launch
25.02.2015 Dr. Ahmad Kamal Nasir 21
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Angles from Gyro-Rate/Accelerometer Sensors
• Gyro-rate sensors: Angles from body rate
–
𝜙𝑡
𝜃𝑡𝜓𝑡
=
𝜙𝑡−1 + 𝜙𝑡 ⋅ Δ𝑡
𝜃𝑡−1 + 𝜃𝑡 ⋅ Δ𝑡
𝜓𝑡−1 + 𝜓𝑡 ⋅ Δ𝑡
• Accelerometer sensors: Angles from gravity vector𝐴𝑏 = 𝐶𝑖
𝑏 𝜃, 𝜙, 𝜓 ∙ 𝐴𝑖 = 𝑅𝑥 𝜙 ∙ 𝑅𝑦 𝜃 ∙ 𝑅𝑧 𝜓 ∙ 𝐴𝑖𝑎𝑥 cos 𝜃 + 𝑎𝑦 sin 𝜙 sin 𝜃 + 𝑎𝑧cos(𝜙)sin(𝜃)
𝑎𝑦 cos 𝜙 − 𝑎𝑧sin(𝜙)
−𝑎𝑥 sin 𝜃 + 𝑎𝑦 sin 𝜙 cos 𝜃 + 𝑎𝑧cos(𝜙)cos(𝜃)
=00𝑔
𝜙𝜃
=
𝑎𝑡𝑎𝑛2 𝑎𝑦, 𝑎𝑥
−𝑎𝑡𝑎𝑛2 𝑎𝑥, 𝑎𝑦2 + 𝑎𝑧
2
25.02.2015 Dr. Ahmad Kamal Nasir 22
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Task1: Hardware Experiment
• Install ardrone_autonomy packages• launch the quadrotor ROS driver, make sure
wireless connection between AR-Drone and Computer is already established
• Plot real-time navdata: [rotX,rotY,rotZ]• Visulize live video stream• Teleop the AR-Drone using keyboard/joypad• Create a rosbag of the real experiment• Estimate roll, pitch, yaw angles from gyroscope • Estimate roll, pitch angles from accelerometer
25.02.2015 Dr. Ahmad Kamal Nasir 23
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Quadrotor model with Gazebo
25.02.2015 Dr. Ahmad Kamal Nasir 24
• To install the quadrotor gazebo simulation model• sudo apt-get install ros-indigo-hector-quadrotor*
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Quadrotor topics visualization in Rviz
25.02.2015 Dr. Ahmad Kamal Nasir 25
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
robot_pose_ekf
• Implements an extended Kalman filter for 3D pose estimation {url}
• roslaunch robot_pose_ekf.launch
25.02.2015 Dr. Ahmad Kamal Nasir 26
<launch><node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
<param name="output_frame" value="odom"/><param name="freq" value="30.0"/><param name="sensor_timeout" value="1.0"/><param name="odom_used" value="true"/><param name="imu_used" value="true"/><param name="vo_used" value="true"/><param name="debug" value="false"/><param name="self_diagnose" value="false"/>
</node></launch>
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Euler Angles From Gyroscope𝜙𝜃𝜓
𝑡
=𝜙𝜃𝜓
𝑡−1
+
1 sin 𝜙 tan(𝜃) cos 𝜙 tan(𝜃)
0 cos 𝜙 −sin(𝜙)
0 sin 𝜙 /cos(𝜃) cos 𝜙 /cos(𝜃)
⋅𝑝𝑞𝑟
⋅ Δ𝑡
25.02.2015 Dr. Ahmad Kamal Nasir 27
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Roll, Pitch angles from Accelerometer
𝜙𝜃
=
𝑎𝑡𝑎𝑛2 𝑎𝑦, 𝑎𝑥
−𝑎𝑡𝑎𝑛2 𝑎𝑥, 𝑎𝑦2 + 𝑎𝑧
2
25.02.2015 Dr. Ahmad Kamal Nasir 28
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Yaw angle from magnetometer
𝑀𝑏 = 𝑅𝑥 𝜙 ∙ 𝑅𝑦 𝜃 ∙ 𝑅𝑧 𝜓 ∙ 𝑀𝑖(𝑚𝑥) cos 𝜃 + (𝑚𝑦) sin 𝜙 sin 𝜃 + (𝑚𝑧)cos(𝜙)sin(𝜃)
(𝑚𝑦) cos 𝜙 − (𝑚𝑧)sin(𝜙)
−(𝑚𝑥) sin 𝜃 + (𝑚𝑦) sin 𝜙 cos 𝜃 + (𝑚𝑧)cos(𝜙)cos(𝜃)
=
𝐵 ∙ cos 𝛿 ∙ cos(𝜓)
−𝐵 ∙ cos 𝛿 ∙ sin(𝜓)
𝐵 ∙ sin 𝛿
𝑦 = 𝑚𝑦cos 𝜙 −𝑚𝑧 sin 𝜙
𝑥 = 𝑚𝑥 cos 𝜃 +𝑚𝑦 sin 𝜃 sin 𝜙 +𝑚𝑧 sin 𝜃 cos 𝜙
𝜓 = −𝑎𝑡𝑎𝑛2(𝑦, 𝑥)
25.02.2015 Dr. Ahmad Kamal Nasir 29
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Task 2: Simulation Experiment
• Install quadrotor model for quadrotor
• Navigate the simulated quadrotor model using keyboard and joypad
• Setup robot_pose_ekf node for quadrotor
25.02.2015 Dr. Ahmad Kamal Nasir 30
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Lab Assignment
• To get the understanding of Kalman filer we shall implement it in a simple case where the quadrotor is stationary. Suppose we wish to filter accelerometer value which is almost constant except some small random noise. Therefore, the process model is as follows
𝑥𝑡 = 𝑥0 +𝒩 0, 𝜎𝑝2
• Accelerometer measurements are also subjected to random noise, therefore, the measurement model is as follows
𝑦𝑡 = 𝑥𝑡 + 𝒩 0, 𝜎𝑚2
• Write a simple node which can subscribe to IMU topic and able to separately filter the three accelerometer values using Kalman filter methodology. Publish the estimated state and variance as a custom message consist of two fields. Using rqt_plot plot the published message.– Record the accelerometer measurements and measure the variance of accelerometer
readings.– Since the measurement variance is fixed, observe the behavior of filter using different process
noise variance– Now observe the estimated state and its variance using different initial values of the state and
its variance.
25.02.2015 Dr. Ahmad Kamal Nasir 31
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Lab Assignment (Cont.)
• Calculate Euler angles for an Attitude and Heading Reference System
(AHRS) using gyro-rate sensor, accelerometer and magnetometer.
– Calculate the Euler angles from gyroscope’s body-rate measurements as
follows
𝜙𝜃𝜓
𝑡
=𝜙𝜃𝜓
𝑡−1
+
1 𝑠𝑖𝑛 𝜙 𝑡𝑎𝑛(𝜃) 𝑐𝑜𝑠 𝜙 𝑡𝑎𝑛(𝜃)
0 𝑐𝑜𝑠 𝜙 −𝑠𝑖𝑛(𝜙)
0 𝑠𝑖𝑛 𝜙 /𝑐𝑜𝑠(𝜃) 𝑐𝑜𝑠 𝜙 /𝑐𝑜𝑠(𝜃)
⋅
𝑔𝑥𝑔𝑦𝑔𝑧
⋅ 𝛥𝑡
– The roll and pitch angle from accelerometer can be calculated as follows
𝜙𝜃
=
𝑎𝑡𝑎𝑛2 𝑎𝑦, 𝑎𝑥
−𝑎𝑡𝑎𝑛2 𝑎𝑥, 𝑎𝑦2 + 𝑎𝑧
2
– The yaw angle from the magnetometer readings can be calculated as follows𝑦 = 𝑚𝑦cos 𝜙 −𝑚𝑧 sin 𝜙
𝑥 = 𝑚𝑥 cos 𝜃 + 𝑚𝑦 sin 𝜃 sin 𝜙 +𝑚𝑧 sin 𝜃 cos 𝜙
𝜓 = −𝑎𝑡𝑎𝑛2(𝑦, 𝑥)
25.02.2015 Dr. Ahmad Kamal Nasir 32
EE565: Mobile Robotics LabTask5: AR Drone setup with ROS and sensor data fusion using AR Drone’s accelerometer and gyroscope
Questions
25.02.2015 Dr. Ahmad Kamal Nasir 33