超冗余机器人运动控制：蛇形机器人 & 8自由度平面机械臂
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_make3 平面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
举报
腾讯云开发者
