随笔分类 - Matlab练习程序
摘要：可以参考之前的B样条曲线插值的方法。 要想利用控制点进行B样条曲面插值，前提是控制点不能是类似点云一样的无序点，一定要是分布在UV坐标空间中的栅格点。 方法就是首先对U方向进行B样条曲线插值，然后利用插值的结果再在V方向上进行B样条曲线插值即可。 下面用matlab自带的Peaks函数作为控制点为例
阅读全文
摘要：方法示意图： 控制率公式： 其中theta是当前航向角与路径航向角之差，e为横向误差，v为车辆速度，lambda为控制参数。 算法步骤如下： 1. 根据当前定位结果找到路径最邻近点。 2. 计算该点与定位结果横向误差e与航线误差theta。 3. 根据控制率公式计算出前轮转角。 4. 将前轮转角转化
阅读全文
摘要：首先要确定反馈量和控制量，这里反馈量用的是车和最近路径点的横向误差，控制量用的是前轮转角。 算法如下： 1. 根据当前定位结果找到路径最邻近点。 2. 计算该点与定位结果横向误差。 3. 以该误差作为反馈测量值通过pid生成控制量即前轮转角。 4. 将前轮转角转化为航向角，带入运动模型计算出下一时刻
阅读全文
摘要：代数黎卡提方程通常会在求解最优控制时有所应用，比如LQR控制。 标准形式有以下两种： 1.连续代数黎卡提方程： 2.离散代数黎卡提方程： 其中P是未知量，A、B、Q、R为已知量。 离散代数黎卡提方程可以迭代求解。 matlab代码如下： clear all;close all;clc; A = [0
阅读全文
摘要：数值雅克比本质就是对函数的每一维分别做数值微分，再组合为雅克比矩阵即可。 通常我们做最优化的时候要计算函数的雅克比矩阵，但是如果函数的解析式比较复杂，求其偏导解析解会非常麻烦。 虽然可以利用Mathematica或者Matlab的符号运算进行求解，不过有时候得到的解析解也是很复杂的，再转写成代码如果
阅读全文
摘要：可以根据状态量(位置，速度，加速度)的起始和结束值列出6个方程，组成方程组解该问题。 1. 列出起始状态： 2. 列出终止状态： 3. 写成矩阵形式： 求解c即可。 下面是从横向-5米到5米的生成的路径。 代码如下： clear all;close all;clc; ts = 0;te = 10;
阅读全文
摘要：如果要准确反求B样条的控制点，有几个参数还是要事先知道的： 1. 样条的控制点个数。 2. B样条曲线的所有点坐标和个数。 3. B样条基函数。 一般条件2容易知道一些，1和3还是比较难事先知道的。 如果待求控制点为四个，B样条曲线点个数为n个，并且已知基函数形式如下面代码中[b0 b1 b2]。
阅读全文
摘要：Logistic映射是一个一维混沌映射，差分方程如下： X(n+1)=X(n)*μ*(1-X(n)) μ∈[0,4] X∈[0,1] u为logistic控制参数，取值为[0,4]，x取[0,1]时，系统处于混沌状态。 该系统在保密通信领域有所应用。 代码如下： clear all;close al
阅读全文
摘要：该算法可以抽稀路径上的数据，在保证几何形状基本不变的情况下，最大限度去除冗余点。 算法逻辑如下： （1）在曲线首尾两点A，B之间连接一条直线AB，该直线为曲线的弦； （2）得到曲线上离该直线段距离最大的点C，计算其与AB的距离d； （3）比较该距离与预先给定的阈值threshold的大小，如果小于t
阅读全文
摘要：之前用simulink仿过一次PID算法，这次用代码实现一遍。 该算法工程性很强，一般需要结合实际调整一个合适的参数用于控制。 matlab代码如下： clear all;close all;clc; target_speed = [5*ones(300,1);ones(300,1);ones(30
阅读全文
摘要：无论是激光、视觉或者是惯导直接推出来的里程计通常会有回环误差，通过图优化的方式能够将回环误差最小化，从而提高建图精度。 图优化也是一种优化，所以能用常见的非线性优化方法来做，这里用到的高斯牛顿法，和之前ndt那一篇类似。 1.定义误差函数： 我们定义Xi为i点位姿，Xj为j点位姿，Rij与Tij为回
阅读全文
摘要：DWA英文全称Dynamic Window Approach即动态窗口方法，是一种局部路径规划算法，主要用来做局部避障。 算法原理是对当前速度和角速度在一定范围内进行采样，并对采样后的参数在一定时间内进行预测生成多组路径，计算每条路径的权重，最后选择最优权重的路径对应的速度和角速度输出给下级模块。
阅读全文
摘要：西尔维斯特方程的形式：AX+XB=C 李雅普诺夫方程的形式：AX+XA'=-C 这两种方程都是已知矩阵A,B,C,求解X的方程。 对于这种方程有两种方法来求解，一种是朴素法，一种是Bartels-Stewart法。 以西尔维斯特方程为例，朴素法是将方程写为下列形式进行直接求解： 其中圆圈中带个X的符
阅读全文
摘要：NDT全称Normal Distributions Transform（正态分布变换），用来计算不同点云之间的旋转平移关系，和ICP功能类似，并且该算法能够写出多线程版本，因此速度可以比较快。 算法步骤如下，以二维点云为例： 1. 比如我们有两组点云A和B，A是固定点云，我们要把B转换到和A对齐，就
阅读全文
摘要：高斯过程是一种非参数模型估计方法。不像最小二乘，需要知道模型的参数，如：y=ax+b，我们就需要知道a和b来对模型进行估计。 高斯过程要设置一个核函数，来给不同观测值确定关系。这里我们需要设置核函数的超参数，比如下面的alpha和beta。 下面是几种常见的计算不同观测关系的核函数： 设置好核函数后
阅读全文
摘要：logistic模型能够对数据进行二分类。 比如我们有两组二维空间数据，最终要求的是一个分类直线，可以设定为计算w(1)+w(2)*x+w(3)*y=0这样的直线。 问题就变为了如何求w的问题。 网上有很多推导，这里就不推导了，不过还是要写几个关键公式。 可以设定logistic函数为： 设定损失函
阅读全文
摘要：上一篇实现了一维波动方程数值解，这一篇实现二维波动方程数值解。 二维波动方程如下： 写成差分形式： 整理一下就能得到u(i+1,j,k)。 matlab代码如下： clear all;close all;clc; t = 3; %时间范围，计算到3秒 x = 1;y = 1; %空间范围，0-1米
阅读全文
摘要：上一篇实现了二维热传导方程数值解，这里我们计算波动方程数值解。 波动方程是一种双曲型偏微分方程。 这里依然用差分法计算。 一维波动方程如下： 写成差分形式： 整理一下就能得到u(i+1,j)。 matlab代码如下： clear all;close all;clc; t = 2; %时间范围，计算到
阅读全文
摘要：上一篇实现了一维热传导方程数值解，这一篇实现二维热传导方程数值解。 套路是一样的，先列微分方程，再改为差分方程，然后递推求解，不同的是一维热传导需要三维显示，而二维热传导需要四维，因此最后做了个三维动态图。 二维热传导方程如下： 另外四条边界都是0。 写成差分方程为： 整理一下就能得到u(i+1,j
阅读全文
摘要：差分法计算一维热传导方程是计算偏微分方程数值解的一个经典例子。 热传导方程也是一种抛物型偏微分方程。 一维热传导方程如下： 该方程的解析解为： 通过对比解析解和数值解，我们能够知道数值解的是否正确。 下面根据微分写出差分形式： 整理得： 已知网格平面三条边的边界条件，根据上面递推公式，不断递推就能计
阅读全文