Matlab BP神經網絡擬合四足機器人足端軌跡線積分方程反函數用於足端軌跡規劃

Matlab BP神經網絡擬合四足機器人足端軌跡線積分方程用於足端軌跡規劃

問題描述

一般情況下，在對四足機器人足端軌跡進行規劃時分別對足端路徑和軌跡加速度進行規劃，然後將規劃好的加速度進行兩次積分得到足端在該加速度條件下的位移，將位移與規劃的足端路徑的線積分長度進行匹配，從而建立起規劃的路徑與控制系統時間的映射關係。
以下對幾個概念進行解釋：
路徑規劃（Path Planning）：路徑規劃不考慮時間因素，只考慮幾何因素，是空間中的一系列點組成的一條曲線。
軌跡規劃（Trajectory Planning）：在路徑規劃的基礎上加入時間因素，即對應系統給定的任意時間能夠計算得到機器人足端對應在空間中的位置。
加速度規劃：加速度規劃爲軌跡規劃中的一種常用方法，其目的在於像規劃好的路徑中添加適當的時間信息，滿足工作空間中運動的連續、弱衝擊等要求。

以下爲機器人足端軌跡規劃中的對角步態路徑規劃與修正梯形加速度曲線規劃：

足端路徑規劃

根據機器人腿部的幾何關係，進行運動學分析，得到從關節座標到足端工作空間的映射關係，然後遍歷關節空間點得到足端的工作空間，並採用MATLAB中的拖拽點點擊觸發回調函數與移動觸發回調函數進行動態拖動工作空間中的貝塞爾控制點，進行足端路徑的設計。其源碼如下所示

%% 1. 足端路徑規劃
% 並聯五杆腿工作空間求解
% 足端F點座標（x，y）
% 生成工作空間
clc; clear;
i = 1;
for Alpha1 = 0:0.01:pi
    for Alpha2 = 0:0.01:pi
        [x(i),y(i)] = forward_kinematic(Alpha1,Alpha2); % 運動學正解
        i = i + 1;
    end
    figure(1);
    plot(x,y,'b.');
    hold on;
    i = 1;   
end
title('工作空間');
xlabel('x/mm');  %x軸
ylabel('x/mm');%y軸
axis equal;
hold on;

% 在工作空間中生成可拖動的貝塞爾曲線
n= 7;  %控制點個數
x=[-80 -130 -90 0 90 130 80];
y=[180 180 140 120 140 180 180];
    length_x = length(x);
    length_y = length(y);
    vertices = zeros(2, length_x);  %初始化座標向量
    vertices(1, 1 : length_x) = x;
    vertices(2, 1 : length_y) = y;
    [x2,y2]=Bezier_n(vertices);
    hold on;
    h=plot(x2, y2, 'r','LineWidth',3);
    
    axis([-250 250 -250 250])
    l=line(x,y,'LineWidth',3);
    % 繪製控制線
    hl = handle(l);
    for i = numel(x):-1:1
        hp(i) = patch('xdata',x(i),'ydata',y(i),'LineWidth',4,...
            'linestyle','none','facecolor','none',...
            'marker','o','markerEdgecolor','g',...
            'buttonDownFcn',@drag,'userdata',i);
    end
    ax = gca; idx = [];
    xl = get(ax,'xlim'); yl = get(ax,'ylim');
%% 拖拽點點擊觸發回調函數
    function drag(src,~)
        idx = get(src,'userdata');
        set(gcbf,'WindowButtonMotionFcn',@move,'WindowButtonUpFcn',@drop);
    end
%% 拖拽點移動觸發回調函數
    function move(~,~)
        cp = get(ax,'currentPoint');
        xn = min(max(cp(1),xl(1)),xl(2));
        yn = min(max(cp(3),yl(1)),yl(2));
        set(hp(idx),'xdata',xn,'ydata',yn)
        hl.XData(idx) = xn;
        hl.YData(idx) = yn;
    end
%% 拖拽點取消點擊觸發回調函數
    function drop(src,~)
        delete(h);
        length_x = length(hl.XData());
        length_y = length(hl.YData());
        vertices = zeros(2, length_x);  %初始化，防止只記錄最高次
        vertices(1, 1 : length_x) = hl.XData();
        vertices(2, 1 : length_y) = hl.YData();
        [x2,y2]=Bezier_n(vertices);
        hold on;
        h=plot(x2, y2, 'r','LineWidth',3);
        x=hl.XData();
        y=hl.YData();
        set(src,'WindowButtonMotionFcn',[],'WindowButtonUpFcn',[]);
        return ;
    end

運行上述代碼後，生成如圖2所示的工作空間與規劃所得路徑。

其中綠色點爲六階貝塞爾曲線的七個控制點，紅色曲線爲規劃得到的足端路徑的騰空相部分。

求曲線線積分

將規劃所得到的六階貝塞爾曲線，利用參數方程的線積分，求出整個足端路徑的長度，以爲之後的加速度規劃和匹配做好準備。

%% 2. 求曲線長度
% 將上一步中設計好的曲線對應控制點記錄下來
% 七個控制點對應六階貝塞爾曲線
clc; clear;
syms t;
x=[-80 -130 -90 0 90 130 80];  % 控制點
y=[180 180 140 120 140 180 180];
% 六階貝塞爾曲線參數方程
fx(t) = x(1)*(1-t)^6 + 6*x(2)*(1-t)^5*t + 15*x(3)*(1-t)^4*t^2 + 20*x(4)*(1-t)^3*t^3 + 15*x(5)*(1-t)^2*t^4 + 6*x(6)*(1-t)*t^5 + x(7)*t^6;
fx(t) = simplify(fx(t)); 
fy(t) = y(1)*(1-t)^6 + 6*y(2)*(1-t)^5*t + 15*y(3)*(1-t)^4*t^2 + 20*y(4)*(1-t)^3*t^3 + 15*y(5)*(1-t)^2*t^4 + 6*y(6)*(1-t)*t^5 + y(7)*t^6;
fy(t) = simplify(fy(t));
dfx(t)=diff(fx(t));
dfy(t)=diff(fy(t));
flengt(t)=sqrt(dfy(t)^2+dfx(t)^2);
% 求數值解
i=1;
for t=0:0.01:1
    dl(i)=flengt(t);
    dt(i)=t;
    i=i+1;
end
l=vpa(trapz(dt,dl),10); %數值積分求得曲線長度爲255.4717

BP神經網絡求線積分函數反函數

由於加速度規劃後經過兩次積分得到的是系統時間t與位移之間的對應關係，而位移與路徑曲線進行匹配後需要知道該位移下對應的路徑曲線參數t的值以求出足端在工作空間中的直角座標。因此需要知道線積分長度l與曲線時間參數t之間的函數關係。即上述時間-線積分長度函數的反函數。
根據上述線積分的參數方程，求出每一個參數下對應的線積分的長度，然後通過神經網絡工具箱，使用BP神經網絡得到每一個時間t對應下的曲線長度。

%% 3. 根據曲線長度求出當前的t
clc; clear;
k=1;
i=1;
for m=0:0.001:1
    for n=0:0.001:m
        dl(k)=CalIntFunc(n);
        dt(k)=n;
        k=k+1;
    end
    l(i)=trapz(dt,dl); 
    t(i)=m;
    i=i+1;
    k=1;
end
plot(t,l, 'b','LineWidth',3);
grid on;
hold on;

下圖爲時間t與線積分長度l的函數圖像

構建BP神經網絡對長度和時間參數t進行擬合。神經網絡採用雙層BP神經網絡，隱藏層有10個神經元，由於時間參數t均爲正數所以採用logsig傳遞函數。學習函數選用traingdx函數。具體的構造代碼爲

% BP神經網絡擬合
net=newff(minmax(l),[10,1],{'logsig' 'logsig'},'traingdx');
net.LW{2,1}= net.LW{2,1}*0.01;        %  調整輸出層權值，加速收斂
net.b{2}= net.b{2}*0.01;        % 調整輸出層權值
 
t1=sim(net,l);
plot(l,t,l,t1,'--');
title('未訓練的網絡輸出結果');
xlabel('長度');
ylabel('仿真輸出-- 原函數 -');
net.performFcn='sse';
net.trainParam.show= 20;                       % 結果顯示間隔
net.trainParam.epochs = 1000000;    % 訓練次數
net.trainParam.goal =0.01;  % 訓練精度設置
net.trainParam.mc=0.95;                         % 動量因子
net= train(net,l,t);        % 訓練·
t2 = sim(net,l);
plot(l,t,l,t1,l,t2);
title('訓練後的網絡輸出結果');
xlabel('長度');
ylabel('仿真輸出');

所設計的神經網絡的結構如圖所示

對設計好的神經網絡進行多次訓練最終得到訓練結果

訓練過程中不同誤差下的擬合效果圖如下所示。
誤差0.5輸出結果

誤差0.1輸出結果

誤差0.02輸出結果

誤差0.005輸出結果

由上圖不難發現，隨着訓練次數的增加，神經網絡對曲線的擬合效果越來越好，在誤差爲0.005時基本已經能夠完全擬合目標曲線。
爲對比BP神經網絡的擬合效果，下圖分別爲使用MATLAB擬合工具箱在使用多項式、Rational函數、高斯函數擬合時的效果圖。