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时基本已经能够完全拟合目标曲线。