| 1 |
1
자세추정 시스템을 포함하는 무인 이동 장치로,상기 무인 이동 장치의 각속도 및 가속도를 측정하는 관성 센서;상기 무인 이동 장치의 지자계 자속 밀도를 측정하는 지자계 센서; 및상기 가속도 센서 및 지자계 센서로부터 획득한 측정 값에 기초하여 상기 무인 이동 장치의 자세를 추정하는 제어부를 포함하고,상기 무인 이동 장치의 자세는 3차원 공간의 3축인 x축, y축, z축에 대하여 x축에 대한 무인 이동 장치의 회전각인 제1 값, y축에 대한 무인 이동 장치의 회전각인 제2 값, 및 z축에 대한 무인 이동 장치의 회전각인 제3 값으로 표현되고,상기 제어부는 상기 가속도 센서로부터 획득한 값을 이용하여 제1 값 및 제2 값을 추정하고, 상기 지자계 센서로부터 획득한 값을 이용하여 제3 값을 추정하고,상기 제어부는 무인 이동 장치의 추정 자세에 대한 초기 오차 공분산 값에 기초하여 제1 오차 공분산 값을 갱신하고, 상기 갱신된 제1 오차 공분산 값에 기초하여 제1 가중치 값을 획득하고, 상기 제1 오차 공분산에 기초하여 제2 오차 공분산을 갱신하고, 상기 제2 오차 공분산에 기초하여 제2 가중치 값을 획득하고, 상기 제1 가중치 값에 기초하여 상기 제1 값 및 제2 값을 보정하고, 상기 제2 가중치 값에 기초하여 상기 제3 값을 보정하며,상기 제어부는상기 관성 센서를 통해 획득한 측정 경사각과 기준 경사각간의 차인 제1 비교값이 제1 임계치 이상이면 상기 제3 값을 보정하지 않고, 상기 지자계 센서를 통해 획득한 자속 밀도의 세기와 기준 자속 밀도 세기간의 비인 제2 비교값이 제2 임계치 이상이면 상기 제3 값을 보정하지 않고, 갱신 주기 동안 상기 지자계 센서로 측정한 방향각 변화량과 상기 갱신 주기 동안 상기 관성 센서로 측정한 방향각 변화량의 차인 제3 비교값이 제3 임계치 이상이면 상기 제3 값을 보정하지 않고,상기 제1 비교값이 제1 임계치 미만이고, 상기 제2 비교값이 제2 임계치 미만이고, 상기 제3 비교값이 제3 비교값 미만인 경우 상기 지자계 센서로부터 획득한 값을 이용하여 방위각을 산출하고, 산출된 방위각과 기준 방위각간의 차인 제4 비교값이 제4 임계치 이상이면 상기 제3 값을 보정하지 않고, 상기 제4 비교값이 제4 임계치 미만인 경우 상기 제2 가중치 값을 획득하여 상기 제2 가중치 값과 상기 방위각을 이용하여 상기 제3 값을 보정하는무인 이동 장치
|
