Задача (1), похоже, записана правильно. Ограничения мощности двигателей можно записать наравне с другими ограничениями.
Сам подход получения задачи нелинейного программирования правильный, но, видимо в описании есть ошибки. Например, я не вижу ограничения мощности двигателей, а
вообще выглядит подозрительно. Еще, обычно можно указать допустимый диапазон переменных, неужели матлаб не дает такой возможности?
Признаюсь, я уже забыл описание метода внутренней точки, т.к. Quick NP делает все сам по описанию. Поэтому здесь помощи от меня ждать не стоит. Никто не мешает использовать любой другой метод, просто решение в ipopt уже проверено и оно работает: ipopt решает задачи с 100 точками (это ~500 переменных) примерно секунд за 40 (процессор ~2ГГ)
********************
Шаг времени я имел в виду одинаковый, но не фиксированный, как это обычно бывает при решении дифференциальных уравнений. Вы в своей модели вынесли T – суть получилось одно и то же.
Общее время в задаче можно ограничить длительностью пробега робота по периметру «стола», взяв с коэффициентом запаса, скажем 2. За это время робот точно должен добраться до цели.
Начальных траекторий, по большому счету, я вижу две: объехать препятствие слева или справа. Если не учитывать специфику задачи – то получите проблему глобальной оптимизации с большим числом переменных. Возможно, что данная задача все-таки решаема современными средствами, но вряд ли Вы сможете найти такой инструмент.
![Сделав замену переменной $\tau = t/T,\; T \in \mathbb R_+, d\tau = (1/T)dt, t \in [0,T], \tau \in [0,1]$, задача примет вид:
\begin{equation}
\label{MathModel3}
\left\{
\begin{array}{rcl}
\frac{dx(\tau)}{d\tau}&=&T\frac{V_L(\tau) + V_R(\tau)}2\cos(\varphi(\tau)),\\
\frac{dy(\tau)}{d\tau}&=&T\frac{V_L(\tau) + V_R(\tau)}2\sin(\varphi(\tau)),\\
\frac{d\varphi(\tau)}{d\tau}&=&T\frac{V_R(\tau) - V_L(\tau)}{L},\\
x(0) &=& x_0,\\
y(0) &=& y_0,\\
\varphi(0) &=& \varphi_0,\\
x(1) &=& x_1,\\
y(1) &=& y_1,\\
(x(\tau)&-&x_c)^2 +(y(\tau)-y_c)^2 \ge r^2, r > 0,\\
\tau &\in& [0,1],\\
T &\to& \min_{|V_L(\cdot)| \le 1, |V_R(\cdot)| \le 1}.\\
\end{array}
\right.
\end{equation}Задача ~\eqref{MathModel3} сводится к задаче нелинейного программирования:
\begin{eqnarray*}
F(z) &\to& min_{z \in \mathbb X}, \quad \mathbb X = \{ z = (x^i, u^j, T) \in\mathbb R^{3(k+1)+2k+1} |\; g(z) = 0,\; h(z) \le 0 \},\\
x^i &\in& \mathbb R^3,\; u^j \in \mathbb R^2,\; i=0..k,\; j=0..k-1,\; T \in \mathbb R,\\
F(z) &=& T,\\
g(z) &=& (x^{i+1} - x^i - \frac{1}{k}f(i/k, x^i, u^i)), i=0..k-1,\\
x^0 &=& (x_0, y_0, \varphi_0),\; x^{k+1}_1 = x_1,\; x^{k+1}_2 = y_1.\\
h(z) &=& (-T,\; x^i_1 - x_{max},\; x^i_2 - y_{max},\; x_{min} - x^i_1,\; y_{min} - x^i_2,
r^2 - (x^i_1 - x_c)^2 - (x^i_2 - y_c)^2), i = 0..k.
\end{eqnarray*}
$f(t,x,u) -$ правая часть системы дифференциальных уравнений~\eqref{MathModel3}.
Полученная задача нелинейного программирования решается методом внутренней точки.
$$\frac{dz(t)}{dt} = -W_z(t,z),\; z(0) = z_0 \in X,$$
где $W(t,z) = F(z) + \tau(t)\sum_{i=1}^{k}\varphi(g^i(z)) + \tau(t)^{-1}\sum_{i=1}^{5(k+1)+1}\xi(h^i(z)),\;$
$\tau(t), \varphi(s), \xi(s) - $ скалярные, непрерывно дифференцируемые функции, определенные для всех $t\ge0,\;s > 0.$
$\tau(t)>0,\; d\tau(t)/dt > 0,\; \xi(s) > 0,\; \lim_{s\to-0}\xi(s) = \infty,\; \varphi(0) = \varphi'(0) = 0,\;
\varphi(s) > 0,\; \varphi''(s) > 0\; \forall s \neq 0.$
Ищем решение $z(t)$ при $t \to \infty,$ точнее $z(t')$, такое что $||W_z(t',z)|| \le \varepsilon(t).$](https://dxdy-02.korotkov.co.uk/f/9/6/a/96ae8e0d9c19b60adaa665e820b96be382.png "Сделав замену переменной $\tau = t/T,\; T \in \mathbb R_+, d\tau = (1/T)dt, t \in [0,T], \tau \in [0,1]$, задача примет вид:
\begin{equation}
\label{MathModel3}
\left\{
\begin{array}{rcl}
\frac{dx(\tau)}{d\tau}&=&T\frac{V_L(\tau) + V_R(\tau)}2\cos(\varphi(\tau)),\\
\frac{dy(\tau)}{d\tau}&=&T\frac{V_L(\tau) + V_R(\tau)}2\sin(\varphi(\tau)),\\
\frac{d\varphi(\tau)}{d\tau}&=&T\frac{V_R(\tau) - V_L(\tau)}{L},\\
x(0) &=& x_0,\\
y(0) &=& y_0,\\
\varphi(0) &=& \varphi_0,\\
x(1) &=& x_1,\\
y(1) &=& y_1,\\
(x(\tau)&-&x_c)^2 +(y(\tau)-y_c)^2 \ge r^2, r > 0,\\
\tau &\in& [0,1],\\
T &\to& \min_{|V_L(\cdot)| \le 1, |V_R(\cdot)| \le 1}.\\
\end{array}
\right.
\end{equation}Задача ~\eqref{MathModel3} сводится к задаче нелинейного программирования:
\begin{eqnarray*}
F(z) &\to& min_{z \in \mathbb X}, \quad \mathbb X = \{ z = (x^i, u^j, T) \in\mathbb R^{3(k+1)+2k+1} |\; g(z) = 0,\; h(z) \le 0 \},\\
x^i &\in& \mathbb R^3,\; u^j \in \mathbb R^2,\; i=0..k,\; j=0..k-1,\; T \in \mathbb R,\\
F(z) &=& T,\\
g(z) &=& (x^{i+1} - x^i - \frac{1}{k}f(i/k, x^i, u^i)), i=0..k-1,\\
x^0 &=& (x_0, y_0, \varphi_0),\; x^{k+1}_1 = x_1,\; x^{k+1}_2 = y_1.\\
h(z) &=& (-T,\; x^i_1 - x_{max},\; x^i_2 - y_{max},\; x_{min} - x^i_1,\; y_{min} - x^i_2,
r^2 - (x^i_1 - x_c)^2 - (x^i_2 - y_c)^2), i = 0..k.
\end{eqnarray*}
$f(t,x,u) -$ правая часть системы дифференциальных уравнений~\eqref{MathModel3}.
Полученная задача нелинейного программирования решается методом внутренней точки.
$$\frac{dz(t)}{dt} = -W_z(t,z),\; z(0) = z_0 \in X,$$
где $W(t,z) = F(z) + \tau(t)\sum_{i=1}^{k}\varphi(g^i(z)) + \tau(t)^{-1}\sum_{i=1}^{5(k+1)+1}\xi(h^i(z)),\;$
$\tau(t), \varphi(s), \xi(s) - $ скалярные, непрерывно дифференцируемые функции, определенные для всех $t\ge0,\;s > 0.$
$\tau(t)>0,\; d\tau(t)/dt > 0,\; \xi(s) > 0,\; \lim_{s\to-0}\xi(s) = \infty,\; \varphi(0) = \varphi'(0) = 0,\;
\varphi(s) > 0,\; \varphi''(s) > 0\; \forall s \neq 0.$
Ищем решение $z(t)$ при $t \to \infty,$ точнее $z(t')$, такое что $||W_z(t',z)|| \le \varepsilon(t).$")
