超冗余机器人运动控制：蛇形机器人 & 8自由度平面机械臂

原创

ZC_Robot机器人技术

修改于 2020-09-25 15:02:56

4.8K10

代码可运行

文章被收录于专栏：机器人技术与系统Robot机器人技术与系统Robot

运行总次数：0

代码可运行

1 概述

超冗余机器人具有适应复杂多变环境的特点，成为机器人研究中的一个热点，超冗余机器人的代表诸如蛇形机器人等。,生物蛇所具有的运动步态是无足脊椎动物行走步态的典范。运动学模型和动力学模型是蛇形机器人的控制基础。基于前人对生物蛇形态曲线的初步探索,为深入研究蛇形机器人步态,运动学模型和动力学模型成为该领域的研究重点。根据蛇形机器人的运动步态特点,可以将运动学模型和动力学模型分为二维步态和三维步态两种。

二维步态：蛇形机器人的二维步态主要指的是蜿蜒、内攀爬和蠕动(也称行波步态)。蜿蜒步态与生物蛇的蜿蜒运动相同,蠕动步态犹如尺蠖蠕动,但其效率很低,内攀爬步态类似于鼓风琴运动,但是利用机器人两个外侧表面与外界的接触摩擦力,及自身部分向前运动。

三维步态：蛇形机器人的三维步态包括侧移步态和攀爬步态,两种步态与生物蛇的运动相同,并且均具有螺旋曲线的特点。

图片位置

2 开源项目

a 7-DoF snake robot. Robotis Dynamixel MX-28AR, Protocol1.0. Ubuntu 16.04 LTS, ROS kinetic, ROS Control.

项目具体链接：

https://github.com/cygni/snakebot
https://github.com/guzhaoyuan/snake_robot

安装步骤如下所示：

# 1. install ros kinetic and create workspace

# 2. install dynamixel lib
git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git
cd DynamixelSDK/c++/build/linux64
make
sudo make install
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib # add dynamixel lib install path

# 3. get code
cd ~/catkin_ws/src
git clone https://github.com/guzhaoyuan/snake_robot.git
git clone https://github.com/guzhaoyuan/snake_moveit_config.git
cd ..
catkin_make

3 平面8自由度运动控制仿真

仿真图形如下：

具体程序如下所示：

%Kinematic Control of Redundant Manipulato
clear;
close all;
%Sampling period
dt = 0.0005; %1 msec
%duration of motion (in secs)
Tf = 1.0; %1 sec
t = 0:dt:Tf;
%Initial position of end-effecto
pd8_0 = [5; 1; 0];
xde_0 = pd8_0(1);
yde_0 = pd8_0(2);
%final position
pd8_f = [6; 0; 0];
xde_f = pd8_f(1);
yde_f = pd8_f(2);
% desired trajectory (1st subtask): position (xd,yd); velocity (xd_,yd_)
Nmax = Tf/dt + 1;
k = linspace(0, Tf, Nmax);
a1 = 6*(xde_f - xde_0)/Tf^2;
a2 = -a1/Tf;
xd =(-a1*k.^3)/(3*Tf) + (a1*k.^2)/2 + xde_0;
yd =(-a1*k.^3)/(3*Tf) + (a1*k.^2)/2 + yde_0;
xd_ = a1*k + a2*k.^2;
yd_ = a1*k + a2*k.^2;
%%%%%%%%%% FIGURE FOR ANIMATION %%%%%%%%%%%%
%%%%%%% (moving robot stick diagram) %%%%%%%
figure;
%set(gcf, 'Position', get(0,'Screensize')); % Maximize figure.
axis([-1, 7 -0.5 3])
axis on
axis equal
hold on
xlabel('x (m)');
ylabel('y (m)');
plot(xd, yd, 'm:');
dtk = 10; %% interval between consecutive plots
pauseTime = 0.1;

% ****** KINEMATIC SIMULATION - Main loop ******
disp('Kinematic Simulation ...');

tt=0; tk=1;
qd(tk,1)=0; qd(tk,2)=0; qd(tk,3)=0; qd(tk,4)=0; qd(tk,5)=0; qd(tk,6)=0; qd(tk,7)=0; qd(tk,8)=0;
while (tt <= Tf)
    %Calculate Jacobian for current q
    c1 = cos(qd(tk,1));
    c12 = cos(qd(tk,1) + qd(tk,2));
    c123 = cos(qd(tk,1) + qd(tk,2) + qd(tk,3));
    c1234 = cos(qd(tk,1) + qd(tk,2) + qd(tk,3) + qd(tk,4));
    c12345 = cos(qd(tk,1) + qd(tk,2) + qd(tk,3) + qd(tk,4) + qd(tk,5));
    c123456 = cos(qd(tk,1) + qd(tk,2) + qd(tk,3) + qd(tk,4) + qd(tk,5) + qd(tk,6));
    c1234567 = cos(qd(tk,1) + qd(tk,2) + qd(tk,3) + qd(tk,4) + qd(tk,5) + qd(tk,6) + qd(tk,7));
    c12345678 = cos(qd(tk,1) + qd(tk,2) + qd(tk,3) + qd(tk,4) + qd(tk,5) + qd(tk,6) + qd(tk,7) + qd(tk,8));
    s1 = sin(qd(tk,1));
    s12 = sin(qd(tk,1) + qd(tk,2));
    s123 = sin(qd(tk,1) + qd(tk,2) + qd(tk,3));
    s1234 = sin(qd(tk,1) + qd(tk,2) + qd(tk,3) + qd(tk,4));
    s12345 = sin(qd(tk,1) + qd(tk,2) + qd(tk,3) + qd(tk,4) + qd(tk,5));
    s123456 = sin(qd(tk,1) + qd(tk,2) + qd(tk,3) + qd(tk,4) + qd(tk,5) + qd(tk,6));
    s1234567 = sin(qd(tk,1) + qd(tk,2) + qd(tk,3) + qd(tk,4) + qd(tk,5) + qd(tk,6) + qd(tk,7));
    s12345678 = sin(qd(tk,1) + qd(tk,2) + qd(tk,3) + qd(tk,4) + qd(tk,5) + qd(tk,6) + qd(tk,7) + qd(tk,8));

    Jac1(1,1) = -c1 - s12 - c123 - s1234 - s12345 - s123456 + c1234567  - s12345678;
    Jac1(1,2) = Jac1(1,1) + c1;
    Jac1(1,3) = Jac1(1,2) + s12;
    Jac1(1,4) = Jac1(1,3) + c123;
    Jac1(1,5) = Jac1(1,4) + s1234;
    Jac1(1,6) = Jac1(1,5) + s12345;
    Jac1(1,7) = c1234567 - s12345678;
    Jac1(1,8) = -s12345678;
    
    Jac1(2,1) = -s1 + c12 - s123 + c1234 + c12345 + c123456 + s1234567 + c12345678;
    Jac1(2,2) = Jac1(2,1) + s1;
    Jac1(2,3) = Jac1(2,2) - c12;
    Jac1(2,4) = Jac1(2,3) + s123;
    Jac1(2,5) = Jac1(2,4) - c1234;
    Jac1(2,6) = c12345678 + c123456 + s1234567;
    Jac1(2,7) = c12345678 + s1234567;
    Jac1(2,8) = c12345678;
    
    %Pseudo-inverse of jacobian
    Jac1_psinv = Jac1'/(Jac1*Jac1');
    
    %Subtask 1
    task1 = Jac1_psinv*[xd_(tk); yd_(tk)];
    %forward kinematics
    xd1(tk) = -s1;
    yd1(tk) = c1;
    xd2(tk) = xd1(tk) + c12;
    yd2(tk) = yd1(tk) + s12;
    
    xd3(tk) = xd2(tk) - s123;
    yd3(tk) = yd2(tk) + c123;
    
    xd4(tk) = xd3(tk) + c1234;
    yd4(tk) = yd3(tk) + s1234;
    
    xd5(tk) = xd4(tk) + c12345;
    yd5(tk) = yd4(tk) + s12345;
    
    xd6(tk) = xd5(tk) + c123456;
    yd6(tk) = yd5(tk) + s123456;
    
    xd7(tk) = xd6(tk) + s1234567;
    yd7(tk) = yd6(tk) - c1234567;
    
    xd8(tk) = xd7(tk) + c12345678;
    yd8(tk) = yd7(tk) + s12345678;
    
    %Subtask 2
    %minimize distance of joints 6 and 7 from middle line between obstacles
    
    %joint 7
    dist7_m(tk) = (yd7(tk) - yde_0)^2;
    
    xi7 = zeros(8,1);
    xi7(1) = -2*(yd7(tk) - yde_0)*(-s1 + c12 - s123 + c1234 + c12345 + c123456 + s1234567);
    xi7(2) = -2*(yd7(tk) - yde_0)*(c12 - s123 + c1234 + c12345 + c123456 + s1234567);
    xi7(3) = -2*(yd7(tk) - yde_0)*(-s123 + c1234 + c12345 + c123456 + s1234567);
    xi7(4) = -2*(yd7(tk) - yde_0)*(c1234 + c12345 + c123456 + s1234567);
    xi7(5) = -2*(yd7(tk) - yde_0)*(c12345 + c123456 + s1234567);
    xi7(6) = -2*(yd7(tk) - yde_0)*(c123456 + s1234567);
    xi7(7) = -2*(yd7(tk) - yde_0)*(s1234567);
    xi7(8) = 0;
    
    %joint 6
    dist6_m(tk) = (yd6(tk) - yde_0)^2;
    
    xi6 = zeros(8,1);
    xi6(1) = -2*(yd6(tk) - yde_0)*(-s1 + c12 - s123 + c1234 + c12345 + c123456);
    xi6(2) = -2*(yd6(tk) - yde_0)*(c12 - s123 + c1234 + c12345 + c123456);
    xi6(3) = -2*(yd6(tk) - yde_0)*(-s123 + c1234 + c12345 + c123456);
    xi6(4) = -2*(yd6(tk) - yde_0)*(c1234 + c12345 + c123456);
    xi6(5) = -2*(yd6(tk) - yde_0)*(c12345 + c123456);
    xi6(6) = -2*(yd6(tk) - yde_0)*(c123456);
    xi6(7) = 0;
    xi6(8) = 0;
    
    %joint 5
    dist5_m(tk) = (yd5(tk) - yde_0)^2;
    
    xi5 = zeros(8,1);
    xi5(1) = -2*(yd5(tk) - yde_0)*(-s1 + c12 - s123 + c1234 + c12345);
    xi5(2) = -2*(yd5(tk) - yde_0)*(c12 - s123 + c1234 + c12345);
    xi5(3) = -2*(yd5(tk) - yde_0)*(-s123 + c1234 + c12345);
    xi5(4) = -2*(yd5(tk) - yde_0)*(c1234 + c12345);
    xi5(5) = -2*(yd5(tk) - yde_0)*(c12345);
    xi5(6) = 0;
    xi5(7) = 0;
    xi5(8) = 0;
    
    k5 = 50;
    k6 = 50;
    k7 = 100;
    task2(:,tk) = (eye(8) - Jac1_psinv*Jac1)*(k5*xi5 + k6*xi6 + k7*xi7);
    
    %angular velocity
    %qd_(tk, :) = task1' + task2(:, tk)';

%****************** Parallagh A *******************
%     if max([dist5_m(tk), dist6_m(tk), dist7_m(tk)]) == dist5_m(tk)
%         task2(:,tk) = (eye(8) - Jac1_psinv*Jac1)*(20*xi5);
%     elseif max([dist5_m(tk), dist6_m(tk), dist7_m(tk)]) == dist6_m(tk)
%         task2(:,tk) = (eye(8) - Jac1_psinv*Jac1)*(20*xi6);
%     else
%         task2(:,tk) = (eye(8) - Jac1_psinv*Jac1)*(20*xi7);
%     end
%     qd_(tk, :) = task1' + task2(:, tk)';
    
%****************** Parallagh B *******************
    if max([dist5_m(tk), dist6_m(tk), dist7_m(tk)]) >= 0.01
        qd_(tk,:) = task2(:, tk)';
    else
        qd_(tk, :) = task1' + task2(:, tk)';
    end
    
    %end-effector velocity
    v8(:, tk) = Jac1 * qd_(tk, :)';
    
    % numerical integration --> kinematic simulation
    qd(tk+1,1) = qd(tk,1) + dt*qd_(tk,1);
    qd(tk+1,2) = qd(tk,2) + dt*qd_(tk,2);
    qd(tk+1,3) = qd(tk,3) + dt*qd_(tk,3);
    qd(tk+1,4) = qd(tk,4) + dt*qd_(tk,4);
    qd(tk+1,5) = qd(tk,5) + dt*qd_(tk,5);
    qd(tk+1,6) = qd(tk,6) + dt*qd_(tk,6);
    qd(tk+1,7) = qd(tk,7) + dt*qd_(tk,7);
    qd(tk+1,8) = qd(tk,8) + dt*qd_(tk,8);
    
    %plot robot
    if (mod(tk, dtk) == 0 || tk == 1)
       % cla %clear plot
        plot(xd, yd, 'm:');
        
        plot(0, 0, 'o');
        
          plot([0, xd1(tk)], [0, yd1(tk)],'b');
          %plot(xd1(tk), yd1(tk), 'ro');
        
          plot([xd1(tk), xd2(tk)], [yd1(tk), yd2(tk)],'b');
          %plot(xd2(tk), yd2(tk), 'ro');
        
          plot([xd2(tk), xd3(tk)], [yd2(tk), yd3(tk)],'b');
          %plot(xd3(tk), yd3(tk), 'ro');
        
        plot([xd3(tk), xd4(tk)], [yd3(tk), yd4(tk)],'b');
          %plot(xd4(tk), yd4(tk), 'ro');
        
        plot([xd4(tk), xd5(tk)], [yd4(tk), yd5(tk)],'b');
          %plot(xd5(tk), yd5(tk), 'ro');
        
        plot([xd5(tk), xd6(tk)], [yd5(tk), yd6(tk)],'b');
          %plot(xd6(tk), yd6(tk), 'ro');
        
        plot([xd6(tk), xd7(tk)], [yd6(tk), yd7(tk)],'b');
          %plot(xd7(tk), yd7(tk), 'ro');
        
        plot([xd7(tk), xd8(tk)], [yd7(tk), yd8(tk)],'b');
          %plot(xd8(tk), yd8(tk), 'ro');
        
         plot(xd8(tk), yd8(tk), 'g+');
        
        %OBSTACLES
        diam = 0.2;
        d = 1;
        %5
        rectangle('Position',[xde_0 - diam/2, yde_0 - d/2+0.15, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %6
        rectangle('Position',[xde_0 - diam/2, yde_0 - d/2+1.2, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %7
        rectangle('Position',[xde_0 - diam/2, yde_0 + d/2, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %8
        rectangle('Position',[xde_0 - diam/2+0.2, yde_0 - d/2+0.15, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %9
        rectangle('Position',[xde_0 - diam/2+0.2, yde_0 - d/2+1.2, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %10
        rectangle('Position',[xde_0 - diam/2+0.4, yde_0 - d/2+0.15, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %21
        rectangle('Position',[xde_0 - diam/2+0.4, yde_0 - d/2+1.2, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %11
        rectangle('Position',[xde_0 - diam/2+0.6, yde_0 - d/2+1.35, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %12
        rectangle('Position',[xde_0 - diam/2+0.6, yde_0 - d/2+1.55, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %13
        rectangle('Position',[xde_0 - diam/2+0.8, yde_0 - d/2+1.55, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %14
        rectangle('Position',[xde_0 - diam/2+1, yde_0 - d/2+1.55, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %15
        rectangle('Position',[xde_0 - diam/2+1.1, yde_0 - d/2+1.35, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %16
        rectangle('Position',[xde_0 - diam/2+1.1, yde_0 - d/2+1.15, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %17
        rectangle('Position',[xde_0 - diam/2+1.1, yde_0 - d/2+0.95, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
        %18
        rectangle('Position',[xde_0 - diam/2+0.9, yde_0 - d/2+0.95, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
       %19
       rectangle('Position',[xde_0 - diam/2+0.6, yde_0 - d/2+0.2, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
       %20
       rectangle('Position',[xde_0 - diam/2+0.6, yde_0 - d/2+0.4, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
         %zuoshang
      %1
      rectangle('Position',[xde_0 - diam/2-0.578, yde_0 + d/2-0.35, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
      %2
      rectangle('Position',[xde_0 - diam/2-0.578, yde_0 + d/2-0.15, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
      %3
      rectangle('Position',[xde_0 - diam/2-0.378, yde_0 + d/2-0.15, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue');
      %4  
      rectangle('Position',[xde_0 - diam/2-0.178, yde_0 + d/2-0.15, diam, diam], 'Curvature', [1,1], 'FaceColor', 'blue')
     
        %%%%%%%%%
        
        pause(pauseTime);
    end
    
    tk = tk + 1;
    tt = tt + dt;
end