Системы технологического управления

Визуализация пути машиныНа основании математической модели объекта, которая была выведена ранее, а также на основании полученные данных о параметрах объекта на всём пути движения, можно построить двумерную модель движения машины. Для этого определим функцию, строящую модель машины в каждый момент времени в зависимости от параметровdraw_car(x=0, y=0, a=0, phi=0), где входными данными являются текущие координаты оси (x,y), угол поворота оси машины, относительно горизонтали – а и угол поворота колёс, относительно, оси машины.ВначалевыясняемR=np.array([[np.cos(a),-np.sin(a)],[np.sin(a),np.cos(a)]])Далееcar=np.array([[0.2,0.5],[-0.2,0.5],[0,0.5],[0,-0.5],[0.2,-0.5],[-0.2,-0.5],[0,-0.5],[0,0],[L,0],[L,0.5],[L+0.2*np.cos(phi),0.5+0.2*np.sin(phi)],[L-0.2*np.cos(phi),0.5-0.2*np.sin(phi)],[L,0.5],[L,-0.5],[L+0.2*np.cos(phi),-0.5+0.2*np.sin(phi)],[L-0.2*np.cos(phi),-0.5-0.2*np.sin(phi)]])Затемcarz=scl*R.dot(car.T)Истроимplt.plot(x+carz[0],y+carz[1],'k',lw=2)plt.plot(x,y,'k.',ms=10)Теперь когда имеется функция построения модели машины, построим её в каждый момент времени из полученного решения оптимизационной задачи.Зададим параметры полотна:plt.figure(figsize=(10,10))Для каждого момента времени, на основании полученных параметров из решенной модели строим модельку машины:forxs,ys,ts,psinzip(x,y,a,phi):draw_car(xs,ys,ts,scl*ps)Строим траекторию движения:plt.plot(x,y,'r--',lw=0.8)Определяем параметры полотна:plt.axis('square')plt.grid(True)Рисунок 3. Путь автомобиля.Масштабирование для включения времени в модель оптимизацииМатематическая модельВ предыдущей постановке задачи оптимизации, была проведена оптимизация лишь физических параметров системы, в то время как временная область была не затронута. В итоге, с точки зрения оптимизации всей системы, решение данного вопроса является неккоректным. Добавим время пути машины, в оптимизируемый параметр и посмотрим что получится.Следующие уравнения описывают простую модель машины:(6.1)(6.2)(6.3)(6.4)Накладываем ограничения на манипулируемые переменные:(6.5)(6.6)(6.7)(6.8)Тогда функциональный объект:(6.9)Введём обычные весовые функции:(6.10)(6.11)(6.12)(6.13)Тогда получаем:(6.14)(6.15)(6.16)(6.17)Добавляем ограничения:(6.18)(6.19)(6.20)(6.21)В перемасштабируемых функциях получаем, оптимизационный функционал:(6.22),где дополнительный членбыл включен, чтобы учесть компромисс между приложенным усилием ускорения и поворота и временем, необходимым для завершения маневра.Создание модели в PyomoПо аналогии с 3, задаётся модель в решатель.Задание ограничений:ar_max=2.8av_max=2.8phi_max=0.7v_max=30v_min=-4Заданиешкалыдлины:L=5Создание модели:m=ConcreteModel()Определение переменных времени:m.tf=Var(domain=NonNegativeReals)m.t=ContinuousSet(bounds=(0,1))Определение контрольных точек входа:m.av=Var(m.t)m.phi=Var(m.t,bounds=(-phi_max,phi_max))Определение независимых переменных положения:m.x=Var(m.t)m.y=Var(m.t)m.a=Var(m.t)m.v=Var(m.t)Определение производных:m.x_dot=DerivativeVar(m.x)m.y_dot=DerivativeVar(m.y)m.a_dot=DerivativeVar(m.a)m.v_dot=DerivativeVar(m.v)Заданиедифференциальныхуравений:m.ode_x=Constraint(m.t,rule=lambdam,t:m.x_dot[t]==m.v[t]*cos(m.a[t]))m.ode_y=Constraint(m.t,rule=lambdam,t:m.y_dot[t]==m.v[t]*sin(m.a[t]))m.ode_a=Constraint(m.t,rule=lambdam,t:m.a_dot[t]==m.v[t]*tan(m.phi[t])/L)m.ode_v=Constraint(m.t,rule=lambdam,t:m.v_dot[t]==m.av[t])Определениепути:m.path_x1=Constraint(m.t,rule=lambdam,t:m.x[t]>=0)m.path_y1=Constraint(m.t,rule=lambdam,t:m.y[t]>=0)m.path_v1=Constraint(m.t,rule=lambdam,t:m.v[t]<=m.tf*v_max/L)m.path_v2=Constraint(m.t,rule=lambdam,t:m.v[t]>=m.tf*v_min/L)m.path_a1=Constraint(m.t,rule=lambdam,t:m.av[t]<=m.tf**2*av_max/L)m.path_a2=Constraint(m.t,rule=lambdam,t:m.av[t]>=-m.tf**2*av_max/L)m.path_a3=Constraint(m.t,rule=lambdam,t:m.v[t]**2*sin(m.phi[t])<=m.tf**2*ar_max/L)m.path_a4=Constraint(m.t,rule=lambdam,t:m.v[t]**2*sin(m.phi[t])>=-m.tf**2*ar_max/L)Задание начальных условий:m.pc=ConstraintList()m.pc.add(m.x[0]==0)m.pc.add(m.y[0]==0)m.pc.add(m.a[0]==0)m.pc.add(m.v[0]==0)Задание конечных условий:m.pc.add(m.x[1]==0)m.pc.add(m.y[1]==4)m.pc.add(m.a[1]==0)m.pc.add(m.v[1]==0)Определение конечных условий контрольных сигналов:m.pc.add(m.av[1]==0)m.pc.add(m.phi[1]==0)Определение оптимизируемого функционала:m.integral=Integral(m.t,wrt=m.t,rule=lambdam,t:m.av[t]**2+(m.v[t]**2*sin(m.phi[t]))**2)m.obj=Objective(expr=m.tf+L**2*m.integral/m.tf**3)Определение численного метода интегрирования и решение в дискретной форме:TransformationFactory('dae.finite_difference').apply_to(m,wrt=m.t,nfe=30)SolverFactory('ipopt').solve(m).write()Результатработы:# ==========================================================# = Solver Results =# ==========================================================# ----------------------------------------------------------# Problem Information# ----------------------------------------------------------Problem: - Lower bound: -inf Upper bound: inf Number of objectives: 1 Number of constraints: 502 Number of variables: 311 Sense: unknown# ----------------------------------------------------------# Solver Information# ----------------------------------------------------------Solver: - Status: ok Message: Ipopt 3.14.4\x3a Optimal Solution Found Termination condition: optimal Id: 0 Error rc: 0 Time: 1.1337249279022217# ----------------------------------------------------------# Solution Information# ----------------------------------------------------------Solution: - number of solutions: 0number of solutions displayed: 0Обработка резульатовt=np.array([t*m.tf()fortinm.t])av=np.array([m.av[t]()*L/(m.tf()**2)fortinm.t])phi=np.array([m.phi[t]()fortinm.t])x=np.array([m.x[t]()*Lfortinm.t])y=np.array([m.y[t]()*Lfortinm.t])a=np.array([m.a[t]()fortinm.t])v=np.array([m.v[t]()*L/m.tf()fortinm.t])ar=v**2*np.sin(phi)/Lplot_results(t,x,y,a,v,av,phi)Рисунок 4. Полученные значения функций параметров системы во времени.Визуализация пути автомобиляscl=0.2plt.figure(figsize=(10,10))forxs,ys,ts,psinzip(x,y,a,phi):draw_car(xs,ys,ts,scl*ps)plt.plot(x,y,'r--',lw=0.8)plt.axis('square')plt.grid(True)Рисунок 5. Путьавтомобиля.Исполнительный органДанная программа может быть запущена как на ОС самого автомобиля, так и на ОС самого шлема. Однако всё же более разумным будет использование ОС самого автомобиля, поскольку это облегчит конструкцию СУ самого шлема, поскольку таким образом шлем будет выполнять роль лишь транслятора данных человеку и ОС на сам шлем можно будет не ставить и гнать данные на него через PCIe, который допустим во второй версии может обеспечить пропускную способность данных до 12.5 Гбит/c. В то же время ОС автомобиля как правило достаточно мощная и её проще встроить в текущую систему автомобиля. Более того, можно даже запустить проект в таком протопированном виде прямо на продемонстрированном коде на Pythonна ОС автомобиля, при наличии на ОС python-транслятора, а поскольку это как правило линукс, там его добавить будет очень просто и быстро.ЗАДЕРЖАНИЕВ данной работе изучался код, представленный в [1]. В ходе исследования был изучен принцип построения математической модели движения автомобиля, на основании чего была выведена постановка задачи оптимизации. В дальнейшем эта задача была решена через библиотеку для решения оптимизационных задач Pyomo. На основе результатов, полученных при масштабируемой оптимизации по времени и оптимизации без временного параметра, траектория движения автомобиля была получена путем адаптации динамической модели Аккермана. В результате было установлено, что масштабируемая временная оптимизация дает более правильный результат для решения задачи оптимизации, чем оптимизация без нее.Список источниковhttps://jckantor.github.io/ND-Pyomo-Cookbook/06.03-Path-Planning-for-a-Simple-Car.htmlJohn T. Betts, "Survey of numerical methods for trajectory optimization", Journal of Guidance, Control, and Dynamics, vol. 21, no. 2, pp. 193-207, 1998.John T. Betts, "Practical Methods for Optimal Control Using Nonlinear Programming", Society for Industrial and Applied Mathematics , 2001.Yang Wang and Stephen Boyd, "Fast model predictive control using online optimization", IEEE Transactions on control systems technology, vol. 18, no. 2, pp. 267--278, 2009.Frank Dellaert, "Factor graphs and GTSAM: A hands-on introduction", Tech. Report, GT-RIM-CP&R-2012-002, 2012.C. R. Hargraves and S. W. Paris, "Direct Trajectory Optimization using Nonlinear Programming and Collocation", J Guidance, vol. 10, no. 4, pp. 338-342, July-August, 1987.DivyaGarg and Michael Patterson and Camila Francolin and Christopher Darby and Geoffrey Huntington and William Hager and Anil Rao, "Direct trajectory optimization and costate estimation ofВ finite-horizon and infinite-horizon optimal control problems using a Radaupseudospectral method", Computational Optimization and Applications, vol. 49, pp. 335-358, 2011.I. Michael Ross and Mark Karpenko, "A review of pseudospectral optimal control: {From} theory to flight", Annual Reviews in Control, vol. 36, no. 2, pp. 182--197, dec, 2012.H. J. Terry Suh and Max Simchowitz and Kaiqing Zhang and Russ Tedrake, "Do Differentiable Simulators Give Better Policy Gradients?", Under Review. , January, 2022. [ link ]