Recommended
More Related Content
What's hot
What's hot (20)
IMU Filter (AHRS)
- 1. An efficient orientation filter for inertial and inertial/magnetic sensor Jinyong Jeong jjy0923@kaist.ac.kr https://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf
- 2. /34 Today’s topic • ROS에서 가장 많이 쓰이는 AHRS IMU filter 알고리즘 2
- 3. /34 IMU filter example video 3https://youtu.be/BXsGWoOMtmU
- 4. /34 IMU는 어떤 데이터를 갖고 있는가? • Gyroscope (자이로센서) • Angular velocity (각속도) • 누적을 통해서 회전량을 계산! • Accelerometer (가속도센서) • Linear acceleration (가속도) • 움직이지 않고 있을 때 중력 가속도의 방향으로 Roll, Pitch 계산가능! • 움직이고 있을 때는 움직임에 따른 가속도가 포함됨 • Magnetometer (지자기센서) • Magnetic filed (지자기) • 지구의 자기장 방향으로 데이터 획득 • 노이즈가 큰편 (주변 환경에 영향을 받음) 4
- 5. /34 AHRS는 무엇인가? • AHRS: Attitude Heading Reference System • Gyroscope: local한 회전을 빠르게 알수 있음! • Accelerometer: 중력 방향을 이용해서 global한 roll, pitch 계산가능! • Magnetometer: 자기장 방향을 이용해서 global yaw 계산가능! • 세가지 데이터 융합으로 global rotation을 계산하는것이 AHRS 5
- 6. /34 왜 Quaternion을 사용하는가? • 오일러(Euler)의 단점: 짐벌락(Gimbal Lock)! 6https://youtu.be/zc8b2Jo7mno
- 7. /34 Quaternion representation • Quaternion representation • A의 rotation (B frame에서) -> 논문 맨처음 설명 틀린듯 • Conjugate 7
- 8. /34 Quaternion representation • 3차원 벡터의 rotation • 같은 vector 의 표현 (각각 A frame, B frame 에서) • Quaternion의 rotation matrix의 변환 • Quaternion의 Euler angle 변환 8
- 9. /34 Orientation from angular rate • Angular rate to derivative of quaternion (미분 과정은 아래 링크 참고) • 실제 구현 • Update: 이전 step에서 추정된 quaternion으로 변환 9https://fgiesen.wordpress.com/2012/08/24/quaternion-differentiation/ 참고 Angular rate 이전 step의 quaternion Quaternion Update
- 10. /34 Orientation from vector observation • 실제 Accelerometer & magnetometer • Accelerometer = gravity + 실제 모션에 따른 가속 • Magnetometer = earth’s magnetic filed + local magnetic flux + distortion • 우선 먼저 가정! • accelerometer 는 gravity만 포함 • Magnetometer 는 earth’s magnetic filed 만 포함 10
- 11. /34 Orientation from vector observation • Optimization Problem! • Sensor의 magnetometer 측정값이 global하게 북쪽을 향하도록! (실제로는 북쪽 + 아랫방향) • Sensor의 accelerometer 측정값이 global하게 z방향으로 되도록! 11 Optimization Objective Function 구하고자 하는 Quaternion Sensor 측정값 알고있는 global 값 (중력, 자기) 알고있는 값을 센서 frame으로 변환
- 12. /34 Orientation from vector observation • Optimization Problem • Gradient descent algorithm • 1. 현재 값에서 선형화 • 2. 작아지는 방향으로 optimization 12 Objective Function 참고 Step size Gradient 에 대한 Jacobian q1으로 미분 q2로 미분 q3로 미분 q4로 미분
- 13. /34 Orientation from vector observation • Accelerometer 데이터를 이용한 Optimization • Gravity는 z 방향! 13 참고 센서 데이터 Gravity (z방향) Object function Jacobian
- 14. /34 Orientation from vector observation • Magnetometer 데이터를 이용한 Optimization • Global의 magnetic filed는 지구 좌표계에서 Z방향과 X방향 (이 값은 어떤 좌표계를 쓰냐에 따라 다름) 14 참고
- 15. /34 Orientation from vector observation • 어떤 좌표계가 있을까? • NED • NWU • ENU 15 현재의 quaternion Global 값 측정값 J*F 전체적으로 직관적인 coordinate와 비교했을 때 방향이 반대로 되어 있는데 처음부터 정의를 반대로 한건지 확인 필.
- 16. /34 Orientation from vector observation • Gradient decent Code! 16 참고 현재 rotation Global target (gravity or magnetic) Sensor measurement result Global target data를 sensor coordinate으로 변형
- 17. /34 Orientation from vector observation • 두 식을 합치면 17 magnetometer Accelerometer Gradient step size (angular velocity의 크기에 비례)
- 18. /34 Filter fusion algorithm • Measurement 1 • angular velocity 를 누적해서 얻은 quaternion • Local measurement (엔코더와 같이 누적 에러가 있음) • Measurement 2 • magnetometer, accelerometer 데이터를 이용하여 gradient decent 방법으로 계산한 quaternion • Global measurement (GPS와 같이 누적에러는 없으나 오차가 있음) • 두개를 잘~ 섞어야 한다! => Fusion 18
- 19. /34 Filter fusion algorithm 19 최종 결과는 Weighted sum Measurement 2 Measurement 1 convergence rate Step size 최적의 감마값은? Divergence rate (measurement error) 최적의 감마값 𝛼가 매우 크다고 가정 = 𝜇도 매우 큼 최종 Fusion!
- 20. /34 Filter fusion algorithm 20 • 1차 fusion 알고리즘 완성
- 21. /34 Magnetic Distortion Compensation • Magnetic Distortion • Magnetometer로 측정되는 데이터는 주변 환경에 의해 외곡이 될 수 있음 • Global frame에서 Z방향은 그대로 사용 • X, Y 방향의 값은 북쪽 방향으로 수정해서 사용! 21 센서 값 현재 추정된 orientation Global coordinate 보정된 값
- 22. /34 Gyroscope bias drift compensation • Gyroscope bias • Angular rate에 포함되어 있는 bias • Kalman filter 의 경우에는 bias를 state로 넣어서 추정 가능 • Mahony[35]가 integral feedback 방법으로도 단순하게 bias를 보정할 수 있음을 증명 22 현재 추정된 quaternion의 미분Angular rate 누적 보정
- 23. /34 Filter fusion algorithm • 최종 fusion 알고리즘 완성 23 Magnetic distortion compensate Bias drift compentation
- 24. /34 Filter fusion algorithm • Gain은? 24 Estimated mean zero gyroscope me asurement error of each axis Estimated rate of gyroscope bias dr ift in each axis
- 26. /34 Experimental Result • Typical result 26
- 27. /34 Experimental Result • Typical result 27
- 28. /34 Experimental Result • Typical result 28
- 29. /34 Experimental Result • Static and dynamic performance 29 Static: angular rate < 5 degree/sec Dynamic: angular rate > 5 degree/sec MARG = AHRS
- 30. /34 Experimental Result • Filter gain vs performance 30 빠르게 움직일수록 gain이 좀더 커야함!
- 31. /34 Experimental Result • Sampling rate vs performance 31 빠르게 움직일수록 sampling rate가 높 아야함!
- 32. /34 Experimental Result • Gyroscope bias drift 32 임의로 bias를 추가해서 실험! 10초 부터 bias gain을 0.015로 세팅!
- 33. Demo Time! 33
- 34. Thank you! 34
Editor's Notes
- Good morning. Thank you for attend my presentation. My name is JinyongJeong from KAIST. I’d like to talk about extrinsic calibration for stereo camera of non-overlapping configuration. This work was collaborated with Lucas Cho, Ayoung kim of KAIST.