roboforum.ru

Казалось бы, зачем нам тригонометрия? Мы ведь не в школе. Да еще сразу арктангенс. Зачем он нужен? А нужен от для того, чтобы определять угол наклона акселерометра.
Вот представьте, у вас двухколесный робот-балансировщик. Пусть ось перед-зад будет осью Y, а ось верх-низ будет осью Z. Акселерометр в состоянии покоя выдает проекции вектора ускорения свободного падения на ортогональный базис. Стоит робот ровно – ось Y параллельна полу, ось Z – перпендикулярна. Но долго так не простоишь, неизбежно начнется падение. Робот наклонится, оси тоже, проекции изменятся. Так вот, отношение данных от оси Y к данным от оси Z есть тангенс угла наклона, где угол выражен в радианах. Хотим знать угол – надо вычислить арктангенс, а хотим в градусах – надо радианы переводить в градусы.
Для вычисления арктангенса есть очень хорошая формула:
Atan(x) = x – (x*x*x/3) + (x*x*x*x*x/5) – (x*x*x*x*x*x*x/7) + (x*x*x*x*x*x*x*x*x/9) и так далее.
То есть меняем знак через шаг, степень и делитель всегда нечетные. Обратите внимание, что Х не превышает единицы, и единицей он станет, когда робот отклонится от вертикали на угол ± 45°. Поэтому от этой формулы легко можно оторвать хвост. Приемлемая точность получается при двух членах ряда.
Перед тем, как перейти к описанию алгоритма, я еще раз напомню: я не люблю работать с дробными числами и знаковыми умножениями. И я хочу получить хорошую точность вычислений, три знака после запятой. Мне кажется, что алгоритм лучше будет понят на примере. Я приведу два примера, для положительного и отрицательного угла.
Пример 1.
Данные от оси Y = FF70, данные от оси Z = 40C3. Для начала посчитаем на калькуляторе, чтобы после сразу проверить алгоритм.
FF70 = –90 = -144; 40C3 = 16579; atan(-144/16579) = -0.498°
1 Берем модуль числителя, знак запоминаем.
2 Работаем с хвостом (х*х*х/3), дополнительно умножим обе части дроби на 0х1000000.
Делаем это следующим образом:
(((((90 00 00 00/40С3)*90)/40С3)*90)/40С3)/3 = 3/1000000
3 Теперь работаем с головой формулы
90 00 00 00/40С3 = 23939/1000000
4 Теперь вспоминаем про знаки, у нас знак был отрицательный, поэтому хвост будем прибавлять, а вот знак головы пока трогать не будем (раз мы пошли по этой ветке, то мы и так знаем, куда робот наклонился)
23939/1000000 + 3/1000000 = 23942/1000000
5 Теперь радианы надо перевести в градусы. Для этого радианы умножаем на 360 и делим на 2PI. PI – нехорошее для целочисленной математики число, поэтому я умножу его на 0х1000000 и сразу же разделю. А градусы я хочу знать три знака после запятой, поэтому умножать буду не на 360, а на 360000 (57E40). PI = 3.14159265358979*0x1000000/0x1000000. PI = 3243F6B/1000000. Действуем:
23942 57E40*1000000
––––––– * --––-––––––––––– = 1F1 (497)
1000000 2*3243F6B
6 Тут мы снова вспоминаем про знак, поэтому угол получается -0.497°.

Пример 2 для положительного угла.
Данные от оси Y = 1070, данные от оси Z = 3EDB. Для начала посчитаем на калькуляторе, чтобы после сразу проверить алгоритм.
1070 = 4208; 3EDB = 16091; atan(4208/16091) = 14.655°
1 Берем модуль числителя, знак запоминаем.
2 Работаем с хвостом (х*х*х/3), дополнительно умножим обе части дроби на 0х1000000.
Делаем это следующим образом:
(((((1070 00 00 00/3EDB)*1070)/3EDB)*1070)/3EDB)/3 = 186B1/1000000
3 Теперь работаем с головой формулы
1070 00 00 00/3EDB = 42F27E/1000000
4 Теперь вспоминаем про знаки, у нас знак был положительный, поэтому хвост будем отнимать, а вот знак головы пока трогать не будем (раз мы пошли по этой ветке, то мы и так знаем, куда робот наклонился)
42F27E/1000000 – 186B1/1000000 = 416BCD/1000000
5 Теперь радианы надо перевести в градусы. Для этого радианы умножаем на 360 и делим на 2PI. PI – нехорошее для целочисленной математики число, поэтому я умножу его на 0х1000000 и сразу же разделю. А градусы я хочу знать три знака после запятой, поэтому умножать буду не на 360, а на 360000 (57E40). PI = 3.14159265358979*0x1000000/0x1000000. PI = 3243F6B/1000000. Действуем:
416BCD 57E40*1000000
––––––– * –––––––––––––--- = 3932 (14642)
1000000 2*3243F6B
6 Тут мы снова вспоминаем про знак, поэтому угол получается +14.642°.

На мой взгляд, погрешность приемлемая. Подозреваю, что при больших углах погрешность будет также больше, но, если мы попадаем в такие углы – значит мы падаем, и проблема точности отходит на десятое место. Я пока не считал, сколько времени занимает расчет (я еще не написал функцию), может быть потом добавлю. Но уверен, что явно быстрее, чем на математике с плавающей точкой (и места в памяти меньше занимается).

Скорость вычисления получилась 785 мкс при кварце 7.3728 МГц. В процедурах многобайтного умножения и деления не использовался умножитель, все программно. Эти процедуры подсмотрены у ELM-CHANа.