Sensor Fusion with ROS
이번 포스트는 ros-sensor-fusion-tutorial 를 참조하였습니다.
1. Concepts
1.1. ROS Nav Stack Refresher
위 그림에서 왼쪽 부분, "/tf"와 "odom" 메시지를 보내는 부분에 주목하세요.
navigation stack은 연속적이고 불연속적인 odometry(twist(속도)와 pose(position)메시지들을 조합하는 메시지 타입)와 다른 센서들로부터 들어오는 tf 메시지들, global pose estimators 이 모두를 사용하여 로봇의 위치를 추정합니다.
다시 말해, odometry data(센서들로부터 얻어진)와 pose estimates(AMCL로부터 얻어진) 뿐 아니라 우리가 사용하려고 하는 다른 추가적인 센서들도 navigation stack의 위치 추정 성능에 영향을 준다는 것입니다.
1.2. Map, Odom and base_link
pose estimates에는 2 종류가 있는데, 하나는 로봇의 local position(연속적이고, 시간에 따라 자주 변합니다), 다른 하나는 로봇의 global position입니다(불연속적이지만, 장기적으로는 더 정확합니다)
그리고 이 두 pos는 지도, odom, base_link frames 이렇게 3개의 프레임 사이의 서로 다른 좌표 변환에 영향을 줍니다.
여기서 생각해야 할 것은, odom은 map의 child frame이며, base_link는 odom frame의 child frame이라는 것입니다. 즉, map >> odom >> base_link 순으로 내려갑니다.
따라서, 여기에는 두 번의 좌표변환이 있습니다.
1. map -> odom
- 로봇의 global pose를 계산합니다.
- 로봇이 움직임에 따라 발생하는 전체적인 sensor drift를 보상하기 위해 필요한 offset을 추적하여, 로봇이 더 강력하게(robustly) global path plan만들 수 있도록 합니다.
- 불연속적입니다
- AMCL node(또는 다른 global pose estimators)에 의해 publish됩니다.
2. odom -> base_link
- 로봇의 local pose를 계산합니다.
- local obstacle을 피하기 위해 사용됩니다.
- 연속적이지만, sensor drift로 인해 시간에 따라 drift하게 됩니다.
- odometry node(wheel encoder, IMU, laser scan 같은 센서들에 의해 계산되는)에 의해 publish됩니다.
1.3. Kalman Filters and The Motivations for Sensor Fusion
분명히, 우리가 센서를 실제로 사용하면 노이즈로 인해 센서의 측정이 잘 되지 않고, 따라서 알고리즘이 잘 동작하지 않게 되는 경우가 많습니다. 이 문제를 해결하기 위한 방법 중 하나는 여러 센서를 활용하는 것입니다. "센서 여러개를 사용하면, 어떤 한 순간에 센서들을 사용하여 측정을 할 때, 그 중 최소한 하나는 제대로 동작할 것이다. 센서 모두가 제대로 측정되지 않을 확률은 낮다" 라는 아이디어를 적용했다고 보면 됩니다.
예시)
우리가 추적하려고 하는 2개의 binary state가 있다고 하겠습니다.
- A
- B
그리고, 이를 추적할 수 있는 4개의 센서가 있다고 가정해보겠습니다. 각 센서의 신뢰도는 80%라고 가정합니다.
센서의 측정값이 다음과 같은 경우를 생각해보겠습니다.
- AABA
- BBBB
- BBBA
- ABBB
위 결과를 보면, 각 측정시 마다 어떤 값이 제대로 측정된 값인지 확인하는 것은 어렵지 않습니다.
더 정확하게 만들기
이번에는 x, y, z축을 따라 continuous한 값을 측정하는 4개의 센서가 있다고 가정해보겠습니다.
x : 0.214 | 0.31123 | 0.413 | 0.215
y : 0.012 | -1.21415 | 2.215 | 0.5421
z : 0.00124 | 0.000 | -12.124124 | 5.0124
위 값에서는 어떤 값이 좋은 추정이라고 할 수 있을까요? 이런 경우에는 보통 평균을 내서 그 값을 사용하긴 하지만, 이 값은 별로 정확하거나 뭔가 똑똑한 방법으로 구한 값은 아닌 것으로 생각될 것입니다.
Kalman Filters
칼만 필터는 다음과 같은 속성이 있습니다.
- 연속적인 숫자 데이터를 조합(fuse) 합니다.
- 센서의 여러 축에 따른 불확실성(uncertainty)을 계산합니다.
robot_localization은 특히 두 종류의 칼만 필터, Extended Kalman Filter(EKF)와 Unscented Kalman Filter(UKF)를 제공합니다.
- UKFs는 느리지만 non-linear transformations에 대해 더 정확합니다.
- EKFs는 빠르지만 더 부정확합니다(하지만 EKF도 충분히 UKF만큼 효과적인 sensor fusion이므로, ROS Navigation에서는 EKF를 사용할 것입니다)
1.4. Covariance
공분산(Covariance)란, 두 변수가 얼마나 상관(correlation)이 있는가를 나타냅니다.
위 그래프에서 x축과 y축은 서로 다른 변수를 나타냅니다. correlation이 크면 변수 하나가 커지면 이에 따라 같이 커지거나 작아지는 경향이 큽니다. 양의 상관관계(positive correlation)의 경우에는 같이 커지는 경향이 있는 것이고, 음의 상관관계(negative correlation)에는 변수 하나가 커지면 다른 변수는 작아지는 경향이 있는 것입니다.
우리가 사용할 robot_localization에서의 칼만 필터에서는 15개의 변수를 다룹니다. 따라서 여러 변수의 관계를 편하게 나타내기 위해서 공분산 행렬(covariance matrix)를 사용하여 나타냅니다.
[0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1]
예시)
아래의 간단한 행렬을 보겠습니다.
x y z x [a, b, c, y d, e, f z g, h, i]
보시다시피, x, y, z에 관한 covariance matrix입니다. 어떤 특정한 행과 열에 대해, 행렬의 인덱스는 그 행과 열의 변수에 대한 covariance를 나타냅니다. 예를 들어,
- b는 (x, y)의 covariance입니다.
- a는 (x, x)의 covariance입니다.
여기서, covariance (x, x)는 x의 variance입니다.
Covariance matrix의 속성
- 대칭행렬이며, 정사각 행렬입니다
- 행렬의 값들은 실수만 가질 수 있습니다.
- Positive Semi-definite입니다.(즉, 대각 성분의 값은 모두 양수이지만(variance는 음수가 될 수 없습니다), 대각 성분이 아닌 값들은 음수도 가능합니다)
Practical Takeaways
칼만 필터에서 사용할 Covariance matrix에서 가장 중요한 값들은 대각 성분의 값입니다.
- variances는 센서의 각 축에 대한 값이 얼마나 신뢰도가 높은가를 나타냅니다.
- variance가 높으면 노이즈가 많고 불확실 하다는 것이고, 따라서 칼만 필터는 이 값에 대해 더 적은 가중치를 둡니다.
- 각각의 센서에 대해 covariance matrix를 튜닝하는 것은 매우 중요합니다. 또한, 초기 covariance estimates와 noise covariance matrix를 처리하는 것도 마찬가지입니다.
즉, 만약 covariance가 수렴하게 된다면, 이는 적절하게 튜닝된 것이고, 발산한다면 데이터의 신뢰도를 떨어뜨리게 되는 것입니다.
2. Practical Sensor Fusion with robot_localization
2.1. Introduction to robot_localization
robot_localization API reference: http://docs.ros.org/melodic/api/robot_localization/html/
ROS는 EKF와 URK를 사용하여, 몇 개든 우리가 원하는 수 만큼의 센서와 pose estimate data sources를 조합할 수 있는 robot_localization package를 제공합니다.
이 패키지는 다음과 같은 특징이 있습니다.
1. EKF 또는 UKF를 선택할 수 있습니다.
2. 원하는 만큼 많은 센서와 pose estimate data sources를 사용할 수 있습니다(단, odometry, imu, pose, twist 형식의 메시지만 사용 가능합니다)
3. 센서가 보내는 값들 중 특정 값을 무시할 수 있습니다.
4. continuous data가 없어도 continuous estimation이 가능합니다.
5. 데이터 전처리(data pre-processing)이 가능합니다(예를 들어, 거리 데이터를 추정하는 값을 만들기 위해 속도를 적분할 수 있습니다.)
6. 다음과 같이 15개의 값을 사용할 수 있습니다.
- position : x, y, z
- orientation : roll, pitch, yaw
- linear velocity : x', y', z'
- angular velocity : yaw', pitch', yaw'
- linear acceleration: x'', y'', z''
robot_localization Usage
아까 위에 언급했듯이, 우리는 map, odom, base_link frames를 추적해야합니다.
- map -> odom은 /tf를 통해 다뤄집니다.
- odom -> base_link 좌표변환은 "odom"을 통해 보내지는 odometry message를 통해 다뤄집니다.
Fusing Sensors for the /map frame (map -> odom)
map -> odom으로의 좌표 변환을 위해서는 ekf/ukf_localization_node에서 "world_frame" 파라미터를 우리가 사용할 map frame의 이름으로 설정해야 합니다.
이 단계에서 조합하는 데이터는 다음과 같습니다
- AMCL과 같은 Global pose estimates
- GPS, Ultrasonic beacons, Global Visual Odometry같은 Absolute global pose data
- 모든 continuous sensor 데이터와 알고리즘의 출력은 global pose estimates에 사용되지 않습니다. 칼만 필터에서 global pose에는 non-global sensor와 algorithm이 사용됩니다.
Fusing Sensors for the /odom frame (odom -> base_link)
odom -> base_link로의 좌표 변환을 위해서는 ekf/ukf_localization_node에서 "world frame"파라미터를 우리가 사용할 odom frame의 이름으로 설정해야 합니다.
이 단계에서 조합하는 데이터는 다음과 같습니다.
- IMU
- Visual Odometry
- Wheel Encoders
- laser-scan-matcher
2.2. Roadmap
이번에는 EKF localization node를 사용할 것입니다. 시작하기 위해 필요한 파일들은 다음과 같습니다.
1. ekf_template.yaml ( 파라미터 설정 )
2. ekf_template.launch ( 파라미터를 불러옴 )
<launch>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
<rosparam command="load" file="$(find robot_localization)/params/ekf_template.yaml" />
</node>
</launch>
Step-By-Step
robot_localizatoin의 EKF 또는 UKF를 사용하는 방법은 어렵지 않습니다. 다음과 같은 절차만 진행하면 됩니다.
1. 데이터 준비
2. 칼만 필터 노드 설정
3. 실행하고, 검증하고, 튜닝하기
2.3. Preparing your Data
robot_localization은 ROS standards를 따릅니다. 따라서, 우리가 사용할 데이터도 다음 협약(conventions)를 따르는지 확인해야 합니다.
추가적으로, 다음 메시지 타입을 따르는 메시지들만 robot_localization nodes에서 입력으로 사용될 수 있습니다.
- nav_msgs/Odometry
- geometry_msgs/PoseWithCovarianceStamped
- geometry_msgs/TwistWithCovarianceStamped
- sensor_msgs/Imu
- 가장 좋은 연습 : Odometry, Pose, Twist
robot_localization documentation은 odometry 데이터를 사용하는 몇 개의 추천 예제를 제공합니다.
1. frame_id와 child_frame_id가 제대로 설정되었는지 확인하세요.
Map, Odom, base_link 이 3개의 프레임들이 사용되어야 합니다.
대부분의 odometry sources는 다음과 같은 설정을 따릅니다.
- frame_id : odom
- child_frame_id : base_link
2. 데이터의 부호가 맞는지 확인하세요
- 로봇이 반시계 방향으로 움직인다면, yaw값이 양수이어야 합니다.
- 로봇이 앞으로 움직인다면, x값이 양수이어야 합니다.
3. odom -> base_link 좌표변환에 사용되는 다른 모든 sources를 제거하세요
이는 매우 중요합니다. 충돌하는 좌표변환을 publish하는 것은 시스템이 동작하지 않거나 로봇의 위치가 크게 진동하도록 만듭니다. 로봇의 위치가 진동한다면, 대부분 이 이유로 인해 발생합니다.
robot_localization node가 필터링 된 odometry 데이터를 publish하도록 하려면 우리는 반드시 odom -> base_link로 좌표변환을 하는 tf publisher를 모두 비활성화 해야합니다.
이는 map -> odom 좌표변환의 경우에도 마찬가지입니다.(예를 들어, AMCL node에서 tf_broadcast 파라미터를 false로 설정해야 합니다.)
4. covariances를 오용하지 마세요
가끔씩 covariance를 정적인(static) 값으로 추정해야하는 경우가 있습니다. 이런 경우에는 직접 계산하는 것이 좋습니다.
하지만 칼만 필터가 해당 값을 무시하도록 만들기 위해(이전에, 공분산이 크면 칼만 필터가 가중치를 적게 둔다고 했습니다) covariance matrix에 일부러 너무 큰 값을 넣으면 안됩니다. 이런 경우를 위해 칼만 필터에서는 해당 값을 무시하도록 설정하는 파라미터가 따로 있습니다.
- 가장 좋은 연습 : IMU
1. frame_id와 부호가 정확한지 확인하세요.
odom, pose, twist 메시지와 같습니다. 제대로 된 로봇의 움직임을 위해 매우 중요합니다.
sensor_msgs/Imu - IMU 메시지는 현재 ROS 커뮤니티에서도 계속 말하는 중이지만, 애매함이 있습니다. 대부분의 IMU 센서는 자기장 센서에 따라서 X축은 지구의 북쪽, Z축은 지구의 중심, Y축은 동쪽을 향한 축을 기준으로 해서 방향 값을 나타냅니다. 이 프레임은 NED(North, Ease, Down)이라고도 합니다.
하지만, REP-103을 보면 실외 내비게이션 사용시 ENU(East, North, Up) 좌표계를 사용한다고 합니다. 이에 따라, robot_localization 패키지는 모든 IMU 데이터가 ENU 프레임을 사용할 것이라고 가정합니다. 따라서, IMU 데이터가 ENU 프레임으로 들어오는지를 반드시 확인해야 합니다.
2. covariance를 오용하지 마세요.
odom의 경우와 같습니다.
3. 중력을 생각하세요
IMU는 중력 가속도 또한 감지하기 때문에, IMU의 방향이 올바른지 확인하세요. 만약 그러지 않았다면, 파라미터 설정 중에서 중력 가속도를 무시하는 설정을 해줘야 합니다. 그렇지 않다면, robot_localization node는 IMU가 위로 향해있다고 가정합니다.
IMU는 다음과 같이 측정되어야 합니다.
Z축 방향으로 +9.81 m/sec^2
센서가 roll 축으로 +90도 회전 시(왼쪽이 위로 향할 경우), Y축 가속도는 +9.81 m/sec^2
센서가 pitch 축으로 +90도 회전 시(앞 쪽이 밑으로 향할 경우), X축 가속도는 -9.81 m/sec^2
만약 IMU가 제대로 놓여있지 않다면, static transform을 사용하여 base_link로부터 회전된 프레임을 만들어야 합니다.
자주 발생하는 오류
- 입력 데이터가 REP-103을 따르지 않는 경우 - 모든 값(특히, 각도 및 방향)이 올바른 방향으로 증감하는지 확인하세요.
- 올바르지 않은 frame_id 값 - 속도 데이터는 반드시 base_link_frame 에서 report되어야 하며, 그렇지 않은 경우 해당 프레임과 base_link_frame 사이의 좌표 변환이 정의되어있어야 합니다.
- 너무 큰 covariance - 변수를 무시하는 적절한 방법은 odom#_config 파라미터에서 정의하는 것입니다.
- covariance를 정의하지 않은 경우 - 주어진 변수를 state estimation node에서 사용하도록 센서를 설정한 경우, 그 값에 해당되는 covariance는 0이 되면 안됩니다. 만약 variance가 0이 되면 state estimation node는 해당 값에 아주 작은 epsilon 값(1e-6)을 추가합니다.
2.4. Configuring the Kalman Filter Nodes
sensor fusion node를 커스터마이징 할 수 있는 파라미터는 매우 많지만, 이 중 다른 파라미터보다 더 중요한 파라미터들이 몇 개 있습니다.
1. EKF와 UKF 중 어떤 것을 사용할지 선택하세요
로봇 위치 추정에는 일반적으로 EKF가 사용됩니다. 사용 방법은 우리가 필요한 node를 launch하면 됩니다.
<launch>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_node_name" clear_params="true">
<rosparam command="load" file="$(find robot_localization)/params/ekf_template.yaml" />
</node>
</launch>
2. frames를 정의하세요
REP-105를 따라서 .yaml 파일에 정의하세요
만약 odometry 데이터를 조합한다면(scan matching, LIDAR, encoders 등)
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
만약 global pose estimates를 조합한다면(AMCL, beacons 등)
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: map # Defaults to the value of odom_frame if unspecified
3. 센서 입력을 정의하세요
센서 nodes가 유효한 메시지를 publish하는지 확인하세요. robot_localization패키지는 센서들의 번호를 0번부터 순서대로 매깁니다. 반드시 0부터 시작해야합니다.
odom0: odom0_topic/odom # Fill in the correct topic here!
odom1: odom1_topic/odom # Make sure it's of the correct message type!
pose0: pose0_topic/pose # Same-same
pose1: pose1_topic/pose
twist0: twist0_topic # But different
twist1: twist1_topic
imu0: imu0_topic # But STILL same
imu1: imu1_topic
4. 각각의 센서에서 사용할 데이터를 설정하세요.
다음과 같이 기본이 되는 벡터가 있습니다. 각 요소는 booleans입니다.
Canonical Data Vector
따라서, 센서를 설정한 예시는 다음과 같습니다.
odom0_config: [true, true, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
# We can see that this sensor's x, y, and yaw_vel values are being used
* 만약 평면 상에서 움직이는 로봇을 사용한다면, 최소한 x, y, x_vel, y_vel, yaw, yaw_vel값들을 다른 값 보다 먼저 조정하세요.
드론 같이 평면 상에서 움직이는 로봇이 아니면, 모든 값이 조정되어야 합니다.
5. covariance matrices를 정의하세요.
정의할 covariance matrices는 두 개가 있습니다. 이 행렬들은 process noise(measurement noise)와 initial estimate covariance matrices입니다. 우리는 이 값들을 튜닝해야합니다.
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is exposed as a configuration parameter. This matrix represents the noise we add to the total error after each prediction step.
# The better the omnidirectional motion model matches your system, the smaller these values can be. However, if users find that a given variable is slow to converge, one approach is to increase the process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error to be larger, which will cause the filter to trust the incoming measurement more during correction.
# The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az.
# Defaults to the matrix below if unspecified.
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal value (variance) to a large value will result in rapid convergence for initial measurements of the variable in question. Users should take care not to use large values for variables that will not be measured directly.
# The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az.
# Defaults to the matrix below if unspecified.
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]
6. 2D mode 설정
만약 2D Navigation을 사용한다면 반드시 설정해줘야 합니다.
two_d_mode: true7. 선택사항
일반적으로 칼만 필터 node에 의해 제공되는 좌표변환만으로도 navigation stack에는 충분하지만, 만약 여러개의 칼만 필터, 또는 GPS message converter를 사용한다거나 디버깅을 한다든지, 이런 이유로 출력 결과의 메시지를 사용해야 한다면, 이를 remap할 수 있습니다.
robot_localization가 publish하는 데이터는 다음과 같습니다.
- odometry/filtered (nav_msgs/Odometry)
- accel/filtered (geometry_msgs/AccelWithCovarianceStamped)
그리고, 다음과 같은 좌표 변환을 publish합니다.
- odom -> base_link
- map -> odom
move_base에 사용될 입력을 remap 태그를 사용하여 수정할 수 있습니다.
<remap from="original_name" to="new_name"/>
<remap from="hello" to="rawr"/>
2.5. Sensor Fusion을 위한 좋은 연습
Sensor Fusion을 위해서 다음과 같이 권장되는 방법이 있습니다.
Fuse Primary Sources
일반적으로 다음과 같이 적용됩니다.
- 만약 odometry가 position과 linear velocity를 제공한다면, linear velocity를 조합합니다.
- 만약 odometry가 orientation과 angular velocity를 제공한다면, orientation을 조합합니다.
* 만약 orientation data에 2개의 sources가 있다면, 조심해야 할 것입니다. 만약 두 sources 모두 정확한 covariance matrices를 가진다면, orientations를 조합해도 괜찮습니다. 하지만 둘 중 하나가 부정확한 covariance를 가진다면, 더 정확한 source로부터 제공되는 데이터를 사용해야 합니다.
Don't Repeat Data
사실, 데이터가 많으면 우리 시스템을 더 강력하게 하는데 도움이 됩니다(제대로 설정이 된 경우에만). 하지만, 때때로 일부 값이 다른 값으로부터 파생되는 경우도 있습니다. 예를 들어, position은 velocity를 적분하면 얻을 수 있으므로, 이는 중복된 데이터입니다.
중복된 데이터는 칼만 필터에 불필요한 바이어스를 더하게 됩니다.
단, 이는 하나의 칼만 필터 노드에서 중복되는 경우입니다. 다른 node끼리 데이터를 교차하는 것은 괜찮습니다.
간단한 예제
x, y, yaw, x_vel, yaw_vel을 report 하는 로봇이 있는데, 이 데이터가 wheel encoders로부터 들어온다고 가정하겠습니다.
우리는 "primary sources"인 x_vel과 yaw_vel만 알면 되는데, 왜냐면 나머지는 이 "primary sources"에서 파생된 값이며, 이는 칼만 필터에 중복되어 제공될 것이기 때문입니다. 따라서, odom node는 다음과 같이 설정합니다.
odomN_config: [false, false, false,
false, false, false,
true, false, false,
false, false, true,
false, false, false]
측정되지 않은 데이터를 전략적으로 사용하기
가끔씩, 우리가 측정하면 안되는 값을 사용하는 것이 유용한 경우가 있습니다.
예를 들어, non-holonomic robot과 y_vel을 생각해보겠습니다. holonomic이란, 모든 방향으로 움직일 수 있는 로봇을 말합니다. 일반적인 자동차 형태의 로봇은 앞 뒤로 움직이는것과 회전 운동만 가능합니다. holonomic robot은 옆으로도 움직일 수 있는 로봇을 말합니다.
따라서, 우리는 로봇이 y방향으로 움직일 수 없다는 것을 압니다. 그러므로, 칼만 필터도 이 값이 항상 0이 되어야 한다는 것을 압니다. 만약 이 값이 0이 아닌 다른 값이 나온다면, 문제가 있는 것이므로 디버깅에 도움이 될 수 있습니다.
데이터 충돌을 주의하세요
만약 pose같이 absolute variables가 겹칠 수 있는 센서가 충분히 많다면(모두 적절하게 튜닝된 값이라면 괜찮습니다), 몇 가지 사항을 알아둬야 합니다.
- 각각의 측정된 방향 변수의 covariance가 제대로 설정되었는가에 대해 매우 신중해야 합니다.
- 예를 들어, 각각의 IMU의 variance가 0.1로 설정했다고 했을 때, 각 IMU들의 yaw 측정값들의 delta가 0.1보다 크다면, 필터의 출력은 각각의 센서에 의해 제공된 값 사이에서 진동하게됩니다.
하지만, 이게 바보같이 높은 covariance를 할당해야 한다는 뜻은 아닙니다. 만약 진동이 생긴다면(covariance로 인한 진동이라는게 확실하다면) 그냥 적당히 높은 값만 주면 됩니다
대안 :
sensor#_differential 파라미터를 설정하는 방법도 있습니다.
robot_localization 패키지는 pose 데이터를 입력으로 취하며, 이 연속적인 pose 데이터의 즉각적인 변화를 통해 속도값을 추론합니다.