CoNi-MPC: Cooperative Non-inertial Frame Based Model Predictive Control

CoNi-MPC: Cooperative Non-inertial Frame Based Model Predictive Control

摘要

• 提出了多机器人协同的系统，可用于在移动的平台上降落、和目标物保持特定的相对运动等任务
• 直接在目标的坐标系中控制UAV，不对目标的运动做假设
• 在非惯性系（non-inertial frame）中使用NMPC
• 需要目标的相对姿态和速度、角速度以及加速度
• 该框架不需要准确的状态估计、对目标运动模型的先验知识、频繁的重规划

引言

• 目标的相对姿态和速度可由相对定位方法获得
• 目标的角速度和加速度可由MEMS IMU传感器获得

主要贡献：

• 提出了在非惯性系中使用MPC的无人机-目标（drone-target）相对运动框架。不需要目标在世界坐标系中的姿态和运动。
• 通过相对运动模型，将和目标相关的元素组合在一起并使用目标坐标系中的IMU信息替换。避免了使用世界坐标系中的数据。
• MPC控制器作为一个统一的框架支持多种无人机-目标交互的任务。无需频繁的重规划。

相关工作

其他工作需要全局状态估计和频繁的全局路径重规划，本文的方法可避免这两项

问题表述和CoNi-MPC

符号

$B$：智能体的自身坐标系；$N$：目标的自身坐标系；$W$：世界惯性坐标系

左侧上标：以该坐标系为参考；右侧下标：该坐标系的物理量

标量：小写字母；向量：加粗小写字母；矩阵：加粗大写字母

$\odot$：四元数的Hamilton乘积；$[\boldsymbol{t}]_{\times}$：向量$\boldsymbol{t}$的反对称矩阵（skew-symmetric matrix）

四元数的Hamilton乘积，也称为四元数的乘法，是定义在四元数集合上的一种二元运算。

向量的反对称矩阵通常用于表示三维向量的叉积。给定一个三维向量$\mathbf{v} = (v_1, v_2, v_3)$，其对应的反对称矩阵$A$定义如下： $A = \begin{pmatrix} 0 & -v_3 & v_2 \ v_3 & 0 & -v_1 \ -v_2 & v_1 & 0 \end{pmatrix}$

EGO-Planner: An ESDF-Free Gradient-Based Local Planner for Quadrotors

EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors

https://github.com/ZJU-FAST-Lab/ego-planner

摘要

• 欧氏符号距离场（Euclidean Signed Distance Field，ESDF）常用于估计梯度大小和方向
• 轨迹规划只在ESDF很小的子空间进行，更新整个ESDF不必要
• 本文提出ESDF-free的基于梯度的规划框架
• 罚函数的碰撞项基于对比有碰撞的轨迹和无碰撞的引导路径，只有轨迹碰撞新的障碍物时规划器才提取必要的障碍物信息
• 若轨迹是动力学不可行的，则延长轨迹的时间

引言

构建ESDF的方式有两种：

• 增量式全局更新（incremental global updating）
• 批量式局部计算（batch local calculation）

二者都没有考虑轨迹本身，不能单独直地接服务于轨迹优化

本文提出了ESDF-free基于梯度的局部规划框架（ESDF-free Gradient-based lOcal planning framework，EGO），包含

• 基于梯度的样条优化器
  • 对比轨迹和无碰撞的引导路径
  • 在有碰撞的轨迹上施加力并生成估计的梯度以使轨迹远离障碍物
  • 轨迹会在附近的障碍物之间反弹几次，并稳定在安全区域内
  • 只计算必要的梯度，避免计算和局部轨迹无关的梯度
• 随后的改进过程
  • 若轨迹不符合动力学约束，则进入改进过程
  • 给轨迹分配更长的时间，产生新的B样条
  • 新轨迹拟合之前的轨迹，在轴向和径向（axial and radial directions）上使用不同的惩罚，以增强鲁棒性

主要贡献：

• 提出新的基于梯度的四旋翼无人机局的部规划方法，其直接从障碍物评估和投影梯度信息
• 提出轻量级的轨迹改进方法，使用各向异性（anisotropic）误差惩罚生成更平滑的轨迹
• 把上述方法集成到四旋翼无人机系统中并开源了软件

相关工作

主要分为两部分：

• 基于梯度的运动规划
  • 把局部轨迹生成建模为无约束的非线性优化问题
  • 常依赖ESDF
• 欧式符号距离场ESDF
  • 常用于从带噪声的传感器参数中构造物体
  • 常包含冗余信息

避碰力估计

优化变量为控制点$\mathbf{Q}$，每个控制点独立拥有自己的环境信息

1. 不考虑碰撞，给出满足初末状态约束的B样条曲线$\Phi$
2. 一次迭代中检测到的每一个碰撞，生成一条无碰撞轨迹$\Gamma$
3. 碰撞段的每个控制点$\mathbf{Q}i$在障碍物表面分配一个锚点$\mathbf{p}{i j}$和相应的排斥力方向$\mathbf{v}_{i j}$
4. 省略了下表的每个${\mathbf{p}, \mathbf{v}}$对只对应一个特定的控制点，其生成过程如算法1和图3所示
5. 从$\mathbf{Q}_i$到第j个障碍物的距离定义为

$$ d_{i j}=\left(\mathbf{Q}i-\mathbf{p}{i j}\right) \cdot \mathbf{v}_{i j} $$

Rethinking Integration of Prediction and Planning in Deep Learning-Based Automated Driving Systems: A Review

Rethinking Integration of Prediction and Planning in Deep…

AD论文1: 集成预测与规划综述

自动驾驶中基于深度学习的预测和规划融合方法综述-CSDN博客

自动驾驶中基于深度学习的预测和规划融合方法综述

摘要

摘要主要有三点信息：

• 模块化的自动驾驶系统常把预测和规划作为连续但单独的任务（sequential separate tasks），这种方式没有考虑交通参与者（traffic participants）对自车行为的反应。
• 近期研究表明，集成预测和规划有助于提升驾驶性能。
• 本文系统性地回顾了最先进的（state-of-the-art，SOTA）基于深度学习（deep learning-based）的预测、规划以及综合预测和规划的模型。

引言

• 解耦的预测和规划：这种顺序结构本质上是被动的（reactive），不能表示自车和其他智能体连续的交互
• 集成的预测和规划：可以表示自车和其他智能体连续的交互，这种方式称为 Integrated Prediction and Planning (IPP)

研究范围

• 关注基于深度学习的方法（DL-based methods）
• 关注智能体之间没有直接或间接通信的场景
• 不包含行人行为的预测（pedestrian motion forecasting）
  • 行人相对于车辆动力学约束较弱，使得其行为预测是不同的问题

贡献和结构

• 回顾了预测、规划和IPP的SOTA方法benchmarks
• 对集成的预测和规划进行了分类，
• 分析了各类方法之间的联系，以及这些联系对安全性和鲁棒性的影响
• 揭示了SOTA方法的不足，并根据分类指出未来方向

自动驾驶系统

架构

DL-based自动驾驶系统（automated driving system, ADS）:

• 模块化（modular）
  • 包含感知、预测、规划和控制模块
  • 可利用专业领域的知识，使训练稳定且提升样本效率
  • 模块间的接口可提升可解释性和复用潜在特征（latent features）
    • 如果所有接口都可微，则可端到端训练
• 端到端（end-to-end）
  • 使用单个神经网络，简化设计过程
  • 无使用接口传递信息导致的信息损失
  • 样本效率和可解释性低
• 可解释（interpretable）的端到端
  • 缓解了样本效率和可解释性低的问题

任务定义

自车：Ego Vehicle (EV)

[TOC]

术语（Terminology）

声明式（declaration）：告诉编译器某个东西的名称和类型（type），但略去细节

[TOC]

条款01：视C++为一个语言联邦（View C++ as a federation of languages）

C++有4个主要的次语言（sublanguage）：

• C。包含区块（blocks）、语句（statements）、预处理器（preprocessor）、内置数据类型（built-in data）、数组（arrays）、指针（pointers）等；没有模板（templates）、异常（exceptions）、继承（inheritance）。
• Object-Oriented C++。这是C with classes部分，包含classes（包括构造函数和析构函数）、封装（encapsulation）、继承（inheritance）、多态（polymorphism）、virtual函数等。
• Template C++。这是C++泛型编程（generic programming）部分。
• STL。涉及容器（containers）、迭代器（iterators）、算法（algorithms）、函数对象（function objects）。

Tips：

• C++高效编程守则视情况而变化，和使用的次语言种类有关

条款02：尽量以const、enum、inline替换#define（Prefer consts, enums, and inlines to #define）

该条款可表达为：宁可以编译器替换预处理器

替换原因

[TOC]

由于本书的翻译腔有点佶屈聱牙，从这一章开始lz会增加转述程度使得文本更流畅

条款05：了解C++默默编写并调用哪些函数（Know what functions C++ silently writes and calls）

自动生成的函数

空类（empty class）会由编译器自动声明一个copy构造函数、一个copy assignment操作符、一个析构函数，若未手动声明构造函数则还会自动生命一个default构造函数

[TOC]

前几章的笔记多有不足，这一章会持续改进

条款13：以对象管理资源（Use objects to manage resources）

关键想法

考虑以下易出错的例子：

[TOC]

条款17：让接口容易被正确使用，不易被误用（Make interfaces easy to use correctly and hard to use incorrectly）

限制类型和值

[TOC]

条款26：尽可能延后变量定义式的出现时间（Postpone variable definitions as long as possible）

应延后变量的定义，知道不得不使用该变量的前一刻为止，甚至直到能够给他初值实参为止

当程序的控制流达到变量的定义式时，会有构造成本；当离开变量的作用域时，会有析构成本

[TOC]

条款32：确定你的public继承塑模出is-a关系（Make sure public inheritance models “is-a”）

本条款内容比较简单，略写

public继承的含义

public继承意味着“is-a”的关系

如果一个类D（derived）public继承自类B（base）：

• 每个类型D的对象也是一个类型B的对象，反之则不然
• B比D表示了一个更为一般的概念，而D比B表现了一个更为特殊的概念
• 任何可以使用类型B的地方，也能使用类型D；但可以使用类型D的地方却不可以使用类型B
• D是B，B不是D