KR101049362B1

KR101049362B1 - 자세각 검출 장치와 자세각 검출 방법

Info

Publication number: KR101049362B1
Application number: KR1020087031073A
Authority: KR
Inventors: 아끼히로 우에다; 이와오 마에다; 기요미 나가미야; 나오또 시바따
Original assignee: 도요타지도샤가부시키가이샤
Priority date: 2006-06-23
Filing date: 2007-06-25
Publication date: 2011-07-13
Also published as: EP2034270A1; CN101473193B; US20090326858A1; KR20090018659A; JP4321554B2; EP2034270A4; JP2008002992A; EP2034270B1; US8200452B2; CN101473193A; WO2007148818A1

Abstract

가속도 센서로부터의 출력치를 보정함으로써 이동체의 이동 중의 자세각을 정밀도 좋게 검출하는 자세각 검출 장치 및 그 방법을 제공한다. 이동체(7)에 가해지는 가속도를 측정하는 가속도 센서(2)와, 이동체(7)의 요 레이트를 측정하는 요 레이트 센서(3)와, 이동체(7)의 속도를 측정하는 속도 센서(4)와, 속도로부터 실제 가속도를 계산하고, 요 레이트와 속도로부터 원심력을 계산하고, 실제 가속도와 원심력의 합력인 이동 성분 가속도를 계산하는 이동 성분 가속도 계산 수단(5)과, 이동 성분 가속도로 가속도를 보정하여 얻어지는 중력 가속도로부터 자세각을 계산하는 자세각 계산 수단(6)으로 이루어지는 것을 특징으로 한다.

자세각 검출 장치, 요 레이트 센서, 가속도 센서, 속도 센서

Description

자세각 검출 장치와 자세각 검출 방법{POSTURE ANGLE DETECTING DEVICE AND POSTURE ANGLE DETECTING METHOD}

본 발명은 이동체의 자세각 검출 장치와 그 방법에 관한 것이다.

차량 등의 이동체 자신의 위치나 속도를 아는 장치(항법 장치)의 일종으로, 지상의 설비의 도움을 필요로 하지 않는 관성 항법 장치(INS)가 있다. 이 INS에 사용되는 관성 센서는 속도의 변화를 검출하는 가속도 센서와 자세의 변화를 검출하는 자이로 센서로 이루어진다. INS는 관성 센서를 사용하여 출발점으로부터의 속도나 자세의 변화를 계산기로 적산해 감으로써, 자신의 위치나 속도를 알 수 있다.

통상, 이동체의 자세 상태를 특정하기 위해, 이동체의 전후(축) 방향을 x(롤)축, 좌우(축) 방향을 y(피치)축, 상하(축) 방향을 z(요)축으로 하는 직교 3축의 좌표계를 사용한다. 이동체가 수평인 지면에 놓였을 때, 이동체의 요축은 지구의 연직축(Z축) 방향으로 대략 향해지고, 롤축 및 피치축은 지구의 국지 수평면(X-Y 평면)에 대략 평행으로 되게 한다. 그리고, 롤축이 국지 수평면 X축으로부터 기울어져 있는 각도를 피치각, 피치축이 국지 수평면 Y축으로부터 기울어져 있는 각도를 롤각, X-Y 평면 상에서 롤축 또는 피치축이 어긋나 있는 각도를 요각의 3각도 로 자세각을 나타낸다. 이 3각도를 측정하기 위해, 3축 자이로 센서가 사용된다.

예를 들어 특허문헌 1의 발명은, 3축 자이로 센서와 가속도 센서의 출력값에 각각 가중을 두어 자세각을 계산하고 있다. 자이로 센서의 출력 결과는 자세각 검출을 위해 적분된다. 그러나, 자이로 센서는 시간의 경과와 함께 오차를 같은 방향으로 출력하는 현상(드리프트)이 있고, 오차를 포함한 값을 적분하면, 오차가 적산된다. 게다가, 자이로 센서는 고가이므로, 저비용으로 기능을 충족시키기 위해서는, 가속도 센서로 중력을 검출하여 자세각을 계산하는 방법이 있다.

특허문헌 2의 발명은, 2개의 가속도 센서로부터의 출력 결과를 기초로 이동체의 자세각을 검출하고 있다. 그러나, 가속도 센서의 출력은 이동체의 이동에 수반하는 가속도를 포함하고 있으므로, 특허문헌 2에 개시되어 있는 발명에서는, 정확한 자세각을 검출하는 것이 어렵다.

특허문헌 1 : 일본 특허 출원 공개 평8-178687호 공보

특허문헌 2 : 일본 특허 출원 공개 제2003-139536호 공보

본 발명은 상기 과제에 비추어 이루어진 것으로, 가속도 센서로부터의 출력값을 보정함으로써 이동체의 이동 중의 자세각을 정밀도 좋게 검출하는 자세각 검출 장치 및 그 방법을 제공하는 것을 목적으로 한다.

본 발명자는, 이동체에 가해지는 가속도로부터만 자세각을 계산한 것에서는, 이동체의 이동 중의 자세각이 정확하게 검출되기 어렵기 때문에, 이동체의 이동에 관한 가속도의 분력(원심력 등)이 이동체의 속도나 각속도로부터 구해지는 것에 착안하여 본 발명을 완성시켰다.

본 발명의 자세각 검출 장치는 이동체에 가해지는 가속도를 측정하는 가속도 센서와, 상기 이동체의 요 레이트를 측정하는 요 레이트 센서와, 상기 이동체의 속도를 측정하는 속도 센서와, 상기 속도를 시간으로 미분하여 실제 가속도를 계산하여 이동 성분 가속도의 전후축 방향의 분력으로 하는 전후축 가속도 계산 수단과, 상기 요 레이트와 상기 속도로부터 원심력을 계산하여 상기 이동 성분 가속도의 좌우축 방향의 분력으로 하는 좌우축 가속도 계산 수단과, 상기 이동체의 상하축 방향에 있어서의 상기 원심력의 분력을 계산하여 상기 이동 성분 가속도의 상하축 방향의 분력으로 하는 상하축 가속도 계산 수단으로 이루어지는 이동 성분 가속도 계산 수단과, 상기 이동 성분 가속도로 상기 가속도를 보정하여 얻어지는 중력 가속도로부터 자세각을 계산하는 자세각 계산 수단을 갖는 것을 특징으로 한다.

본 발명의 자세각 검출 장치에서는, 가속도 센서가 이동체에 가해지는 가속도를 롤축 방향, 피치축 방향, 요축 방향의 3방향에 대해 측정하고, 요 레이트 센서가 이동체의 요축 방향의 회전에 대한 요 레이트(각속도)를 측정한다. 그리고, 이동체의 속도로부터 이동(운동)에 관한 가속도(운동량)을 계산에 의해 구하여 실제 가속도로 한다. 마찬가지로, 이동체에 관한 원심력을 이동체의 속도 및 요 레이트를 사용하여 계산한다. 실제 가속도와 원심력은 이동체의 이동에 의해 발생하는 가속도(이동 성분 가속도)이며, 가속도 센서에서 얻어진 가속도에 포함되어 있다. 그래서, 가속도로부터 이동 성분 가속도를 제거함으로써, 이동체의 이동에 관한 운동량이 제거되어 중력 가속도를 얻을 수 있다. 그리고, 중력 가속도를 이용하여 자세각을 구할 수 있다.
또한, 이동 성분 가속도는 이동체의 이동(운동)에 관한 운동량이며, 이동체의 직교 3축에 대응한 분력을 나타낸다. 따라서, 이동체의 직교 3축마다 구하여 이동체에 관한 가속도로부터 제거함으로써 이동체의 자세각을 검출하므로, 더욱 정확한 중력 가속도를 구할 수 있다.

본 발명의 자세각 검출 장치에서는, 이동체의 직교 3축의 가속도와 이동체의 속도를 검출함으로써, 자이로 센서는 요각을 검출하기 위한 1개를 사용하면, 이동체의 자세각을 구할 수 있다. 그로 인해, 고가의 자이로 센서의 수를 줄일 수 있다. 그러나, 자이로 센서의 개수를 1개에 한정하지 않고, 직교 3축에서 사용하고 또한 가속도와 속도로부터 이동체의 자세각을 검출함으로써, 더욱 정확한 검출 결과를 얻을 수 있다. 또한, 속도 센서 및 가속도 센서의 출력 결과를 그대로 자세각의 검출에 사용하는 것이 아니며, 이동체의 이동에 수반하는 가속도(운동량)를 계산하고, 가속도 센서의 가속도로부터 제거함으로써, 정확한 자세각을 계산할 수 있다.
그리고, 이동체 성분 가속도를 이동체의 직교 3축마다 구하여, 이동체에 관한 가속도로부터 제거함으로써 이동체의 자세각을 검출하므로, 더욱 정확한 중력 가속도를 구할 수 있다.

본 발명의 자세각 검출 장치에서 사용되는 자세각 계산 수단은, 이동 성분 가속도를 가속도로부터 제거한 중력 가속도를 계산하는 중력 가속도 계산 수단과, 중력 가속도로부터 이동체의 롤각을 계산하는 롤각 계산 수단과, 중력 가속도로부터 이동체의 피치각을 계산하는 피치각 계산 수단으로 이루어지는 것이 바람직하다. 이동체의 자세각은 롤각과 피치각으로 나타낸다. 중력 가속도 계산 수단에서는, 각 방향의 중력 가속도를 구할 수 있으므로, 피치축 방향의 중력 가속도로부터 롤각, 롤각축 방향의 중력 가속도로부터 피치각을 계산할 수 있다. 이 중력 분력을 이용하여 이동체의 자세각을 롤각과 피치각에 대해 구할 수 있으므로, 더욱 정확한 자세각을 검출할 수 있다.

삭제

본 발명의 자세각 검출 장치에서 사용되는 상하축 가속도 계산 수단에 있어서의 원심력의 분력의 계산은, 자세각 계산 수단에 의해 계산한 롤각 또는 피치각을 기초로 하여 행하고, 자세각 계산 수단은 계산한 원심력의 분력을 기초로 롤각 및 피치각 중 적어도 한쪽을 계산하는 것이 바람직하다. 상하축 가속도 계산 수단에 있어서, 원심력의 분력의 계산을 롤각 또는 피치각을 기초로 하여 행하고, 원심력의 분력을 피치각 및 롤각의 계산에 반영시킴으로써 이동체의 자세각을 더욱 정확하게 구할 수 있다.

본 발명의 자세각 검출 방법은 이동체에 가해지는 가속도와, 상기 이동체의 요 레이트와, 상기 이동체의 속도를 측정하는 측정 스텝과, 상기 속도를 시간으로 미분하여 실제 가속도를 계산하여 이동 성분 가속도의 전후축 방향의 분력으로 하는 전후축 가속도 계산 스텝과, 상기 요 레이트와 상기 속도로부터 원심력을 계산하여 상기 이동 성분 가속도의 좌우축 방향의 분력으로 하는 좌우축 가속도 계산 스텝과, 상기 이동체의 상하축 방향에 있어서의 상기 원심력의 분력을 계산하여 상기 이동 성분 가속도의 상하축 방향의 분력으로 하는 상하축 가속도 계산 스텝으로 이루어지는 이동 성분 가속도 계산 스텝과, 상기 이동 성분 가속도로 상기 가속도를 보정하여 얻어진 중력 가속도로부터 자세각을 계산하는 자세각 계산 스텝을 갖는 것을 특징으로 한다.

본 발명의 자세각 검출 방법은, 가속도 센서가 이동체에 가해지는 가속도를 롤축 방향, 피치축 방향, 요축 방향의 3방향에 대해 측정하고, 요 레이트 센서가 이동체의 요축 방향의 회전에 대한 요 레이트를 측정한다. 그리고, 이동체의 속도로부터 이동에 관한 가속도를 계산에 의해 구하여 실제 가속도로 한다. 마찬가지로, 이동체에 관한 원심력을 이동체의 속도를 이용하여 계산한다. 실제 가속도와 원심력은 이동체의 이동에 의해 발생하는 가속도(이동 성분 가속도)이며, 가속도 센서에서 얻어진 가속도에 포함되어 있다. 그래서, 가속도로부터 이동 성분 가속도를 제거함으로써, 이동체의 이동에 관한 운동량이 제거되어 중력 가속도를 얻을 수 있다. 그리고, 중력 가속도를 이용하여 자세각을 구할 수 있다.
또한, 이동 성분 가속도는 이동체의 이동(운동)에 관한 운동량이며, 이동체의 직교 3축에 대응한 분력을 나타낸다.

본 발명의 자세각 검출 방법에서는, 이동체의 속도 및 직교 3축의 가속도를 검출함으로써, 이동체의 자세각을 구할 수 있으므로, 요각을 검출하는 자이로 센서가 1개만으로 고가의 자이로 센서의 수를 줄일 수 있다. 또한, 속도 센서 및 가속도 센서의 출력 결과를 그대로 자세각의 검출에 사용하는 것은 아니며, 이동체의 이동에 수반하는 가속도를 계산하고, 가속도 센서의 가속도로부터 제거함으로써, 정확한 자세각을 계산할 수 있다.
그리고, 이동체의 직교 3축마다 구하여 이동체에 관한 가속도로부터 제거함으로써 이동체의 자세각을 검출하므로, 더욱 정확한 중력 가속도를 구할 수 있다.

본 발명의 자세각 검출 방법에서 사용되는 자세각 계산 스텝은, 이동 성분 가속도를 가속도로부터 제거한 중력 가속도를 계산하는 중력 가속도 계산 스텝과, 중력 가속도로부터 이동체의 롤각을 계산하는 롤각 계산 스텝과, 중력 가속도로부터 이동체의 피치각을 계산하는 피치각 계산 스텝으로 이루어지는 것이 바람직하다. 이동체의 자세각은 롤각과 피치각으로 나타낸다. 중력 가속도 계산 스텝에서는 각 방향의 중력 가속도를 구할 수 있으므로, 피치축 방향의 중력 가속도로부터 롤각, 롤각축 방향의 중력 가속도로부터 피치각을 계산할 수 있다. 이 중력 분력을 이용하여 이동체의 자세각을 롤각과 피치각에 대해 구할 수 있으므로, 더욱 정확한 자세각을 검출할 수 있다.

삭제

본 발명의 자세각 검출 방법에서 사용되는 중력 가속도 계산 스텝은, 중력 가속도를 전후축 방향, 좌우축 방향 및 상하축 방향의 분력으로 나누어 계산하고, 롤각 계산 스텝 및 피치각 계산 스텝 중 한쪽에서 계산한 롤각 또는 피치각을 기초로 하여 각 분력 중 일부를 계산하는 스텝이다. 그리고, 롤각 계산 스텝 및 피치각 계산 스텝 중 한쪽은 각 분력 중 잔량부를 기초로 하여 롤각 또는 피치각을 계산하고, 롤각 계산 스텝 및 피치각 계산 스텝 중 다른 쪽은 각 분력 중 일부 내지 전부를 기초로 하여 롤각 또는 피치각을 계산하는 것이 바람직하다. 본 발명의 자세각 검출 방법에서는, 롤각 및 피치각을 계산하기 위해 중력 가속도 계산 스텝에서 계산하는 중력 가속도의 각 축 방향의 분력을, 롤각 또는 피치각을 기초로 하여 계산한다. 이에 의해, 롤각 및 피치각의 계산에 중력 가속도의 분력이 반영되므로, 이동체의 자세각을 더욱 정확하게 구할 수 있다.

본 발명의 자세각 검출 장치 및 그 방법에 따르면, 고가의 자이로 센서를 직교 3축분 사용하는 것이 아닌 1개라도 기능하므로, 저비용으로 이동체의 자세각을 검출할 수 있다. 또한, 자이로 센서의 출력은 각속도(요 레이트)를 이용하여 자이로 센서의 드래프트에 의한 오차를 포함한 값을 계산에 사용하는 일이 없기 때문에, 정확한 자세각을 검출할 수 있다.

그리고, 가속도 센서의 출력값을 그대로 계산에 사용하는 것이 아니라, 이동체의 운동에 관한 가속도를 제거한 중력 가속도를 구하고 있으므로, 이동체가 이동하고 있어도 정확하게 자세각을 검출할 수 있다.

도1은 본 실시예의 자세각 검출 장치의 구성도이다.

도2는 본 실시예의 자세각 검출 장치 및 방법에서 사용되는 절대축과 상대축을 나타내는 도면이다.

도3은 본 실시예의 자세각 검출 방법의 흐름도이다.

도4는 본 실시예의 이동체의 순뱅크 상태를 나타내는 도면이다.

도5는 본 실시예의 이동체의 역뱅크 상태를 나타내는 도면이다.

도6은 본 실시예의 자세각 검출 방법에서 사용되는 뱅크와 선회를 판정하는 흐름도이다.

[부호의 설명]

1 : 자세각 검출 장치

2 : 가속도 센서

3 : 요 레이트 센서

4 : 속도 센서

5 : 이동 성분 가속도 계산 수단

6 : 자세각 계산 수단

7 : 이동체

51 : 전후축 가속도 계산 수단

52 : 좌우축 가속도 계산 수단

53 : 상하축 가속도 계산 수단

61 : 중력 가속도 계산 수단

62 : 롤각 계산 수단

63 : 피치각 계산 수단

71 : 피치각

72 : 롤각

73 : 요각

81 : 이동체(7)의 상하축(요축)

82 : 이동체(7)의 전후축(롤축)

83 : 이동체(7)의 좌우축(피치축)

91 : 지구의 연직축(Z축)

92 : 국지 수평면 X축

93 : 국지 수평면 Y축

94 : 지구의 수평면(X-Y 평면)

이하, 실시예를 이용하여 본 발명을 구체적으로 설명한다.

도1은 본 실시예의 자세각 검출 장치의 구성도이다.

자세각 검출 장치(1)는 가속도 센서(2)와, 요 레이트 센서(3)와, 속도 센서(4)와, 이동 성분 가속도 계산 수단(5), 자세각 계산 수단(6)으로 이루어진다. 이동 성분 가속도 계산 수단(5), 자세각 계산 수단(6)은 컴퓨터 상의 로직으로서 실현된다.

자세각 검출 장치(1)가 설치되는 이동체(7)는, 도2에 도시된 바와 같이 이동체(7)를 수평인 지면에 두었을 때에, 이동체(7)의 상하축(요축)(81)이 지구의 연직축(Z축)(91) 방향으로 대략 향해지고, 이동체(7)의 전후축(롤축)(82) 및 좌우축(피치축)(83)이 지구의 수평면(X-Y 평면)(94)에 대략 평행이 되게 한다. 이동체(7)의 롤축(82)이 국지 수평면 X축(92)으로부터 기울어져 있는 각도를 피치각(71), 피치축(83)이 국지 수평면 Y축(93)으로부터 기울어져 있는 각도를 롤각(72), X-Y 평면 상에서 롤축(82) 또는 피치축(83)이 어긋나 있는 각도를 요각(73)으로 하여, 이동체(7)의 자세각을 3각도로 나타낸다.

가속도 센서(2)는 롤축(82), 피치축(83) 및 요축(81)의 직교하는 3축의 가속도를 각각 측정하는 Ax(21), Ay(22), Az(23)으로 이루어진다. 가속도 센서(2)는 이동체의 가속도를 측정하고, 이동 성분 가속도 계산 수단(5) 및 자세각 계산 수단(6)에 출력한다. 가속도 센서(2)는 직교 3축의 롤축(82)이 이동체(7)의 전후 방향, 피치축(83)이 이동체(7)의 좌우 방향, 요축(81)이 이동체(7)의 상하 방향에 일치하도록 설치되고, 이동체(7)의 회전과 동시에 회전한다.

요 레이트 센서(3)는 이동체(7)의 상하축인 요축(81)의 각속도(요 레이트)를 측정하여 이동 성분 가속도 계산 수단(5)에 출력하는 자이로 센서이다. 속도 센서(4)는 이동체(7)의 속도를 측정하여 이동 성분 가속도 계산 수단(5)에 출력하는 수단이다. 이동체가 차량인 경우에는 차륜 속도를 측정한다.

이동 성분 가속도 계산 수단(5)은 요 레이트 센서(3) 및 속도 센서(4)로부터 입력된 요 레이트 및 속도를 기초로, 이동체(7)의 이동에 관한 가속도를 직교 3축의 축마다 계산하는 수단이며, 전후축 가속도 계산 수단(51)과, 좌우축 가속도 계산 수단(52)과, 상하축 가속도 계산 수단(53)으로 이루어진다. 이동 성분 가속도 계산 수단(5)은 계산 결과를 자세각 계산 수단(6)에 출력한다.

전후축 가속도 계산 수단(51)은 이동체(7)의 이동에 관한 전후 방향의 가속도를 계산하는 수단이다. 좌우축 가속도 계산 수단(52)은 이동체(7)의 이동에 관한 좌우 방향의 가속도를 계산하는 수단이다. 상하축 가속도 계산 수단(53)은 이동체(7)의 이동에 관한 상하 방향의 가속도를 계산하는 수단이다.

자세각 계산 수단(6)은 중력 가속도 계산 수단(61)과 롤각 계산 수단(62)과 피치각 계산 수단(63)으로 이루어진다. 중력 가속도 계산 수단(61)은 가속도 센서(2)로부터 입력되는 가속도로부터 이동 성분 가속도 계산 수단(5)으로부터 입력되는 이동 성분 가속도를 사용하여 보정한 중력 가속도를 계산하는 수단이다. 롤각 계산 수단(62)은, 중력 가속도 계산 수단(61)으로부터 입력되는 중력 가속도를 기초로 롤각(72)을 계산하는 수단이다. 피치각 계산 수단(63)은, 중력 가속도 계산 수단(61)으로부터 입력되는 중력 가속도를 기초로 피치각(73)을 계산하는 수단이다.

본 실시예의 자세각 검출 장치(1)에서 사용되는 자세각 검출 방법(이하, 자세각 검출 루틴)을 흐름도로 나타낸 것이 도3이다. 본 루틴은 자세각 검출 장치(1)가 기동 후, 소정 기간 중에 소정 시간마다 실행되는 것이다.

본 루틴의 처리 개시 후, 가속도 센서(2)가 이동체(7)에 가해지는 가속도를 측정하고, 요 레이트 센서(3)가 요 레이트(ω)를 측정하고, 속도 센서(4)가 속도(Vw)를 측정하여, 이동 성분 가속도 계산 수단(5) 및 자세각 계산 수단(6)으로 출력한다(측정 스텝 S1). 가속도 센서(2)는, Ax(21)에서 롤축 방향의 가속도(Ax_sensor)가 측정되고, Ay(22)에서 피치축 방향의 가속도(Ay_sensor)가 측정되고, Az(23)에서 요축 방향의 가속도(Az_sensor)가 측정된다.

다음에, 이동 성분 가속도 계산 수단(5)은 요 레이트 및 속도로부터 이동체(7)에 관한 이동(운동)에 관한 가속도(이동 성분 가속도)의 계산을 행한다(이동 성분 가속도 계산 스텝).

그리고, 자세각 계산 수단(6)은 가속도 센서(2)에서 측정한 가속도를 이동 성분 가속도 계산 수단(5)으로부터 입력된 이동 성분 가속도로 보정하여 구한 중력 가속도로, 롤각(Roll)(72), 피치각(Pitch)(71)의 계산을 행한다(자세각 계산 스텝).

이동 성분 가속도 계산 수단(5)은 전후축 가속도 계산 수단(51)이 속도(Vw)를 시간으로 미분하여 실제 가속도를 계산하고, 이동 성분 가속도의 전후 방향의 분력(Ax_cor)으로 하는 전후축 가속도 계산 스텝 S21을 갖는다. 그리고, 좌우축 가속도 계산 수단(52)이 요 레이트(ω)와 속도(Vw)로부터 원심력을 계산하고, 이동 성분 가속도의 좌우 방향의 분력(Ay_cor)으로 하는 좌우축 가속도 계산 스텝 S22를 갖고, 상하축 가속도 계산 수단(53)이 이동체(7)의 상하 방향에 있어서의 원심력의 분력을 계산하여 이동 성분 가속도의 상하 방향의 분력(Az_cor)으로 하는 상하축 가속도 계산 스텝 S23을 갖는다.

원심력(Ay_cor)은,

로 구할 수 있다.

이동 성분 가속도의 상하 방향의 분력(Az_cor)은 자세각 계산 수단(6)의 자세각 계산 스텝에 의해 계산되는 롤(Roll)을 기초로 계산된다.

자세각 계산 수단(6)은 중력 가속도 계산 수단(61)이 가속도를 이동 성분 가속도로 보정한 중력 가속도를 계산하는 중력 가속도 계산 스텝 S31과, 중력 가속도로부터 이동체(7)의 롤(Roll)을 계산하는 롤각 계산 스텝 S32와, 중력 가속도로부 터 이동체(7)의 피치(Pitch)를 계산하는 피치각 계산 스텝 S33이 있다.

S32에 있어서, 이동체(7)가 기울어지는 방향에 의해, Ay_sensor은, 도4에 도시되는 순뱅크와 도5에 도시되는 역뱅크에서 다른 수식으로 계산한다.

순뱅크의 경우 :

g = 그 지역에서의 중력 가속도

역뱅크의 경우 :

g = 그 지역에서의 중력 가속도

수학식2 및 수학식3의 gㆍsin(Roll)이 이동 성분 가속도의 좌우 방향의 분력이다. 수학식2 및 수학식3을 좌우 방향의 분력을 구하도록 이항한 경우, 이동체(7)가 어느 쪽으로 기울어져 있는지로, 측정된 Ay_sensor에 원심력이 가해지고 있는지 빠지고 있는 것인지 다르게 된다. 그래서, 도6에 도시된 바와 같이, Ay_sensor의 절대치와 원심력의 절대치를 비교하여, 원심력이 크면 순뱅크이고, 원심력이 작으면 역뱅크이다. 순뱅크일 때에 1을, 역뱅크일 때에 -1을 뱅크 플래그(B_flag)의 값으로 한다. 또한, 이동체(7)의 선회에 대해서도 좌선회와 우선회에서 Ay_sensor과 원심력의 힘이 작용하는 방향이 다르므로, 도6에 도시된 바와 같이 ω에 의해 판단하고, 우선회일 때에 1을, 좌선회일 때에 -1을 방향 플래그(RL_flag)의 값으로 한다. g의 중력 가속도는 정수이므로, 좌우 방향의 분력을 구할 수 있으면, 롤(Roll)을 계산할 수 있다.

다음에, S33에 있어서, 피치(Pitch)가 가속도(Ax_sensor, Ay_sensor, Az_sensor)를 이동 성분 가속도의 직교 3축의 각 분력(Ax_cor, Ay_cor, Az_cor)에 의해 보정한 중력 가속도로부터 계산된다.

이동 성분 가속도의 직교 3축의 각 분력(Ax_cor, Ay_cor, Az_cor)은 이동 성분 가속도 계산 수단(5)의 스텝 S21, 스텝 S22, 스텝 S23에서 구해지고 있다.

본 실시예의 자세각 검출 장치(1) 및 그 방법에 따르면, 가속도 센서와 속도 센서를 사용함으로써 고가의 자이로 센서를 직교 3축 모두에 사용하지 않고, 저비용으로 이동체의 자세각을 검출할 수 있다. 또한, 자이로 센서의 드래프트에 수반하는 오차를 포함한 값을 계산에 사용하는 일도 없기 때문에, 정확한 자세각을 검출할 수 있다.

그리고, 가속도 센서의 출력값을 그대로 계산에 사용하는 것은 아니며, 이동체의 운동에 관한 가속도를 제거한 후에 중력 가속도를 구하고 있으므로, 이동체가 이동하고 있어도 정확하게 자세각을 검출할 수 있다.

이상, 본 발명의 적합한 실시예에 대해 설명하였지만, 본 발명은 상기 실시예에 한정되어 있는 것은 아니다. 예를 들어, 이동체의 속도는 이동체가 차륜을 갖는 자동차 등이면 차륜속을 측정하고, 피치(Pitch)를 구할 때는, 이동 성분 가속도의 상하 방향의 분력을 구하는 방법에 대해 피치각과 속도에 의해 계산한 이동체의 가속도를 추가함으로써, 보다 고정밀도의 자세각을 검출할 수 있다. 또한, 이동체가 차륜을 갖지 않는 항공기 등이면, 이동체의 속도로서 프로펠러의 회전 속도를 측정하는 것을 생각할 수 있다.

본 발명의 자세각 검출 장치와 자세각 검출 방법은 공업, 예를 들어 자동차 산업, 항공기 산업 등의 분야에 사용할 수 있다.

Claims

이동체에 가해지는 가속도를 측정하는 가속도 센서와,

상기 이동체의 요 레이트를 측정하는 요 레이트 센서와,

상기 이동체의 속도를 측정하는 속도 센서와,

상기 속도를 시간으로 미분하여 실제 가속도를 계산하여 이동 성분 가속도의 전후축 방향의 분력으로 구하는 전후축 가속도 계산 수단과, 상기 요 레이트와 상기 속도로부터 원심력을 계산하여 상기 이동 성분 가속도의 좌우축 방향의 분력으로 구하는 좌우축 가속도 계산 수단과, 상기 이동체의 상하축 방향에 있어서의 상기 원심력의 분력을 계산하여 상기 이동 성분 가속도의 상하축 방향의 분력으로 구하는 상하축 가속도 계산 수단으로 이루어지는 이동 성분 가속도 계산 수단과,

상기 가속도로부터 상기 이동 성분 가속도를 제거하여 얻어지는 중력 가속도로부터 자세각을 계산하는 자세각 계산 수단으로 이루어지는 것을 특징으로 하는, 자세각 검출 장치.
제1항에 있어서, 상기 자세각 계산 수단은, 상기 이동 성분 가속도를 상기 가속도로부터 제거한 상기 중력 가속도를 계산하는 중력 가속도 계산 수단과, 상기 중력 가속도로부터 상기 이동체의 롤각을 계산하는 롤각 계산 수단과, 상기 중력 가속도로부터 상기 이동체의 피치각을 계산하는 피치각 계산 수단으로 이루어지는, 자세각 검출 장치.
삭제
제2항에 있어서, 상기 상하축 가속도 계산 수단에 있어서의 상기 원심력의 분력의 계산은, 상기 자세각 계산 수단에 의해 계산한 상기 롤각을 기초로 하여 행하고,

상기 자세각 계산 수단은, 계산한 상기 원심력의 분력을 기초로 상기 롤각을 계산하는, 자세각 검출 장치.
이동체에 가해지는 가속도와, 상기 이동체의 요 레이트와, 상기 이동체의 속도를 측정하는 측정 스텝과,

상기 속도를 시간으로 미분하여 실제 가속도를 계산하여 이동 성분 가속도의 전후축 방향의 분력으로 구하는 전후축 가속도 계산 스텝과, 상기 요 레이트와 상기 속도로부터 원심력을 계산하여 상기 이동 성분 가속도의 좌우축 방향의 분력으로 구하는 좌우축 가속도 계산 스텝과, 상기 이동체의 상하축 방향에 있어서의 상기 원심력의 분력을 계산하여 상기 이동 성분 가속도의 상하축 방향의 분력으로 구하는 상하축 가속도 계산 스텝으로 이루어지는 이동 성분 가속도 계산 스텝과,

상기 가속도로부터 상기 이동 성분 가속도를 제거하여 얻어진 중력 가속도로부터 자세각을 계산하는 자세각 계산 스텝을 갖는 것을 특징으로 하는, 자세각 검출 방법.
제5항에 있어서, 상기 자세각 계산 스텝은, 상기 이동 성분 가속도를 상기 가속도로부터 제거한 상기 중력 가속도를 계산하는 중력 가속도 계산 스텝과, 상기 중력 가속도로부터 상기 이동체의 롤각을 계산하는 롤각 계산 스텝과, 상기 중력 가속도로부터 상기 이동체의 피치각을 계산하는 피치각 계산 스텝으로 이루어지는, 자세각 검출 방법.
삭제
제6항에 있어서, 상기 상하축 가속도 계산 스텝에 있어서의 상기 원심력의 분력의 계산은, 상기 자세각 계산 스텝에 의해 계산한 상기 롤각을 기초로 하여 행하고,

상기 자세각 계산 스텝은, 계산한 상기 원심력의 분력을 기초로 상기 롤각을 계산하는, 자세각 검출 방법.