雯欂の修仙笔记

1. 重要函数 def is_convex_and_ordered(self, points: NDArray[np.float64]) -> tuple[bool, str] 这个函数的作用是判断按照顺序排列的顶点是否构成凸集，并判断点是顺时针排列"CW"还是逆时针排列"CCW" 实现思路：按顺序取三个点o,a,bo,a,bo,a,b，判断oa→\overrightarrow{oa}oa和ob→\overrightarrow{ob}ob的叉积，如果所有叉积同号则说明aaa点在ob→\overrightarrow{ob}ob的外侧，那么所有内角都为凸角，同时根据叉积的正负还可以判断点是顺时针排列（负）还是逆时针排列（正） def gen_inequal_global(self, vertex: NDArray[np.float64])-> tuple[NDArray[np.float64], NDArray[np.float64]] 此函数的作用是根据顶点生成半空间不等式Ax≤b\bm{Ax} \leq \bm{b}Ax≤b 实...

GitHub TRO Arxiv YouTube Bilibili ROS 此论文以OBCA为基础，具体解释请参考OBCA 3. 问题陈述 3.1 障碍物模型 障碍物可以看成凸机，用锥不等式表示 Om={o∈ROm∣Dmo⪯Ombm}\mathbb{O}_m = \left\{ \bm{o} \in \mathbb{R} ^{O_m} \mid \bm{D}_m \bm{o} \preceq_{\mathcal{O}_m} \bm{b}_m \right\} Om​={o∈ROm​∣Dm​o⪯Om​​bm​} 其中mmm代表障碍物的序号 3.2 机器人模型 可以将机器人本体看成一个紧凑的凸集，初始状态用锥不等式表示，对初始状态进行旋转和平移便可以得到不同时刻的占用空间表示 C={z∈Rnr∣Gz⪯Krh}Zt(st)={Rt(st)z+p(st)∣z∈C}\begin{gather*} \mathbb{C} = \left\{ \bm{z} \in \mathbb{R} ^{n_r} \mid \bm{Gz} \preceq_{\mathcal{K}_r}\bm{h} \r...

GitHub TRO Arxiv YouTube Bilibili Website ROS 3. 问题陈述 3.1 机器人运动学 3.2 机器人模型 可以将机器人本体看成一个紧凑的凸集，初始状态用锥不等式表示为 C={x∈Rn∣Gx⪯Kh}\mathbb{C} = \left\{ \bm{x} \in \mathbb{R} ^n \mid \bm{Gx} \preceq_{\mathcal{K}}\bm{h} \right\} C={x∈Rn∣Gx⪯K​h} 具体解释请参考OBCA 由于机器人在不断运动，因此给定状态st\bm{s}_tst​，第ttt帧的占用空间表示为凸紧集Zt\mathbb{Z}_tZt​ Zt(st)={Rt(st)x+tt(st)∣x∈C}\mathbb{Z}_t(\bm{s}_t) = \left\{ \bm{R}_t(\bm{s}_t)\bm{x} + \bm{t}_t(\bm{s}_t) \mid \bm{x} \in \mathbb{C} \right\} Zt​(st​)={Rt​(st​)x+tt​(st​)∣x∈C} 本质上是对C\ma...

GitHub TCST Arxiv 2. 问题描述 2.1 障碍物建模 障碍物用如下公式表示 O(m)={y∈Rn∣A(m)y⪯Kb(m)}\mathbb{O}^{(m)} = \left\{ \bm{y} \in \mathbb{R} ^n \mid \bm{A}^{(m)} \bm{y} \preceq _{\mathcal{K}} \bm{b}^{(m)}\right\} O(m)={y∈Rn∣A(m)y⪯K​b(m)} 上标(m)^{(m)}(m)表示在时间步为mmm时的障碍物位置；A(m)y⪯Kb(m)\bm{A}^{(m)} \bm{y} \preceq _{\mathcal{K}} \bm{b}^{(m)}A(m)y⪯K​b(m)可以展开为b(m)−A(m)y∈K\bm{b}^{(m)} - \bm{A}^{(m)} \bm{y} \in \mathcal{K}b(m)−A(m)y∈K。若令K=R+l\mathcal{K} = \mathbb{R} ^l_+K=R+l​即非负象限锥，则此广义不等式变为逐元素不等式≤\leq≤，此时障碍物由不同的半空间组成，是...

1. 前言 随机排列算法，目的是将一个数组中的元素随机打乱随机排列 2. Fisher-Yates洗牌算法 2.1 思路 获取数组长度为nnn 随机选取[0,n−1][0,n-1][0,n−1]位置上的数据，并与第一个数据交换 随机选取[1,n−1][1,n-1][1,n−1]位置上的数据，并与第二个数据交换 重复这个过程 2.2 代码参考 void rand_permutation(const int n, int *p) { typedef std::uniform_int_distribution<int> rand_int; typedef rand_int::param_type rand_range; static std::mt19937_64 gen; static rand_int rdi(0, 1); int j, k; for (int i = 0; i...

1. 前言 重点介绍Seidel’s线性规划算法，适用于低纬度线性规划 完整代码详见github 2. 代码详解 2.1 linprog函数 求解的问题为 min⁡C⊤xs.t.Ax≤b\begin{gather*} \min \bm{C}^{\top}\bm{x} \\ s.t. \bm{Ax} \leq \bm{b} \end{gather*} minC⊤xs.t.Ax≤b​ prev存储的是链表中第i个元素的前一个元素的索引 next存储的是链表中第i个元素的后一个元素的索引 perm存储的是随机排列数列 2.2 rand_permutation函数 随机排列，参考随机排列算法博客

cwiseAbs()方法 逐元素取绝对值，返回一个新的同尺寸矩阵 maxCoeff()方法 找到矩阵中数值最大的那个元素，并返回该值 topRightCorner(m,n)方法 取矩阵的右上角子矩阵，大小为m行×n列 bottomRightCorner(m,n)方法 取矩阵的右下角子矩阵，大小为m行×n列 colwise().操作()方法 对矩阵的每一列单独执行同一个操作 normalize()/normalized()方法 对向量归一化，normalize()修改向量本身；normalized()返回新向量，不修改原向量本身 head(d)方法 取向量的前d个元素，引用取出，可修改原值

1. 低维线性规划 1.1 问题描述 最大化（最小化）目标函数 f(x1,x2⋯xd)=c1x1+c2x2+⋯+cdxdf(x_1,x_2 \cdots x_d) = c_1 x_1 + c_2 x_2 + \cdots + c_d x_d f(x1​,x2​⋯xd​)=c1​x1​+c2​x2​+⋯+cd​xd​ 需满足的约束为 a1,1x1+⋯+a1,dxd≤b1a2,1x1+⋯+a2,dxd≤b2⋮⋮⋮an,1x1+⋯+an,dxd≤bna_{1,1}x_1 + \cdots + a_{1,d}x_d \le b_1 \\ a_{2,1}x_1 + \cdots + a_{2,d}x_d \le b_2 \\ \vdots \quad\quad\quad\quad \vdots \quad\quad\quad \vdots \\ a_{n,1}x_1 + \cdots + a_{n,d}x_d \le b_n a1,1​x1​+⋯+a1,d​xd​≤b1​a2,1​x1​+⋯+a2,d​xd​≤b2​⋮⋮⋮an,1​x1​+⋯+an,d​xd​≤bn​ 此线性规划的维度为...

1. 代码结构 1.1 lidar_model 这里的体素索引依靠的是水平角度索引+垂直角度索引，二者的分辨率都是按每2度递增。其中垂直角度索引可以看成行索引，水平角度索引可以看成列索引 水平角度是以map坐标系下的x方向为零，在代码中将-x方向为索引的开始，逆时针为正；垂直角度是以map坐标系下的z方向为零，但是此方向一般为激光雷达的盲区，在代码中将视场角的上边作为索引的开始，通过kVerticalAngleOffset实现偏移 GetHorizontalNeighborNum和GetVerticalNeighborNum像是经验公式，即射线的邻居数量与距离成反比（距离越远，点云越稀疏）与分辨率成反比（分辨率越大，体素越稀疏） 1.2 viewpoint 定义了ViewPoint类用来表示视点，每一个视点中都有一个lidar_model，用来存储这个视点观测到的体素距离 1.2 grid 维护了一组栅格，并实现了线性索引、向量索引、维度下标索引和坐标值间的互相转换 1.3 rolling_grid 实现了3D滚动网格的功能，只维护机器人周围一个固定大小的局部网格。注意这里的网格...

1. 前言 光线投射算法(Ray Casting)是指在一个空间体素模型中，已知起点和终点，求光线从起点到终点经过的所有体素 此算法与直线绘制算法的不同之处在于：一个是寻找点，一个是寻找体素。因此当直线穿过对角点时，直线绘制算法直接加入这个点就行了；光线投射算法需要把对角线周围的体素加入进去 2. 快速体素遍历算法 2.1 思路 已知起点P0P_0P0​和终点PnP_nPn​，可以分别计算两点距离在x轴方向和y轴方向的投影Δx,Δy\Delta x,\Delta yΔx,Δy。可以把每个栅格的边长设为1(栅格索引以1为单位进行移动)，则移动一个格子时的比例（或者称为时间）为tΔx=1/Δxt_{\Delta x}=1/\Delta xtΔx​=1/Δx或tΔy=1/Δyt_{\Delta y}=1/\Delta ytΔy​=1/Δy。分别累积计算水平方向穿过网格边和竖直方向穿过网格边的ttt值，记为txt_{x}tx​和tyt_{y}ty​ 如果tx<tyt_{x} < t_{y}tx​<ty​，那么光线先穿过垂直边界，那么向x方向移动一步 如果tx≥t...