1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
| import numpy as np
import math
try:
from objmodel import calcWLWR, observe_ROBOT_pars
except:
print("unable to import from objmodel")
exit()
print("--> good import from objmodel !")
# -------------------------------------------------------------
# здесь располагаются вспомогательные функции
# -------------------------------------------------------------
# Нахождение длины вектора r
def sqnorm(r):
return np.sqrt(sum(r*r))
# корректировка угла fi: вычисление нормированного угла -pi < fi < pi
def normAngle(fi):
while (abs(fi) > np.pi):
fi -= 2 * np.pi * np.sign(fi)
return fi
# оценка ошибки по углу: нормированного угла между углом fi и директивным направлением (rg-r)
def epsfi(R, Rg, fi):
x, y = R[0], R[1]
xg, yg = Rg[0], Rg[1]
return normAngle(math.atan2((yg - y), (xg - x)) - fi)
# расчет ошибки по расположению робота: расстояние между текущим положением робота R и целевой точкой Rg
def epsR(R, Rg):
return sqnorm(R-Rg)
# ...
# -------------------------------------------------------------
# --- Эту функцию программирует обучающийся!!! ----
# -------------------------------------------------------------
# функция расчета параметров управления траекторией движения робота
# Вход:
# ROBOT_state - трассировка состояний робота; последнее состояние rcur = ROBOT_state[-1]
# rcur - список координат робота в текущий момент времени - numpy array размером (4,)
# содержит текущие координаты робота t,x,y,fi
# GOAL_state - трассировка состояний цели; последнее состояние rg= ROBOT_state[-1]
# rg - список координат цели в текущий момент времени - numpy array размером (4,)
# содержит текущие координаты цели t,xg,yg,fig
# dt - шаг дискретизации времени
# Выход:
# пара значений параметров управленя wL, wR
def set_NEW_REGUL(ROBOT_state, GOAL_state, dt):
Pars = observe_ROBOT_pars()
ROBOT_L = Pars['ROBOT_L']
ROBOT_R = Pars['ROBOT_R']
# Wmin, Wmax - ограничения по угловой скорости колес робота
Wmin = Pars['ROBOT_Wmin']
Wmax = Pars['ROBOT_Wmax']
Wmax = 10000000
# считываем параметры цели (зайца)
curGoalState = GOAL_state[-1] # текущее состояние цели
xg = curGoalState[1] # - координата по x текущего расположения цели
yg = curGoalState[2] # - координата по y текущего расположения цели
fig = curGoalState[3] # - угол напраления движения цели
# определяем текущее местоположение робота:
rcur = ROBOT_state[-1]
x = rcur[1] # - координата по x текущего расположения робота
y = rcur[2] # - координата по y текущего расположения робота
fi = rcur[3] # - угол напраления движения робота
# задаем вычисление кортежа параметров
# далее следует код обучающегося ....
kPv=1
kPw=1000
r=rcur[1:3]
rg=curGoalState[1:3]
newFi = normAngle(fi)
ef=epsfi(r,rg,fi)
er=epsR(r,rg)
vC=kPv*er
wC=kPw*ef
return calcWLWR(vC,wC) |
(
Сообщение от Ayart
