Устойчивость без складок: параллельное моделирование и устойчивое MPC для сертифицированного манипулирования деформируемыми объектами

Аннотация

Мы представляем CORD-SLS, метод управления в реальном времени для безопасной манипуляции деформируемыми объектами, с акцентом на веревки и ткань. В его основе лежит параллельный на GPU дифференцируемый симулятор со сглаживанием контактов, который обеспечивает эффективное градиентное планирование при прерывистом контакте. Для надежного соблюдения ограничений в условиях неопределенности модели и сенсорики мы разработали параллельный на GPU алгоритм робастного MPC с обратной связью по выходу, работающий в реальном времени и планирующий с использованием этого симулятора. Мы также показываем, что симулятор ускоряет model-based RL для обучения нейросетевых стратегий манипуляции. Для повышения робастности в реальных условиях мы используем conformal prediction для калибровки границ визуальной обратной связи и ошибок восприятия для MPC, создавая достижимые трубки, которые обеспечивают безопасное управление с высокой вероятностью. Мы оцениваем CORD-SLS на высокоразмерных задачах манипуляции веревками и тканью с частыми контактами в симуляции и на аппаратном обеспечении, включая обход препятствий, прокладку маршрута, складывание и разглаживание. Во всех сценариях CORD-SLS достигает миллисекундной скорости планирования, превосходя базовые методы по безопасности, скорости и успешности выполнения задач..

Ключевые слова: манипуляция, дифференцируемое моделирование, управление на основе моделей и RL

⟦Figure 1: Результаты на аппаратном обеспечении демонстрируют эффективность CORD-SLS в реальных условиях. В а), CORD-SLS надежно решает задачу манипуляции деформируемой веревкой с ограничениями, поднимая ее и перемещая через препятствие с помощью нашего дифференцируемого симулятора со сглаживанием контактов. В b) мы показываем соответствующие надежные трубки, вычисленные CORD-SLS при обходе препятствия (красный цвет). В c) CORD-SLS применяется для складывания ткани.

1 Введение

Манипулирование деформируемыми объектами, такими как веревки и ткань, важно для критически важных с точки зрения безопасности и производительности приложений в домашних роботах, логистике и хирургии [56]. Тем не менее, управление этими объектами затруднено: деформируемым объектам часто не хватает компактных представлений состояний, а удобные аппроксимации, такие как ткань на основе сеток, требуют планирования с учетом множества степеней свободы и высокоразмерной нелинейной динамики. Манипулирование также требует принятия решений о том, где и когда осуществлять контакт, а также соблюдения ограничений, например, избегания препятствий, достижения целей и выполнения задач, несмотря на неопределенность в динамике и восприятии. Симуляторы могут предоставить полезные модели для планирования такого поведения, если они достаточно быстры, точны и удобны для планировщика. Однако предыдущие работы оставляют разрыв между масштабируемым планированием, динамикой деформируемых контактов и робастностью. Классические методы планируют задачи с помощью геометрических абстракций, но создают планы с разомкнутым контуром с ограниченной робастностью. Обученные политики могут планировать в визуальных или латентных пространствах, но им не хватает гарантий робастности, и они требуют длительного обучения. Дифференцируемые симуляторы обеспечивают быстрое планирование на основе градиентов, но разрывные градиенты, возникающие из-за наивных формулировок контактов, могут препятствовать эффективному обнаружению контактов, необходимому для манипулирования..

Чтобы устранить эти пробелы, мы предлагаем CORD-SLS (Contact-smoothed Output-feedback Robust control of Deformables via System Level Synthesis) — фреймворк для дифференцируемого моделирования в реальном времени и надежного планирования манипуляций с деформируемыми объектами на основе восприятия. Сначала мы разрабатываем параллельный на GPU дифференцируемый симулятор со сглаживанием контактов, обеспечивающий удобные для планирования градиенты при скорости моделирования в реальном времени. Затем мы выполняем планирование с помощью этого симулятора посредством параллельного на GPU метода надежного MPC, основанного на System Level Synthesis (SLS), что позволяет осуществлять планирование в реальном времени в многомерных условиях. Для обработки неопределенности восприятия мы вводим параллельную на GPU формулировку SLS с обратной связью по выходу для надежного управления на основе зрения. Мы также показываем, что симулятор может ускорить MBRL, позволяя обучать нейронные стратегии управления эффективным с точки зрения выборки способом через аналитические градиенты стратегии. Наконец, мы калибруем основанные на данных границы неопределенности наихудшего случая для надежного MPC, обеспечивая вероятностные гарантии надежности в реалистичных условиях. Наш вклад заключается в следующем::

• ⟦ Параллельный на графическом процессоре, дифференцируемый, работающий в реальном времени симулятор деформируемых объектов со сглаживанием контактов.
• ⟦ Параллельный на графическом процессоре метод синтеза в реальном времени робастных стратегий управления с обратной связью по выходу для манипуляции деформируемыми объектами посредством SLS с ограничениями достижимости, а также метод MBRL для обучения нейронных стратегий управления, ускоряемый за счет аналитических градиентов стратегии из симулятора.
• ⟦ Реальная реализация на основе технического зрения с использованием отслеживания ключевых точек, а также калибровки границ ошибок восприятия и модели с помощью conformal prediction (CP) для обеспечения безопасности с высокой вероятностью.
• ⟦ Оценка на задачах манипуляции веревкой и тканью с частыми контактами в симуляции и на аппаратном обеспечении (обход препятствий, складывание и разглаживание), превосходящая базовые методы по безопасности, скорости и успешности выполнения задач.

2 Родственные работы

Моделирование деформируемых объектов

Динамика на основе положений (PBD) и её расширение, XPBD, моделируют деформируемые объекты путем проецирования частиц на геометрические ограничения, что обеспечивает быстрое и стабильное моделирование [38, 39, 33]. Однако методы на основе PBD достигают устойчивости ценой физической точности: эффективная жесткость зависит от шага по времени и количества итераций решателя, а существующие дифференцируемые варианты не полностью решают проблему контакта и трения между жесткими и деформируемыми телами. Для деформируемых линейных объектов (DLO) были разработаны более физически обоснованные модели. В частности, формулировки стержней Коссера и дискретных упругих стержней учитывают изгиб, кручение, растяжение и сдвиг, а также были расширены для обработки фрикционного контакта [18, 27, 7, 6]. Дифференцируемые симуляторы стержней поддерживают идентификацию системы, но менее пригодны для управления в условиях интенсивных контактов, поскольку планирование требует явного обнаружения режима контакта [12]. Для моделирования ткани методы, основанные на дискретных оболочках, оболочках Кирхгофа-Лява и проективной динамике, эффективно моделируют тонкие поверхности и могут включать сухой фрикционный контакт [19, 51, 34]. Однако невыпуклость разрешения контактов часто приводит к несогласованным градиентам в дифференцируемых симуляторах ткани [30, 29]. Наш метод использует выпуклую формулировку контакта и сглаженную динамику для улучшения обнаружения контактов во время планирования, создавая градиенты, направленные на поиск контакта, в то время как прямое развертывание и выполнение используют негладкую динамику для сохранения точности [46, 47, 28]⟧.

Планирование

Планирование манипуляций с деформируемыми объектами затруднено из-за тесной взаимосвязи восприятия, динамики и управления [56], а модели должны находить компромисс между точностью и скоростью [21, 4]. В отличие от обучения с подражанием [43, 41], планировщики не требуют данных от экспертов. Классические планировщики используют упрощенные геометрические абстракции для повышения скорости [5, 40], а современные гибридные методы сочетают глобальное планирование с локальным управлением или упрощенной динамикой [36, 50]. Эти методы повышают вычислительную эффективность, но приводят к созданию планов с разомкнутым контуром, обладающих ограниченной устойчивостью. Обученные модели прогнозируют перцептивную или латентную динамику [44, 61, 54, 35, 26] для планирования, но требуют обширного обучения и не имеют гарантий устойчивости. Более высокоуровневые абстракции, такие как латентные дорожные карты, расширяют горизонты планирования и могут кодировать контактные переходы [32, 53, 31], но зависят от библиотек навыков, специфичных для конкретных задач. Дифференцируемые симуляторы позволяют выполнять планирование на основе градиентов [10, 60, 11, 57], однако разрывные градиенты препятствуют обнаружению контактов при планировании [11, 46, 28]. Наш метод вместо этого использует сглаживание контактов для планирования через контакт без использования специализированных навыков, одновременно учитывая ограниченную ошибку модели при переносе из симуляции в реальность [8]⟧.

Безопасность и устойчивость изучены в меньшей степени. Некоторые методы планируют номинально безопасные движения [20, 60] или используют барьерные функции для переопределения небезопасных действий [48, 1]; однако они опираются на приближенные модели без учета модельной ошибки, обеспечивают только локальные корректировки и не решают задачу обнаружения контактов. Методы достижимости оценивают безопасность DLO [59], но не учитывают планирование с интенсивными контактами или манипуляции с тканью в условиях ошибок восприятия. Другие методы повышают устойчивость за счет областей доверия модели [37], уточнения градиента в возмущенных системах DLO [16] или демонстраций MPC [49], но им не хватает формальных гарантий и планирования с интенсивными контактами. Хотя безопасное управление с обратной связью по выходу рассматривалось в недавних работах [55, 13, 14], эти методы с трудом масштабируются до высокоразмерных пространств состояний деформируемых объектов. В отличие от них, мы объединяем дифференцируемое моделирование, сглаживание контактов и робастное MPC с обратной связью по выходу посредством SLS, создавая градиенты поиска контактов при робастном учете неопределенности модели и восприятия. Мы опираемся на параллельную на GPU достижимость [17] через SLS [2, 23], расширяя ее за пределы обратной связи по полному состоянию. В отличие от существующих медленных методов SLS с обратной связью по выходу, ограниченных CPU [25], наш параллельный на GPU решатель обеспечивает робастное управление в реальном времени..

3 Постановка задачи

Мы рассматриваем известное представление состояния $q\in\mathcal{Q}\subseteq\mathbb{R}^{n_{q}}$ для деформируемого объекта (например, трехмерные координаты звеньев для веревки или узлов сетки для ткани), приводимого в действие управляющим сигналом $u\in\mathcal{U}\subseteq\mathbb{R}^{n_{u}}$ и подчиняющегося неопределенной динамике (1a), где $f:\mathcal{Q}\times\mathcal{U}\rightarrow\mathcal{Q}$. Чтобы учесть задержку восприятия, мы моделируем систему как наблюдаемую с помощью неопределенных измерений с временной задержкой $y\in\mathcal{Y}\subseteq\mathbb{R}^{n_{y}}$ в соответствии с картой наблюдений $h:\mathcal{Q}\rightarrow\mathcal{Y}$ (1b),

где $E:\mathcal{Q}\rightarrow\mathbb{R}^{n_{q}\times n_{q}}$ и $F:\mathcal{Q}\rightarrow\mathbb{R}^{n_{y}\times n_{y}}$ и $w\in\mathcal{B}^{n_{q}}:=\{w\in\mathbb{R}^{n_{q}}\mid\|w\|_{2}\leq 1\}$ и $e\in\mathcal{B}^{n_{y}}$ являются ограниченными возмущениями. Мы стремимся разработать 1) дифференцируемый симулятор деформируемых объектов, который аппроксимирует динамику (1a), оставаясь при этом удобным для планирования и обучения, и 2) эффективный и робастный алгоритм планирования и управления, который использует этот симулятор для решения задач MPC с учетом достижимости для безопасного манипулирования деформируемыми объектами. Мы рассматриваем две конкретные задачи:

Задача 1. Дифференцируемый симулятор деформируемых объектов со сглаживанием контактов. Разработать симулятор для деформируемых линейных объектов и тканей, который 1) является дифференцируемым, 2) параллельным на GPU и 3) предоставляет информативные градиенты для планирования и обучения стратегий через контакты, определяя динамику планировщика.

$$q_{k+1}=f(q_{k},u_{k}).$$ (2)

Задача 2. MPC с учетом достижимости для робастного манипулирования деформируемыми объектами. Оптимизируйте номинальную траекторию состояния $\textbf{z}:=\{z_{k}\}_{k=0}^{N}$ и управления $\textbf{v}:=\{v_{k}\}_{k=0}^{N-1}$ длиной $N$, которая является допустимой для динамики симулятора (2), совместно с причинно-следственным контроллером обратной связи по выходу $\pi:=(\pi_{0},\ldots,\pi_{N-1})$, где $\pi_{t}:\mathcal{Y}^{t+1}\rightarrow\mathcal{U}$ стабилизирует истинную динамику деформируемого объекта (1a) относительно $(\textbf{z},\textbf{v})$, используя историю наблюдений, и обеспечивает выполнение ограничений на состояние и управление в замкнутом контуре $\mathcal{S}\subseteq\mathcal{Q}\times\mathcal{U}$.

4 Метод

Мы разрабатываем дифференцируемый симулятор деформируемых объектов со сглаживанием контактов для планирования на основе градиентов (разд. 4.1), параллельную на графическом процессоре структуру SLS с обратной связью по выходу, которая использует его для обеспечения надежного управления в реальном времени в условиях неопределенности восприятия (разд. 4.2), стратегию model-based RL с эффективным использованием выборок для обучения нейронных стратегий манипулирования, ускоряемую аналитическими градиентами стратегии из дифференцируемого симулятора (разд. 4.3), и процедуру калибровки на основе CP для преобразования изученных ошибок восприятия в гарантии достижимости трубки с высокой вероятностью (разд. 4.4⟧).

4.1 Дифференцируемый симулятор деформируемых объектов со сглаживанием контактов

Существующие симуляторы деформируемых объектов часто предполагают, что деформируемые объекты остаются прикрепленными к роботу, или полагаются на явные схемы пошагового интегрирования по времени, которые требуют малых шагов по времени для численной устойчивости. В отличие от них, мы представляем новый симулятор, который корректно учитывает переключение между неактивными и активными ограничениями, используя при этом неявную схему пошагового интегрирования по времени, сохраняя при этом дифференцируемость..

Рассмотрим узловую систему с $n_{v}$ узлами, чьи трехмерные координаты вместе с $n_{u}$ приводимыми степенями свободы (DoFs) образуют вектор конфигурации $q\in\mathbb{R}^{n_{q}}$, где $n_{q}=3n_{v}+n_{u}$. Для упругой энергии $E(q)$ соответствующая внутренняя упругая сила задается выражением $-\partial E(q)/\partial q$. В рамках квазистатического допущения система удовлетворяет

$$M\frac{q^{+}-q}{\delta t^{2}}+\frac{\partial E(q^{+})}{\partial q^{+}}=\tau(u)+\sum_{i=1}^{n_{c}}J_{i}(q)^{\top}\,\lambda_{i},$$ (3)

где $M$ — матрица масс, $\tau(u)$ — внешние обобщенные силы, включая гравитацию, действующую на узловую систему, и силы привода, приложенные к управляемым DoFs, $\lambda_{i}\in\mathbb{R}^{3}$ — $i$-я сила связи, $\delta t$ — дискретный временной шаг, а $J_{i}\in\mathbb{R}^{3\times n_{q}}$ — соответствующий якобиан связи. Как упругие силы, так и силы связи вычисляются на следующем временном шаге, что дает неявную схему временного шага для повышения численной устойчивости. Для голономных связей, таких как захват, жестко взаимодействующий с узловой системой, мы предполагаем, что активная связь $c_{i}(q)=0\in\mathbb{R}^{3}$ обеспечивает фиксацию захвата. Следующая конфигурация $q^{+}$ и сила связи $\lambda_{i}$ должны удовлетворять условию комплементарности

$$0=J_{i}(q)\,(q^{+}-q)+c_{i}(q)\;\perp\;\lambda_{i}\in\mathbb{R}^{3},$$ (4)

где якобиан ограничений определяется как $J_{i}(q):=\partial c_{i}(q)/\partial q$. Для контактных ограничений мы используем выпуклую релаксацию Анитеску [3]. Здесь конфигурация следующего шага $q^{+}$ и контактная сила $\lambda_{i}$ удовлетворяют

$$\mathcal{F}_{\mu}^{*}\ni J_{i}(q)\,(q^{+}-q)+\begin{bmatrix}\phi_{i}(q)&0&0\end{bmatrix}^{\top}\;\perp\;\lambda_{i}\in\mathcal{F}_{\mu},$$ (5)

где $\phi_{i}(q)$ — это знаковое расстояние контактной пары $i$, $\mathcal{F}_{\mu}$ — конус трения, а $\mathcal{F}_{\mu}^{*}$ — его двойственный конус.

Решатель симуляции Мы решаем динамику в (3) с учетом голономных и контактных ограничений в (4) и (5), используя двухэтапную процедуру. Первый этап вычисляет отклик системы при голономных ограничениях, в то время как второй этап разрешает контактные взаимодействия. На первом этапе мы находим промежуточную конфигурацию $q^{*}$, удовлетворяющую

$$F(q^{*};q,u):=M\frac{q^{*}-q}{\delta t^{2}}+\frac{\partial E(q^{*})}{\partial q^{*}}-\tau(u)+\sum_{i=1}^{n_{h}}J_{i}(q)^{\top}D_{i}\,\bigl(J_{i}(q)\,(q^{*}-q)+c_{i}(q)\bigr)=0,$$ (6)

где $n_{h}$ обозначает количество голономных связей, а $D_{i}\in\mathbb{R}^{3\times 3}$ представляет собой диагональную матрицу жесткости, связанную с $i$-й голономной связью. Когда связь активна, $D_{i}$ выбирается достаточно большим для обеспечения (4); в противном случае $D_{i}=0$. (6) решается с использованием метода Ньютона и неявно определяет отображение из текущей конфигурации $q$ в промежуточную конфигурацию $q^{*}$. На втором этапе мы решаем задачу оптимизации

$$\min_{q^{+}} \frac{1}{2}(q^{+}-q^{*})^{\top}\,\bigl[\underbrace{M+\frac{\partial^{2}E(q^{*})}{\partial(q^{*})^{2}}+\sum_{i=1}^{n_{h}}J_{i}(q)^{\top}D_{i}J_{i}(q)}_{P}\bigr]\,(q^{+}-q^{*})$$ (7)

$$\text{s.t.} J_{i}(q^{*})\,(q^{+}-q^{*})+\begin{bmatrix}\phi_{i}(q^{*})&0&0\end{bmatrix}\in\mathcal{F}_{\mu}^{*},\quad\forall i=n_{h}\!+\!1,\dots,n_{c}.$$

Условия KKT для (7) восстанавливают ограничение контакта в (5) вместе с $\textstyle P\,(q^{+}\!-\!q^{*})\!=\!\sum_{i=n_{h}\!+\!1}^{n_{c}}J_{i}(q)^{\top}\,\lambda_{i}$, что соответствует линеаризации динамики относительно промежуточной конфигурации $q^{*}$. При наличии пары $(q,u)$ решение (6)–(7) дает следующую конфигурацию $q^{+}$. Решатели для (6)–(7) реализованы в JAX, что позволяет выполнять матричные разложения с ускорением на GPU..

Дифференцирование через симуляцию Для получения градиентов через динамику мы дифференцируем через задачи оптимизации в (6) и (7). Вместо дифференцирования через развернутые итерационные решатели мы применяем неявное дифференцирование. Для задачи первого этапа (6) пусть $\theta:=(q,u)$ обозначает входные данные для динамики. Дифференцирование неявного уравнения, определяющего $q^{*}$, дает

$$\frac{\partial F}{\partial q^{*}}\frac{\partial q^{*}}{\partial\theta}+\frac{\partial F}{\partial\theta}=0\implies\frac{\partial q^{*}}{\partial\theta}=-\Bigl(\frac{\partial F}{\partial q^{*}}\Bigr)^{-1}\frac{\partial F}{\partial\theta}.$$ (8)

Для задачи второго этапа (7) мы дифференцируем её условия KKT, обозначенные как $G(q^{+},\lambda;q^{*})=0$, по отношению к $q^{*}$. Это дает

$$K\begin{bmatrix}\frac{\partial q^{+}}{\partial q^{*}}\\ \frac{\partial\lambda}{\partial q^{*}}\end{bmatrix}+\frac{\partial G}{\partial q^{*}}=0\implies\frac{\partial q^{+}}{\partial q^{*}}=-\begin{bmatrix}I&0\end{bmatrix}K^{-1}\frac{\partial G}{\partial q^{*}},$$ (9)

где $K$ обозначает матрицу Каруша-Куна-Таккера задачи оптимизации. Таким образом, симулятор можно рассматривать как дифференцируемый слой. Для скалярной целевой функции $\mathcal{L}$ сопряженное значение относительно $\theta=(q,u)$ определяется как

$$\frac{\partial\mathcal{L}}{\partial\theta}=\frac{\partial\mathcal{L}}{\partial q^{+}}\frac{\partial q^{+}}{\partial q^{*}}\frac{\partial q^{*}}{\partial\theta}=\begin{bmatrix}\dfrac{\partial\mathcal{L}}{\partial q^{+}}&0\end{bmatrix}K^{-1}\frac{\partial G}{\partial q^{*}}\left(\frac{\partial F}{\partial q^{*}}\right)^{-1}\frac{\partial F}{\partial\theta}.$$ (10)

Данное выражение естественным образом определяет пользовательский обратный проход для слоя симулятора. Начиная с сопряженного значения $q^{+}$, мы сначала решаем линейную систему, включающую $K^{\top}$, и вычисляем векторно-якобиево произведение с $\partial G/\partial q^{*}$, чтобы получить сопряженное значение $q^{*}$. Затем мы решаем вторую линейную систему, включающую $(\partial F/\partial q^{*})^{\top}$, и вычисляем векторно-якобиево произведение с $\partial F/\partial\theta$, чтобы получить сопряженное значение $\theta$. Поскольку факторизации $K$ и $\partial F/\partial q^{*}$ являются побочными результатами прямого решения, обратный проход не требует дополнительных затрат..

Сглаживание контактов. Динамика контактов часто демонстрирует проблему исчезающего градиента: до момента активации контакта управляющие воздействия, связанные с приводимыми в движение степенями свободы, не оказывают влияния на состояние деформируемого объекта [28]. В результате градиентные оптимизаторы могут стагнировать из-за отсутствия информативных градиентных сигналов, не достигая поставленной цели..

Для решения этой проблемы мы используем идею суррогатных градиентов [15], в которой градиент, используемый для планирования, заменяется суррогатным оценщиком, не соответствующим истинному градиенту, полученному путем дифференцирования через физическое развертывание. Это вносит контролируемое смещение в обмен на улучшенную градиентную информацию в режимах, где истинный градиент неинформативен. Голономное ограничение в (4) активно только тогда, когда $c_{i}(q)=0$, и в этот момент ненулевая матрица жесткости $D_{i}$ используется в (6) для обеспечения выполнения ограничения. Это переключение между неактивными и активными ограничениями приводит как к разрывам, так и к затухающим градиентам. Чтобы сгладить этот переход, мы переопределяем $D_{i}$ как $D_{i}=D_{i,\max}\exp(-\|c_{i}(q)\|_{2}/\kappa)$, где $\kappa$ — параметр сглаживания. По мере $\kappa\to 0$ эта формулировка приближается к исходному поведению активации, подобному индикаторной функции..

Контактное ограничение в 5, которое кодирует режимы отсутствия контакта, статического трения и скольжения, аналогичным образом вводит разрывные и исчезающие градиенты. Мы сглаживаем это путем релаксации условия дополняемости в 5 до $(J_{i}(q)\,(q^{+}-q)+\begin{bmatrix}\phi_{i}(q)&0&0\end{bmatrix})^{\top}\lambda_{i}=\kappa$, что восстанавливает исходное ограничение дополняемости при $\kappa=0$. Сглаженная динамика используется исключительно для вычисления градиента, а негладкая динамика используется для прямого моделирования, что позволяет сохранить точность модели..

4.2 Симулятор-информированный параллельный на графическом процессоре SLS с обратной связью по выходу

Для планирования устойчивых стратегий обратной связи по выходу для деформируемого объекта мы стремимся приближенно решить идеализированную задачу оптимизации (11), чтобы получить стратегию обратной связи по выходу $\boldsymbol{\pi}:=(\pi_{0}(\cdot),\ldots\pi_{N-1}(\cdot))$,

$$\min_{\boldsymbol{\pi}} J(\boldsymbol{\pi})$$ (11)

$$\text{s.t.} q_{k+1}=f^{*}(q_{k},u_{k})+E(q_{k})w_{k}, \forall k\in[N],\qquad q_{0}\in\mathcal{Q}_{0},$$

$$y_{k+1}=h(q_{k})+F(q_{k})e_{k}, \forall k\in[N],\qquad u_{k}=\pi_{k}(y_{0},\ldots,y_{k}), \forall k\in[N],\hskip-5.0pt$$

$$(q_{k},u_{k})\in\mathcal{S}, \forall w_{k}\in\mathcal{B}^{n_{q}},\;\forall e_{k}\in\mathcal{B}^{n_{y}},\;\forall k\in[N].$$

где $[N]:=\{0,\ldots,N-1\}$, $\mathcal{Q}_{0}$ обозначает множество возможных начальных условий, $\mathcal{S}=\left\{(q,u)\in\mathbb{R}^{n_{q}+n_{u}}\;\middle|\;c_{i}^{\top}(q,u)+b_{i}\leq 0,\;i=1,\ldots,n_{c}\right\}$ определяет безопасное множество. Хотя мы рассматриваем нелинейные ограничения в разд. 5, для упрощения обозначений мы описываем метод для случая аффинных ограничений. Нелинейные ограничения могут быть надежно соблюдены с использованием подхода [58]. Динамика и наблюдения определены в (1). Для эффективного решения (11) мы расширяем GPU-SLS [17], который ограничен обратной связью по состоянию (т.е. требует полного знания состояния), до настройки с обратной связью по выходу, основываясь на [25]. Следуя [25], задачу синтеза SLS с обратной связью по выходу можно записать как,

$$\hskip-10.0pt\min_{\mathbf{z},\mathbf{v},\mathbf{\Phi},\boldsymbol{\tau}}\quad J(\mathbf{z},\mathbf{v},\boldsymbol{\tau},\mathbf{\Phi})$$ (12a)

s.t. $$z_{k+1}=f(z_{k},v_{k}),\qquad\forall k\in[N]$$ (12b)

$$\hskip-4.0pt\begin{bmatrix}\mathbf{I-ZA(z,v)},\ \mathbf{-ZB(z,v)}\end{bmatrix}\mathbf{\Phi}=\begin{bmatrix}\mathbf{I},\ \mathbf{0}\end{bmatrix},\ \mathbf{\Phi}\begin{bmatrix}(\mathbf{I-ZA(z,v)})^{\top}\hskip-6.0pt,\ \mathbf{(-ZC(z,v))^{\top}}\end{bmatrix}^{\top}=\begin{bmatrix}\mathbf{I},\ \mathbf{0}\end{bmatrix}^{\top}\hskip-8.0pt,$$ (12c)

$$\textstyle\sum_{j=0}^{k-1}\left\|c_{i}^{\top}\mathbf{\Phi}_{k,j}^{\text{w}}E_{j}\right\|_{2,r}\hskip-3.0pt+\left\|c_{i}^{\top}\mathbf{\Phi}_{k,j}^{\text{e}}F_{j}\right\|_{2,r}\hskip-3.0pt+c_{i}^{\top}\begin{bmatrix}z_{k}\\ v_{k}\end{bmatrix}\leq-b_{i},\qquad\forall i\in[n_{c}],\;\forall k\in[N]$$ (12d)

$$\textstyle\sum_{j=0}^{k-1}\left\|\mathbf{\Phi}_{k,j}^{\text{w}}E_{j}\right\|_{2,r}+\left\|\mathbf{\Phi}_{k,j}^{\text{e}}F_{j}\right\|_{2,r}\leq\tau_{k},\qquad\forall k\in[N]$$ (12e)

где $\mathbf{A(z,v),B(z,v)}$ и $\mathbf{C(z,v)}$ обозначают линеаризации моделей динамики и наблюдений относительно номинальной траектории $\mathbf{(z,v)}$, $\|\cdot\|_{2,r}$ обозначает построчную евклидову норму, а $E_{j}$ и $F_{j}$ представляют собой соответствующие масштабирующие коэффициенты линеаризованных возмущений состояния и наблюдений. Матрица $\mathbf{Z}$ обозначает оператор блочного сдвига вниз с $I_{n_{q}}$ на первой поддиагонали и нулями в остальных позициях. Операторы замкнутого отклика $\mathbf{\Phi}^{\text{w}}$ и $\mathbf{\Phi}^{\text{e}}$ характеризуют распространение возмущений процесса и наблюдений соответственно посредством ограничения (12c), а $\tau_{k}$ определяет ширину достижимой трубки. (12d)–(12e) вычисляют достижимые трубки с обратной связью по выходу при наличии возмущений $w_{k}$ и $e_{k}$ и используют их для сужения ограничений таким образом, чтобы достижимое множество замкнутой системы оставалось внутри $\mathcal{S}$. Для решения (12), [25] декомпозирует оптимизацию на три этапа: 1) генерация номинальной траектории $\mathbf{(z,v)}$, 2) оптимизация контроллера обратной связи по состоянию $\mathbf{\bar{\Phi}}$ и, наконец, 3) оптимизация контроллера обратной связи по выходу $\mathbf{\hat{\Phi}}$. Первые два этапа обрабатываются непосредственно с помощью GPU-SLS [17]. Поэтому мы фокусируемся на параллельной реализации третьего этапа, выражая рекурсии наблюдателя как ассоциативные операторы, что позволяет использовать параллельное префиксное сканирование на GPU. Оптимальное решение для выходного контроллера может быть найдено с помощью одной обратной рекурсии Калмана,

$$\Pi_{k,0}=\Xi\Xi^{\top};\quad L_{k,j+1} =-(F_{j}F_{j}^{\top}+C_{j}\Pi_{k,j}C_{j}^{\top})^{-1}C_{j}\Pi_{k,j}A_{j}^{\top}$$ (13)

$$\Pi_{k,j+1} =E_{j}E_{j}^{\top}+A_{j}\Pi_{k,j}A_{j}^{\top}+A_{j}\Pi_{k,j}C^{\top}_{j}L_{k,j+1},$$

где $\Xi$ обозначает неопределенность начального состояния, а $N$ независимые прямые распространения,

$$\hat{\mathbf{\Phi}}^{x}_{k,k}=I; \hat{\mathbf{\Phi}}^{y}_{k,j}=\hat{\mathbf{\Phi}}^{x}_{k,j}L^{\top}_{k,j}, \hat{\mathbf{\Phi}}^{x}_{k,j}=\hat{\mathbf{\Phi}}^{x}_{k,j+1}(A_{j}+L_{k,j}^{\top}C_{j}),\quad\forall k\in[N],\ \ \forall j\in[k],$$ (14)

Матрицы $\mathbf{\Phi}^{\text{w}}$ и $\mathbf{\Phi}^{\text{e}}$ могут быть затем собраны с использованием $\mathbf{M=I-ZA}$ и

$$\mathbf{\Phi}^{\text{xw}}=\bar{\mathbf{\Phi}}^{\text{x}}+\hat{\mathbf{\Phi}}^{\text{x}}-\bar{\mathbf{\Phi}}^{\text{x}}\mathbf{M}\hat{\mathbf{\Phi}}^{\text{x}}, \mathbf{\Phi}^{\text{uw}}=\bar{\mathbf{\Phi}}^{\text{u}}\mathbf{(I-M\hat{\Phi}^{x})}, \mathbf{\Phi}^{\text{xe}}=\mathbf{\hat{\Phi}}^{\text{y}}-\bar{\mathbf{\Phi}}^{\text{x}}\mathbf{M}\hat{\mathbf{\Phi}}^{\text{y}}, \mathbf{\Phi}^{\text{ue}}=-\bar{\mathbf{\Phi}}^{\text{u}}\mathbf{M}\hat{\mathbf{\Phi}}^{\text{y}}.$$ (15)

И (13), и (14) требуют последовательного выполнения умножений плотных матриц на протяжении всего горизонта планирования. В связи с этим мы используем параллельные ассоциативные сканирования на GPU. Мы определяем условную функцию ценности, $V_{i\rightarrow l}(\mathbf{\Phi}_{k,i},\mathbf{\Phi}_{k,l})\!=\!\max_{\gamma}\tfrac{1}{2}\mathbf{\Phi}_{k,i}^{\top}\tilde{P}_{i,l}\mathbf{\Phi}_{{k,i}}-\tfrac{1}{2}\gamma^{\top}\tilde{D}_{i,l}\gamma-\gamma^{\top}(\mathbf{\Phi}_{k,l}-\tilde{A}_{i,l}\mathbf{\Phi}_{k,i})$, и правила комбинации,

$$\tilde{P}_{i,l} =\tilde{P}_{i,m}\otimes\tilde{P}_{m,l}:=(\tilde{A}_{i,m}^{\top}(I+\tilde{P}_{m,l}\tilde{D}_{i,m})^{-1}\tilde{P}_{m,l}\tilde{A}_{i,m}+\tilde{P}_{i,m},$$ (16)

$$\tilde{A}_{i,l} =\tilde{A}_{i,m}\otimes\tilde{A}_{m,l}:=\tilde{A}_{m,l}(I+\tilde{D}_{i,m}\tilde{P}_{m,l})^{-1}\tilde{A}_{i,m},$$

$$\tilde{D}_{i,l} =\tilde{D}_{i,m}\otimes\tilde{D}_{m,l}:=\tilde{A}_{m,l}(I+\tilde{D}_{i,m}\tilde{P}_{m,l})^{-1}\tilde{D}_{i,m}(\tilde{A}_{m,l})^{\top}+\tilde{D}_{m,l},$$

с начальными значениями как

$$\begin{gathered}\tilde{A}_{i+1,i}=A_{i}^{\top},\ \ \ \tilde{D}_{i+1,i}=C_{i}^{\top}(F_{i}F_{i}^{\top})^{-1}C_{i},\ \ \ \tilde{P}_{i+1,i}=E_{i}E_{i}^{\top},\ \ \ \tilde{P}_{1,0}=\Xi\Xi^{\top},\ \ \ \tilde{A}_{1,0}=\tilde{D}_{1,0}=0.\end{gathered}$$ (17)

Затем мы можем восстановить коэффициенты усиления Калмана $L_{k,j}$ посредством прямого параллельного ассоциативного сканирования, используя (16)–(17) для восстановления $\Pi_{k,j}=\tilde{P}_{0,j}:=\tilde{P}_{0,1}\otimes\tilde{P}_{1,2}\otimes\cdots\otimes\tilde{P}_{j-1,j}$, а затем $L_{k,j}$ из (13). Чтобы извлечь $\mathbf{\hat{\Phi}}$, мы используем аналогичную процедуру и определяем распространение условного наблюдателя $\hat{N}^{(k)}_{i\rightarrow l}(\hat{\mathbf{\Phi}}^{x}_{k,i},\hat{\mathbf{\Phi}}^{x}_{k,l})=\hat{\mathbf{\Phi}}^{x}_{k,i}\hat{A}_{i,l}^{(k)}$ для каждого введения возмущения $k$ с правилом комбинации $\hat{A}^{(k)}_{m,l}\hat{A}^{(k)}_{i,m}$ и инициализацией $\hat{A}^{(k)}_{i+1,i}=A_{i}+L_{k,i}^{\top}C_{i}$. Мы получаем решение для (14) с помощью прямого параллельного ассоциативного сканирования, $\hat{\mathbf{\Phi}}^{x}_{k,i}=\hat{N}_{j+1\rightarrow j}^{(k)}\otimes\hat{N}_{j\rightarrow j-1}^{(k)}\otimes\cdots\otimes\hat{N}_{i+1\rightarrow i}^{(k)}$ и $\hat{\mathbf{\Phi}}^{y}_{k,j}=\hat{\mathbf{\Phi}}^{x}_{k,j}L^{\top}_{k,j}$, решая зависящий от наблюдателя замкнутый отклик за логарифмическое время, что аналогично сложности GPU-SLS.

4.3 Ускорение обучения с подкреплением на основе моделей посредством аналитических градиентов стратегии (APG)

Дифференцируемый симулятор, предложенный в разд. 4.1, может быть использован для поддержки обучения с подкреплением (RL) на основе моделей несколькими способами; здесь мы укажем два из них. Во-первых, он может генерировать экспертные траектории для управления обучением стратегий. Это особенно полезно при манипулировании деформируемыми объектами с частыми контактами, где разреженные вознаграждения и прерывистые контакты могут сделать исследование с нуля крайне неэффективным с точки зрения выборки. Во-вторых, за счет включения симулятора со сглаженными контактами в качестве дифференцируемого слоя (10), прямые градиенты стратегии могут направлять стратегии к обнаружению и поддержанию контакта с меньшей зависимостью от стохастического исследования, повышая стабильность и эффективность выборки при обучении стратегий. Мы обозначаем этот вариант MBRL нашего метода как APG, поскольку он использует аналитические градиенты стратегии для ускорения обучения стратегий, и оценивается в разд. 5⟧.

4.4 Калиброванные гарантии достижимости посредством конформного прогнозирования ⟦K0⟧)

Формулировка SLS в разд. 4.2 требует наличия множеств неопределенности для динамики замкнутого контура и модели наблюдения для подтверждения безопасности реальной системы (1). Однако на аппаратном уровне независимые метки для истинного возмущения процесса или ошибки оценки состояния, как правило, недоступны. Вместо этого мы измеряем расхождение между следующим наблюдением, предсказанным моделью, и следующим наблюдением, полученным от системы восприятия, и используем CP для калибровки совокупной ошибки одношагового прогнозирования в координатах контроллера. В нашей реализации (разд. 5) система восприятия оценивает полное состояние веревки по изображениям RGB-D. Таким образом, в (1b) мы принимаем $h(q)=q$, следовательно $y_{k+1}=q_{k}+e_{k}$ с $\|e_{k}\|_{2}\leq\rho$, где $\rho$ — это граница ошибки измерения в худшем случае, исходящая из спецификаций точности камеры. Мы определяем воспринимаемое состояние $\bar{q}_{k}:=y_{k+1}$ как зашумленную оценку полного состояния, используемую контроллером. Хотя физическая система развивается в латентном пространстве $q_{k}$, наша реализация SLS выполняет планирование по $\bar{q}_{k}$; откалиброванный с помощью CP остаток подтверждает трубки для $\bar{q}_{k}$, которые консервативно расширяются на $\rho$ для получения трубок истинного состояния..

Пусть $\mathcal{D}_{\mathrm{cal}}:=\{(\bar{q}_{i},u_{i},\bar{q}_{i}^{+})\}_{i=1}^{n}$ — это отложенный калибровочный набор переходов воспринимаемых состояний, где $\bar{q}_{i}^{+}$ — следующее воспринимаемое состояние; эквивалентно, выборка представляет собой $(y_{k+1},u_{k},y_{k+2})$. Мы определяем оценки несоответствия $s_{i}:=\|\bar{q}_{i}^{+}-f(\bar{q}_{i},u_{i})\|_{2}$, и пусть $s_{(1)}\leq\cdots\leq s_{(n)}$ — это отсортированные оценки. Для уровня покрытия траектории $\delta$ на горизонте $T$, установим $\bar{\delta}:=\delta/T$ и $q:=s_{(\lceil(n+1)(1-\bar{\delta})\rceil)}$, с $q=+\infty$, если индекс превышает $n$. Множество CP для следующего воспринимаемого состояния имеет вид $\mathcal{C}(\bar{q},u):=\left\{\bar{q}^{+}\in\mathcal{Q}\;\middle|\;\|\bar{q}^{+}-f(\bar{q},u)\|_{2}\leq r_{\textrm{CP}}\right\}$. Мы применяем SLS к суррогату воспринимаемого состояния $\bar{q}_{k+1}=f(\bar{q}_{k},u_{k})+r_{\textrm{CP}}Iw_{k}$, с $w_{k}\in\mathcal{B}^{n_{q}}$. Это откалиброванная модель оценки полного состояния, используемая контроллером, а не независимо идентифицированная модель шума процесса для латентного состояния $q_{k}$. Пусть $\bar{\mathcal{T}}_{k}^{q}$ и $\mathcal{T}_{k}^{u}$ обозначают результирующие трубки достижимости воспринимаемого состояния и управления. Предполагая взаимозаменяемость между оценками остатков замкнутого контура $\|\bar{q}_{k+1}-f(\bar{q}_{k},u_{k})\|_{2}$ и калибровочными оценками, CP и робастный SLS обеспечивают (доказательство в приложении A).⟧):

Theorem 1 (Ограничение истинного состояния с помощью трубок CP ⟦K0⟧).

Пусть $y_{k+1}=q_{k}+e_{k}$ с $\|e_{k}\|_{2}\leq\rho$, и определим $\bar{q}_{k}:=y_{k+1}$. Предположим, что конформная калибровка и робастный SLS синтез дают трубки воспринимаемого состояния и управления $\bar{\mathcal{T}}_{k}^{q}$ и $\mathcal{T}_{k}^{u}$, удовлетворяющие $\mathbb{P}\left[\bar{q}_{k}\in\bar{\mathcal{T}}_{k}^{q},\;u_{k}\in\mathcal{T}_{k}^{u},\;k=0,\ldots,N\right]\geq 1-\delta$. Тогда истинные траектории состояния и управления удовлетворяют $\mathbb{P}\left[q_{k}\in\bar{\mathcal{T}}_{k}^{q}\oplus\rho\mathbb{B}^{n_{q}},\;u_{k}\in\mathcal{T}_{k}^{u},\;k=0,\ldots,N\right]\geq 1-\delta$⟧.

Соответственно, мы определяем трубку истинного состояния как $\mathcal{T}_{k}^{q}:=\bar{\mathcal{T}}_{k}^{q}\oplus\rho\mathcal{B}^{n_{q}}$. Таким образом, для обеспечения ограничений безопасности истинного состояния $q_{k}\in\mathcal{Q}_{\mathrm{safe}}$ достаточно наложить ужесточенное условие воспринимаемого состояния $\bar{\mathcal{T}}_{k}^{q}\subseteq\mathcal{Q}_{\mathrm{safe}}\ominus\rho\mathcal{B}^{n_{q}}$. Зависящие от состояния границы неопределенности могут быть включены путем замены скалярного радиуса $r_{\textrm{CP}}$ на откалиброванную границу, зависящую от состояния, например, путем адаптации [45] к условиям воспринимаемого состояния.

5 Результаты

Мы сравниваем CORD-SLS и APG с MPPI [52] и методами обучения с подкреплением без модели (а именно, PPO [42]) в задачах манипуляции деформируемыми объектами, эмпирически подтверждаем устойчивость нашего решателя с обратной связью по выходу и оцениваем эффективность нашего метода на аппаратном обеспечении..

Task	Method	Dynamics Evaluations Count	Execution Time Per-Step [s]	Safety Rate [%]	Goal Error [m]
Lift Rope	Ours	5400	0.050 $\pm$ 0.001	100.0 $\pm$ 0.0	0.0548 $\pm$ 0.0003
	MPPI	1024000	0.177 $\pm$ 0.043	100.0 $\pm$ 0.0	0.0681 $\pm$ 0.0038
	PPO	16000000	0.0004 $\pm$ 0.0001	96.5 $\pm$ 0.2	0.0587 $\pm$ 0.0016
	APG	16000000	0.0004 $\pm$ 0.0001	100.0 $\pm$ 0.0	0.0573 $\pm$ 0.0017
Drag Rope	Ours	13500	0.070 $\pm$ 0.012	99.9 $\pm$ 0.1	0.0360 $\pm$ 0.0077
	MPPI	2560000	0.234 $\pm$ 0.018	1.0 $\pm$ 0.1	0.0814 $\pm$ 0.0034
	PPO	40000000	0.0004 $\pm$ 0.0001	97.0 $\pm$ 0.4	0.0320 $\pm$ 0.0092
	APG	40000000	0.0004 $\pm$ 0.0001	93.0 $\pm$ 0.4	0.0302 $\pm$ 0.0061
Fold cloth	Ours	3400	0.758 $\pm$ 0.107	100.0 $\pm$ 0.0	0.0611 $\pm$ 0.0072
	MPPI	64000	18.81 $\pm$ 6.11	64.0 $\pm$ 6.1	0.0738 $\pm$ 0.0079
Flatten cloth	Ours	3400	0.711 $\pm$ 0.077	100.0 $\pm$ 0.0	0.0234 $\pm$ 0.0089
	MPPI	64000	11.10 $\pm$ 4.66	80.0 $\pm$ 0.2	0.0526 $\pm$ 0.0102

⟦Table 1: Метрики времени выполнения и производительности для различных задач.

Сравнение эталонных тестов симуляции

См. Прил. F для сравнения нашего дифференцируемого симулятора с предыдущими симуляторами [12, 11] по точности и времени выполнения. Мы показываем, что наш симулятор достигает сопоставимой точности, ускоряя прямое моделирование и вычисление Якобиана до 700$\times$ и 2000$\times$ раз соответственно. Эта эффективность, наряду с интеграцией сглаживания контактов, обеспечивает частоту управления в реальном времени и низкую ошибку достижения цели в наших экспериментах по планированию деформируемых объектов, которые мы обсудим далее..

Сравнение эталонных тестов планирования в задачах манипуляции деформируемыми объектами

Мы проводим сравнительный анализ нашего подхода на четырех задачах манипуляции деформируемыми объектами, представленных на рис. 2. В задаче подъема веревки пара захватов должна установить контакт с веревкой, чтобы активировать голономную связь, необходимую для ее подъема. Наш метод использует градиенты динамики для направления действий в сторону установления контакта. В отличие от него, MPPI требует значительно большего количества оценок динамики для определения действий, приводящих к контакту, что приводит к увеличению времени выполнения. Среди методов RL, APG использует дифференцируемую динамику для вычисления градиентов стратегии, в то время как PPO инициализируется с помощью 10 экспертных демонстраций, сгенерированных CORD-SLS, что позволяет ему решить задачу в рамках того же бюджета взаимодействий. Без этой предварительной инициализации стратегия PPO не может установить контакт с веревкой после того же количества взаимодействий со средой и, следовательно, не справляется с выполнением задачи..

В задаче перемещения веревки захват должен установить контакт с веревкой и транспортировать ее в целевое местоположение, избегая препятствия. CORD-SLS явным образом учитывает возмущения и вычисляет траекторные трубки (Рис. 2(b), см. Прил. D для полных снимков трубок), обеспечивая надежное избегание препятствий. Хотя избегание препятствий закодировано как штраф в функции стоимости MPPI, лучшие выбранные траектории часто проходят через препятствие, что приводит к очень низкому уровню безопасности. Дальнейшее увеличение штрафа за препятствие приводит к тому, что веревка остается вблизи своего начального положения. APG находит политику с более низкой стоимостью, чем PPO, с меньшим количеством выборок симулятора (Рис. 6b, Прил. C), но оба метода RL демонстрируют нарушения ограничений, поскольку они не учитывают явным образом возмущения во время выполнения..

Для задач складывания и разглаживания ткани, несмотря на многомерное пространство состояний (300 степеней свободы), параллелизованный на GPU симулятор и решатель SLS позволяют вычислять каждое обновление MPC менее чем за одну секунду (см. прил. D для достижимых трубок). MPPI, для сравнения, требует значительно большего количества оценок динамики для достижения сопоставимой производительности, что приводит к увеличению времени выполнения, а также к более высоким показателям ошибки достижения цели и нарушения ограничений. Мы не включаем базовые модели RL для этих задач, поскольку система ткани с 300 степенями свободы находится в режиме, где оптимизация траектории в режиме реального времени остается выполнимой, в то время как обучение нейронной стратегии становится чрезмерно затратным с точки зрения выборки..

⟦(a) ⟧

⟦(b) ⟧

⟦(c) ⟧

⟦(d) ⟧

⟦Figure 2: Таймлапс нашего метода для каждой задачи. (а) Подъем веревки, (b) перемещение веревки вокруг препятствия с использованием трубок неопределенности, (c) складывание ткани, (d) расправление ткани. Формулируя управление как оптимизацию траектории, наш метод обобщается на различные задачи без необходимости переобучения под конкретную задачу..

⟦Figure 3: (a): Решенная траектория для задачи манипуляции веревкой с использованием CORD-SLS. (b): Устойчивые трубки для четырех узлов веревки и соответствующие им развертки. Все развертки остаются внутри своих трубок, демонстрируя устойчивость как к динамической ошибке, так и к неопределенности измерений. См. Приложение E для достижимых трубок по всем размерностям состояния..

Выходная обратная связь

На рис. 3 мы ставим перед контроллером задачу надежного перемещения веревки через препятствие с учетом симулированного шума измерений и динамического шума. На рис. 3(a) мы показываем надежные трубки и траекторию, вычисленные нашим решателем, которые учитывают динамическую неопределенность и неопределенность измерений. На рис. 3(b) мы показываем подмножество надежных трубок для 4 узлов веревки и их развертки, дополнительно иллюстрируя, что контроллер остается устойчивым к шуму измерений и динамическому шуму на протяжении всего выполнения..

Аппаратное обеспечение

Наконец, мы проверяем наш подход на аппаратном обеспечении. Как показано на рис. 1, мы рассматриваем две задачи манипулирования деформируемыми объектами: манипулирование веревкой с препятствием и складывание ткани. Для обеих задач мы оцениваем состояние, используя RGB-D наблюдения, где SAM [9] инициализирует и привязывает ключевые точки, которые затем отслеживаются в режиме реального времени с помощью TAPNext++ [22]. Затем мы используем эти позы ключевых точек для аппроксимации и реконструкции полного состояния объекта. Более подробную информацию о конвейере восприятия см. в приложении B. Эксперименты проводятся с двумя манипуляторами KUKA LBR iiwa, при этом решатель MPC работает на графическом процессоре NVIDIA RTX4070 Ti. Чтобы откалибровать границу CP, мы применяем 100 случайных управлений на аппаратном обеспечении и сравниваем предсказанные и измеренные одношаговые переходы состояний. Затем мы используем округленный эмпирический радиус ошибки 0,03 м, который покрывает 87% калибровочных остатков. На рис. 1(a) мы показываем, что благодаря гладкости контактов и дифференцируемости нашего симулятора манипуляторы способны вступать в контакт с веревкой и перемещать ее через препятствие. На рис. 1(b) мы визуализируем надежные трубки, сгенерированные в выбранные моменты времени, которые учитывают как динамическую неопределенность, так и неопределенность измерений, показывая, что мы остаемся в безопасности, несмотря на неопределенность. Наконец, на рис. 1(c) мы показываем, как манипуляторы успешно завершают задачу складывания ткани, демонстрируя, что наш симулятор и решатель применимы к задачам с высокоразмерными деформируемыми объектами..

6 Обсуждение, ограничения и заключение

В данной статье представлена CORD-SLS, структура для безопасного манипулирования деформируемыми объектами с частыми контактами, которая объединяет дифференцируемое моделирование, надежное управление с обратной связью по выходу с ускорением на GPU и калибровку неопределенности на основе данных. Благодаря плавной контактной динамике, дифференцируемому моделированию и конформному прогнозированию, CORD-SLS обеспечивает оптимизацию траектории в реальном времени для многомерных деформируемых объектов при прерывистом контакте, сохраняя при этом высокую вероятность безопасности..

Ограничения

Сглаживание контактов вносит погрешности вблизи моментов перехода контактов, с которыми можно справиться с помощью [28]. Восприятие деформируемых объектов остается сложной задачей в условиях окклюзии и сложных топологий, что может быть решено за счет более совершенных конвейеров восприятия, таких как многоракурсное RGB-D зондирование или обученные выходные уравнения. Наконец, использование памяти для систем более высокой размерности и длинных горизонтов становится узким местом, что стимулирует использование низкоранговых операторов отклика SLS..

Приложения

Ниже представлен обзор наших приложений. В приложении A мы приводим доказательство теоремы 1, которое показывает, как конформные трубки воспринимаемого состояния подразумевают удержание истинного состояния после расширения трубок на величину границы ошибки восприятия. В приложении B мы приводим дополнительные сведения об аппаратных конвейерах восприятия, используемых для экспериментов по манипулированию веревкой и складыванию ткани, включая настройку камер, сегментацию на основе SAM3, отслеживание ключевых точек TAPNext++, реконструкцию RGB-D, аппроксимацию сплайнами и процедуры реконструкции состояния. В приложении C мы приводим дополнительные результаты обучения с подкреплением, сравнивающие PPO и APG в задачах подъема и перетаскивания веревки, демонстрируя повышенную эффективность выборки, достигнутую за счет дифференцирования через наш симулятор. В приложении D мы приводим дополнительные визуализации трубок MPC с обратной связью по состоянию для задач перетаскивания веревки и складывания ткани, а также обсуждаем, как перепланирование влияет на взаимосвязь между выполненными траекториями и ранее вычисленными робастными трубками. В приложении E мы приводим дополнительные визуализации трубок с обратной связью по выходу для задачи манипулирования веревкой, демонстрируя развертывания при случайных динамических и измерительных возмущениях под управлением робастного контроллера на основе восприятия, синтезированного с помощью CORD-SLS. В приложении F мы сравниваем наш дифференцируемый симулятор с предыдущими методами с точки зрения точности и скорости моделирования..

Appendix A Доказательства

Для удобства мы переформулируем теорему 1, а затем приведем доказательство.

Theorem A.1 (Удержание истинного состояния с помощью конформных трубок воспринимаемого состояния).

Пусть $y_{k+1}=q_{k}+e_{k}$ с $\|e_{k}\|_{2}\leq\rho$, и определим $\bar{q}_{k}:=y_{k+1}$. Предположим, что конформная калибровка и робастный синтез SLS дают трубки воспринимаемого состояния и управления $\bar{\mathcal{T}}_{k}^{q}$ и $\mathcal{T}_{k}^{u}$, удовлетворяющие $\mathbb{P}\left[\bar{q}_{k}\in\bar{\mathcal{T}}_{k}^{q},\;u_{k}\in\mathcal{T}_{k}^{u},\;k=0,\ldots,N\right]\geq 1-\delta$. Тогда истинные траектории состояния и управления удовлетворяют $\mathbb{P}\left[q_{k}\in\bar{\mathcal{T}}_{k}^{q}\oplus\rho\mathbb{B}^{n_{q}},\;u_{k}\in\mathcal{T}_{k}^{u},\;k=0,\ldots,N\right]\geq 1-\delta$.

Доказательство.

В случае, если $\bar{q}_{k}\in\bar{\mathcal{T}}_{k}^{q}$ и $u_{k}\in\mathcal{T}_{k}^{u}$ для всех $k$, мы имеем

$$q_{k}=\bar{q}_{k}-e_{k}.$$

С момента $\|e_{k}\|_{2}\leq\rho$,

$$q_{k}\in\bar{\mathcal{T}}_{k}^{q}\oplus\rho\mathbb{B}^{n_{q}}$$

для всех $k$. Событие включения воспринимаемого состояния имеет вероятность не менее $1-\delta$, поэтому событие включения расширенного истинного состояния имеет ту же нижнюю границу вероятности. ∎

Appendix B Восприятие

Мы более подробно описываем аппаратные экспериментальные установки и архитектуру восприятия для эксперимента по манипулированию веревкой, а затем для эксперимента по складыванию ткани. Для обоих экспериментов мы используем два манипулятора Kuka iiwa с приводами Schunk для захвата. Мы используем два компьютера: один, оснащенный NVIDIA RTX4090, отвечает за работу конвейера восприятия и связь с манипуляторами, а другой, оснащенный NVIDIA RTX4070 Ti Super, предназначен для работы решателя. Два компьютера взаимодействуют через ROS2 посредством Ethernet-соединения..

Мы устанавливаем две камеры глубины Orbbec Gemini 336L в рабочую зону: одна камера направлена на переднюю часть рабочей зоны, а другая — на заднюю. Обе камеры и манипуляторы откалиброваны для передачи измерений в единой системе координат рабочей станции..

B.1 Веревка

⟦Figure 4: Конвейер восприятия для оценки состояния веревки. Исходные изображения с фронтальной камеры (a) обрабатываются сегментационной сетью SAM3 (b). Полученная маска скелетируется (c), а точки скелета отслеживаются с помощью TAPNext++ (d). Затем точки RGB сопоставляются с соответствующей картой глубины RGB-D (e). Сплайн подгоняется через восстановленные трехмерные точки (f), а фиксированные расстояния между узлами выбираются вдоль сплайна (g) для реконструкции состояния веревки (h).

Мы используем плетеную веревку диаметром приблизительно 1 см и общей длиной 1,5 метра. Чтобы разместить веревку в пределах рабочего пространства, мы связываем оба конца веревки, уменьшая ее эффективную длину примерно до 0,8 м. Для нашей модели симуляции мы используем 10 звеньев.

Наш основной конвейер восприятия объединяет семантическую сегментацию с отслеживанием ключевых точек. На рис. 4 мы показываем снимки нашего конвейера восприятия. Сначала мы используем Segment Anything Model 3 (SAM3) с подсказкой «rope» для сегментации веревки на RGB-изображении. Полученная маска скелетируется для создания центральной линии шириной в один пиксель вдоль веревки. Затем мы выбираем фиксированное количество точек вдоль этой центральной линии и используем их для инициализации и периодического повторного привязывания трекера TapNext++. Получая эти RGB-пиксели, TapNext++ отслеживает движение этих ключевых точек по кадрам видео, обеспечивая непрерывное отслеживание ключевых точек веревки между обновлениями сегментации..

Такая гибридная конструкция помогает сбалансировать семантическую устойчивость и эффективность выполнения. Хотя SAM3 обеспечивает обнаружение веревки на основе концепций, в наших экспериментах мы обнаружили, что один вывод занимает примерно 200 мс, что создает слишком большую задержку для аппаратного управления в реальном времени. TapNext++, с другой стороны, работает с меньшей задержкой и отслеживает ключевые точки веревки кадр за кадром, но со временем может отклоняться, поскольку не опирается на язык или семантику объектов. Поэтому мы используем SAM3 периодически для повторной инициализации или перепривязки TapNext++, в то время как TapNext++ обеспечивает отслеживание ключевых точек с более высокой частотой между обновлениями SAM3..

Отслеженные двумерные ключевые точки затем сопоставляются с картой глубины для восстановления их трехмерных положений в системе координат рабочей станции. Чтобы уменьшить шум и преобразовать эти измерения в представление состояния, используемое в нашем симуляторе, мы аппроксимируем трехмерные точки сплайном и выполняем передискретизацию вдоль сплайна в соответствии с требуемой длиной сегмента веревки. В результате мы получаем полную оценку состояния веревки в виде упорядоченной последовательности трехмерных узлов с фиксированным расстоянием между сегментами..

Мы обнаружили, что частота перепривязки SAM3 создает компромисс между надежностью и временем выполнения. Запуск SAM3 один раз в секунду приводит к тому, что TapNext++ работает с частотой примерно 15 Гц, в то время как запуск SAM3 один раз в 5 секунд позволяет TapNext++ работать с частотой примерно 25 Гц. Такое замедление, вероятно, связано с тем, что SAM3 конкурирует с TapNext++ за память графического процессора и ресурсы ядра. Хотя этот конвейер работает эмпирически, результирующая скорость восприятия все еще ниже, чем у решателя, время решения которого в среднем составляет менее 20 мс. В результате решатель иногда приходится задерживать, чтобы позволить системе восприятия наверстать упущенное..

Для устранения этого узкого места в виде задержки мы также внедряем более быстрый конвейер реконструкции веревки, который жертвует некоторой точностью восприятия ради более высокой частоты управления. На начальном этапе захвата, когда веревка неподвижна, мы используем полный конвейер сегментации и отслеживания для оценки состояния веревки. После того как манипуляторы захватывают веревку, мы реконструируем ее, используя известные положения захватов и известную длину веревки между двумя манипуляторами. Эта реконструкция предполагает, что форма веревки определяется преимущественно гравитационным провисанием. Хотя это приближение менее точно, чем прямое восприятие, оно обеспечивает достаточно разумную оценку конфигурации веревки при существенно более высоких частотах, что делает его более подходящим для управления с обратной связью..

B.2 Ткань

⟦Figure 5: Конвейер восприятия для оценки состояния ткани. Исходные изображения с фронтальной камеры (a) обрабатываются сетью сегментации SAM3 (b). Полученная RGB-маска сопоставляется с картой глубины для восстановления частичного трехмерного облака точек ткани (c). К наблюдаемой границе подбирается сплайн, который расширяется с учетом скрытых участков, что гарантирует соответствие длины сплайна известной длине ткани (d). Затем к облаку точек подбираются узлы сетки, а точки в скрытых областях генерируются в соответствии со сплайном (e). Это приводит к полному представлению состояния ткани, которое передается в симулятор (f).

Мы используем ткань размером приблизительно 38 см x 38 см. Для нашей модели симуляции ткань дискретизирована в сетку 7 x 7 ячеек, что соответствует решетке из 8 x 8 узлов и дает в общей сложности 64 узла ткани. Полное состояние имеет размерность ($64\times 3+3\times 2=198$), состоящую из 3D-позиций узлов ткани и двух 3D-позиций захватов..

Оценка состояния ткани является более сложной задачей, чем оценка состояния веревки, из-за ее более высокой размерности, высокой деформируемости и частого самоперекрытия. Чтобы сделать оценку состояния для складывания ткани более управляемой, мы упрощаем восприятие, предполагая, что координаты (x) и (z) являются постоянными вдоль каждого ряда ткани. Другими словами, каждый ряд представлен как прямая линия, охватывающая ширину ткани в направлении (y), что сводит задачу оценки состояния к восстановлению геометрии осевой линии по рядам. Чтобы сделать физическую задачу согласованной с этим подходом, мы ограничиваем положение захватов при оптимизации так, чтобы они различались в плоскости (x) и (z) не более чем на 5 см. Мы обобщаем и показываем снимки конвейера восприятия (рис. 5).

Наш конвейер восприятия ткани следует той же общей структуре, что и конвейер восприятия веревки. Сначала мы пропускаем необработанные изображения через конвейер сегментации SAM3, используя подсказку «cloth». Полученная маска сегментации сопоставляется с картой глубины для восстановления трехмерных точек на видимой поверхности ткани. Однако во время складывания части ткани могут быть перекрыты сложенным слоем и, следовательно, не видны фронтальной камере и ее камере глубины. Чтобы справиться с этим, мы проецируем восстановленные точки на плоскость (x)-(z), получая распределение двумерных точек, и аппроксимируем эти точки сплайном центральной линии. Используя этот сплайн, мы вычисляем общую длину дуги и сравниваем ее с ожидаемой длиной ткани. Если длина дуги значительно меньше известной длины ткани, мы рассматриваем недостающую длину как перекрытую часть ткани..

Наконец, мы подгоняем узлы сетки, соответствующие тканевой решетке, к видимому облаку точек и добавляем дополнительные узлы сетки позади видимой ткани для учета перекрытых областей. Затем это реконструированное состояние передается в симулятор для планирования..

Appendix C Сходимость обучения RL

⟦(a) ⟧

⟦(b) ⟧

⟦Figure 6: Сходимость обучения с подкреплением для задачи (а) подъема веревки и (б) перетаскивания веревки.

На рис. 6 мы сравниваем скорость сходимости PPO и APG в задачах подъема веревки и перетаскивания веревки. Благодаря дифференцируемости нашего симулятора, APG может использовать аналитические градиенты стратегии, полученные путем дифференцирования через симулятор, что ускоряет эффективность обучения. PPO, с другой стороны, не может использовать эту градиентную информацию. В обеих задачах APG сходится к более низким затратам быстрее, чем PPO. Это улучшение обусловлено динамикой со сглаженным контактом, которая обеспечивает оценки градиента с меньшей дисперсией и повышает эффективность выборки. В частности, в задаче подъема веревки APG сходится к тем же затратам, что и PPO, в среднем за 45,95% (медиана 36,01%) меньшее количество шагов, чем PPO, и за 16,63% (медиана 27,43%) меньшее количество шагов для задачи перетаскивания веревки. Эти результаты демонстрируют, что наш дифференцируемый симулятор может быть использован для ускорения обучения стратегий RL..

Appendix D Трубки MPC с обратной связью по состоянию

⟦Figure 7: Прямые достижимые трубки в рамках MPC для задачи с буксируемым тросом для всех 66 состояний.

На рис. 7 показаны трубки достижимости для задачи перетаскивания веревки, представленной на рис. 2(b), на выбранных шагах MPC, наложенные на траекторию (темная линия), реализованную с помощью замкнутого контура MPC с перепланированием. Мы отмечаем, что в условиях MPC выполненная траектория может не совпадать с трубками, вычисленными на предыдущих временных шагах, поскольку контроллер выполняет перепланирование после каждого шага выполнения. Возмущения и конечный горизонт прогнозирования могут привести к тому, что последующие оптимизации выберут траекторию с меньшими затратами, которая отклоняется от ранее запланированного пути и связанной с ним робастной трубки. Для обеспечения гарантированной робастной рекурсивной допустимости может быть применен такой подход, как [24]. В качестве альтернативы, развертки с использованием робастного контроллера обратной связи, созданного с помощью SLS, гарантированно остаются внутри вычисленных трубок, но только в пределах конечного горизонта, для которого был синтезирован контроллер, как показано на рис. 9, где представлены трубки для полного горизонта..

На рис. 8 показаны трубки достижимости для задачи складывания ткани, представленной на рис. 2(c), на выбранных шагах MPC. Ограничение лимитирует высоту ткани до 3 см в направлении $z$. Поскольку MPC перепланирует траекторию после каждого шага выполнения, исполняемая траектория не обязательно остается внутри трубок, вычисленных на предыдущих временных шагах. Тем не менее, за счет соблюдения ограничений на предсказания в виде трубок во время каждого перепланирования, контроллер надежно удовлетворяет ограничению по высоте $z$ на протяжении всей задачи..

⟦Figure 8: Прямые достижимые трубки в рамках MPC для задачи складывания ткани для состояний с координатами $z$.

Appendix E Трубки выходной обратной связи

Мы показываем трубки для всех 39 состояний для эксперимента с обратной связью по выходу на рис. 3. Прогоны оцениваются при случайных динамических и измерительных возмущениях, в то время как управляющие воздействия генерируются робастным наблюдателем-контроллером, синтезированным с помощью CORD-SLS..

⟦Figure 9: Трубки достижимости вперед для задачи манипулирования веревкой с обратной связью по выходу для всех 39 состояний. Красные траектории представляют состояния, соответствующие размерности $x$, зеленые — размерности $y$, а синие — размерности $z$.

Appendix F Сравнение симуляторов

В таблице 2 представлены показатели времени выполнения прямого моделирования и вычисления якобиана динамики для веревки из 20 узлов и ткани из 100 узлов. Наш симулятор стабильно демонстрирует самое быстрое время выполнения среди сравниваемых дифференцируемых симуляторов. Кроме того, предварительная факторизация матриц значительно снижает накладные расходы на вычисление градиента, делая время вычисления якобиана динамики сопоставимым с прямым моделированием..

DEFORM [12] демонстрирует более низкую скорость симуляции, поскольку его вычислительный конвейер не скомпилирован в код, исполняемый непосредственно на графическом процессоре. DaXBench [11] использует различные методы симуляции для веревок и ткани, применяя метод материальных точек (MPM) для веревок и масс-пружинную модель для ткани, что приводит к различным вычислительным затратам..

		Forward simulation [ms]	Dynamics Jacobian computation [ms]
Rope	Ours	1.095 $\pm$ 0.162	5.198 $\pm$ 2.533
	DEFORM [12]	49.03 $\pm$ 20.04	2128.81 $\pm$ 55.14
	DaXBench [11]	786.48 $\pm$ 76.23	11561.57 $\pm$ 122.07
Cloth	Ours	15.08 $\pm$ 1.209	10.06 $\pm$ 0.983
	DaXBench [11]	21.71 $\pm$ 0.722	110.19 $\pm$ 2.663

⟦Table 2: Сравнение времени симуляции в различных дифференцируемых симуляторах.

Далее мы проводим сравнительный анализ точности симуляции нашего метода по отношению к DEFORM. Для создания эталонного набора данных мы собираем 100 образцов рандомизированных манипуляционных действий на аппаратном обеспечении, одновременно запуская наш конвейер восприятия для оценки состояния веревки. Для каждого образца мы записываем текущее состояние, управляющее воздействие и результирующее следующее состояние, т.е. ($x^{+}=f(x,u)$). Мы отмечаем, что из-за задержки в конвейере восприятия мы не можем напрямую наблюдать мгновенное состояние после действия и, следовательно, не можем надежно зафиксировать переходные колебания или динамику, обусловленную инерцией. Вместо этого записанное следующее состояние соответствует установившейся конфигурации веревки после затухания движения, что делает набор данных более репрезентативным для квазистатического поведения..

Мы оцениваем оба симулятора в двух конфигурациях. Во-первых, мы приводим точность одношагового прогнозирования базового симулятора, измеряемую как среднее L2-расстояние между предсказанным и наблюдаемым следующими состояниями. Во-вторых, мы оцениваем каждый симулятор с дополненной обученной остаточной моделью. Оригинальная структура DEFORM использует обученный остаток, состоящий из сетевой архитектуры для компенсации ошибок моделирования. Чтобы обеспечить справедливое сравнение, мы обучаем идентичную остаточную архитектуру для обоих симуляторов, используя ту же процедуру обучения и гиперпараметры. Набор данных разделен на 70% обучающих и 30% валидационных данных, и мы приводим среднюю ошибку валидации для моделей с остаточной коррекцией. Для всех экспериментов мы устанавливаем длину веревки равной 0.8м и дискретизируем ее на 10 звеньев.

Ключевое различие между нашим методом и симулятором DEFORM заключается в том, что DEFORM сформулирован как полностью динамический симулятор, тогда как наш метод предполагает квазистатическое поведение. Следовательно, прямое сравнение одного шага прямого моделирования было бы некорректным, поскольку записанные аппаратные данные соответствуют установившимся равновесным конфигурациям, а не переходным процессам. Поэтому, чтобы учесть это расхождение при оценке DEFORM, после выполнения заданной команды мы многократно распространяем динамику с нулевым управляющим воздействием до тех пор, пока веревка не придет в равновесную конфигурацию. Это позволяет получить прогнозы, согласующиеся с квазистатическими состояниями, наблюдаемыми нашей системой восприятия..

В таблице 3 обобщена итоговая точность одношагового прогнозирования. DEFORM достигает примерно на 7% меньшей ошибки прогнозирования, чем наш симулятор. Этот разрыв в производительности ожидаем, поскольку DEFORM моделирует дополнительные физические степени свободы, включая деформацию кручения, посредством формулировки дискретных упругих стержней, в то время как наш симулятор использует модель упругой энергии, которая учитывает только деформацию растяжения и изгиба. В результате некоторые эффекты деформации более высокого порядка не представлены явно, что приводит к потере точности модели. Тем не менее, мы отмечаем, что вычислительная эффективность нашего симулятора и планировщика обеспечивает быстрое перепланирование во время выполнения, позволяя системе оперативно компенсировать неточности моделирования и возмущения. Наконец, мы показываем, что оба симулятора выигрывают от использования обученной модели остаточной коррекции, при этом DEFORM сохраняет небольшое преимущество благодаря своей более сильной базовой физической модели..

		Mean L2 Error [m]
Rope	Ours	0.0150 $\pm$ 0.0040
	DEFORM [12]	0.0139 $\pm$ 0.0029
	Ours with learned Residual	0.0075 $\pm$ 0.0068
	DEFORM with learned Residual [12]	0.0071 $\pm$ 0.0032

⟦Table 3: Сравнение точности моделирования нашего метода с DEFORM.