---
# System prepended metadata

title: 運動控制系統導論
tags: [' 運動控制', Lecture Note]

---

---
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