An SOM-Like Approach to Inverse Kinematics Modeling

Robot kinematics modeling has been one of the main research issues in robotics research. For realtime control of robotic manipulators with high degree of freedom, a computationally efficient solution to the inverse kinematics modeling is required. In this paper, an SOM-Like inverse kinematics modeling methodis proposed. The principal idea behind the proposed modeling method is the use of a first-order Taylor series expansion to build the inverse kinematics model from a set of training data. The work space of a robot arm is discretized into a cubic lattice consisting of Nx×Ny×Nz sampling points. Each sampling point corresponds to a reciprocal zone and is assigned to one neural node, storing four different data items(e.g., coordinates position vector, template position vector,the joint angle vector, and the Jacobian matrix) about the first-order Taylor series expansionof the inverse kinematics function at that sampling point. The proposed inverse kinematics modeling method was tested on a 3-D printed robot arm with 5 degrees of freedom (DOF). The performance of the proposed method was tested on two simulated examples. The average approximation error could be decreased to 0.283 mm in the workspace, 200.0 mm×200.0 mm×72.0 mm and 0.25 mm in the workspace, 200.0 mm×200.0 mm. An SOM-Like Approach to Inverse Kinematics Modeling Publication History: Received: January 14, 2017 Accepted: February 16, 2017 Published: February 18, 2017


Introduction
Robot kinematics modeling has been one of the main research issues in robotics research.It can be divided into forward kinematics and inverse kinematics.Forward kinematics refers to the calculation of the position and orientation of an end effector in terms of the joint angles, where represents the Cartesian position of the end effector and represents the joint angles where we assume there are n joints in the joint configuration.Inverse kinematics refers to find the transformation from the position of the end effector in the external Cartesian position space to the joint angles in the internal joint space.While there is always a straightforward solution to forward kinematics, the solution to inverse kinematics is usually more difficult, complex, and computationally expensive.For real-time control of robotic manipulators with high degree of freedom, a computationally efficient solution to the inverse kinematics modeling is one of the main requirements.
Approaches to the inverse kinematics problem can be roughly categorized into four classes: the analytical approach (e.g., [1]- [5]), the numerical approach (e.g., [6]- [11]), the computational intelligence-based approach (e.g., [12]- [20]), and the lookup tablebased approach (e.g., [21]- [23]).While the analytical approach solves the joint variables analytically according to given configuration data to provide closed form solutions, the numerical method provides a numerical solution (e.g., the use of the Jacobian matrix of the forward kinematics function, to approximate the optimal joint angles [24].Real-time applications usually prefer closed form solutions than numerical solutions because the latter one either requires heavy computations or fails to converge when a singularity exists.The computational intelligence-based approach provides an alternative solution to the inverse kinematics problem [12]- [20].Many co1mputational intelligence-based methods were based on the use of the self-organizing feature map (SOM) [12]- [14], [16]- [20].Recently, the lookup table-based approach has been introduced to solve the inverse kinematics problem due to its simplicity [21]- [23].Basically, the lookup table-based approach consists of two phases: the phase of the off-line construction of the lookup table and the on-line interpolation phase.The lookup table-based approach may encounter the following problems.First of all, the amount of memory required for constructing an effective table may increase as the number of joints and the resolution of the table increase.In addition, a further approximation procedure may be adopted to search for a better solution once an initial table entry has been located.Without any doubt, each one of the aforementioned four approaches has its advantages and limitations.
The goal of this paper is to endow a 3-D printed humanoid robot arm with the ability of positioning its fingertip to a target position in real time.To achieve this goal, the robot system has to seek a high efficiency solution to inverse kinematics modeling.In this paper we propose an SOM-like approach to solving the inverse kinematics problem.The proposed approach integrates the SOMbased approach and the lookup table-based approach.Our approach is the use of a Taylor series expansion to build the transformation from the position of the end effector in the external Cartesian position space to the joint angles in the internal joint space from a set of training data.The principal idea behind the proposed modeling method is to discretize the work space of a robot arm into a cubic lattice consisting of N x ×N y ×N z sampling points.Each sampling point corresponds to a reciprocal zone and is assigned to one neural node.Each neural node storesfour weight vectors or data items: the coordinates position weight vector , the template position weight vector , the joint angle weight vector , and the Jacobian matrix W j J .All these four data terms can be quickly learned by the proposed modeling method from a collected training data set.The training data set can be constructed by either the uniformly discretization scheme or the real-life data generation scheme.The computations of the joint angles corresponding to a target position in the work space involve the following two steps.First of all, we search the reciprocal zone which is

International Journal of Computer & Software Engineering
Mu-Chun Su * and Chung-Cheng Hsueh Department of Computes Science and Information Engineering, National Central University, Taiwan closest to the target position.Secondly, the joint angles are approximated by he first-order Taylor series expansion of the transfor mation via the target position vector target , the joint angle vector , and the Jacobian matrix W j J within the reciprocal zone.
The performance of the proposed SOM-like inverse kinematics modeling method was tested on a 3-D printed robot arm with 5 degrees of freedom (DOF).Two simulated examples were designed to test whether the robot arm could successfully position its fingertip to target positions in the work space.This paper is organized as follows.Following this introduction is a brief review of the Taylor series expansion and the SOM algorithm.Section III explains the detailed descriptions of the proposed SOMlike inverse kinematics modeling method.Simulation results are given in Section IV.The final section contains the discussions and conclusions.

Brief Review of the Taylor Series Expansion and the SOM Algorithm The Taylor Series Expansion
In mathematics, a vector-valued function can be approximatedvia the first-order Taylor expansion as follows: (1) where is a data point, is a template point, is a vector-valued function, and is the Jacobian matrix at the template point The Jacobian matrix is the matrix of the all first order partial derivatives of the vector-valued function as follows: ( An immediate problem needed to be solved is the estimation of the Jacobian matrix at the point, .There are two methods to estimate the Jacobian matrix from the N+1 data pairs.One popular method is the use of the Moore-Penrose generalized Inverse operator.Assume we have N+1 data pairs, . Let us rewrite Eq. ( 1) as follows: ( Since we have N+1 data pairs, , Eq. ( 3) can be expanded to be as follows: The solution of the matrix is computed as follows: where is the Moore-Penrose generalized inverse of the matrix .

The SOM
The training algorithm proposed by Kohonen for forming a selforganizing feature map (SOM) is summarized as follows [25]- [26]: Step 1. Initialization: Consider the network on a rectangular grid with M rows and N columns.Each neuron in the neural network is associated with an n-dimensional weight vector , Randomly choose values for the initial weight vectors .
Step 2. Winner Finding: Present an input pattern to the network and search for the winning neuron.The winning neuron at time k is found by using the minimum-distance Euclidean criterion: (8) where represents the kth input pattern and indicates the Euclidean norm.
Step 3. Weight Updating: Adjust the weights of the winner and its neighbors using the following updating rule: where is a positive constant and is the topological neighborhood function of the winner neuron at time k.
Step 4. Iterating: Go to step 2 until some pre-specified termination criterion is satisfied.

The Proposed SOM-Like Approach to Inverse Kinematics Modeling
The goal of the proposed SOM-like approach to inverse kinematics modeling is to derive an corresponding joint angles, from any fingertip position, .It involves two phases: (1) the off-line training phase and (2) the real-time manipulating phase.While the off-line training phase is to derive the inverse kinematics model for each sampling point over the discretized work space of the robot arm from a collected training data set, the real-time manipulating phase is to compute the corresponding angles for a particular fingertip position based on the trained inverse kinematics model in real-time.

The off-line training phase
The proposed off-line training phase integrates the merits of the SOM-based approach and the lookup table-based approach.It fully utilizes the topology-preserving property of the SOM algorithm to generalize the modeling capability from collected data to unknown data space.In addition, similar to the table-based approach, it is able to calculate the necessary information to derive the inverse kinematics with no need of a complicated learning procedure.The principal idea behind the proposed modeling method is to discretize the workspace of a robot arm into a cubic lattice consisting of N x ×N y ×N z sampling points.Each sampling point corresponds to a non-overlapping reciprocal zone in the workspace.For each sampling point, we will stores four weight vectors or data terms (i.e. ) in order to quickly derive corresponding joint angles .for any fingertip position located inside the corresponding reciprocal zone of the sampling point via the first-order` Taylor expansion.All these four data terms can be quickly learned by the proposed off-line training phase.
The off-linetraining phase is describedas follows: Step 1: Workspace Specification-First of all, we need to specify where the workspace of the robot arm is.Assume that the workspace of the robot arm is located in the region defined by [m where the parameters, m x ,M x , m y ,M y ,m z , and M z are the lower bounds and upper bounds for the workspace with respective to the axes, X, Y, and Z, respectively.
Step 2: Lattice Determination-Discretize the work space into an equidistant cubic lattice consisting of N x ×N y ×N z template points.The more template points the workspace has, the smaller the approximation error the training phase will achieve.Accordingly, we set the SOM network structure to be a 3-dimensional lattice structure with the network size,N x ×N y ×N z .Each neural node isthenrespectively assigned to its correspondingtemplate point.Therefore, the reciprocal zone of each neural nodeis .
Each neural node then needs to store the its corresponding fourweight vectors: the coordinates vector of the sampling point w c , .
, the template position vector the joint angle vector , and the Jacobian matrix W (k,l,m) .These four weight vectors will be computed in the following steps from a set of training data.
Step 3: Collecting training data-Assume the forward kinematics model of the robot arm has been developed via some kind of forward kinematics modeling method.Based on the generated forward kinematics model, we can easily derive the position of the fingertip of the robot arm from a given combination of joint angles.The training data can be constructed by either the uniformly discretization scheme or the real-life data generation scheme.If we have generated the forward kinematics model then we suggest to use the uniformly discretization scheme.Let RA i and ∆θ i represent the physical range limit and the sampling step for the ith joint, respectively.Therefore, we will collect training data, , where N act represent the number of joint angles.The more training data we collect, the higher the performance of our method will achieve.The prices paid for the high performance are: (1) the need of larger memory for storing the corresponding four weight vectors and (2) the more training time.
Step A flag is attached to each neural node to indicate whether the node has already won one competition.If the node has won for at least one competition then the value of its flag will be set to be one; otherwise, zero.After all Ns training data have been presented to the SOM network, the neural nodes with non-zero flag value are regarded as "template nodes".On the contrary, neural nodes with a zero flag value are claimed to be "novice nodes".All novice nodes have to enter the next sub-step to update their weight vectors.One thing should be pointed out is that several neural nodes may have won the competitions for more than one time.If this case happens then the training data with the smallest distance to the coordinates position vector, is adopted to update the particular node's template position vector via (15).

(4.4) Updating the empty nodes' weights:
In this sub-step, we fully utilize the topology-preserving property of the SOM.We assume that neural nodes have similar responses as their neighboring nodes.
Based on this assumption, the weight vectors of a novice node can be computed from its neighboring nodes.For each novice node (k, l, m), we will find how many neighbors of the novice node are already template nodes.We only consider its 3×3×3 neighboring nodes.The novice nodes are sorted in a decreasing order according to the numbers of their neighboring nodes which are template nodes.
According to the sorted order, the joint angle vector .ofa novice node is updated as follows: , , and ( 1) ( 1) where NS (k,l,m) represents the set of template nodes within the 3×3×3 region.After the updating change, a novice node then becomes a template node.This process is repeated until all novice nodes are updated and become template nodes.
Step 5: Computationof Jacobian matrix-According to Eq. ( 1), if the template point is very close to the data point then the joint angles .corresponding to a particular location can be linearly approximated as follows: Where is a template point with the Jacobian matrix J( ).An immediate problem is how to compute the Jacobian matrix for each template point.After Step 4, the neural node located at the sampling point has been assigned a template position weight vector, , and a joint angle weight vector, .Assume each node has N nb neighboring nodes.For example, nodes located at the eight corners have only 3 neighboring node but most of the nodes inside the lattice have 26 neighboring nodes.Then we can construct a set of N nb data pairs for the neural nodelocated at to estimate the corresponding Jacobian matrix via (7).To be concise and clear, we denote the N nb data pairs for h = 1,…, N nb as follows: These N nb data pairs should meet the following conditions: where the Jacobian matrix J is a N act ×3 matrix since there are N act joints and the workspace is a 3-dimensional space.Via (7) the Jacobian matrix can be computed as follows: where

The real-time manipulating phase
After the off-line training procedure, an inverse kinematics model has been approximated via a trained SOM network with N x ×N y ×N z lattice structure.Each neural node stores the necessary information (e.g., the template position weight vector the joint angle Page 4 of 7 weight vector , and the Jacobian matrix weight vector . within the reciprocal zone) for the inference of the first-order Taylor expansion.The trained inverse kinematics model can be used to predict a set of appropriate joint angles given a special targeted position vector .

Results and Discussion
Step 1: Initialization: Set the iteration parameter k=0 and set the initial real position vector to be the targeted position vector, .( = ).
Step 2: Winner finding: Present the present positionx to the network and search for the winning neuron.The winning neuron (k * , l * , m * ) at time k is found by using the minimum-distance Euclidean criterion: We search the sampling point of which reciprocal zone is the closest to the present real position vector .If k> 0, then , which will be calculated at the next step.
Step 3: Calculating the joint angles: The joint angles are approximated by he first-order Taylor series expansion via the joint angle vector , and the Jacobian matrix : Where and Step 4: Actuating the joint angles: Let the robot arm move to the new real position, , according to the joint angles, θ(k+1), computed by the previous step.
Step 5: Termination criteria: If the new real position is close enough to the target position (e.g., the approximation error between the target position and the final real position where ϵ is a pre-specified threshold) then we terminate the procedure; otherwise, we set k = k + 1 and go to step 2 until some pre-specified iteration number is reached.

Simulation Results
To test the performance of the proposed SOM-Like inverse kinematics modeling method, humanoid robots arm with 5 degrees of freedom was our test platform.The design of the robot arm was from the InMoov project [27].The InMoov was the first open source 3D printed life-size robot.Based on the open source and a 3D printer, we implemented a humanoid robot arm with 5 degrees of freedom as shown in Figure 1.The Denavit-Hartenberg (DH) method is the most common method to construct a forward kinematics for a robot platform based on four parameters (e.g., the link length, link twist, link offset or distance, and joint angle) [28].A coordinate frame is then attached to each joint to determine the DH parameters.Figure 2 shows the coordinate frame assignment for the robot arm.
Based on the forward kinematics, 20,800 training data points and 9,072 testing data points were generated, respectively.Figure 3 shows the workspace of these data points.The physical boundaries of these data points and the way for generating the data points are tabulated in Table 1.We then used five different network sizes to discuss whether the network size would influence the approximation performance.
( ) ( , , ) Arg min ( ) * * * ( , , ) Table 2 tabulates the average approximation error, the maximum approximation error, and the computational time.The simulation was done on a PC with 4.0 GB memory, Intel Core i7-2600 CPU @ 3.40 GHz, and the operating system Windows 7. Two observations can be concluded.First of all, the larger the network size is the smaller the average approximation error is reached.Secondly, the larger the network size is the longer the computational time is required.
We then took a tradeoff between the approximation error and the computational efficiency to choose an appropriate network size for the following two simulated examples.
After the inverse kinematics model of the robot arm has been constructed, two simulated examples were designed to further test the performance of the proposed SOM-Like inverse kinematics modeling method.

Example one: Tracking a Circular Helix
The first simulation was designed to track the circular helix defined as: The circular helix was sampled for every ∆θ=1° and there were total 720 sampling points.Figure 4 shows the circular helix tracked in the workspace, 200.0 mm×200.0mm×72.0mm, with an average error 5.73 mm if the maximum iteration number is only 1.To further decrease the approximation error, the termination criterion parameter ϵ was set to be 0.5 mm so that the real-time manipulating phase took about 0.0306 milliseconds to repeat the iterative procedure for 200 iterations.Finally, the average approximation error decreased to be 0.283 mm.

Example two: Writing a letter "B"
The second simulation was designed to write a letter "B" as follows: Line 1: x = -207.7mm,z = 638.8mm,-493.3mm≤ y ≤ -293.3mm Ellipse 1: 90≤θ≤-90     90≤θ≤-90 The ellipses were sampled for every ∆θ=1° and there were total 360 sampling points.The line was sampled for every 1 mm and there were 200 samples.Figure 5 shows the letter "B" tracked in the workspace, 200.0 mm×200.0mm, with an average error 6.58 mm if the maximum iteration number is only 1.To further decrease the approximation error, the termination criterion parameter ϵ was set to be 0.5 mm so that the real-time manipulating phase took about 0.0175milliseconds to repeat the iterative procedure.Finally, the average approximation error decreased to be 0.247 mm.

4 : 4 . 1 )
Training of the SOM network-It involves the following four sub-steps.(Initialization: For each neural node, its corresponding fourweight vectors are initialized as follows: (4.2) Winner Finding:Present an input pattern to the network and search for the winning neuron.The winning neuron(k*, l*, m*) corresponding to the input pattern is found by using the minimumdistance Euclidean criterion: (4.3) Weight Updating:Adjust the weights of the winning neuron using the following updating rule:

Figure 1 :
Figure 1: The humanoid robot arm with 5 degrees of freedom used as the test platform.

Figure 3 :
Figure 3: The workspace of the training data and the testing data.

Figure 2 :
Figure 2: The coordinate frame assignment for the robot arm.

Table 2 :
The physical boundaries of the robot arm.The approximation performance of the proposed SOM-Like inverse kinematics modeling method with five different network sizes.