gophae的博客

In this section, we provide a function that supports generate a quintic polynomial.xs = 3;vxs = 0.3;axs = 0;xe = 0;vxe = 0;axe = 0;T = 8;[a0, a1, a2, a3, a4,a5] = quintic_polynomial(xs, vxs, axs, xe, vxe, axe,T);figurefor t = 0:0.1:T d = a0

对于地图采样获得的轨迹，通过cross entropy的方式，进行噪声过滤。曲率计算采用三点式计算圆半径获得。clcclear allload('D:\ccs\test.mat')% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%use your own recorded map info%%%...

本文根据LQR轨迹跟踪算法Python/Matlab算法实现_LQRmatrix推导(2)使用代码实现，进行仿真：clcclear allKp = 1.0 ; dt =0.1 ;% [s]L = 2.9 ;% [m] wheel base of vehicleQ = eye(3);% Q(1,1) = 10;% Q(2,2) =20;% Q(3,3) = 5;% Q(4,4)...

mathworks社区中的这个资料还是值得一说的。openExample('mpc/mpccustomqp')我们从几个角度来解析两者关系，简单的说就是MPC是带了约束的LQR.下面我们从代码的角度解析这个问题：1， 定义被控系统：A = [1.1 2; 0 0.95];B = [0; 0.0787];C = [-1 1];D = 0;Ts = 1;sys = ss(A,B,...

有噪声的离散点，计算曲率。首先用滤波法降噪，然后用三点法计算半径clcclear allload('D:\ccs\DaischTest\TraceC10.mat') spacelimit = 1; start =2; finish =1;x= PosLat(start : spacelimit:length(PosLat)-finish);y = PosLon(star...

%% clc clear all p0 = [ 0, 0]; p1 = [7,0]; p2= [14,0]; p3 = [21,4]; p4 = [27,4]; p5 = [35,4]; %生成五阶贝塞尔曲线的笛卡尔坐标系点位 i =1; for u =0: 0.01:1 p(i,:)= (1-u)^5*p0 + 5*(1-u)^4*u*p1 + 10*(1-u)^3...

Continue:经常用在for和while语句中，continue一旦被执行，就会终止当前循环，进行下一次循环。eg, 在for 循环中：clcclear allclose alli = 1;COND = 1;for i = 1:20 if mod(i,2)==0 continue else fprintf('the arr...

本文介绍如何实现基于Frenet坐标系的动态障碍物避障。其中包括：cubic spline generationFrenet transformation to gloval coordnatessampling-based search methodBezier curvecost function formalizationlazy collision checkingpure...

Python:"""Path tracking simulation with LQR steering control and PID speed control.author Atsushi Sakai (@Atsushi_twi)"""import syssys.path.append("H:\Project\TrajectoryPlanningModelDesign\Codes...

本文介绍贝塞尔五阶曲线最为curve smoother的使用，分析关于时间的一阶和二阶导数的连续性，以及曲率的连续性，并给出一个smoother的实例进行效果展示。clc clear all p0 = [ 0, 0]; p1 = [4 4*sqrt(3)]; p2= [5.5 5.5*sqrt(3)]; p3 = [8.5 5.5*sqrt(3)]; p4 = [ 10 4*sqr...

检测对生成出的期望轨迹的跟踪效果，期望轨迹为连续变道的蛇形轨迹：clcclear all load('D:\ccs\DaischTest\CurveCurve.mat')% load('D:\ccs\DaischTest\TraceC10.mat') spacelimit = 1; start =10; finish =1;x= PosLat(start : spacel...

clcclear all% load('D:\ccs\DaischTest\CurveCurve.mat')load('D:\ccs\DaischTest\TraceC10.mat') spacelimit = 1; start =10; finish =1;x= PosLat(start : spacelimit:length(PosLat)-finish);y = P...

推荐使用点到直线距离进行计算：首先找到离本车最近点，在取改点的前后点最为直线的端点，计算直线到本车的质点的距离：function [dist] = FindDistance (p1, p2, currentP) % find the distance between the current points to the curve lineA = p1(2)- p2(2);B = p2...

正北坐标系由组合导航给出，以正北为0，顺时针为正。本车的heading与期望heading 之间的夹角，delta_heading 计算方法如下：function [delta_heading] = CalHeading(heading_desire, heading_current)index_store = 0;index_current = 0;index_store = floo...

首先规划出reference line, 探测到障碍物后，进行贝塞尔曲线轨迹规划，绕开障碍物，给出新的reference line, 然后根据pure pursuit模型进行软件在环测试，pure pursuit部分参考之前的博文。clcclear allk = 0.1; % look forward gainLfc = 0.1; % look-ahead distanceglob...

对于给定的一连串waypoints我们需要对其进行平滑线处理，这里介绍一种三阶spline的平滑方法：call function:clcclear all% x = [-4 -2 0 2 4 6 10];% y = [1.2 0.6 0 1.5 3.8 5 3];x = [0,10,50,100,120];y = [0,50,100,150,200]; figure...

首先我们需要两台机子，一台ubuntu系统，这里我们选用Xavier,另一台是我们的电脑，转配最新的Matlab2019b.Ubuntu中安装ROS Melodic.对于Matlab的配置，需要做如下操作：初始化matlab ros:setenv('ROS_MASTER_URI','http://192.168.1.101:11311') setenv('ROS_IP','192.168...

提供动态轨迹的Bezier轨迹，计算cost minimization 后取cost最小值clc clear all p0 = [ 0, 0];% p1 = [7.5,7.5*4/30];% p2= [15,0];% p3 = [15,4];% p4 = [22.5,22.5*4/30];% p5 = [30,4];for i = 1:10 j =10;...

提供动态轨迹的Bezier轨迹，计算cost minimization 后取cost最小值clc clear all p0 = [ 0, 0]; for i = 1:10 j =10; p3(i*j-9, 1 ) = 19+i; p3(i*j-8, 1 ) = 19+i; p3(i*j-7, 1 ) = 19+i; p3(i*j-6, 1 ) = ...

存储函数： BezierGenerationfunction p= BezierGeneration (xs,ys,xe,ye,Latoff) % x_start, and x_end means s and e% this function as you can see here, we generate a Bezier Curve in Frenet% Coordinate.% ...

注意：MATLAB中调色板色彩强度[0,1]，0代表最暗，1代表最亮。        常用颜色的RGB值  --------------------------------------------    颜色   R   G   B    &...

本文接上文，最后展示A* 算法的Matlab实现。相关代码函数如下：声明： 课程代码部分来自我上过的课，HKUST ELEC 5660 Roboticsmain 函数， 主函数，用于执行主程序：在本函数中，我们设定了搜索路径的起点终点，调用两个可视化函数 obstacle_map, 和visualize_map， 第一个函数生成随机障碍物，另一个讲障碍物和计算出来的path进行可视化。cl...

对于形如y = a + bx + c * x^2 + d * x^3 的三次spline曲线拟合的数学原理，我就不多说了。我接了一个图给大家看看：数值计算的伪代码如下：书名是：numerical_methods_for_engineers_for_engineers_chapra_canale_6th_editionspline interpolation 在18.6章，想了解如...

本文针对Python/Matlab实现Stanley进行介绍：Stanley 的数学几何原理来自：https://blog.youkuaiyun.com/gophae/article/details/100012763Python代码来自：https://github.com/gameinskysky/PythonRobotics/blob/master/PathTracking/stanley_co...

本文的python源代码来自：https://github.com/gameinskysky/PythonRobotics/blob/master/PathTracking/pure_pursuit/pure_pursuit.py我们对纯跟踪算法进行一次仿真，python 我已经改过，如下：import numpy as npimport mathimport matplotlib.py...

>> setenv('ROS_MASTER_URI','http://192.168.43.20:11311')>> setenv('ROS_IP','192.168.43.3')>> rosinit('192.168.43.20')

matlab的dare 函数在解决解离散的里卡多方程的问题：数学原理就不多解释了，但是需要注意的是，care, dare, 一个是解决连续方程，一个是解决离散方程，区别和lqr, 和lqrd是一样的。】举个例子：有这样一个连续系统，需要计算最优的LQR 的 增益 K ：Matlab 代码仿真如下：%%clcclear allformat shortA = [0 -1;0 0 ...

Matlab 提供了很多实例， 我们通过以下这个实例来解释以下在线仿真的方式：open_system(‘robotROSConnectToRobotExample’);在MATLAB中打开这个实例：打开是这个样子的，为了更好地演示如何来做在线仿真，我们把上面的部分拿出去，放在另一个新的simulink里面。上面的部分就是在模拟发送机器人的控制命令，通过publish模块发出。下面的部分则...

本文根据https://blog.youkuaiyun.com/AdamShan/article/details/78458325中提到的的动态调节PID参数的方法使用Matlab simulink进行了一次仿真，值得申明的是这个方法不是上述博文作者的原创，有知道原创出处的朋友评论一下，我好感谢。下面看simulink模型：整个结构很简单：这里我随便设置了一个二阶延迟系统中间部分就是PID这...

这次我们分以下几部分来讲解LQR及其应用：线性二次型最优控制器概述连续系统线性二次型最优控制离散系统线性二次型最优控制线性二次型Gauss最优控制应用经典控制理论设计控制系统，能够解决很多简单、确定系统的实际设计问题。但是对于诸多新型而复杂的控制系统，例如多输入多输出系统与阶次较高的系统，往往得不到满意的结果。这时就需要有在状态空间模型下建立的最优控制策略。最优控制是现代控制理论的核心...