仿生足式机器人与SLIP模型4: 2D-SLIP的仿真件框架优化与复杂地形越障

1. 仿真框架的优化

在前文中给出了搭建2D-SLIP模型的基本方法，该框架也能推广到其他的机器人模型中，如无人机仅需要替换飞行相中的动力学和微分方程，另外在动画绘制中基于质心状态和姿态结合运动学绘制出机构末端位置，对于矢量平面的简化四足机器人或双足机器人来说也可以用这样的方法来实现，总之基于这套框架能快速地在matlab中搭建一个简单的动力学仿真环境并开展一些算法的研究，当然你也可以直接使用现成的动力学仿真软件如Webots、Gazebo等，在之前的框架还有一个严重的问题，前文为了保证系统能一直弹跳需要给定额外的控制输入，我们将这部分控制的代码放置在了动力学模型里面：

这虽然不影响代码的仿真，但是在软件架构上来说不太合理，另外为了记录摆动腿的位置，我们临时增加了一个没有用的系统状态，虽然其不会影响主要的动力学解算，但是这样对动力学模型的描述是不合理的，而且在测试中发现再增加该部分后，虽然在动力学中让其状态微分变化为0但是对最终的仿真结果仍然有一定的影响，因此本文首先对软件框架进行新的优化，将控制和动力学独立开，同时使用另外的方法来记录仿真中的期望信号、控制变量或者其他绘图用的中间变量，首先修改控制函数。

首先定义控制函数接口将所需要的变量传入，注意如果是需要随仿真过程实时变化的变量需要在前面的（）中也写入,如下面的接口传入了时间t和系统状态q：

ctrlFun =@(t,q)( fb_ctrl(t,q,t_opt_t,z_opt_t,u_opt,[x0;y0],dy1,Ts,fly_reach_z));

其对应的控制函数为：

function u = fb_ctrl  (t,q,traj_t,traj_q,traj_u,support_n,d_support_y,Ts_last,fly_z_peak)%反馈控制
        l1=sqrt(q(1)^2+q(2)^2); 
        if spd_z_reg*q(4)<0 && en_out==0%速度切换支撑最低点
            en_out=1;
        end
        spd_z_reg=q(4);
  
        if en_out==1%伸长阶段 开始注入能量
            u=Kp_z*(l_0+fly_z-fly_reach_z+d_support_y);%能量控制
        else 
            u=0;%高度控制
        end
    end

上面的函数基于上一次跳跃高度与期望高度的误差构建支撑后半阶段伸腿过程的固定力输出，其中spd_z_reg缓存了仿真中z方向速度的变化，通过其符号切换判断是否达到压缩最低点fly_reach_z为飞行过程中其相对当前支撑点最大的飞行高度，d_support_y为支撑点高度的变化，其用于消除地形下落时的多余重力势能，保证触地后弹跳不会跳很高（由于仿真系统在没增加D微分阻尼耗散能量时，弹簧从任意高度掉落支撑后会重新弹跳至掉落高度，这是符合能量守恒原则的）

则动力学部分需要实时调用该控制函数并将u输入系统动力学模型：

dynFun = @(t,q)( EoM_single_ctrl(q, ctrlFun(t,q)) );
动力学函数修改如下：
    function dqdt = EoM_single_ctrl(q,u)
        dqdt(1,1) = q(3);
        dqdt(2,1) = q(4);
        l1=sqrt(q(1)^2+q(2)^2); % spring length
        a1=K*(l_0-l1)/mass+u/mass;      % acceleration of point mass
        dqdt(3,1) = a1/l1*q(1);%ddx
        dqdt(4,1) = a1/l1*q(2) -q(4)*D- g;%ddy
        if q(2)<0 %弹簧压缩限制
            if dqdt(4,1)<0
                dqdt(2,1)=0;
                dqdt(4,1)=0;
            else
                dqdt(4,1)=0;
            end
        end
        %记录
        q_swing_r=[q_swing_r;0];
        u_out_r=[u_out_r;u];
    end

综上，修改后的仿真框架将控制和动力学函数区分开来，剩下需要对数据记录部分进行修改，首先定义几个全局变量数组，在每个状态相切换前清零：

%初始化记录缓存
 q_swing_r=[];
 u_out_r=[];
 z_plan_r=[];
 dz_plan_r=[];

在相应的动力学函数中记录所需要的数据：

 % 飞行动力学
 function dqdt = EoM_flight(~,q)
 dqdt(1,1) = q(3);%x
 dqdt(2,1) = q(4);%y
 dqdt(3,1) = 0;
 dqdt(4,1) = -g;
 if q(2)<pos_z_reg && en_peak==0%记录最高点
 fly_reach_z=q(2);
 en_peak=1;
 end
 pos_z_reg=q(2);
 %记录        
 q_swing_r=[q_swing_r;q_swing];
 u_out_r=[u_out_r;0];
 z_plan_r=[z_plan_r;0];  
 dz_plan_r=[dz_plan_r;0];  
 end

最终在状态相切换时将其赋值给全局记录缓存：

 sim_data(2*i-1).record.q_swing=q_swing_r(1:size(q(:,1) ));
 sim_data(2*i-1).record.u_out=u_out_r;
 sim_data(2*i-1).record.z_plan_r=z_plan_r;
 sim_data(2*i-1).record.dz_plan_r=dz_plan_r;

综上，我们将基于修改完的软件框架来对Raibert三通道解耦控制进行实现，并在复杂地形上进行越障测试。

2. 地形越障仿真

最终，基于上述原理我们可以开展在复杂地形下的越障仿真，首先构建地形：

profile=[-1 0;      %起点X Y
 50 0];    %终点X Y

数组构建了两个点间的连线地形，如果要构建一个台阶状的地形，则值需要增加新的坐标点就行，这里需要注意的是x坐标两个点不能重合因此如果需要构建一个90°的台阶，需要给下一个坐标点增加一个小的偏移量，则一个随机离散地形构建方法如下：

 temp=randi([-100 100],1,1)/100.0;
 %边缘
 profile(j,1) =profile(j-1,1)+0.03;%X斜面长度
 profile(j,2) =profile(j-1,2)+0.1*temp;%Y台阶高度
 j=j+1;
 %平台
 profile(j,1) =profile(j-1,1)+0.2+0*0.1*temp;%X台阶长度
 profile(j,2) =profile(j-1,2);%Y
 j=j+1;
end
 profile(j,1) =1000;%无限平台
 profile(j,2) =profile(j-1,2);%Y

仿真结果如下：

3. 总结

通过几个简单文章我们可以了解到如何搭建基本的matlab下的仿真环境并开展算法测试，目前的软件框架基本完整但是还存在着几个问题：

（1）地形判断不准确，会出现穿模的现象；

（2）飞行控制中没增加摆动轨迹，因此上台阶时腿部有可能会发生提前的碰撞；

（3）动画中如果绘制运动轨迹由于没有动态清除过久轨迹点的部分，导致随仿真时间增长仿真运算速度会越来越慢；

（4）弹跳控制规划太简单，仅能实现类似blind jump的移动，如果需要引入地形数据跳跃到特殊位置目前无法实现；

希望感兴趣的同学能帮优化以上问题，相关结果可以分享至舵狗之家QQ群中！

仿真代码：
链接：https://pan.baidu.com/s/1d7lF1izLl_TYH7fulXFRrg 
提取码：oflc