# Motion Planning for Mobile Robots -- Assignment 05 Minimum-Snap Trajectory Generation
**NOTE** Please open this in **VSCode** with **MATLAB plugin**
Solution guide for **Assignment 05, Minimum-Snap Trajectory Generation**.
---
## Introduction
Welcome to **Solution Guide for Assignment 05**! Here I will guide you through the **MATLAB** implementations of both
* **Minimum-Snap Numeric Solver**
* **Minimum-Snap Analytic Solver**
---
## Q & A
Please send e-mail to alexgecontrol@qq.com with title **Motion-Planning-for-Mobile-Robots--Assignment-04--Q&A-[XXXX]**. I will respond to your questions at my earliest convenience.
**NOTE**
* I will **NOT** help you debug your code and will only give you suggestions on how should you do it on your own.
---
## Minimum-Snap, Numeric Solver
### Overview
The workflow of **numeric solver** can be summed up as follows:
* Build **objective matrix**, which is defined by **Minimum-Snap** or **Minimum-Jerk**
* Build **equality constraint matrix**, which is defined by:
* **Boundary conditions**, **start** / **end** ego states
* **Intermediate waypoint continuity**
* Solve the above **QP** problem
* Use the result monomial coeffs. to generate the optimal trajectory
The workflow can be implemented in MATLAB as follows:
```matlab
% plan the minimum snap trajectory
poly_coef_x = MinimumSnapQPSolver(path(:, 1), ts, K, t_order);
poly_coef_y = MinimumSnapQPSolver(path(:, 2), ts, K, t_order);
% display the trajectory
X_n = [];
Y_n = [];
k = 1;
tstep = 0.01;
for i=0:K-1
%#####################################################
% STEP 3: get the coefficients of i-th segment
%#####################################################
% 1. get segment index:
segment_index = (i*N + 1):((i+1)*N);
% 2. extract segment coeffs:
Pxi = flipud(poly_coef_x(segment_index));
Pyi = flipud(poly_coef_y(segment_index));
% 3. calculate planned waypoints:
for t = 0:tstep:ts(i+1)
X_n(k) = polyval(Pxi, t / ts(i + 1));
Y_n(k) = polyval(Pyi, t / ts(i + 1));
k = k + 1;
end
end
% minimum snap trajectory generator:
function poly_coef = MinimumSnapQPSolver(waypoints, ts, K, t_order)
start_cond = [waypoints(1), 0, 0, 0];
end_cond = [waypoints(end), 0, 0, 0];
%#####################################################
% TODO -- STEP 1: compute Q of p'Qp
%#####################################################
%#####################################################
% TODO -- STEP 2: compute Aeq and beq
%#####################################################
%#####################################################
% TODO -- STEP 3: solve the problem with QP
%#####################################################
end
```
### Objective Matrix
The objective matrix is defined by **the L2-norm of the optimization target**:
* **Minimum Snap**, which is equivalent to **t_order = 4** in the implementation below
* **Minimum Jerk**, which is equivalent to **t_order = 3** in the implementation below
```matlab
function Q = getQ(K, t_order, ts)
% num. of polynomial coeffs:
N = 2*t_order;
% ###############################################
% 1. pre-compute constants used in Q construction
% ###############################################
% 1.1 factorial from derivative
Q_k = zeros(N - t_order);
Q_v = zeros(N - t_order);
for n = t_order:(N - 1)
Q_k(n - t_order + 1) = n;
Q_v(n - t_order + 1) = factorial(n) / factorial(n - t_order);
end
Q_factorial = containers.Map(Q_k, Q_v);
% 1.2 time power:
ts_power = ts .^ t_order;
% ###############################################
% 2. populate Q
% ###############################################
Q_i = [];
Q_j = [];
Q_v = [];
index = 1;
for k = 1:K
for m = t_order:(N - 1)
for n = t_order:(N - 1)
% TODO -- fill in elements of Q
index = index + 1;
end
end
end
Q = sparse(Q_i, Q_j, Q_v);
end
```
**Implementation Nodes**
* First, the above code uses **time normalization** to improve numeric stability
<img src="images/hints/01-time-normalization.png" alt="Normalize Each Trajectory Segment Duration to 1.0" width="100%">
* Pay attention to the extra multiplication of **ts** introduced by change of variable in integration
* Here **sparse matrix** is used for efficient matrix manipulation. Please search the corresponding doc in MATLAB for details.
### Equality Constraint Matrix
The equality constraint matrix consists of following constraints:
* **Boundary Conditions**, which are defined by **start** and **end** states of target trajectory
* **Intermediate Waypoint Continuity**, which requires the planned trajectory should be **t_order - 1** order continuous at each intermediate waypoint
```matlab
function [Aeq, beq]= getAbeq(K, t_order, waypoints, ts, start_cond, end_cond)
% num. of polynomial coeffs:
N = 2*t_order;
% num. of constraints:
C = (K + 1)*t_order + (K - 1);
% ###############################################
% 1. pre-compute constants used in A construction
% ###############################################
% 1.1 factorial from derivative
A_factorial_k = [];
A_factorial_v = [];
index = 1;
for c = 1:t_order
for n = c:N
A_factorial_k(index) = (c-1)*N + n - 1;
A_factorial_v(index) = factorial(n - 1) / factorial(n - c);
index = index + 1;
end
end
A_factorial = containers.Map(A_factorial_k, A_factorial_v);
% ###############################################
% 2. populate A & b
% ###############################################
A_i = [];
A_j = [];
A_v = [];
beq = zeros(C, 1);
index = 1;
c_index = 1;
% 2.1 start & goal states:
for c = 1:t_order
% TODO -- fill in start state:
index = index + 1;
% move to next constraint:
c_index = c_index + 1;
% TODO -- fill in end state:
for n = c:N
index = index + 1;
end
% move to next constraint:
c_index = c_index + 1;
end
% 2.2 intermediate waypoint passing constraints:
for k = 1:(K - 1)
% TODO -- next segment start position:
index = index + 1;
% TODO -- should equal to the specified value:
% move to next constraint:
c_index = c_index + 1;
end
% 2.3 intermediate waypoint continuity constraints:
for c = 1:t_order
for k = 1:(K - 1)
% TODO -- current segment end state:
for n = c:N
index = index + 1;
end
% TODO -- should equal to next segment start state:
index = index + 1;
% move to next constraint:
c_index = c_index + 1;
end
end
Aeq = sparse(A_i, A_j, A_v);
end
```
**Implementation Nodes**
* According to the original paper, if your objective function is the L2-norm of **t_order** trajectory derivative, then the trajectory should be **t_order - 1** order continuous at each intermediate waypoint.
---
## Minimum-Snap, Analytic Solver
### Overview
The workflow of **analytic solver** can be summed up as follows:
* Build **objective matrix**, which is defined by **Minimum-Snap** or **Minimum-Jerk**. This is the same as previous question.
* Map **equality constraints** to **actual decision variables**:
* The **equality constraints** are the same as previous question
* The **actual decision variables** are the **t_order - 1** derivatives at the start and end of each trajectory segment
* Solve the above **least square** problem.
* Map the optimal decision variables to monomial coe
没有合适的资源?快使用搜索试试~ 我知道了~
移动机器人运动规划的课程分配方案_C++_Python_下载.zip
共961个文件
cpp:148个
hpp:125个
png:92个
1.该资源内容由用户上传,如若侵权请联系客服进行举报
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
版权申诉
5星 · 超过95%的资源 2 下载量 162 浏览量
2023-04-25
11:50:11
上传
评论
收藏 154.17MB ZIP 举报
温馨提示
移动机器人运动规划的课程分配方案_C++_Python_下载.zip
资源推荐
资源详情
资源评论
收起资源包目录
移动机器人运动规划的课程分配方案_C++_Python_下载.zip (961个子文件)
.gtkrc-2.0 616B
solar_system.agr 226KB
CHANGELOG 171B
FindEigen3.cmake 3KB
apt-fast.conf 5KB
supervisord.conf 2KB
lxde.conf 1KB
webportal.conf 501B
desktop-items-0.conf 290B
pip.conf 123B
resolv.conf 61B
trajectory_optimizer.cpp 32KB
osqp++.cpp 26KB
osqp++.cpp 26KB
stepper_copying.cpp 23KB
trajectory_generator_waypoint.cpp 20KB
trajectory_generator_node.cpp 19KB
traj_server.cpp 16KB
aerialmap_display.cpp 16KB
aerialmap_display.cpp 16KB
aerialmap_display.cpp 16KB
aerialmap_display.cpp 16KB
aerialmap_display.cpp 16KB
aerialmap_display.cpp 16KB
probmap_display.cpp 16KB
probmap_display.cpp 16KB
probmap_display.cpp 16KB
probmap_display.cpp 16KB
probmap_display.cpp 16KB
probmap_display.cpp 16KB
odom_visualization.cpp 15KB
symplectic_steppers.cpp 14KB
trajectory_generator_node.cpp 13KB
pose_utils.cpp 13KB
Astar_searcher.cpp 13KB
multi_probmap_display.cpp 13KB
multi_probmap_display.cpp 13KB
multi_probmap_display.cpp 13KB
multi_probmap_display.cpp 13KB
multi_probmap_display.cpp 13KB
multi_probmap_display.cpp 13KB
path_finder.cpp 13KB
JPS_utils.cpp 12KB
stepper_with_units.cpp 11KB
quadrotor_simulator_so3.cpp 11KB
demo_node.cpp 11KB
demo_node.cpp 10KB
Quadrotor.cpp 10KB
default_operations.cpp 9KB
JPS_searcher.cpp 9KB
integrate.cpp 9KB
waypoint_generator.cpp 9KB
waypoint_generator.cpp 9KB
waypoint_generator.cpp 9KB
waypoint_generator.cpp 9KB
waypoint_generator.cpp 9KB
waypoint_generator.cpp 9KB
random_complex_generator.cpp 9KB
stepper_with_ranges.cpp 9KB
demo_node.cpp 8KB
pcl_render_node.cpp 8KB
implicit_euler_mtl.cpp 7KB
fusion_algebra.cpp 7KB
random_complex_generator.cpp 7KB
random_complex_generator.cpp 7KB
random_complex_generator.cpp 7KB
random_complex_generator.cpp 7KB
adams_bashforth.cpp 7KB
so3_control_nodelet.cpp 7KB
hw_tool.cpp 7KB
range_algebra.cpp 7KB
runge_kutta_controlled_concepts.cpp 7KB
integrate_implicit.cpp 6KB
integrate_times.cpp 6KB
stepper_details.cpp 6KB
resizing_lattice.cpp 5KB
pose_tool.cpp 5KB
pose_tool.cpp 5KB
pose_tool.cpp 5KB
pose_tool.cpp 5KB
pose_tool.cpp 5KB
pose_tool.cpp 5KB
runge_kutta_concepts.cpp 5KB
runge_kutta_error_concepts.cpp 5KB
solar_system.cpp 5KB
demo_node.cpp 5KB
gmp_integrate.cpp 5KB
resizing.cpp 5KB
check_gmp.cpp 5KB
runge_kutta.cpp 5KB
heun.cpp 5KB
fpu.cpp 4KB
two_dimensional_phase_lattice.cpp 4KB
rosenbrock4.cpp 4KB
harmonic_oscillator.cpp 4KB
black_hole.cpp 4KB
bulirsch_stoer.cpp 4KB
gauss_packet.cpp 4KB
phase_oscillator_ensemble.cpp 4KB
spreading.cpp 4KB
共 961 条
- 1
- 2
- 3
- 4
- 5
- 6
- 10
资源评论
- STX199502062023-07-19资源有一定的参考价值,与资源描述一致,很实用,能够借鉴的部分挺多的,值得下载。
- 2301_763164362024-03-23资源是宝藏资源,实用也是真的实用,感谢大佬分享~
快撑死的鱼
- 粉丝: 1w+
- 资源: 9154
下载权益
C知道特权
VIP文章
课程特权
开通VIP
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
最新资源
- com.aesq.zb_v1.0.35_danji100.com.apk
- 760996331259605建立门派1.360.apk
- 下面提供一些C语言的入门示例代码,并附有注释,以帮助理解每个部分的功能 1. Hello World程序 #include
- 下面提供一些C语言的入门示例代码,并附有注释,以帮助理解每个部分的功能 1. Hello World程序 #include
- 下面提供一些C语言的入门示例代码,并附有注释,以帮助理解每个部分的功能 1. Hello World程序 #include
- C语言是一种广泛使用的计算机编程语言,它是许多其他编程语言的基础 以下是一些C语言入门的例子和代码,适合初学者学习和实践
- C语言是一种广泛使用的计算机编程语言,它是许多其他编程语言的基础 以下是一些C语言入门的例子和代码,适合初学者学习和实践
- C语言是一种广泛使用的计算机编程语言,它是许多其他编程语言的基础 以下是一些C语言入门的例子和代码,适合初学者学习和实践
- C语言 入门例子和代码学习
- C语言 入门例子和代码学习
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功