Matlab Robotics 逆运动学求解函数（ikine）分析机器人

注：全文为本人见解，如有错误请指点！
一. 源代码的解释以下代码直接对robotics工具箱中的ikine函数进行解释

%SerialLink.ikine Inverse kinematics by optimization without joint limits % % Q = R.ikine(T) are the joint coordinates (1xN) corresponding to the robot % end-effector pose T which is an SE3 object or homogenenous transform % matrix (4x4), and N is the number of robot joints. % % This method can be used for robots with any number of degrees of freedom. % 这个方法适用于任意自由度的机器人 % % Options:: % 'ilimit',Lmaximum number of iterations (default 500)迭代次数 % 'rlimit',Lmaximum number of consecutive step rejections (default 100)最大连续拒绝次数 % 'tol',Tfinal error tolerance (default 1e-10)最终误差容限 % 'lambda',Linitial value of lambda (default 0.1)步长 % 'lambdamin',Mminimum allowable value of lambda (default 0) 步长的最小值 % 'quiet'be quiet 输出保持安静 % 'verbose'be verbose输出内容详细 % 'mask',Mmask vector (6x1) that correspond to translation in X, Y and Z, %and rotation about X, Y and Z respectively.mask向量，表示机器人自由度 % 'q0',Qinitial joint configuration (default all zeros)迭代的初始关节形位 % 'search'search over all configurations搜索所有形位 % 'slimit',Lmaximum number of search attempts (default 100)最大搜索尝试数目 % 'transpose',Ause Jacobian transpose with step size A, rather than %Levenberg-Marquadt使用步长为A的雅可比转置，而不是LM方法 % % Trajectory operation:: % % In all cases if T is a vector of SE3 objects (1xM) or a homogeneous % transform sequence (4x4xM) then returns the joint coordinates % corresponding to each of the transforms in the sequence.Q is MxN where % N is the number of robot joints. The initial estimate of Q for each time % step is taken as the solution from the previous time step. % 使用上一步的解作为当前步的初始值 % % Underactuated robots::欠驱动机器人 % % For the case where the manipulator has fewer than 6 DOF the solution % space has more dimensions than can be spanned by the manipulator joint % coordinates. % 欠驱动机器人解空间维数比关节坐标维数多？ % % In this case we specify the 'mask' option where the mask % vector (1x6) specifies the Cartesian DOF (in the wrist coordinate % frame) that will be ignored in reaching a solution.The mask vector % has six elements that correspond to translation in X, Y and Z, and rotation % about X, Y and Z respectively.The value should be 0 (for ignore) or 1. % The number of non-zero elements should equal the number of manipulator DOF. % 使用“mask”矢量指定失去的自由度，机器人自由度等于改矢量的非零元素的个数。 % % For example when using a 3 DOF manipulator rotation orientation might be % unimportant in which case use the option: 'mask', [1 1 1 0 0 0]. % 举个例子，当使用三自由度机械手时，旋转方向可能不重要（不影响逆运动学求解），mask向量设置为[1 1 1 0 0 0] % % For robots with 4 or 5 DOF this method is very difficult to use since % orientation is specified by T in world coordinates and the achievable % orientations are a function of the tool position. % 对于4或5个自由度的机器人，这种方法较难使用 % % References:: % - Robotics, Vision & Control, P. Corke, Springer 2011, Section 8.4. % % Notes:: % - This has been completely reimplemented in RTB 9.11 % - Does NOT require MATLAB Optimization Toolbox.不需要matlab优化工具箱 % - Solution is computed iteratively.采用迭代的方法 % - Implements a Levenberg-Marquadt variable step size solver. %通过LM变步长求解器实现 % - The tolerance is computed on the norm of the error between current %and desired tool pose.This norm is computed from distances %and angles without any kind of weighting. %误差是根据当前工具姿态和期望（求解）姿态的范数计算 % - The inverse kinematic solution is generally not unique, and %depends on the initial guess Q0 (defaults to 0). %求解的值不唯一，且与初值有关，初值Q0默认为0 % - The default value of Q0 is zero which is a poor choice for most %manipulators (eg. puma560, twolink) since it corresponds to a kinematic %singularity. %Q0设置为[0 0 0 0 0 0]，这对于部机器人（puma560，twolink）不是好选择，因为其对应运动学奇点 % - Such a solution is completely general, though much less efficient %than specific inverse kinematic solutions derived symbolically, like %ikine6s or ikine3. %这是通用的解法，所以效率不高 % - This approach allows a solution to be obtained at a singularity, but %the joint angles within the null space are arbitrarily assigned. %此方法可能在奇异点求出解 % - Joint offsets, if defined, are added to the inverse kinematics to %generate Q. %如果定义了关节偏移，将添加到逆运动学中以生成Q % - Joint limits are not considered in this solution. %不考虑关节的范围限制 % - The 'search' option peforms a brute-force search with initial conditions %chosen from the entire configuration space. %“search”选项形成一个强力搜索，初始条件从整个关节空间中随机选择 % - If the 'search' option is used any prismatic joint must have joint %limits defined. %如果使用“搜索”选项，则任何移动关节都必须定义关节范围 % % See also SerialLink.ikcon, SerialLink.ikunc, SerialLink.fkine, SerialLink.ikine6s.% Copyright (C) 1993-2017, by Peter I. Corke % % This file is part of The Robotics Toolbox for MATLAB (RTB). % % RTB is free software: you can redistribute it and/or modify % it under the terms of the GNU Lesser General Public License as published by % the Free Software Foundation, either version 3 of the License, or % (at your option) any later version. % % RTB is distributed in the hope that it will be useful, % but WITHOUT ANY WARRANTY; without even the implied warranty of % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the % GNU Lesser General Public License for more details. % % You should have received a copy of the GNU Leser General Public License % along with RTB.If not, see . % % http://www.petercorke.com%TODO: % search do a broad search from random points in configuration spacefunction qt = ikine(robot, tr, varargin)n = robot.n; %关节变量的个数TT = SE3.check(tr); %转换为SE3矩阵%set default parameters for solution默认参数设置 opt.ilimit = 500; %默认迭代次数 opt.rlimit = 100; %最大连续拒绝次数 opt.slimit = 100; %最大尝试次数 opt.tol = 1e-10; %默认迭代误差 opt.lambda = 0.1; %默认步长 opt.lambdamin = 0; %默认步长最小值 opt.search = false; %默认关闭搜索初值 opt.quiet = false; %保持安静，减少输出 opt.verbose = false; %输出的信息详细 opt.mask = [1 1 1 1 1 1]; %默认六自由度 opt.q0 = zeros(1, n); %初始关节形位为零位 opt.transpose = NaN; %默认使用LM方法[opt,args] = tb_optparse(opt, varargin); %输入参数传递if opt.search%如果为true，执行，随机选取迭代初值 % randomised search for a starting pointopt.search = false; %关闭搜索初值 opt.quiet = true; %保持安静 %args = args{2:end}; for k=1:opt.slimit%循环到最大尝试次数 for j=1:n qlim = robot.links(j).qlim; %传递机器人关节变量的范围 if isempty(qlim)%是否为空集 if robot.links(j).isrevolute%是否为旋转关节 q(j) = rand*2*pi - pi; %产生-pi到pi的随机初值 else error('For a prismatic joint, search requires joint limits'); %对于移动关节，需要定义关节范围 end else q(j) = rand*(qlim(2)-qlim(1)) + qlim(1); %产生关节范围内的随机初值 end end fprintf('Trying q = %s\n', num2str(q)); %输出“尝试q的值为 ”q = robot.ikine(tr, q, args{:}, 'setopt', opt); %以此为初值，计算结果 if ~isempty(q) qt = q; %如果不为空集，q为所求值 return; end end error('no solution found, are you sure the point is reachable?'); %找不到解，你确定该点可到达？（尝试完最大尝试次数） qt = []; return endassert(numel(opt.mask) == 6, 'RTB:ikine:badarg', 'Mask matrix should have 6 elements'); %如果mask没有6个参数，报错 assert(n >= numel(find(opt.mask)), 'RTB:ikine:badarg', 'Number of robot DOF must be >= the same number of 1s in the mask matrix'); %如果机器人自由度小于mask矩阵的1元素个数，报错 W = diag(opt.mask); %W为以mask矩阵为对角线的矩阵qt = zeros(length(TT), n); % preallocate space for results为解预分配空间 tcount = 0; % total iteration count初始化总迭代次数 rejcount = 0; % rejected step count初始化持续拒绝的迭代次数q = opt.q0; %给迭代初值failed = false; %迭代失败的标签 revolutes = robot.isrevolute(); %给机器人的关节构型（移动为0或旋转为1，全为旋转为[1 1 1 1 1 1]）for i=1:length(TT)%循环到输入的SE3的个数 T = TT(i); %给其中某个SE3 lambda = opt.lambda; %给默认步长iterations = 0; %初始化迭代次数if opt.debug%是否调试程序，默认不调试，跳 if e = tr2delta(robot.fkine(q), T); %给从初始零位的位姿到输入位姿的对应微分运动，d=(dx, dy, dz, dRx, dRy, dRz) fprintf('Initial:|e|=%g\n', norm(W*e)); endwhile true%开始进行迭代 % update the count and test against iteration limit iterations = iterations + 1; %计数迭代次数 if iterations > opt.ilimit%超过迭代最大次数 if ~opt.quiet%quiet值为0，不保持安静，执行 warning('ikine: iteration limit %d exceeded (pose %d), final err %g', ... opt.ilimit, i, nm); end failed = true; break%迭代失败，并跳出循环 ende = tr2delta(robot.fkine(q), T); % are we there yet if norm(W*e) < opt.tol break; %如果小于容许误差，跳出循环 end% compute the Jacobian J = jacobe(robot, q); %给当前形位下的雅各比矩阵JtJ = J'*W*J; if ~isnan(opt.transpose)%是否使用LM方法，进行 % do the simple Jacobian transpose with constant gain dq = opt.transpose * J' * e; else % do the damped inverse Gauss-Newton with Levenberg-Marquadt % 用LM法做阻尼逆高斯牛顿 dq = inv(JtJ + (lambda + opt.lambdamin) * eye(size(JtJ)) ) * J' * W * e; %迭代公式% compute possible new value of得到新的关节值 qnew = q + dq'; % and figure out the new error得到新的误差值 enew = tr2delta(robot.fkine(qnew), T); % was it a good update? if norm(W*enew) < norm(W*e)%该步是否成功 % step is accepted if opt.debug%是否调试程序，默认不调试，跳 if fprintf('ACCEPTED: |e|=%g, |dq|=%g, lambda=%g\n', norm(W*enew), norm(dq), lambda); end q = qnew; e = enew; %给迭代新值 lambda = lambda/2; rejcount = 0; %持续拒绝的迭代次数置0 else % step is rejected, increase the damping and retry % 该步被拒绝，增加阻尼并重试 if opt.debug%是否调试程序，默认不调试，跳 if fprintf('rejected: |e|=%g, |dq|=%g, lambda=%g\n', norm(W*enew), norm(dq), lambda); end lambda = lambda*2; rejcount = rejcount + 1; if rejcount > opt.rlimit%超过最大拒绝次数，进行 if ~opt.quiet%是否保持输出安静，默认保持，跳 else warning('ikine: rejected-step limit %d exceeded (pose %d), final err %g', ... opt.rlimit, i, norm(W*enew)); end failed = true; break; %迭代失败，跳出循环 end continue; % try again修改步长后继续尝试 end end% wrap angles for revolute joints将旋转关节的关节值移到-pi~pi中 k = (q > pi) & revolutes; q(k) = q(k) - 2*pi; k = (q < -pi) & revolutes; q(k) = q(k) + 2*pi; nm = norm(W*e); end% end ikine solution for this pose qt(i,:) = q'; tcount = tcount + iterations; %总共迭代次数，包括多个SE3值的情况 if opt.verbose && ~failed%是否保持输出详细且迭代成功 fprintf('%d iterations\n', iterations); end if failed if ~opt.quiet%是否失败且不保持安静 warning('failed to converge: try a different initial value of joint coordinates'); %未能收敛，尝试不同的初值 end qt = []; end endif opt.verbose && length(TT) > 1%如果详细且有多个SE3值 fprintf('TOTAL %d iterations\n', tcount); %输出总迭代次数 end end

二. 一些解释

【Matlab Robotics 逆运动学求解函数（ikine）分析】设置的”search“选项不起作用
我经过多次尝试，发现无论其随机选取的迭代初值q为多少，程序实际上还是从q0=[0 0 0 0 0 0]开始迭代。
我猜测，q = robot.ikine(tr, q, args{:}, 'setopt', opt);这行代码有问题。
随机选取的迭代初值q，并没有被传递进去。
将这行代码改为 q = robot.ikine(tr, 'setopt', opt, 'q0',q, args{:}); 有效果，但不知是否有其他影响，请慎重
迭代公式
未完待续~