When performing inverse kinematics (IK) on a complicated bone chain, it can become too complex for an analytical solution. Cyclic Coordinate Descent (CCD) is an alternative that is both easy to implement and efficient to process。逆運動學問題一般采用解析法和基於Jacobian矩陣的迭代方法，前者雖然精度高而且能達到實時的效果，但是隨着關節的增多，自由度隨着增多，數學建模也變得很困難，甚至不可解。而后者很難達到實時的效果。
CCD 算法的思想
Cyclic Coordinate Descent (CCD) 是一個啟發式的迭代搜索算法，它通過每一次只改變一個關節的參數來逐步減少位置誤差和姿態誤差，每個迭代過程包括一個從關節鏈結構的末端到基點的遍歷過程。由於CCD 方法將多關節的關節鏈問題簡化為單關節問題，可以用解析法處理，因此每一步的迭代可以相當快。當求得每個關節的參數 (轉角)θ后，將其代入正向運動學方程求得末端效器和每個關節的位置。從運動鏈的末端開始 , 逐步改變每個關節的旋轉角度。先是改變最末端的關節，末端關節到末段執行器的向量為圖中藍色線段，末端關節到目標點的向量為圖中紅色線段。求出 2 個向量的夾角α，讓末端關節下的子鏈繞旋轉軸轉α角度，則末端執行器達到一個新位置。若沒有達到目標，則繼續取當前關節的上一關節，改變其旋轉角度，直到選到根節點。若末端還沒有達到目標位置，則又從末端關節開始新一輪運動，直到位置誤差足夠小或者到達了給定的循環次數。
After our first loop through the bone chain, we have moved the end effector much closer to the target position. By repeating this process, we will continue to get closer and closer. Once we have reached a tolerable distance from the target position or once we have performed too many iterations (for performance reasons), we can stop looping.下面三幅圖展示了CCD算法的3次迭代過程，可以看出隨着迭代的進行，末端離目標點越來越近。
下面在V-rep中建立平面3連桿機構，各桿長均為0.5m，使用Python腳本計算運動學逆解並控制V-rep中的模型，使其達到目標位置。
# -*- coding: utf-8 -*-
import vrep # V-rep library
import sys import time import math # This function will convert an angle to the equivalent rotation in the range [-pi,pi]
def ConfineAngle(angle): angle = angle % (2.0 * math.pi) if( angle < -math.pi ): angle += (2.0 * math.pi) if( angle > math.pi ): angle -= (2.0 * math.pi) return angle def CalcIK(): id = linkNum - 1
while id >= 0: retcode, J_pos = vrep.simxGetObjectPosition(clientID,joint_handle[id],-1,vrep.simx_opmode_oneshot_wait) retcode, tip = vrep.simxGetObjectPosition(clientID,tip_handle, -1, vrep.simx_opmode_oneshot_wait) # Get the vector from the current bone to the end effector position.
curToEndX = tip[0] - J_pos[0] curToEndY = tip[1] - J_pos[1] curToEndMag = math.sqrt( curToEndX*curToEndX + curToEndY*curToEndY ) # Get the vector from the current bone to the target position.
curToTargetX = target[0] - J_pos[0]; curToTargetY = target[1] - J_pos[1]; curToTargetMag = math.sqrt(curToTargetX*curToTargetX+curToTargetY*curToTargetY) # Get rotation
endTargetMag = curToEndMag*curToTargetMag if endTargetMag <= 0.0001: # prevent division by small numbers
cosRotAng = 1 sinRotAng = 0 else: cosRotAng = (curToEndX*curToTargetX + curToEndY*curToTargetY) / endTargetMag sinRotAng = (curToEndX*curToTargetY - curToEndY*curToTargetX) / endTargetMag # Clamp the cosine into range when computing the angle(might be out of rangedue to floating point error)
rotAng = math.acos(max(-1, min(1,cosRotAng))) if sinRotAng < 0.0: rotAng = -rotAng q[id] = q[id] + rotAng # Rotate the current link
if(id == 0): vrep.simxSetJointPosition(clientID,joint_handle[id], ConfineAngle(q[id])+math.pi/2, vrep.simx_opmode_oneshot) else: vrep.simxSetJointPosition(clientID,joint_handle[id], ConfineAngle(q[id]), vrep.simx_opmode_oneshot) # Check for termination
retcode, tip = vrep.simxGetObjectPosition(clientID,tip_handle, -1, vrep.simx_opmode_oneshot_wait) endToTargetX = (target[0] - tip[0]) endToTargetY = (target[1] - tip[1]) error = math.sqrt(endToTargetX*endToTargetX + endToTargetY*endToTargetY) if( error <= stol ): # We found a valid solution.
return 1, error id = id - 1
return 0, error # cannot get to the target in this iteration
if __name__ == "__main__": # Starts a communication thread with the server
clientID = vrep.simxStart('127.0.0.1', 20001, True, True, 5000, 5) # clientID: the client ID, or -1 if the connection to the server was not possible
if clientID != -1: #check if client connection successful
print 'Connected to remote API server'
else: print 'Connection not successful' sys.exit('Could not connect') # Exit from Python
# Retrieves an object handle based on its name.
errorCode,tip_handle = vrep.simxGetObjectHandle(clientID,'tip',vrep.simx_opmode_oneshot_wait) errorCode,target_handle = vrep.simxGetObjectHandle(clientID,'target',vrep.simx_opmode_oneshot_wait) errorCode,consoleHandle = vrep.simxAuxiliaryConsoleOpen(clientID,'info',4,1+4,None,None,None,None,vrep.simx_opmode_oneshot_wait) joint_handle = [-1,-1,-1] # store the joint handles
for i in range(3): errorCode,joint_handle[i] = vrep.simxGetObjectHandle(clientID,'j'+str(i+1),vrep.simx_opmode_oneshot_wait) ilimit = 100 # maximum iteration
stol = 1e-2 # tolerance
q = [0,0,0] # initial joint value
linkNum = 3 # number of links
retcode, target = vrep.simxGetObjectPosition(clientID,target_handle, -1, vrep.simx_opmode_oneshot_wait) retcode, tip = vrep.simxGetObjectPosition(clientID,tip_handle, -1, vrep.simx_opmode_oneshot_wait) count = 0 isOK = 0 while ( not isOK ): isOK,err = CalcIK() vrep.simxAuxiliaryConsolePrint(clientID,consoleHandle,None,vrep.simx_opmode_oneshot_wait) count = count + 1 vrep.simxAuxiliaryConsolePrint(clientID,consoleHandle,str(count)+' iterations err:'+str(err),vrep.simx_opmode_oneshot_wait) if count > ilimit: vrep.simxAuxiliaryConsolePrint(clientID,consoleHandle,"Solution wouldn't converge\r\n",vrep.simx_opmode_oneshot_wait) break
#time.sleep(0.1)
# Ends the communication thread. This should be the very last remote API function called on the client side
vrep.simxFinish(clientID)
點擊仿真按鈕並運行Python控制腳本后，可以看到V-rep中的連桿模型不斷調整其關節角，同時誤差err逐漸減小。當誤差減小到一定程度，就可以停止迭代。下面三幅圖中目標處於不同位置，可以發現目標位置對迭代次數有較大的影響（為什么會這樣？）
參考：
1.Cyclic Coordinate Descent in 2D ：http://www.ryanjuckett.com/programming/cyclic-coordinate-descent-in-2d/
2. 陽小濤 ,楊克儉. CCD 算法及其在逆運動學中的應用與實現[J]. 重 慶 工 學 院 學 報 (自然科學),2008 年 5 月