V-rep学习笔记:机器人逆运动学数值解法(The Jacobian Transpose Method) ...

发布时间:2016年10月28日 09:15:45    浏览数:1120次    来自:博客园
机器人运动学逆解的问题经常出现在动画仿真和工业机器人的轨迹规划中:We want to know how the upper joints of the hierarchy would rotate if we want the end effector to reach some goal.    IK Solutions ...

    机器人运动学逆解的问题经常出现在动画仿真和工业机器人的轨迹规划中:We want to know how the upper joints of the hierarchy would rotate if we want the end effector to reach some goal.


  IK Solutions:

  • Analytical solutions are desirable because of their speed and exactness of solution.

  • For complex kinematics problems, analytical solutions may not be possible.

  • Use iterative methods (迭代法)--Optimization methods (e.g., minimize the distance between end effector and goal point)


  What is Jacobian? A linear approximation to f(x). 雅克比矩阵相当于函数f(x)的一阶导数,即线性近似。

  Computing the Jacobian Numerically:(雅克比矩阵的计算有解析法和数值法,当难以根据解析法得到雅克比时可以用差分法代替微分求解)


  下面是Jacobian Transpose方法的主要推导过程:

    It is recommended that you choose a small positive scalar α < 1 and update the joint angles θ by adding Δθ. Then proceed iteratively by recomputing the Jacobian based on the updated angles and positions, finding new values for Δθ and again updating with a small fraction α. This is repeated until the links are sufficiently close to the desired positions. The question of how small α needs to be depends on the geometry of the links; it would be a good idea to keep α small enough so that the angles are updated by at most 5 or 10° at a time.


  Operating Principle:

  Project difference vector DX on those dimensions qwhich can reduce it the most. It is a plausible, justifyable approach, because it is related to the method of steepest decent.( It follows a force that pulls the end-effector towards its desired target location).

1. Simple computation (numerically robust)
2. No matrix inversions

1. Needs many iterations until convergence in certain configurations
2. Unpredictable joint configurations
3. Non conservative


    下面以V-rep中官方自带的例子(在文件夹scenes/ik_fk_simple_examples中)为基础进行修改,添加代码手动实现运动学逆解的求解。为了实现同样的功能先将两个旋转关节从Inverse kinematics mode设为Passive mode,然后将target和tip的Linked dummy设为none,并在Calculation Modules的Inverse kinematics选项卡中取消IK groups enabled。下图中红色的dummy为target dummy,仿真开始后程序会计算连杆末端(tip dummy)与target之间的误差,然后根据Jacobian Transpose方法不断计算关节调整量,直到误差小于容许值。

    如下图所示,迭代计算61次收敛后,点击图中的Compute IK按钮,连杆末端能根据计算出的关节角q1,q2移动到target的位置(可以随意拖动target的位置,在合理的范围内经过逆解计算tip都会与target重合)

  在child script中由lua API实现了该方法

if (sim_call_type==sim_childscriptcall_initialization) then
    J1_handle = simGetObjectHandle('j1')
    J2_handle = simGetObjectHandle('j2')
    target_handle = simGetObjectHandle('target')
    consoleHandle = simAuxiliaryConsoleOpen('info', 5, 2+4, {100,100},{800,300})
    --link length
    L1 = 0.5
    L2 = 0.5

    gamma = 1       --step size
    stol = 1e-2     --tolerance
    nm = 100        --initial error
    count = 0       --iteration count
    ilimit = 1000   --maximum iteratio
    --initial joint value
    q1 = 0   
    q2 = 0

if (sim_call_type==sim_childscriptcall_actuation) then
    local a=simGetUIEventButton(ui)
    local target_pos = simGetObjectPosition(target_handle, -1)

    if(nm > stol) 
        simAuxiliaryConsolePrint(consoleHandle, nil)

        local x, y = L1*math.cos(q1)+L2*math.cos(q1+q2), L1*math.sin(q1)+L2*math.sin(q1+q2)
        local delta_x, delta_y = target_pos[1] - x, target_pos[2] - y
        local dq_1 = (-L1*math.sin(q1)-L2*math.sin(q1+q2))*delta_x + (L1*math.cos(q1)+L2*math.cos(q1+q2))*delta_y
        local dq_2 = (-L2*math.sin(q1+q2))*delta_x + (L2*math.cos(q1+q2))*delta_y
        q1, q2 = q1 + gamma*dq_1,  q2 + gamma*dq_2

        nm = math.sqrt(delta_x * delta_x + delta_y * delta_y)

        count = count + 1
        if count > ilimit   then
            simAuxiliaryConsolePrint(consoleHandle,"Solution wouldn't converge\r\n")

        simAuxiliaryConsolePrint(consoleHandle, string.format("q1:%.2f", q1*180/math.pi)..'  '..string.format("q2:%.2f", q2*180/math.pi)..'\r\n') 
        simAuxiliaryConsolePrint(consoleHandle, string.format("x:%.2f",x)..'  '..string.format("y:%.2f", y)..'\r\n') 
        simAuxiliaryConsolePrint(consoleHandle, string.format("%d", count)..'iterations'..'  '..string.format("err:%.4f", nm)..'\r\n')   

    -- if the button(a is the button handle) is pressed
    if a==1 then
        simSetJointPosition(J1_handle, q1+math.pi/2)  -- the angle between L1 and X-axis is 90 degree
        simSetJointPosition(J2_handle, q2)