V-rep學習筆記：機器人逆運動學數值解法（The Jacobian Transpose Method）

　　機器人運動學逆解的問題經常出現在動畫仿真和工業機器人的軌跡規划中：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)

　　機器人學的教材上一般只介紹了運動學逆解的解析解法，很少涉及數值解（迭代解法）。其中有兩方面的原因，一是數值解相對於解析解需要的運算量更大，不太適合實時性要求較高的場合；另一方面是因為一般在設計機器人的結構時就會考慮其逆解的可解性，比如相鄰的三個關節軸線相交即存在解析解。下面以最簡單的二連桿機構為例子，研究一下機器人運動學逆解的數值解法。如下圖所示，其運動學正解很容易得到，根據運動學正解可以寫出機器人的雅克比矩陣J。

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).

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

Disadvantages:
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
    ui=simGetUIHandle('UI')
    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
end


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

    if(nm > stol) 
    then
        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")
        end

        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')   
    end

    -- 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)      
    end
end