GPS навигатор

## Описание

Система автоматического управления транспортного средства по введенным координатам. Управление рулем и дроссельной заслонкой двигателя.

## Механическая схема

Для определения положения транспортного средства в пространстве нам понадобится два GPS датчика. Первый на носу, второй на корме, расположенные на одной прямой. Они не обязательно должны быть далеко друг от друга, точность от этого не пострадает.

## Принципиальная схема работы

### **Схема поворота**

Мы имеем три точки в пространстве со своими координатами.

* Первая точка

(

x
1

,

y
1

)

{\displaystyle (x\_1,y\_1) }

 - координаты кормы судна
* Вторая точка

(

x
2

,

y
2

)

{\displaystyle (x\_2,y\_2) }

 - координаты носа судна
* Третья точка

(

x
3

,

y
3

)

{\displaystyle (x\_3,y\_3) }

 - координаты пункта назначения

Первым действием найдем два нужных нам вектора. Для этого вычтем из координат второй точки, координаты первой точки.

v
1

=
{

x
2

−

x
1

,

y
2

−

y
1

}

{\displaystyle v\_1=\{x\_2-x\_1, y\_2-y\_1\}
}

v
2

=
{

x
3

−

x
1

,

y
3

−

y
1

}

{\displaystyle v\_2=\{x\_3-x\_1, y\_3-y\_1\}
}

И найдем угол между ними. В общем случае:

cos
⁡
# θ

x

1

x

2

y

1

y

2

x

1

2

y

1

2

⋅

x

2

2

y

2

2

{\displaystyle \cos\theta=\frac{x\_{1}x\_{2}+y\_{1}y\_{2}}{\sqrt{x\_{1}^{2}+y\_{1}^{2}}\cdot\sqrt{x\_{2}^{2}+y\_{2}^{2}}}}

В нашем случае формула будет выглядеть так:

cos
⁡
# θ

v

x
1

v

x
2

v

y
1

v

y
2

l
e
n

(

v

x
1

,

v

y
1

)

⋅
l
e
n

(

v

x
2

,

v

y
2

)

{\displaystyle \cos\theta=\frac{v\_{x1}v\_{x2}+v\_{y1}v\_{y2}}{len\left(v\_{x1},\ v\_{y1}\right)\cdot len\left(v\_{x2},\ v\_{y2}\right)}}

И на этом было бы всё, но

cos
⁡
# 0
1

{\displaystyle \cos0=1}

 , что нам совершенно не удобно, так как рули принимают значение от -1 до 1, где 0 это прямо. Поэтому найдем

sin
⁡
θ

{\displaystyle \sin \theta }

 с помощью основного тригонометрического тождества:

sin
2

⁡
θ

cos
2

⁡
# θ
1

{\displaystyle \sin^2 \theta + \cos^2 \theta = 1}

sin
⁡
# θ

1
−

cos

2

⁡
θ

{\displaystyle \sin\theta=\sqrt{1-\cos^{2}\theta}}

Также нам нужно определить с какой стороны от транспортного средства находится точка в которую нам нужно попасть - слева или справа (- или +):

# D

(

x

3

−

x

1

)

(

y

2

−

y

1

)

(

y

3

−

y

1

)

(

x

2

−

x

1

)

{\displaystyle D=\left(x\_{3}-x\_{1}\right)\left(y\_{2}-y\_{1}\right)+\left(y\_{3}-y\_{1}\right)\left(x\_{2}-x\_{1}\right)}

D будет принимать положительные значения если точка находится с правой стороны и отрицательные если с левой. Нам нужен только знак. Итого на рули будем подавать значение равное:

# P

D

D

sin
⁡
θ

{\displaystyle P=\frac{D}{\left|D\right|}\sin\theta}

### **Схема движения**

С движением все еще проще. Вычисляем по координатам расстояние от носа корабля до исходной точки:

# S

(

x

3

−

x

2

)

2

(

y

3

−

y

2

)

2

{\displaystyle S=\sqrt{\left(x\_{3}-x\_{2}\right)^{2}+\left(y\_{3}-y\_{2}\right)^{2}}}

Далее мы передаем это число на дроссельную заслонку. Но мы встаём перед проблемой - так как дроссельная заслонка принимает значение только от 0 до 1, то мотор начнет сбавлять обороты только за 1 метр (условную единицу координат). И это слишком малое расстояние, двигатель не успеет затормозить судно, оно проедет условную точку и начинёт кружить вокруг неё.

Чтобы сгладить этот эффект начнем тормозить несколько раньше (насколько раньше зависит от того насколько у вас быстрое и инертное судно), для этого введём коэффициент k.

# S

S
k

{\displaystyle S=\frac{S}{k}}

, и вот уже это значение и будем передавать на двигатель.

## Логическая схема

```

```