The present invention proposes an inverse kinematics modeling and solving principle for multi-axis systems based on axis invariant, including: the D-H and D-H parameter determination principle based on fixed axis invarian, “Ju-Gibbs” quaternion and class direction cosine matrix principle, the inverse solution principle of general 6R and 7R robotic arms based on axial invariant. These principles are versatile, convenient, and precise. They can be set up as circuits, code, directly or indirectly, partially or completely within a multi-axis robot system. In addition, the present invention also includes analysis verification system constructed on these principles for designing and verifying multi-axis robot systems.

Various approaches to solve for inverse kinematics may be used for teleoperation of a surgical robotic system. In one approach, an iterative solver solves for the linear component of motion independently from solving for the angular component of motion. One solver may be used to solve for both together. In another approach, all limits (e.g., position, velocity, and acceleration) are handled in one solution. Where a limit is reached, the limit is used as a bound in the intermediate solution, allowing solution even where a bound is reached. In another approach, a ratio of limits of position are used to create a slow-down region near the bounds to more naturally control motion. In yet another approach, the medical-based teleoperation uses a bounded Gauss-Siedel solver, such as with successive-over-relaxation.

In a method of obtaining an angle of each joint of a 6-axis vertical articulated robot when a position and a posture of an end effector attached on a sixth axis are given, a predetermined amount of offset exists between a sixth axis and a fourth axis, and the method includes sequentially determining a point of interest, which is a point on a circumference of a circle having the predetermined amount as a radius, around a first intersection point, on a plane which includes the first intersection point which is an intersection point of the sixth axis and the fifth axis and the plane which is orthogonal to the sixth axis, calculating a second intersection point, which is an intersection point of the fourth axis and the third axis, when it is assumed that the point of interest is an intersection point of the fifth axis and the fourth axis, calculating an inner product value of a first vector directed from the calculated second intersection point to the point of interest and a second vector directed from the point of interest to the first intersection point, and estimating that the point of interest, when an absolute value of the inner product value is less than or equal to a predetermined threshold, is an intersection point of the fifth axis and the fourth axis.

A complete model numerical solver resides on an embedded processor for real time control of a system. The solver eliminates the need for custom embedded code, requiring only model equations, definition of the independent and dependent variables, parameters and input sources information as input to solve the model equations directly. Through elimination of the need for custom code, the solver speeds up the model deployment process and provides the control application sophisticated features such as Automatic Differentiation, sensitivity analysis, sparse linear algebra techniques and adaptive step size in solving the model concurrently.

The invention proposes an axis-invariant based multi-axis system modeling and control principle. Iterative modeling, real-time solution and control of multi-axis system engineering are completely solved from different levels of system topology, forward kinematics, inverse kinematics and dynamics. Parametric modeling and control is completed including topology, coordinate frame, polarity, structural parameters, mass and inertia, etc.. It can be set to circuit, code, directly or indirectly, partially or fully executed inside a multi-axis robot system. In addition, the present invention also includes analytical verification system constructed on these principles for designing and verifying a multi-axis robot system.

The present invention proposes an inverse kinematics modeling and solving principle for multi-axis systems based on axis invariant, including: the D-H and D-H parameter determination principle based on fixed axis invarian, Ju-Gibbs quaternion and class direction cosine matrix principle, the inverse solution principle of general 6R and 7R robotic arms based on axial invariant. These principles are versatile, convenient, and precise. They can be set up as circuits, code, directly or indirectly, partially or completely within a multi-axis robot system. In addition, the present invention also includes analysis verification system constructed on these principles for designing and verifying multi-axis robot systems.

A complete model numerical solver resides on an embedded processor for real time control of a system. The solver eliminates the need for custom embedded code, requiring only model equations, definition of the independent and dependent variables, parameters and input sources information as input to solve the model equations directly. Through elimination of the need for custom code, the solver speeds up the model deployment process and provides the control application sophisticated features such as Automatic Differentiation, sensitivity analysis, sparse linear algebra techniques and adaptive step size in solving the model concurrently.

In a method of obtaining an angle of each joint of a 6-axis vertical articulated robot when a position and a posture of an end effector attached on a sixth axis are given, a predetermined amount of offset exists between a sixth axis and a fourth axis, and the method includes sequentially determining a point of interest, which is a point on a circumference of a circle having the predetermined amount as a radius, around a first intersection point, on a plane which includes the first intersection point which is an intersection point of the sixth axis and the fifth axis and the plane which is orthogonal to the sixth axis, calculating a second intersection point, which is an intersection point of the fourth axis and the third axis, when it is assumed that the point of interest is an intersection point of the fifth axis and the fourth axis, calculating an inner product value of a first vector directed from the calculated second intersection point to the point of interest and a second vector directed from the point of interest to the first intersection point, and estimating that the point of interest, when an absolute value of the inner product value is less than or equal to a predetermined threshold, is an intersection point of the fifth axis and the fourth axis.

Various approaches to solve for inverse kinematics may be used for teleoperation of a surgical robotic system. In one approach, an iterative solver solves for the linear component of motion independently from solving for the angular component of motion. One solver may be used to solve for both together. In another approach, all limits (e.g., position, velocity, and acceleration) are handled in one solution. Where a limit is reached, the limit is used as a bound in the intermediate solution, allowing solution even where a bound is reached. In another approach, a ratio of limits of position are used to create a slow-down region near the bounds to more naturally control motion. In yet another approach, the medical-based teleoperation uses a bounded Gauss-Siedel solver, such as with successive-over-relaxation.

A complete model numerical solver resides on an embedded processor for real time control of a system. The solver eliminates the need for custom embedded code, requiring only model equations, definition of the independent and dependent variables, parameters and input sources information as input to solve the model equations directly. Through elimination of the need for custom code, the solver speeds up the model deployment process and provides the control application sophisticated features such as Automatic Differentiation, sensitivity analysis, sparse linear algebra techniques and adaptive step size in solving the model concurrently.