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

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

机械臂实际图像

注意

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

help loadrobot

结果

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

Algorithm one

最后一幅图是7个关节角度随时间变化的图像，有一条线始终在0线上，小虎从workspcae变量发现是最后一个关节的数据。仔细一看，最后一个关节就像是第6个关节的延长，经过这么一条轨迹，转动的量相当的小，远小于其他几个关节转动角度。

Algorithm two

最后一幅图是7个关节角度随时间变化的图像，这次有三条线始终在0线上，两条线(负值)重合。实际上是关节$3,4,5$转动量很小，关节$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)$而言，本人认为第二种算法没有第一种算法好，因为第二种算法有几个关节几乎没有移动，总的来说，其他关节的工作量就大了一些，更容易损坏。