Matlab 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',L        maximum number of iterations (default 500)      迭代次数
% 'rlimit',L        maximum number of consecutive step rejections (default 100)      最大连续拒绝次数
% 'tol',T           final error tolerance (default 1e-10)   最终误差容限
% 'lambda',L        initial value of lambda (default 0.1)   步长
% 'lambdamin',M     minimum allowable value of lambda (default 0)	步长的最小值
% 'quiet'           be quiet	输出保持安静
% 'verbose'         be verbose		输出内容详细
% 'mask',M          mask vector (6x1) that correspond to translation in X, Y and Z, 
%                   and rotation about X, Y and Z respectively.		mask向量，表示机器人自由度
% 'q0',Q            initial joint configuration (default all zeros)     迭代的初始关节形位
% 'search'          search over all configurations      搜索所有形位
% 'slimit',L        maximum number of search attempts (default 100)     最大搜索尝试数目
% 'transpose',A     use 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.gnu.org/licenses/>.
%
% http://www.petercorke.com

%TODO:
% search do a broad search from random points in configuration space

function 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 point
        
        opt.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
    end
    
    assert(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));
        end
        
        while 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                           %迭代失败，并跳出循环
            end
            
            e = 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
    end
    
    
    if opt.verbose && length(TT) > 1              %如果详细且有多个SE3值
        fprintf('TOTAL %d iterations\n', tcount);       %输出总迭代次数
    end      
end

二. 一些解释

设置的”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{:}); 有效果，但不知是否有其他影响，请慎重

Q: 求逆解时出现“警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.239176 ”是什么原因？
A: 迭代步数超过100次（位姿1），最终误差为0.239176。 猜测可能是该位姿机器人不能到达
————————————————

原文链接：https://blog.csdn.net/y_s_h1998/article/details/105787748