OpenRobotics浅学——机械臂逆运动学超真实轨迹仿真(附代码)

最近小虎在学习robotics的时候,利用7关节(joints)机械臂进行机器人轨迹求解仿真,这个程序的“毛病”也是亮点之一就是引用了很多新版MATLAB的函数,应该是Robotics tool box里面的东西。Anyway,这里就编程思想本身进行分析,编程工具是MATLAB。里提供两种算法,一种是任务空间计算、一种是关节空间计算。

全文目录

    • 机械臂实际图像
    • 注意
  • 结果
    • Algorithm one
    • Algorithm two
  • 代码分析
    • Algorithm one
      • exampleHelperTimeBasedTaskInputs
      • trejectory_xiaohu1.m
    • Algorithm two
      • exampleHelperTimeBasedJointInputs.m
      • trejectory_xiaohu2.m
  • 代码来源
  • 改进代码——已存github
  • 小结

机械臂实际图像

OpenRobotics浅学——机械臂逆运动学超真实轨迹仿真(附代码)_第1张图片

注意

请在配置了Robotics tool box的较新版本的MATLAB(小虎的是2019b)。可以试试在commad window输入下面的语句,如果找不到loadrobot,那么代表你的MATLAB没有相关工具箱程序。

help loadrobot

OpenRobotics浅学——机械臂逆运动学超真实轨迹仿真(附代码)_第2张图片

结果

测试目的地坐标为 ( 1.3 , 0.4 , 0.9 ) (1.3,0.4,0.9) (1.3,0.4,0.9)

Algorithm one

OpenRobotics浅学——机械臂逆运动学超真实轨迹仿真(附代码)_第3张图片
OpenRobotics浅学——机械臂逆运动学超真实轨迹仿真(附代码)_第4张图片
OpenRobotics浅学——机械臂逆运动学超真实轨迹仿真(附代码)_第5张图片
最后一幅图是7个关节角度随时间变化的图像,有一条线始终在0线上,小虎从workspcae变量发现是最后一个关节的数据。仔细一看,最后一个关节就像是第6个关节的延长,经过这么一条轨迹,转动的量相当的小,远小于其他几个关节转动角度

Algorithm two

OpenRobotics浅学——机械臂逆运动学超真实轨迹仿真(附代码)_第6张图片
OpenRobotics浅学——机械臂逆运动学超真实轨迹仿真(附代码)_第7张图片
OpenRobotics浅学——机械臂逆运动学超真实轨迹仿真(附代码)_第8张图片
最后一幅图是7个关节角度随时间变化的图像,这次有三条线始终在0线上,两条线(负值)重合。实际上是关节 3 , 4 , 5 3,4,5 3,4,5转动量很小,关节 1 , 7 1,7 1,7转动量很相近。

代码分析

两种算法前一个程序的区别在于,前者输入始末位置的点信息矩阵;后者输入的是每个关节在每个时刻需要到达的位置。

Algorithm one

exampleHelperTimeBasedTaskInputs

实时计算齐次变换矩阵的值。

function stateDot = exampleHelperTimeBasedTaskInputs(motionModel, timeInterval, initialTform, finalTform, t, state)

[refPose, refVel] = transformtraj(initialTform, finalTform, timeInterval, t);

 % Compute state derivative
stateDot = derivative(motionModel, state, refPose, refVel);
end

trejectory_xiaohu1.m

任务空间法(部分代码)。

% Algorithm one
% Task-Space
% PID控制器
tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','EndEffector_Link');
tsMotionModel.Kp(1:3,1:3) = 0;%
tsMotionModel.Kd(1:3,1:3) = 0;

q0 = currentRobotJConfig; %初始状态
qd0 = zeros(size(q0));
%模拟机器人运动
[tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state),timeInterval,[q0; qd0]);

Algorithm two

exampleHelperTimeBasedJointInputs.m

实时计算齐次变换矩阵的值。

function stateDot = exampleHelperTimeBasedJointInputs(motionModel, timeInterval, configWaypoints, t, state)

    % Use a B-spline curve to ensure the trajectory is smooth and moves
    % through the waypoints with non-zero velocity
    [qd, qdDot] = bsplinepolytraj(configWaypoints,  timeInterval , t);
    
    % Compute state derivative
    stateDot = derivative(motionModel, state, [qd; qdDot]);
end

trejectory_xiaohu2.m

关节空间法(部分代码)。

% Algorithm two
%joint space
ik = inverseKinematics('RigidBodyTree',robot);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1 1 1 1 1 1];
%运动学逆解
initialGuess = wrapToPi(jointInit);
jointFinal = ik(endEffector,taskFinal,weights,initialGuess);
jointFinal = wrapToPi(jointFinal);
%插值
ctrlpoints = [jointInit',jointFinal'];
jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes);
jointWaypoints = bsplinepolytraj(jointConfigArray,timeInterval,1);

jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');
q0 = currentRobotJConfig;
qd0 = zeros(size(q0));
%模拟机器人运动
[tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state),timeInterval,[q0; qd0]);

代码来源

源代码来自于作者wuhaoran996(又名银河帝国暗黑卿)的github:
https://github.com/wuhaoran996/openRobotics#openrobotics
作者并没有很多个人介绍,从名字996推断,代码的作者是一个程序员。
另外,源代码经过本人修改

改进代码——已存github

可以在我的github下载:robot_arm_trajectories

小结

就测试目的地坐标为 ( 1.3 , 0.4 , 0.9 ) (1.3,0.4,0.9) (1.3,0.4,0.9)而言,本人认为第二种算法没有第一种算法好,因为第二种算法有几个关节几乎没有移动,总的来说,其他关节的工作量就大了一些,更容易损坏。

你可能感兴趣的:(Robotics,MATLAB)