Элемент управления Python не может имитировать статическую нелинейность
Я новичок в использовании python для разработки элементов управления (из MATLAB). Он кажется достаточно способным для решения основных задач, но более сложные вещи становятся громоздкими. Я пытаюсь создать систему обратной связи для управления ориентацией спутника, используя взаимосвязанные системы ввода-вывода. Нелинейные системы создаются с помощью
Это нормальное поведение для решателя? Во всяком случае, я ожидал, что
Функции могут выполнять деление на входе (разрешено, поскольку некоторые переменные, передаваемые между связанными системами, никогда не могут быть равны нулю, кватернионы), если эти функции вызываются с нулевым входом, симуляция прерывается, но, что более важно, в общем случае выход может не быть релевантен для моделирования, если используется неверный (нулевой) ввод.
Ниже (часть) вывода, когда код запускается с операторами печати для отладки. Возникновение outfcn относительно updfcn вызывает беспокойство и передача входных данных (u), которые имеют нулевое значение.
Called rotKin_outfcn with u:
[0. 0. 0.]
Called rotKin_outfcn with u:
[0.53 0.53 0.053]
Called markleyPD_outfcn with u:
[0. 0. 0. 0. 0. 0. 0.]
Called rotKin_outfcn with u:
[0. 0. 0.]
Called rotKin_outfcn with u:
[0.53 0.53 0.053]
Called markleyPD_outfcn with u:
[0.53 0.53 0.053 0.6853 0.6953 0.1531 0.1531]
Called rotKin_outfcn with u:
[0. 0. 0.]
Called rotKin_outfcn with u:
[0.53 0.53 0.053]
Called markleyPD_outfcn with u:
[0.53 0.53 0.053 0.6853 0.6953 0.1531 0.1531]
Called rotKin_outfcn with u:
[0. 0. 0.]
Called rotKin_outfcn with u:
[0.53 0.53 0.053]
Called rotKin_updfcn with u:
[0.53 0.53 0.053]
Called rotKin_outfcn with u:
[0. 0. 0.]
Called rotKin_outfcn with u:
[0.52958536 0.5297074 0.0532223 ]
Called markleyPD_outfcn with u:
[0. 0. 0. 0. 0. 0. 0.]
Called rotKin_outfcn with u:
[0. 0. 0.]
Called rotKin_outfcn with u:
[0.52958536 0.5297074 0.0532223 ]
Called markleyPD_outfcn with u:
[0.52958536 0.5297074 0.0532223 0.6854992 0.69598091 0.15311521
0.14910081]
Called rotKin_outfcn with u:
[0. 0. 0.]
Called rotKin_outfcn with u:
[0.52958536 0.5297074 0.0532223 ]
Called markleyPD_outfcn with u:
[0.52958536 0.5297074 0.0532223 0.6854992 0.69598091 0.15311521
0.14910081]
Called rotKin_outfcn with u:
[0. 0. 0.]
Called rotKin_outfcn with u:
[0.52958536 0.5297074 0.0532223 ]
Called rotKin_updfcn with u:
[0.52958536 0.5297074 0.0532223 ]
Called rotKin_outfcn with u:
[0. 0. 0.]
Called rotKin_outfcn with u:
[0.52922002 0.52944959 0.05341816]
Called markleyPD_outfcn with u:
[0. 0. 0. 0. 0. 0. 0.]
Called rotKin_outfcn with u:
[0. 0. 0.]
Called rotKin_outfcn with u:
[0.52922002 0.52944959 0.05341816]
Я знаю, что могу кодировать деление на ноль, но это не решает основную проблему, связанную с тем, что выходные функции изначально обнуляются. По мере того, как я создаю более сложные системы, кажется, что большее количество взаимосвязанных систем вносит еще большую непредсказуемость.
Я спрашиваю себя:
- Я использую
- Почему системный решатель пытается решить статическую нелинейность (PD-регулятор без состояний) с помощью решателя RK45? Он может просто выполнять арифметику один раз за шаг?
- Зачем столько раз звонить в outfcn? Это, кажется, только увеличивается, когда количество взаимосвязанных систем увеличивается.
- Может ли быть проблема с лежащей в основе библиотекой управления?
Пример След ошибки, которую я получаю при использовании ввода (u) для операций кватерниона:
---------------------------------------------------------------------------
ValueError Traceback (most recent call last)
Input In [16], in <cell line: 117>()
114 omega_0 = [0.53, 0.53, 0.053]
115 x_0 = np.concatenate([omega_0, q_0])
--> 117 y = control.input_output_response(system, time, X0=x_0)
File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/control/iosys.py:1839, in input_output_response(sys, T, U, X0, params, transpose, return_x, squeeze, solve_ivp_kwargs, t_eval, **kwargs)
1836 if not hasattr(sp.integrate, 'solve_ivp'):
1837 raise NameError("scipy.integrate.solve_ivp not found; "
1838 "use SciPy 1.0 or greater")
-> 1839 soln = sp.integrate.solve_ivp(
1840 ivp_rhs, (T0, Tf), X0, t_eval=t_eval,
1841 vectorized=False, **solve_ivp_kwargs)
1842 if not soln.success:
1843 raise RuntimeError("solve_ivp failed: " + soln.message)
File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/scipy/integrate/_ivp/ivp.py:546, in solve_ivp(fun, t_span, y0, method, t_eval, dense_output, events, vectorized, args, **options)
543 if method in METHODS:
544 method = METHODS[method]
--> 546 solver = method(fun, t0, y0, tf, vectorized=vectorized, **options)
548 if t_eval is None:
549 ts = [t0]
File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/scipy/integrate/_ivp/rk.py:94, in RungeKutta.__init__(self, fun, t0, y0, t_bound, max_step, rtol, atol, vectorized, first_step, **extraneous)
92 self.max_step = validate_max_step(max_step)
93 self.rtol, self.atol = validate_tol(rtol, atol, self.n)
---> 94 self.f = self.fun(self.t, self.y)
95 if first_step is None:
96 self.h_abs = select_initial_step(
97 self.fun, self.t, self.y, self.f, self.direction,
98 self.error_estimator_order, self.rtol, self.atol)
File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/scipy/integrate/_ivp/base.py:138, in OdeSolver.__init__.<locals>.fun(t, y)
136 def fun(t, y):
137 self.nfev += 1
--> 138 return self.fun_single(t, y)
File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/scipy/integrate/_ivp/base.py:20, in check_arguments.<locals>.fun_wrapped(t, y)
19 def fun_wrapped(t, y):
---> 20 return np.asarray(fun(t, y), dtype=dtype)
File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/control/iosys.py:1832, in input_output_response.<locals>.ivp_rhs(t, x)
1831 def ivp_rhs(t, x):
-> 1832 return sys._rhs(t, x, ufun(t))
File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/control/iosys.py:1024, in InterconnectedSystem._rhs(self, t, x, u)
1021 u = np.array(u, ndmin=1)
1023 # Compute the input and output vectors
-> 1024 ulist, ylist = self._compute_static_io(t, x, u)
1026 # Go through each system and update the right hand side for that system
1027 xdot = np.zeros((self.nstates,)) # Array to hold results
File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/control/iosys.py:1074, in InterconnectedSystem._compute_static_io(self, t, x, u)
1071 state_index, input_index, output_index = 0, 0, 0
1072 for sys in self.syslist:
1073 # Compute outputs for each system from current state
-> 1074 ysys = sys._out(
1075 t, x[state_index:state_index + sys.nstates],
1076 ulist[input_index:input_index + sys.ninputs])
1078 # Store the outputs at the start of ylist
1079 ylist[output_index:output_index + sys.noutputs] = \
1080 ysys.reshape((-1,))
File ~/Repositories/aocs-utils/.venv_aocs_utils/lib/python3.8/site-packages/control/iosys.py:849, in NonlinearIOSystem._out(self, t, x, u)
848 def _out(self, t, x, u):
--> 849 y = self.outfcn(t, x, u, self._current_params) \
850 if self.outfcn is not None else x
851 return np.array(y).reshape((-1,))
Input In [16], in markleyPD_outfcn(t, x, u, params)
44 print('\nCalled markleyPD_outfcn with u: ')
45 print(u)
---> 46 rot = Rotation.from_quat(u[3:7])
48 ## Markley p.291
49 # τ = -k_p*sign(dq_4)*dq_0:3 - k_d*ω
50 tau = -k_p*np.sign(u[6])*u[3:6] - k_d*u[0:3]
File _rotation.pyx:627, in scipy.spatial.transform._rotation.Rotation.from_quat()
File _rotation.pyx:531, in scipy.spatial.transform._rotation.Rotation.__init__()
ValueError: Found zero norm quaternions in `quat`.
Ниже приведен минималистичный код, который может воспроизвести проблему:
import numpy as np
import control
from scipy.spatial.transform import Rotation
# Rotational dynamics state update function
def rotDyn_updfcn(t, x, u, params):
I = params.get('sat_inertia')
## Markley p.84
# I*dω = τ_e - ω x (Iω)
dx = np.matmul(np.linalg.inv(I), (u - np.cross(x, np.matmul(I,x))))
return dx
# Rotational kinematics state update function
def rotKin_updfcn(t, x, u, params):
print('\nCalled rotKin_updfcn with u: ')
print(u)
## Markley p.71
# dq = 1/2 * Ξ(q)*ω
dq = np.array([[ x[3]*u[0] - x[2]*u[1] + x[1]*u[2]],
[ x[2]*u[0] + x[3]*u[1] - x[0]*u[2]],
[-x[1]*u[0] + x[0]*u[1] + x[3]*u[2]],
[-x[0]*u[0] - x[1]*u[1] - x[2]*u[2]]]) / 2
return dq
# Rotational kinematics output function
def rotKin_outfcn(t, x, u, params):
print('\nCalled rotKin_outfcn with u: ')
print(u)
return x
# Basic quaternion attitude feedback controller
def markleyPD_outfcn(t, x, u, params):
k_p = params.get('k_p')
k_d = params.get('k_d')
print('\nCalled markleyPD_outfcn with u: ')
print(u)
rot = Rotation.from_quat(u[3:7])
## Markley p.291
# τ = -k_p*sign(dq_4)*dq_0:3 - k_d*ω
tau = -k_p*np.sign(u[6])*u[3:6] - k_d*u[0:3]
return tau
params = {
"sat_inertia": [[10000, 0, 0],
[0, 9000, 0],
[0, 0, 12000]],
"k_p": 50,
"k_d": 500
}
## Plant construction
rotDyn = control.NonlinearIOSystem(rotDyn_updfcn,
None,
name='rotDyn',
inputs=['L','M','N'],
outputs=['P','Q','R'],
states=['P','Q','R'],
params=params,
dt=0
)
rotKin = control.NonlinearIOSystem(rotKin_updfcn,
rotKin_outfcn,
name='rotKin',
inputs=['P','Q','R'],
outputs=['q0','q1','q2','q3'],
states=['q0','q1','q2','q3'],
params=params,
dt=0
)
plant = control.interconnect((rotDyn, rotKin),
name='plant',
inputs=rotDyn.input_labels,
outputs=rotDyn.output_labels + rotKin.output_labels,
states=rotDyn.state_labels + rotKin.state_labels
)
plant.name='plant' #Required due to bug in interconnect?
## Controller construction
ctrl = control.NonlinearIOSystem(None,
markleyPD_outfcn,
inputs = ['P', 'Q', 'R', 'q0', 'q1', 'q2', 'q3'],
outputs = ['L', 'M', 'N'],
params=params,
dt=0
)
## Closed loop system
system = control.interconnect([plant, ctrl],
name='system',
outputs=ctrl.output_labels + plant.output_labels,
states=plant.state_labels
)
system.name = 'system'
## Run simulation
time = np.linspace(0,300,3000)
# initial condition
q_0 = [0.6853, 0.6953, 0.1531, 0.1531]
omega_0 = [0.53, 0.53, 0.053]
x_0 = np.concatenate([omega_0, q_0])
y = control.input_output_response(system, time, X0=x_0)
Я знаю, что это довольно сложная проблема с математической точки зрения, но надеюсь, что кто-то может хотя бы пролить свет на мое использование