조작자 - 6 축 로봇 핸들러 공간 모션 분석

매니퓰레이터 모델링 분석 : https://blog.csdn.net/Kalenee/article/details/81990130

관절 간격의 동작 분석에 MoveIt 계획 : http://www.guyuehome.com/752

 

I. 서론

      MoveIt ROS 플랫폼에서 수행 머니퓰레이터 제어를 사용할 때, 기본 통화 AddTimeParameterization 완전한 트랙 이동 계획 모듈, 상기 시간 프레임의 각속도에 대응하는 관절의 각 관절 위치의 출력 및 움직임 정보를 필요로하는 로봇 아암의 단부 아니다 rosbag 공간에 의해 완전한 기록 동작 계획 매트랩 동작 분석과 결합 작업을 완료한다.

 

둘째, 야 코비 행렬

2.1 수학적 센스

수학적 코비 행렬 (자 코비안 행렬) 다기능의 편미분 행렬이다. 육은 육 개 변수가 다음과 같은 기능을 가진다 :

이후 여섯 개 함수 차동이었다

그것은로 축약 할 수있다 

상기  코비안 행렬이고, J는 가능한 나타낸다.

2.2 매니퓰레이터 코비안

조작자 코비안 행렬 J는 조작 공간에 전달 공동 공간 속도 사이의 선형 관계로 어느 하나이고, 또한 선형 차동 운동 변환에서 볼 수있다.

                                                                                          

                                                                                         

인 6 축 로봇 코비안 행렬 J (Q)에 대이며 3 행 각속도 변속비 (W)의 끝을 나타내는 후 매트릭스 및 각 열에 대응하는 공동을 나타내고, 순서 행렬 변속비 단자 선 속도 V 대신 처음 세 열의 상기 전송 팁 속도 비와 각속도에 대한 대응 조인트 속도 조인트 부 N, 및 충돌 속도 조인트 유닛은 각각 선형 및 각속도의 접합 단부에 대응하는 N.

                                                                            

셋째, 동작은 달성

3.1 트랙 레코드

작업 공간 가방에있는 파일, 디렉토리에 .bag 문서 및 스크립트의 트랙 레코드를 만듭니다.

노드 (1)의 제조 상기 기록 트랙을 사용 rosbag

rosbag API를 공식 튜토리얼 : http://wiki.ros.org/rosbag/Code%20API

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64MultiArray.h>
#include <string>
#include <trajectory_msgs/JointTrajectory.h>

ros::Subscriber write_sub, motion_sub;
int nums;

bool writeJudge;
rosbag::Bag writeRobot, writePos, writeVel;

void data_process(trajectory_msgs::JointTrajectory getData) {
    writeRobot.write("/moveit/JointTrajectory", ros::Time::now(), getData);
  for (int j_num = 0; j_num < 6; j_num++) {
    std_msgs::Float64MultiArray pos_data, vel_data;
    for (int i = 0; i < getData.points.size(); i++) {
      pos_data.data.push_back(getData.points[i].positions[j_num]);
      vel_data.data.push_back(getData.points[i].velocities[j_num]);
    }
    writePos.write("pos" + std::to_string(j_num), ros::Time::now(), pos_data);
    writeVel.write("val" + std::to_string(j_num), ros::Time::now(), vel_data);
  }
}

void write_callback(const std_msgs::Bool::ConstPtr &data) {
  if (data->data) {
    writeJudge = true;
    std::string robotFileName = "bag/motion";
    robotFileName += std::to_string(nums);
    robotFileName += ".bag";
    writeRobot.open(robotFileName, rosbag::bagmode::Write);

    std::string posFileName = "bag/motion_pos";
    posFileName += std::to_string(nums);
    posFileName += ".bag";
    writePos.open(posFileName, rosbag::bagmode::Write);

    std::string velFileName = "bag/motion_vel";
    velFileName += std::to_string(nums);
    velFileName += ".bag";
    writeVel.open(velFileName, rosbag::bagmode::Write);

  } else {
    writeJudge = false;
    writeRobot.close();
    writePos.close();
    writeVel.close();
    nums++;
  }
}

void motion_callback(const trajectory_msgs::JointTrajectory::ConstPtr &data) {
  if (writeJudge) {
    std::cout << "write" << std::endl;
    data_process(*data);
  }
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "record");
  ros::NodeHandle nh;
  nums = 1;
  writeJudge = false;
  write_sub =
      nh.subscribe<std_msgs::Bool>("/robot/bagWirte", 1, write_callback);
  motion_sub = nh.subscribe<trajectory_msgs::JointTrajectory>(
      "/moveit/JointTrajectory", 10, motion_callback);
  ros::spin();
}

(2) .bag합니다 .CSV 파일로 스크립트 파일을 사용하여

스크립트 bagtocsv.sh 만들기

#！ /bin/bash
echo "Enter bag name"
read bagname
for topic in `rostopic list -b $bagname.bag`;
do rostopic echo -p -b $bagname.bag $topic >$bagname-${topic//\//_}.csv;
echo "finish"
done

디렉토리에 CD를 .bag 파일 이름을 입력하라는 메시지가 떠들썩한 파티 명령을 실행합니다.

 

3.2 추적 과정

MATLAB 프로그램

clear;

%% 前期准备
%启动工具箱
startup_rvc;

%角度转换
du=pi/180;     %度
radian=180/pi; %弧度
 
%关节长度，单位m
L1=0.40;L2=0.39;L3=0.17;L4=0.20;L5=0.08;
 
%% 建立数学模型
% DH法建立模型,关节角，连杆偏移，连杆长度，连杆扭转角，关节类型（0转动，1移动）
L(1) = Link( 'd',L1    ,  'a',0  , 'alpha',-pi/2, 'offset',0          );
L(2) = Link( 'd',0     ,  'a',-L2, 'alpha',0    , 'offset',pi/2       );
L(3) = Link( 'd',0     ,  'a',0  , 'alpha',pi/2 , 'offset',0          );
L(4) = Link( 'd',L3+L4 ,  'a',0  , 'alpha',-pi/2, 'offset',0          );
L(5) = Link( 'd',0     ,  'a',0  , 'alpha',pi/2 , 'offset',0          );
L(6) = Link( 'd',L5    ,  'a',0  , 'alpha',-pi/2, 'offset',0          );
plotopt = {'noraise', 'nowrist', 'nojaxes', 'delay',0};
tool_char=[1 0 0 0;
           0 1 0 0;
           0 0 1 0;
           0 0 0 1];
six_link=SerialLink(L,'name','six link','tool',tool_char);

%% 显示机械臂
figure('name','机器臂')
hold on 
six_link.plot([0 0 0 0 0 0], plotopt{:});
hold off

%% 计算雅可比矩阵
syms theta1 theta2 theta3 theta4 theta5 theta6;
j_trans = six_link.jacob0([theta1 theta2 theta3 theta4 theta5 theta6],'trans');
save('JacobData','j_trans');

%% 读取关节运动参数
%获取关节角度
j1 = csvread('robotMotion_pos-pos0.csv');
j2 = csvread('robotMotion_pos-pos1.csv');
j3 = csvread('robotMotion_pos-pos2.csv');
j4 = csvread('robotMotion_pos-pos3.csv');
j5 = csvread('robotMotion_pos-pos4.csv');
j6 = csvread('robotMotion_pos-pos5.csv');

%获取角速度
j_v = zeros(6,length(j1));
j_v(1,:) = csvread('robotMotion_vel-val0.csv');
j_v(2,:) = csvread('robotMotion_vel-val1.csv');
j_v(3,:) = csvread('robotMotion_vel-val2.csv');
j_v(4,:) = csvread('robotMotion_vel-val3.csv');
j_v(5,:) = csvread('robotMotion_vel-val4.csv');
j_v(6,:) = csvread('robotMotion_vel-val5.csv');

%% 计算末端运动参数
eff_p = zeros(3,length(j1));
eff_v = zeros(3,length(j1));
for  i=1:1:length(j1)
    theta1 = j1(i);
    theta2 = j2(i);
    theta3 = j3(i);
    theta4 = j4(i);
    theta5 = j5(i);
    theta6 = j6(i);
    
    % 计算末端位置
    eff_p(:,i) = six_link.fkine([theta1 theta2 theta3 theta4 theta5 theta6]).t;
     % 计算末端速度
    eff_v(:,i) = double(subs(j_trans))*j_v(:,i);
end

%% 画图
time = csvread('robotMotion_time.csv');

hold on;
figure('name','关节空间运动分析')
subplot(2,1,1);   
plot(time,j1,time,j2,time,j3,time,j4,time,j5,time,j6);
subplot(2,1,2);
plot(time,j_v(1,:),time,j_v(2,:),time,j_v(3,:),time,j_v(4,:),time,j_v(5,:),time,j_v(6,:));
hold off;

hold on;
figure('name','工作空间运动分析')
subplot(2,1,1);   
plot(time,eff_p(1,:),time,eff_p(2,:),time,eff_p(3,:));
subplot(2,1,2);
plot(time,eff_v(1,:),time,eff_v(2,:),time,eff_v(3,:));
hold off;

3.3 결과

                                                                                        공동 공간 동작 분석 

                                                                                       작업 공간 동작 분석 

 

참조 :

"로봇 기술"