---
tags: Lecture Note, 運動控制
---
運動控制系統導論
===
<style>
.blue {color: blue;}
.ph {
display : block;
margin-left : auto ;
margin-right: auto;}
.ph_8 {
display : block;
margin-left : auto ;
margin-right: auto;
width : 80%}
.ph_6 {
display : block;
margin-left : auto ;
margin-right: auto;
width : 60%}
.ph_4 {
display : block;
margin-left : auto ;
margin-right: auto;
width : 40%}
.ph_2 {
display : block;
margin-left : auto ;
margin-right: auto;
width : 20%}
</style>
## Motion Planning
### Scurve
- Discription
- S-curve motion planning is often adopted in a point-topoint (PTP) motion. The position trajectory described by an S-curve is a 3rd order polynomial of time t. The corresponding acceleration curve is continuous, and the magnitude of jerk is finite.
- S-curve motion planning can reduce the chance of causing abrupt ACC/DEC. As a result, the chance of endangering the machine safety can be substantially reduced.
- Requestment
To specify an S-curve users must provide the following information:
A. total moving distance
B. maximum speed
C. maximum acceleration
D. average of acceleration
#### Main
```Matlab=
%% ==== Parameter ====
acc_lim = 4; % acceleration limitation (m/sec^2)
acc_avg = 0.7; % average accleration = acc_lim * acc_avg (m/sec^2)
vec_lim = 4; % velocity limitation (m/sec)
samp_t = 0.01; % sampling time(s)
%% ==== Main ====
end_point = 20; % distance
boundary = [0,0;0,end_point];
par = struct('acc',acc_lim,'vec',vec_lim,'avg',acc_avg*acc_lim,'samp_t',samp_t,'boundary',boundary);
%% ==== Scurve ====
[res,T_total,tc,tb] = Scurve_single_axis(end_point,par);
% Accleration, Velocity, position
a = res(1,:); v = res(2,:); x = res(3,:);
```
- Scurve Motion planning
<img src = https://i.imgur.com/W9q79Ch.png class="ph_8">
<img src = https://i.imgur.com/5DdzI8w.png class="ph_8">
<img src = https://i.imgur.com/cRpeeHt.png class="ph_4">
#### Scurve_single_axis
```Matlab=
function [res,T_total,tc,tb] = Scurve_single_axis(end_point,par)
%% ==== 解參數 ====
a_max = par.acc;
v_max = par.vec;
a_avg = par.avg;
samp_t = par.samp_t;
boundary = par.boundary;
%% ==== 計算時間 ====
ta = v_max / a_avg;
ts = (end_point - v_max*ta)/v_max;
T_total = 2 * ta + ts;
tb = 2*v_max/a_max - ta;
tc = (ta-tb)/2;
n = floor(T_total/samp_t) + 1;
a = zeros(1,n); i = 2;
%% ==== 給定各時間的加速度 ====
for t = samp_t:samp_t:T_total/2
if t<= tc
a(i) = a_max*t/tc;
a(end-i+1) = -a(i);
elseif t<= tc+tb
a(i) = a_max;
a(end-i+1) = -a(i);
elseif t<= ta
a(i) = a_max*(2*tc+tb-t)/tc;
a(end-i+1) = -a(i);
else
a(i) = 0;
end
i = i+1;
end
%% ==== 積分出各點的速度及位置 ====
res = Integral(a,2,samp_t,boundary);
end
```
#### Integral
```Matlab=
function res = Integral(a,order,samp_t,boundary)
n = length(a);
x = zeros(1+order,n);
x(1,:) = a(:);
for j = 1:order
for i = 2 : n-1
x(1+j,i) = x(1+j,i-1) + (x(j,i)+x(j,i-1))*samp_t/2;
end
x(1+j,1) = boundary(j,1);
x(1+j,n) = boundary(j,2);
end
res = x;
end
```
---
### BSpline
<img src = https://i.imgur.com/Mdr3iCa.png class = "ph_4">
where $P_i$ are the control points, $k$ is the order, and $N_{i,k}(u)$ is the blending function
<img src = https://i.imgur.com/2w78ojv.png class = "ph_6">
Notice：
1. k(階數) = p(級數) + 1
2. number of knot vector = number of control point(n+1) + k
3. example of bending function [ 0, 0 , 0 , 1/4 , 1/2 , 3/4 , 1 , 1 , 1 ];
<img src = https://i.imgur.com/cuX9SfT.png class = "ph_6">
#### Main
```Matlab=
%% ==== Parameter ====
clc,clear,close all;
% Control Point
Ctrl_P = [ 0.0 , 1.5;
1.0 , 4.0;
4.0 , 4.5;
2.5 , 0.0;
7.0 , 0.0;
5.0 , 3.0];
% Knot vector (p = 2)
knot = [ 0, 0 , 0 ,...
1/4 , 1/2 , 3/4 ,...
1 , 1 , 1 ];
p = 2; % Power
k = p+1; % Order
n = size(Ctrl_P,1); % number of control point
knot = Knot_gen(knot,k,n); % Knot_generator
samp = 0.01; % Sampling
n = length(knot)-1;
Auto = 1;
%% ==== Bending Function ====
N = zeros(n, k, max(knot)/samp+1); % N initialization
for j = 1:k % N(,j)
for i = 1:n % N(i,)
if j == 1
if i <= k
start = 1; end_ = k + 1;
elseif i > length(knot)-k
start = length(knot)-k+1; end_ = length(knot);
else
start = i; end_ = i+1;
end
for u = 0:samp:max(knot)
if j == 1 && i >= length(knot)-(k) && u == knot(end)
N(i,j, round(u/samp + 1)) = 1;
elseif knot(start) <= u && u < knot(end_)
N(i,j, round(u/samp + 1)) = 1;
end
end
else
if i + j > n+1
break;
end
for u = 0:samp:max(knot)
if knot(i+j-1)-knot(i) == 0
par1 = 0;
else
par1 = (u - knot(i))/(knot(i+j-1)-knot(i));
end
if knot(i+j)-knot(i+1) == 0
par2 = 0;
else
par2 = (knot(i+j) - u)/(knot(i+j)-knot(i+1));
end
N(i,j, round(u/samp + 1)) = ...
par1*N(i,j-1,round(u/samp + 1))+...
par2*N(i+1,j-1,round(u/samp + 1));
end
end
end
end
%% ==== Curve ====
for u = 0 : samp : max(knot)
sum = 0;
for i = 1 : size(Ctrl_P,1)
sum = sum + N(i,k, round(u/samp + 1)) * Ctrl_P(i,:);
end
C(round(u/samp + 1),:) = sum;
end
```
#### knot_gen
```Matlab=
function knot = Knot_gen(knot_1, k , n )
% B-spline knot vector generator.
%
% Input arguments:
% knot_1:
% knot vector in setting.
% k:
% B-spline order (2 for linear, 3 for quadratic, etc.)
% n:
% number of Control point -1
%
% Output arguments:
% knot:
% B-spline knot vector
%
% Copyright Hank Yu 2020/10/02
% Compute number of knot vector
len = n + k;
% Distinguish different conditions
if len == length(knot_1)
knot = knot_1;
else
knot = zeros(1,len);
knot(1:k) = 0;
knot(end-k+1:end) = 1;
if mod ( length(knot) , 2 ) == 1
knot (round(length(knot)/2)) = 0.5;
end
for i = k+1:length(knot)-k
if i < round(length(knot)/2)
if (round(length(knot)/2)-i)*0.1 < 0.5
knot(i) = (round(length(knot)/2)-i)*0.1;
else
knot(i) = 0.5;
end
else
if ( i )*0.1 < 1
knot(i) = ( i )*0.1 ;
else
knot(i) = 1;
end
end
end
end
end
```
---
### NURBS_Circle
<img src = https://i.imgur.com/jeTvfIi.png class = "ph_6">
where
- u : parameter
- order = k
- degree = k-1 = p
- number of control points = n+1
- number of knots = n+k+1=r+1
- $P_i$: control point
- $W_i$: weights
Notice:
<img src = https://i.imgur.com/B0GSMTB.png class = "ph_4">
```Matlab=
%% Parameter
clc,clear,close all;
% Control Point
Ctrl_P = [ 0.0 , 0.0;
0.0 , 25.0;
50.0 , 25.0;
50.0 , 0.0;
50.0 ,-25.0;
0.0 ,-25.0;
0.0 , 0.0];
% Knot vector (p = 2)
knot = [ 0, 0 , 0 ,...
1/4 , 1/2 , 1/2 , 3/4 ,...
1 , 1 , 1 ];
% Weight
weight = [1, 0.5, 0.5, 1, 0.5, 0.5, 1];
p = 2; % Power
k = p+1; % Order
rp = Repeat(knot); % Knot_generator
samp = 0.01; % Sampling
n = length(knot)-1;
Auto = 1;
%% Bending Function
N = zeros(n, k, max(knot)/samp+1); % N initialization
for j = 1:k % N(,j)
for i = 1:n % N(i,)
if j == 1
if i <= rp(1)
start = 1; end_ = rp(1) + 1;
elseif i > length(knot)-rp(2)
start = length(knot)-rp(2); end_ = length(knot);
else
start = i; end_ = i+1;
end
for u = 0:samp:max(knot)
if j == 1 && i >= length(knot)-(rp(2)) && u == knot(end)
N(i,j, round(u/samp + 1)) = 1;
elseif knot(start) <= u && u < knot(end_)
N(i,j, round(u/samp + 1)) = 1;
end
end
else
if i + j > n+1
break;
end
for u = 0:samp:max(knot)
if knot(i+j-1)-knot(i) == 0
par1 = 0;
else
par1 = (u - knot(i))/(knot(i+j-1)-knot(i));
end
if knot(i+j)-knot(i+1) == 0
par2 = 0;
else
par2 = (knot(i+j) - u)/(knot(i+j)-knot(i+1));
end
N(i,j, round(u/samp + 1)) = ...
par1*N(i,j-1,round(u/samp + 1))+...
par2*N(i+1,j-1,round(u/samp + 1));
end
end
end
end
%% Curve
for u = 0 : samp : max(knot)
sum = 0; N_sum = 0;
for i = 1 : size(Ctrl_P,1)
N_sum = N_sum + N(i,k, round(u/samp + 1)) * weight(i);
end
for i = 1 : size(Ctrl_P,1)
sum = sum + N(i,k, round(u/samp + 1)) * Ctrl_P(i,:) * weight(i) / N_sum ;
end
C(round(u/samp + 1),:) = sum;
end
```
---
## Velocity
Sign in with Wallet
Connect another wallet