phasespace optical navigation development & multi-uav closed- loop control demonstration texas...
TRANSCRIPT
PhaseSpace Optical Navigation Development & Multi-UAV Closed-
Loop Control Demonstration
Texas A&M UniversityTelecon with BoeingDecember 7, 2009
PhaseSpace Optical Navigation Adaptation (PSONA) Team
Faculty Mentors: Ms. Magda Lagoudas, Dr. John L. Junkins, Dr. Raktim Bhattacharya
Overview• PSONA Semester Goals• PhaseSpace calibration (Albert Soto & Zach Sunberg)• Outfitting quad-rotors with PhaseSpace/Vicon (Erin
Mastenbrook)• Quad-rotor custom electronics board design (Winnie
Lung & Kate Stuckman)• A look at the next semester
2
Semester GoalsCharacterizing and Calibrating PhaseSpace
• Characterize the PhaseSpace system• Develop a method of calibration• Evaluate the accuracy and reliability of
PhaseSpace by comparing to Indoor GPS• Compare all results to the capabilities of Vicon
3
Semester Goals
• Demonstrate the capability of utilizing PhaseSpace as a vision-based localization solution in multiple, UAV coordination
• Equip two DraganFlyer Quad-Rotors and LASR Lab to enable multi-vehicle autonomous flight– Next semester: develop user interfaces and control
algorithms for autonomous flight
UAV Demonstration
4
PhaseSpace Calibration
5
• Error in the PhaseSpace system arises primarily from optical noise and from misalignments in the camera’s internal geometry (biases).
• By determining the true geometry of the camera, bias error may be compensated for, resulting in a more accurate “best guess” of the beacon location.
• Noise characterization then describes how reliable the “best guess” measurement is.
Approach
6
• Develop a model that describes a camera’s output as a function of its intrinsic parameters and beacon position.
• Construct an algorithm to determine more accurate (or less biased) camera parameters.
• Using these improved parameters, run several tests to gather data reflecting the correlation of noise with beacon position in each dimension.
Mathematical Model
7
Extrinsic parameters:u – beacon locationu0 – mounting pointCamera's Euler angles
Intrinsic parameters:pz – pinhole depthsz – sensor center depth
α – angle of sensor's axis from horizontal within ideal planeß – angle between sensor and ideal planepx' – distance from mount to pinhole along sensor's axissx' – distance from mount to sensor center along sensor's axis
pinhole
Mathematical Model
8
The following set of four equations describes an output pixel in terms of the basic intrinsic and extrinsic parameters.
b0, f, and δ are intermediate values. R is the rotation matrix containing the camera's Euler angles. ξ is the pixel value returned by the camera.
0uub R0
Calibration Simulation
9
Ideal Camera Sensor 1 Sensor 2
α (deg.) -45 45
ß (deg.) 0 0
sx' (in.) 1.77 1.77
sz (in.) .100 .100
px' (in.) 1.77 1.77
pz (in.) 1.81 1.81
True Camera Sensor 1 Sensor 2
α (deg.) -47.5 42
ß (deg.) 2.0 -1.5
sx' (in.) 1.81 1.74
sz (in.) .105 .095
px' (in.) 1.76 1.80
pz (in.) 1.80 1.84
The ideal intrinsic parameter values shown below were passed into the GLSDC algorithm as initial guess values. Biased “true” values were used to generate output for eight beacon locations.
Calibration Simulation
10
Error (GLSDC) Average Max
α 0.000% 0.000%
ß 0.000% 0.000%
sx' 0.000% 0.000%
sz 0.000% 0.000%
px' 0.000% 0.000%
pz 0.000% 0.000%
Shown below is the % error for the ideal and GLSDC-derived cameras vs. the true camera. For this trial, noiseless measurements from the hypothetical true camera were used and the parameters were found accurately to several decimal places.
Error (ideal) Sensor 1 Sensor 2
α 5.263% 7.143%
ß 100.0% 100.0%
sx' 2.210% 1.724%
sz 4.762% 5.263%
px' 0.568% 1.667%
pz 0.556% 1.630%
Calibration Simulation
11
Error (ideal) Sensor 1 Sensor 2
α 5.263% 7.143%
ß 100.0% 100.0%
sx' 2.210% 1.724%
sz 4.762% 5.263%
px' 0.568% 1.667%
pz 0.556% 1.630%
Error (GLSDC) Average Max
α 0.003% 0.010%
ß 0.343% 0.443%
sx' 0.043% 0.101%
sz 2.896% 4.740%
px' 0.045% 0.100%
pz 0.160% 0.273%
Next, the noise was set to a value slightly greater than the maximum observed in testing. The average and maximum % errors from a set of 5 trials are shown. In all cases, the GLSDC code produced more accurate parameters, often by a large margin. Sensor depth was estimated less accurately than the other parameters.
Calibration Testing
12
Ideal Camera Sensor 1 Sensor 2
α (deg.) -45 45
ß (deg.) 0 0
sx' (in.) 0.707 -0.707
sz (in.) 0.100 0.100
px' (in.) 0.707 -0.707
pz (in.) 1.81 1.81
True Camera Sensor 1 Sensor 2
α (deg.) -44.08 45.49
ß (deg.) 2.13 -0.75
sx' (in.) 0.462 -0.566
sz (in.) -2.370 -2.424
px' (in.) 0.461 -0.566
pz (in.) -0.512 -0.560
• Two cameras were tested using three different arrangements of 6 beacons in known locations
• GLSDC was run on all three data sets simultaneously• Results were similar for both cameras
Distortion Testing
13
• Characterized the lens distortion by taking measurements across the FOV of each imager.
• For each imager, the camera was tilted +45⁰ or -45⁰ to isolate one sensor (set one imager vertical and one horizontal)
• The test camera was leveled in the plane of the test by ensuring that the vertical sensor outputs for a beacon at the FOV endpoints have equivalent values (horizontal imager is parallel to the optical table assuming the imagers are orthogonal).
0 500 1000 1500 2000 2500 3000 3500 4000-50
-40
-30
-20
-10
0
10
20
30
40
50
Model Pixels
Diff
ere
nce
from
Mod
elDistortion Test Results
Distortion Testing
14
13
3 ddd
Calibration Software
15
• The calibration software will take in data previously gathered by the cameras.
• The cameras will gather data by viewing the calibration rig (a circle of beacons at many positions and orientations).
• The software will calibrate the positions and orientations of all of the cameras and their intrinsic parameters. (It will also calibrate the position of the rig in each frame, but this is of no use to us)
Calibration Software Initialization
16
y – measurementsx – calibration parameters
Main Function
Read in Measurements
Load Typical Intrinsic Parameters
Assume Initial Rig State is Zero
Estimate Camera States (Initial Rig State)
Estimate Rig States (Camera States)
While Difference in Y > Tolerance
GLSDC Iteration (Measurements, Calibration Parameters)
[Next Slide]
GLSDC Iteration
17
While Difference in Y > Tolerance
GLSDC Iteration ( y , x )
Difference in Y = y – New Y
New Y = Simulate System ( x )
H = Generate H ( H = Generate H ( xx , Constraints ) , Constraints )
Difference in X = ( HT W H )-1 W HT * Difference in Y
x = x + Difference in X
W = Generate Weight MatrixW = Generate Weight Matrix
y – measurementsx – calibration parameters
Coding Progress
18
Created Matlab classes for the measurements and calibration parameters. These will handle access to all of the parameters and allow them to be used as vectors.
Example: x is an object of class CalParamsx.Vec acts as a vector for using in GLSDCx.IntParams(2,1).F allows access to the intrinsic parameter F of sensor 1 in camera 2.
I have never worked with classes in Matlab, so the development has taken longer than initially expected, but we hope that a strong framework will pay off in the future.
Previous Semester Accomplishments
19
• Familiarized with Matlab and GLSDC• Researched and purchased computer
hardware for QuadRotor offboard computing.• Wrote software for finding an initial estimate
of a camera’s position and orientation.
Tasks for Next Semester
20
• Finish writing calibration software.• Help to write software for determining
positions using the data we gather from calibration.
• Help to write control software for the quadrotors.
Quad-Rotor Structure
21
22
X
Y
LP
LVR
R=131.6 mmLP= 8.3 mmLV= 14.3 mm
Rotors
PhaseSpace Beacon
Vicon Reflector
Basic Stabilization Kit Rods
Upper Layer Rods
Φ
cossin
sincos
ii
ii
LRY
LRX
Use the top sign if beacon is to the clockwise side
Semester Summary
23
• We designed a T joint, corner piece, center piece, and mounts for PhaseSpace beacons and Vicon reflectors in SolidWorks and had the parts created using a rapid prototyping machine.
• Outfitted 2 quad rotors with parts, beacons, reflectors, and electrical wiring.
• Created files for Vicon and PhaseSpace to allow them to recognize the individual quad rotors.
• Will outfit 3 more quad rotors next semester.
Quad-Rotor Video
24
Schematic
25
Current Board Layout
26
• Completed Schematic– Learned circuit design basics– Learned EAGLE– Learned how to interpret datasheets
• Finalized Parts List• Began Board Layout
Semester Summary
27
• Before End of Semester– Finish board layout– Order board and parts
• Early Next Semester– Assemble board– Test board– Make necessary changes
Future Plans
28
• Assemble calibration rig and perform comparative testing between Vicon and PhaseSpace using iGPS as truth
• Complete PhaseSpace calibration code• Assemble & debug custom electronics board• Interface quad-rotor board with off-board computing
& Vicon/PhaseSpace feedback• Outfit three additional quad-rotors (total of 5)• Develop models & control laws for autonomous
flight
Next Semester
29
Questions? Comments?
30