CN1985773A - Celebral operating robot system based on optical tracking and closed-loop control and its realizing method - Google Patents

Abstract

The cerebral operating robot system based on optical tracking and closed-loop control for receiving medical image information, detecting and determining disease focus position, assisting operation plan and performing operation guidance is one closed-loop pose measuring and real-time feedback controlling robot system comprising one computer, one five-freedom robot, one optical tracking device and one passive marker. The passive marker is mounted in the end of the five-freedom robot, and the five-freedom robot includes one mechanical arm and one mechanical arm controller. The operation plan assisting and operation guiding software includes a digital image inputting and pre-treating module, a disease focus extracting and 3D reconstructing module, an operation planning module, and an operation executing module. The present invention has the advantages of raised precision, low cost, simplified calibration process, etc.

Description

Based on optical tracking closed loop control celebral operating robot system and implementation method

Technical field

The invention belongs to the robotics field, be particularly related to a kind ofly, specifically be meant a kind of high precision machines robot system and its implementation that can assist the doctor to carry out meticulous department of cerebral surgery Minimally Invasive Surgery based on optical tracking closed loop control celebral operating robot system and implementation method.

Background technology

The department of cerebral surgery micro-wound operation robot is a kind of robot system that can assist the doctor to implement frameless stereotactic surgery, generally all includes to assist undergo surgery planning and the computer of guiding and the robot that carries out the operation of stereotaxis assisted surgery.Wherein, auxiliary planning and the computer of guiding of undergoing surgery, supporting assisted surgery planning and guiding software are installed, mainly finish the processing and the three-dimensionalreconstruction of patient's brain medical image information, auxiliary doctor carries out virtual operation planning, and sends movement instruction with the control robot motion to robot controller in operation process.This software generally includes digitized video input and functional modules such as pretreatment, focus extraction and three-dimensionalreconstruction, surgery planning and operation enforcement.Digitized video input is the medical image data that receives patient's brain of outside input with the effect of pretreatment module, and converts the form that computer can show to, on this basis image is carried out pretreatment, comprise image reinforcement, remove dry, Histogram statistics etc.; Focus is extracted and the effect of three-dimensionalreconstruction module is to allow the doctor focus be cut apart on image with alternant way, and extracts the head profile, thereby generates the 3 D medical model of focus and head; In the surgery planning module, the doctor can carry out the sign work of target spot and gauge point, and the path planning that can undergo surgery in reality environment; Implement in the module in operation, can send movement instruction with the control robot motion to robot controller according to the good operation pathway of planning in advance in good time, the virtual reality function can provide real-time monitoring for operation simultaneously.

The robot that system adopted is made up of mechanical arm and controller, generally has 5 joint freedom degrees, and all there is driving mechanism in each joint, is used to drive joint motions, can guarantee operating theater instruments with any attitude arrive in the operative space more arbitrarily.Robot controller can be accepted the movement instruction that computer sends, and each joint motions of real time control machine device people arrive assigned address.

The greatest problem that existing celebral operating robot exists is the high precision requirement that positioning accuracy can not satisfy meticulous cerebral surgery operation.Robotic surgical device not only requires the repeatable accuracy height, the absolute precision of apparatus work is required stricter because the surgery planning that carries out in the virtual 3 D medical model space finally will be realized by the motion of robot.Because the factor of mechanism and departure, there are error in the position command of expectation and robot between the actual position that reaches inevitably.This error may reach several millimeters usually to about one centimetre, hinders operating required precision considerably beyond Wicresoft, must manage to be overcome.In commercial Application, the absolute fix error of robot can overcome by the demarcation of variety of way, but robotic surgical device is as a kind of surgical apparatus, and its applicable cases difference is very big, does not generally have strict fixed installation position.Obviously, to each example operation all requires to carry out strictness, loaded down with trivial details field calibration is unaccommodated, moreover the influence of some non-geometric error factors such as mechanism's elastic deformation, joint space and flexibility etc., also be difficult to rely on and demarcate solution.At present, many work all are the methods of considering to improve precision from machinery, rely on mechanism's processing, assembling and high-precision control and the compensation of high precision to guarantee system accuracy, be difficult to reach ideal effect, and increased the design and the manufacturing cost of robot greatly.Existing celebral operating robot is open loop from the overall situation, promptly provide position command to guided robot motion and location process from computer, lack the feedback and the correction of robot end's pose, various error components can't obtain the closed-loop corrected of the overall situation, and absolute pose accuracy is difficult to guarantee.

In addition, from disclosed report (for example Chinese patent publication number CN1243690A), the so-called mapping localization method of determining the spatial position of the relative robot manipulation of focus is too complicated, need use three-dimensional positioning framework, mapping nail or double template, six joint digitized mechanical arms, power control human-computer interaction technology etc., not only time-consuming but also require great effort, also bring misery and pressure to patient; Also have utilization vision measurement technology to carry out process registration (for example Chinese patent publication number CN1554315A), but precision and environmental suitability also need to improve, difficult practical.

Summary of the invention

The purpose of this invention is to provide a kind ofly, can overcome the shortcoming of prior art based on optical tracking closed loop control celebral operating robot system and implementation method.It is that optictracking device is combined with celebral operating robot, utilize the current location and the attitude of optictracking device real-time tracking robot end apparatus, carry out real-time pose closed loop control in the optical measurement space, make the celebral operating robot end accurately to move according to the desirable operation pathway of planning in advance and to locate, realization can be satisfied the high accuracy celebral operating robot system of meticulous cerebral surgery operation requirement.Simultaneously, by means of the optical tracking technology, simplify the process and the method for the mapping transformation of the mapping 3 D medical model space and robot base coordinate space greatly.Use the present invention can make celebral operating robot reach high accuracy, low cost, and save time, convenient, practical, easy to operate.

What the present invention proposed comprises based on optical tracking closed loop control celebral operating robot system:

Robot with five degrees of freedom, computer (selecting model PC P42.0GHz/256M for use), optictracking device (selecting the model NDI POLARIS of company optictracking device for use), optics registration tool (selecting the passive instrument of the model NDI POLARIS of company standard configuration for use), passive marker (selecting the spherical passive marker of the model NDI POLARIS of company standard configuration for use), (that medically uses a kind ofly makes with the tantalum metal material medical science marker, the opaque pellet shapes index point of correlation electrical height, with the organism compatibility, implant into body for a long time), the hardware components that equipment such as moulding pillow and operation table are formed; Assisted surgery planning and guiding software section promptly receive the position of medical image information, mensuration and definite focus, the auxiliary celebral operating robot that undergos surgery and plan and carry out surgical guide; It is characterized in that:

Said computer, robot with five degrees of freedom, optictracking device and passive marker constitute the robot pose measurement and real-time feedback control system of a closed loop, passive marker is installed in the end of robot with five degrees of freedom, is followed the tracks of by optictracking device all the time; Said robot with five degrees of freedom comprises five degree-of-freedom manipulator and mechanical arm controller; Said assisted surgery planning comprises digitized video input and pretreatment module with guiding software, focus extracts and module is implemented in three-dimensionalreconstruction module, surgery planning module and operation.

Use the undergo surgery concrete operations step in stage of the present invention to be:

One. when before undergoing surgery, preparing: on patient's head, paste four medical science markers, brain is carried out medical image scanning, and will scan gained medical image information input computer, the planning of utilization assisted surgery is determined focus with guiding software, the 3 D medical model of reconstruct focus and head, the sign of target spot and medical science sign image position and planning operation pathway undergo surgery.

Two. registering timing signal: allow patient lie on the operation table, head uses moulding pillow and operation table relative fixed, measure the coordinate of four medical science markers on patient's head on the one hand with the optics registration tool, measured value is provided by optictracking device, and send into computer, by the mapping transformation between the COMPUTER CALCULATION 3 D medical model space and the optical measurement space; With the assignment test method of the present invention's design, cooperate on the other hand, measure the mapping transformation between robot with five degrees of freedom pedestal coordinate space and the optical measurement space automatically by robot with five degrees of freedom, optictracking device, passive marker and computer.

Three. when undergoing surgery enforcement: at first, will transform to the optical measurement space at the operation pathway that the 3 D medical model space is planned in advance, and calculate the desirable pose that need reach at current path point robot with five degrees of freedom end by computer; Then, by the desirable pose of computer according to current path point, carry out by of the coordinate transform of optical measurement space, and, obtain the ideal position in each joint of robot with five degrees of freedom by finding the solution the robot with five degrees of freedom inverse kinematics to robot with five degrees of freedom pedestal coordinate space; Then, import the ideal position in each joint to robot with five degrees of freedom, the motion of control robot with five degrees of freedom by computer; Simultaneously, cooperate by optictracking device and the passive marker that is installed in the robot with five degrees of freedom end, the terminal pose of The real time measure robot with five degrees of freedom is also sent into computer, control method with the present invention's design is controlled in real time to the terminal pose of robot with five degrees of freedom, realizes accurate track following and location; At last, robot with five degrees of freedom locking, doctor's operation that under robot with five degrees of freedom auxiliary, undergos surgery.

Five degree-of-freedom manipulator is made up of arm and wrist, has five joints, takes the PPRRR configuration of two Gliding joints and three cradle heads; Arm segment has three joints, is respectively I, II and III joint, and Gliding joint is adopted in the I joint, and the direction of motion is perpendicular to horizontal plane; Gliding joint is adopted in the II joint, and is "T"-shaped vertical with the I joint; Cradle head is adopted in the III joint, and axis is parallel with I joint motions direction; Wrist partly has two joints, is respectively IV and V joint, and cradle head is adopted in the IV joint, and axis is parallel with the III joints axes; Cradle head is adopted in the V joint, and axis is vertical with the IV joints axes; Terminal apparatus is installed in V joint at mechanical arm, and it is parallel with the IV joints axes that axis is installed.All there is independent driving mechanisms in each joint, is made up of motor, decelerator, and arthrodial driving mechanism also includes ball screw.

The mechanical arm controller is made up of programmable logic controller (PLC) PLC and stepper motor driver, adopt three PLC and five stepper motor driver combinations, five joints of corresponding control five degree-of-freedom manipulator, the mechanical arm controller communicates by RS232C Serial Communication Component and outer computer, can accept the joint position order of outer computer input, the motion of control mechanical arm arrives assigned address.

The POLARIS optictracking device of NDI company is a kind of space measurement location instrument of extensive use in the present medical industry.The POLARIS equipment of passive type comprises the position sensor that can launch and receive infrared illumination, the supporting passive instrument that some passive markers is provided or passive marker is installed; POLARIS equipment just can be determined the position and the direction of instrument in real time by the locus of passive marker on the survey tool, and 3D mean square error scope is usually in the 0.35mm scope; By the RS-232/RS-422 communication, the Data Update frequency of Continuous Tracking reaches 60HZ between POLARIS equipment and the computer.

The method of the robot pose measurement of closed loop and real-time feedback control system is: the terminal pose of being measured robot with five degrees of freedom by optictracking device, calculate controlled quentity controlled variable by computer according to pose that measures and the pose that the expectation robot with five degrees of freedom reaches, the control robot with five degrees of freedom further moves to revise deviation.

The passive marker that optictracking device is installed in the robot with five degrees of freedom end by tracking is measured the terminal pose of robot with five degrees of freedom.

Passive marker is installed in the end of robot with five degrees of freedom, and quantity is at least 3.

Passive marker satisfies at the terminal geometry site of installing of robot with five degrees of freedom: the distance between any two passive markers can not be less than 50mm, minimal spatial separation between any two line segments that are made of passive marker line can not be less than 5mm, any two line segments are not parallel, and the angle between any two line segments can not be lower than 0.5 degree.

Implementation method based on the celebral operating robot system of optical tracking closed loop control, it is characterized in that, carry out pose measurement by means of the optical tracking technology, comprising: the closed loop control method of the terminal pose of the automatic assignment test method of the easy assignment test method of the 3 D medical model space and the conversion of optical measurement spatial mappings, robot with five degrees of freedom pedestal coordinate space and the conversion of optical measurement spatial mappings and robot with five degrees of freedom.

Described three kinds of methods relate to five coordinate systems, as Fig. 2: set up a 3 D medical model coordinate systems { V} in the 3 D medical model space; On real patient's head, set up a patient coordinate system { P}; In the optical measurement space, set up an optical measurement coordinate system { M}; On the pedestal of robot with five degrees of freedom, set up a robot base coordinate sys-tem { R}; Set up an end-of-arm tooling coordinate system { T} at the end of robot with five degrees of freedom; { M} is an absolute reference system to whole system with the optical measurement coordinate system; Wherein, patient coordinate system is based on that the point at four medical science marker places that stick on patient's head describes, and this coordinate system is chosen the some M at any one marker place in four medical science markers ₀As coordinate origin, simultaneously with M ₀Some M with other three marker places ₁, M ₂, M ₃Line as three coordinate axess to.

Provide the detailed step and the mathematical description of described three kinds of methods below.

One. the spatial mapping transformation of the 3 D medical model space and optical measurement

Carry out pose measurement by means of the optical tracking technology, the easy assignment test method of the 3 D medical model space and the conversion of optical measurement spatial mappings is: paste four medical science markers by the doctor in patient head, these four markers are not or not same plane, and any three markers are not on same straight line; Patient head is carried out CT or the scanning of MRI medical image, gained scan-image input computer; Allow patient lie on the operation table, head uses moulding pillow and operation table relative fixed, and four medical science markers using optics registration tool mensuration patient head are at the spatial coordinate of optical measurement, and measured value is imported computer by optictracking device; By the COMPUTER CALCULATION 3 D medical model space and the spatial mapping transformation of optical measurement.

This mapping transformation is made up of two groups of Coordinate Conversion, promptly from the 3 D medical model coordinate systems V} to patient coordinate system the conversion of P} and from patient coordinate system { { conversion of M}, two groups of conversions all are based on four medical science markers pasting on patient's head and determine P} to the optical measurement coordinate system.

Obviously, { definition of P}, four markers are in that { coordinate among the P} is respectively: M according to patient coordinate system ₀(0,0,0), M ₁(1,0,0), M ₂(0,1,0), M ₃(0,0,1).

In addition, because four medical science markers can be discerned in the medical scanning image, { coordinate among the V} also can obtain, and supposes to be designated as: M ' in the 3 D medical model coordinate systems for they ₀(x _V0, y _V0, z _V0), M ' ₁(x _V1, y _V1, z _V1), M ' ₂(x _V2, y _V2, z _V2), M ' ₃(x _V3, y _V3, z _V3).

Because the 3 D medical model of patient's brain is by patient's brain scans data reconstruction, therefore can think that { { mapping of P} is rigid transformation (comprising translation, rotation and stretching) to the 3 D medical model coordinate systems, can be with a homogeneous transformation matrix for V} and patient coordinate system ^VT _PFinish two position mappings in the coordinate system.By P} to the homogeneous transformation matrix of V} is:

${}^{V}T_P=\left[\begin{array}{cccc}{x}_{v1}-x_{v0}& x_{v2}-x_{v0}& x_{v3}-z_{v0}& x_{v0}\\ y_{v1}-x_{v0}& y_{v2}-y_{v0}& y_{v3}-z_{v0}& y_{v0}\\ z_{v1}-x_{v0}& z_{v2}-y_{v0}& z_{v3}-z_{v0}& z_{v0}\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$

In like manner, { P} is to the optical measurement coordinate system { transition matrix of M} can to determine patient coordinate system ^MT _P{ position among the M} can be made as: M by o'clock obtaining to four medical science markers respectively with the optics registration tool in the optical measurement coordinate system for four medical science markers ₀(x _M0, y _M0, z _M0), M ₁(x _M1, y _M1, z _M1), M ₂(x _M2, y _M2, z _M2), M ₃(x _M3, y _M3, z _M3).Then by P} to the homogeneous transformation matrix of M} is:

${}^{M}T_P=\left[\begin{array}{cccc}{x}_{m1}-x_{m0}& x_{m2}-x_{m0}& x_{m3}-x_{m0}& x_{m0}\\ y_{m1}-y_{m0}& y_{m2}-y_{m0}& {\text{y}}_{m3}-y_{m0}& y_{m0}\\ z_{m1}-z_{m0}& z_{m2}-z_{m0}& z_{m3}-z_{m0}& z_{m0}\\ 0& 0& 0& 1\end{array}\right]---\left(2\right)$

At last, { { homogeneous transformation of M} is V} to the optical measurement coordinate system by the three dimensional virtual models coordinate system ^MT _VBe:

^MT _V＝ ^MT _P ^PT _V＝ ^MT _P( ^VT _P) ^-1 (3)

Two. the spatial mapping transformation of robot with five degrees of freedom pedestal coordinate space and optical measurement

Carry out pose measurement by means of the optical tracking technology, the automatic assignment test method of robot with five degrees of freedom pedestal coordinate space and the conversion of optical measurement spatial mappings is: at first, four points in the selected robot with five degrees of freedom work space, these four points should be in the measuring range of optictracking device simultaneously, and four points are coplane not, and any three points are conllinear not; By computer according to default program, send the joint position order that arrives above-mentioned four points to robot with five degrees of freedom, control robot with five degrees of freedom end moves to above-mentioned four points, the coordinate of above-mentioned four points in robot with five degrees of freedom pedestal coordinate space under the computer recording successively; Simultaneously, cooperate by optictracking device and the passive marker that is installed in the robot with five degrees of freedom end, the coordinate of its end in the optical measurement space when sequentially determining robot with five degrees of freedom moves to above-mentioned four points, and send into computer; At last, by computer according to above-mentioned four points at robot with five degrees of freedom pedestal coordinate space and the spatial coordinate figure of optical measurement, calculate the spatial mapping transformation of robot with five degrees of freedom pedestal coordinate space and optical measurement, thereby finish automatic mapping.

Suppose that { coordinate among the R} is described four points in robot base coordinate sys-tem ^RP _i(r _i, s _i, t _i), (i=1,2,3,4), simultaneously, the coordinate of described four points in the optical measurement coordinate system is ^MP _i(u _i, v _i, w _i), (i=1,2,3,4), then by robot base coordinate sys-tem R} to the optical measurement coordinate system homogeneous transformation matrix of M} is:

${}^{M}T_R=\left[\begin{array}{cccc}{u}_1& u_2& u_3& u_4\\ v_1& v_2& v_3& v_4\\ w_1& w_2& w_3& w_4\\ 1& 1& 1& 1\end{array}\right]{\left[\begin{array}{cccc}{r}_1& r_2& r_3& r_4\\ s_1& s_2& s_3& s_4\\ t_1& t_2& t_3& t_4\\ 1& 1& 1& 1\end{array}\right]}^{-1}---\left(4\right)$

Three. the closed loop control method of the terminal pose of robot with five degrees of freedom

Carry out pose measurement by means of the optical tracking technology, the closed loop control method of the terminal pose of robot with five degrees of freedom is: with the optical measurement space is the reference space, at first, to transform to the optical measurement space at the operation pathway that the 3 D medical model space is planned in advance by computer, and calculate the desirable pose that need reach at current path point robot with five degrees of freedom end; Then, by the desirable pose of computer according to current path point, carry out by of the coordinate transform of optical measurement space, and, obtain the ideal position in each joint of robot with five degrees of freedom by finding the solution the robot with five degrees of freedom inverse kinematics to robot with five degrees of freedom pedestal coordinate space; Then, import the ideal position in each joint to robot with five degrees of freedom, the motion of control robot with five degrees of freedom by computer; Simultaneously, cooperate by optictracking device and the passive marker that is installed in the robot with five degrees of freedom end, the pose of The real time measure robot with five degrees of freedom end, and send into computer; Then, the desirable pose of the posture information of The real time measure with the current path point of planning in advance compared, obtain both pose deviations by computer; At last, by computer according to the pose deviation according to predetermined control law calculation correction controlled quentity controlled variable, revise the desirable pose of current path point, and begin the motor control of a new round, thereby realize accurate track following and location.

The control law that is adopted during according to pose deviation calculation correction controlled quentity controlled variable by computer is: Position Control adopts proportional-integral-differential to regulate; Attitude control employing ratio is regulated.

The terminal pose of robot with five degrees of freedom is to carry out closed loop control in the optical measurement space.

As Fig. 5, { T} is at optical measurement coordinate system { the pose X=[P among the M} to establish the end-of-arm tooling coordinate system ^Tφ ^T] ^TExpression.P=[xyz wherein] ^TExpression { the position of T}; φ=[α β γ] ^TExpression the attitude of T} is made up of one group of Z-Y-X Eulerian angles,

Note X _d=[P _d ^Tφ _d ^T] ^TRepresent desirable pose, P _d=[x _dy _dz _d] ^T, φ _d=[α _dβ _dγ _d] ^TX _r=[P _r ^Tφ _r ^T] ^TThe pose that the expression actual measurement obtains, P _r=[x _ry _rz _r] ^T, φ _r=[α _rβ _rγ _r] ^TΔ X=[Δ P ^TΔ φ ^T] ^TExpression X _dAnd X _rDeviation, Δ P=[Δ x Δ y Δ z] ^T, Δ φ=[Δ α Δ β Δ γ] ^T,

Position deviation can directly be calculated, that is:

ΔP＝P _d-P _r (5)

The calculating of attitude misalignment relates to rotation transformation, uses R _d{ T} is with respect to { the desirable rotation transformation matrix of M} in expression; R _rThe real transform matrix that expression obtains according to measured value; Δ R represents by R _rTo R _dConversion, then:

$R_d=R_Z\left({\mathrm{\α}}_d\right)R_Y\left({\mathrm{\β}}_d\right)R_X\left({\mathrm{\γ}}_d\right)$

$=\left[\begin{array}{ccc}{\cos \mathrm{\α}}_d{\cos \mathrm{\β}}_d& {\cos \mathrm{\α}}_d{\sin \mathrm{\β}}_d{\sin \mathrm{\γ}}_d-{\sin \mathrm{\α}}_d{\cos \mathrm{\γ}}_d& {\cos \mathrm{\α}}_d{\sin \mathrm{\β}}_d{\cos \mathrm{\γ}}_d+{\sin \mathrm{\α}}_d{\sin \mathrm{\γ}}_d\\ {\sin \mathrm{\α}}_d{\cos \mathrm{\β}}_d& {\sin \mathrm{\α}}_d{\sin \mathrm{\β}}_d{\sin \mathrm{\γ}}_d+{\cos \mathrm{\α}}_d{\cos \mathrm{\γ}}_d& {\sin \mathrm{\α}}_d{\sin \mathrm{\β}}_d{\cos \mathrm{\γ}}_d-{\cos \mathrm{\α}}_d{\sin \mathrm{\γ}}_d\\ -{\sin \mathrm{\β}}_d& {\cos \mathrm{\β}}_d{\sin \mathrm{\γ}}_d& {\cos \mathrm{\β}}_d{\cos \mathrm{\γ}}_d\end{array}\right]---\left(6\right)$

$R_r=R_Z\left({\mathrm{\α}}_r\right)R_Y\left({\mathrm{\β}}_r\right)R_X\left({\mathrm{\γ}}_r\right)$

$=\left[\begin{array}{ccc}{\cos \mathrm{\α}}_r{\cos \mathrm{\β}}_r& {\cos \mathrm{\α}}_r{\sin \mathrm{\β}}_r{\sin \mathrm{\γ}}_r-{\sin \mathrm{\α}}_r{\cos \mathrm{\γ}}_r& {\cos \mathrm{\α}}_r{\sin \mathrm{\β}}_r{\cos \mathrm{\γ}}_r+{\sin \mathrm{\α}}_r{\sin \mathrm{\γ}}_r\\ {\sin \mathrm{\α}}_r{\cos \mathrm{\β}}_r& {\sin \mathrm{\α}}_r{\sin \mathrm{\β}}_r{\sin \mathrm{\γ}}_r+{\cos \mathrm{\α}}_r{\cos \mathrm{\γ}}_r& {\sin \mathrm{\α}}_r{\sin \mathrm{\β}}_r{\cos \mathrm{\γ}}_r-{\cos \mathrm{\α}}_r{\sin \mathrm{\γ}}_r\\ {-\sin \mathrm{\β}}_r& {\cos \mathrm{\β}}_r{\sin \mathrm{\γ}}_r& {\cos \mathrm{\β}}_r{\cos \mathrm{\γ}}_r\end{array}\right]---\left(7\right)$

Note: $\mathrm{\ΔR}=R_d{R}_r^{-1}=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]---\left(8\right)$

Have again:

$\mathrm{\ΔR}=R_Z\left(\mathrm{\Δ\α}\right)R_Y\left(\mathrm{\Δ\β}\right)R_X\left(\mathrm{\Δ\γ}\right)$

$=\left[\begin{array}{ccc}\cos \mathrm{\Δ\α}\cos \mathrm{\Δ\β}& \cos \mathrm{\Δ\α}\sin \mathrm{\Δ\β}\sin \mathrm{\Δ\γ}-\sin \mathrm{\Δ\α}\cos \mathrm{\Δ\γ}& \cos \mathrm{\Δ\α}\sin \mathrm{\Δ\β}\cos \mathrm{\Δ\γ}+\sin \mathrm{\Δ\α}\sin \mathrm{\Δ\γ}\\ \sin \mathrm{\Δ\α}\cos \mathrm{\Δ\β}& \sin \mathrm{\Δ\α}\sin \mathrm{\Δ\β}\sin \mathrm{\Δ\γ}+\cos \mathrm{\Δ\α}\cos \mathrm{\Δ\γ}& \sin \mathrm{\Δ\α}\sin \mathrm{\Δ\β}\cos \mathrm{\Δ\γ}-\cos \mathrm{\Δ\α}\sin \mathrm{\Δ\γ}\\ -\sin \mathrm{\Δ\β}& \cos \mathrm{\Δ\β}\sin \mathrm{\Δ\γ}& \cos \mathrm{\Δ\β}\cos \mathrm{\Δ\γ}\end{array}\right]$

(9)

Can solve attitude misalignment Δ φ=[Δ α Δ β Δ γ] by (6), (7), (8), the associating of (9) formula ^T, wherein:

A. when sin Δ β ≠ 0,

$\mathrm{\Δ\β}=A\tan 2(\sqrt{r_{31}^2+r_{32}^2},r_{33}),$

Δα＝Atan2(r ₂₃/sinΔβ，r ₁₃/sinΔβ)，

Δγ＝Atan2(r ₃₂/sinΔβ，-r ₃₁/sinΔβ) (10)

B. when sin Δ β=0, if Δ β=0.0, then:

Δα＝0.0，

Δγ＝Atan2(-r ₁₂，r ₁₁) (11)

If Δ β=180.0 °, then:

Δα＝0.0，

Δγ＝Atan2(r ₁₂，-r ₁₁) (12)

Position correction and attitude updating problem are discussed respectively below, are adopted the discrete form row to write formula, the subscript k in the bracket represents to control the circulation k step.

Proportional-integral-differential (PID) control is adopted in position correction, proofreaies and correct controlled quentity controlled variable and is designated as U _P=[u _xu _yu _z] ^T, then:

$U_P\left(k\right)=K_{\mathrm{PP}}\mathrm{\ΔP}\left(k\right)+K_{\mathrm{PI}}T\underset{j=0}{\overset{k}{\mathrm{\Σ}}}\mathrm{\ΔP}\left(j\right)+K_{\mathrm{PD}}\frac{\mathrm{\ΔP}\left(k\right)-\mathrm{\ΔP}(k-1)}{T}---\left(13\right)$

Wherein T is the controlling of sampling cycle of system, and such as pressing the highest measurement data updating rate 60HZ that POLARIS sets, T can be taken as 16.7ms; K _PPThe diagonal matrix of forming by three proportional control factors; K _PIThe diagonal matrix of forming by three integral control coefficients; K _PDThe diagonal matrix of forming by three derivative control coefficients.Following formula is rewritten into incremental form:

$U_P\left(k\right)=U_P(k-1)+(K_{\mathrm{PP}}+T{K}_{\mathrm{PI}}+\frac{K_{\mathrm{PD}}}{T})\mathrm{\ΔP}\left(k\right)$

$-(K_{\mathrm{PP}}+\frac{{2K}_{\mathrm{PD}}}{T})\mathrm{\ΔP}(k-1)+\frac{K_{\mathrm{PD}}}{T}\mathrm{\ΔP}(k-2)---\left(14\right)$

Note $A=K_{\mathrm{PP}}+{\mathrm{TK}}_{\mathrm{PI}}+\frac{K_{\mathrm{PD}}}{T},$ $B=K_{\mathrm{PP}}+\frac{{2K}_{\mathrm{PD}}}{T},$ $C=\frac{K_{\mathrm{PD}}}{T},$ Then further algorithm is write as computer implemented form:

$\left\{\begin{array}{c}{U}_P\left(k\right)=\mathrm{A\ΔP}\left(k\right)+F(k-1)\\ F\left(k\right)=U_P\left(k\right)-\mathrm{B\ΔP}\left(k\right)+\mathrm{C\ΔP}(k-1)\end{array}---\left(15\right)\right.$

The desirable F of initial value (k-1)=0, Δ P (k-1)=0, each step of algorithm all will be calculated Δ P (k), U _P(k), F (k), wherein F (k) is used for next step and calculates U _P(k).

Ratio (P) control is adopted in attitude updating, proofreaies and correct controlled quentity controlled variable and is designated as U _φ=[U _αu _βu _γ] ^T, corresponding spin matrix is Then:

U _φ(k)＝K _φPΔφ(k) (16)

K wherein _{φ P}The diagonal matrix of forming by three proportional control factors,

$\mathrm{\Δ}\hat{R}=R_Z\left(u_{\mathrm{\α}}\right)R_Y\left(u_{\mathrm{\β}}\right)R_X\left(u_{\mathrm{\γ}}\right)---\left(17\right)$

Remember that revised pose instruction is ${\hat{X}}_d=[{\hat{P}}_d^T{\widehat{\mathrm{\φ}}}_d^T{]}^T,$ ${\hat{P}}_d=[{\hat{x}}_d{\hat{y}}_d{\hat{z}}_d{]}^T,$ ${\widehat{\mathrm{\φ}}}_d=[{\widehat{\mathrm{\α}}}_d{\widehat{\mathrm{\β}}}_d{\widehat{\mathrm{\γ}}}_d{]}^T,$ Corresponding spin matrix is

Then:

${\hat{P}}_d\left(k\right)=P_d\left(k\right)+U_p\left(k\right)---\left(18\right)$

${\hat{R}}_d\left(k\right)=R_z\left({\widehat{\mathrm{\α}}}_d\left(k\right)\right)R_Y\left({\widehat{\mathrm{\β}}}_d\left(k\right)\right)R_X\left({\widehat{\mathrm{\γ}}}_d\left(k\right)\right)=R_d\left(k\right)\mathrm{\Δ}\hat{R}\left(k\right)---\left(18\right)$

Unite by (6), (17), (19) formula and to find the solution ${\widehat{\mathrm{\φ}}}_d\left(k\right)=[{\widehat{\mathrm{\α}}}_d\left(k\right){\widehat{\mathrm{\β}}}_d\left(k\right){\widehat{\mathrm{\γ}}}_d\left(k\right){]}^T,$ Its process can be found the solution Δ φ=[Δ α Δ β Δ γ] with reference to above-mentioned the associating by (6), (7), (8), (9) formula ^T, repeat no more.

At last,

Replace X _dInstruction is sent as pose, is used for the guided robot motion.

The advantage that the present invention has is: by robot end's pose is carried out real-time closed loop control, making influences robot motion and localized main error component, mechanism and departure as robot body, and the factors such as various measurements and calculations errors in the operation process can both obtain proofreading and correct, thereby really guaranteed the motion and the positioning accuracy of the surgical robot system overall situation, the track following and the positioning accuracy that have solved existing surgical operation robot existence can not satisfy the problem that clinical delicate procedure utilization requires with imitating.On the other hand, adopted after the overall Pose Control, can relax requirement of mechanism design, manufacturing and the control accuracy of robot body etc., thereby reduce the design and the manufacturing cost of robot body, and the operation motility and the operability of having ready conditions and paying close attention to robot more, design the man-machine operating robot train of mechanism of coordinating more.The method that the present invention proposes is suitable for some industrial robot application strict to absolute precision equally.

The present invention can also demarcate robot base coordinate space and the spatial mapping transformation of optical measurement automatically, removed the artificial calibration process of loaded down with trivial details robot base coordinate sys-tem from, and, just can obtain very quickly and easily by the 3 D medical model space and the spatial mapping transformation of optical measurement by pasting the medical science marker in patient head, adopting the optics registration tool to measure the coordinate of medical science marker.

In a word, one aspect of the present invention can obviously improve the track following and the positioning accuracy of robot system, not only solve the critical absolute precision problem that the restriction surgical operation robot is promoted the use of, and helped reducing the design and the manufacturing cost of robot body; Simplify the registration calibration process on the other hand greatly, not only alleviated doctor's workload, psychological burden and the probability that goes wrong, and alleviated patient's misery and pressure.

Description of drawings

Fig. 1 is a structural representation of the present invention.

Fig. 2 is the sketch map of operation principle block diagram of the present invention and the definition of related coordinate system.

Fig. 3 is the FB(flow block) of the assignment test method of the 3 D medical model space of the present invention and the conversion of optical measurement spatial mappings.

Fig. 4 is the FB(flow block) of the automatic assignment test method of robot with five degrees of freedom pedestal coordinate space of the present invention and the conversion of optical measurement spatial mappings.

Fig. 5 is the flow process and the theory diagram of the closed loop control method of the terminal pose of robot with five degrees of freedom of the present invention.

Fig. 6 is a robot with five degrees of freedom mechanical arm structural representation of the present invention.

The specific embodiment

With reference to accompanying drawing the present invention is elaborated:

As shown in the figure, the implication of a label is among equipment composition and the figure, it is by computer 1, robot with five degrees of freedom 2, optictracking device 3, optics registration tool 4, passive marker 5, medical science marker 6, moulding pillow 7, operation table 8 hardware components, form with assisted surgery planning and guiding software 9 parts, and five degree-of-freedom manipulator 11 and mechanical arm controller 10.

Constitute the robot pose measurement and real-time feedback control system of a closed loop by said computer 1, robot with five degrees of freedom 2, optictracking device 3 and passive marker 5, passive marker 5 is installed in the end of robot with five degrees of freedom 2, is followed the tracks of by optictracking device 3 all the time; Said robot with five degrees of freedom 2 comprises five degree-of-freedom manipulator 11 and mechanical arm controller 10; Said assisted surgery planning comprises digitized video input and pretreatment module with guiding software 9, focus extracts and module is implemented in three-dimensionalreconstruction module, surgery planning module and operation.The quantity of passive marker 5 is at least 3.

The concrete operations step is:

One. when before undergoing surgery, preparing: on patient's head, paste four medical science markers 6, brain is carried out medical image scanning, and will scan gained medical image information input computer 1, the planning of utilization assisted surgery is determined focus with guiding software 9, the 3 D medical model of reconstruct focus and head, the sign of target spot and medical science sign image position and planning operation pathway undergo surgery;

Two. registering timing signal: allow patient lie on the operation table 8, head uses moulding pillow 7 and operation table 8 relative fixed, measure the coordinate of four medical science markers 6 on patient's head on the one hand with optics registration tool 4, measured value is provided by optictracking device 3, and send into computer 1, by the mapping transformation between the computer 1 calculating 3 D medical model space and the optical measurement space; With the assignment test method of the present invention's design, cooperate on the other hand, measure the mapping transformation between robot with five degrees of freedom pedestal coordinate space and the optical measurement space automatically by robot with five degrees of freedom 2, optictracking device 3 and computer 1;

Three. when undergoing surgery enforcement: at first, will transform to the optical measurement space at the operation pathway that the 3 D medical model space is planned in advance, and calculate the desirable pose that need reach at current path point robot with five degrees of freedom end by computer 1; Then, by the desirable pose of computer 1 according to current path point, carry out by of the coordinate transform of optical measurement space, and, obtain the ideal position in robot with five degrees of freedom 2 each joints by finding the solution the robot with five degrees of freedom inverse kinematics to robot with five degrees of freedom pedestal coordinate space; Then, by the ideal position of computer 1 to robot with five degrees of freedom 2 each joints of input, 2 motions of control robot with five degrees of freedom; Simultaneously, cooperate by optictracking device 3 and the passive marker 5 that is installed in the robot with five degrees of freedom end, the terminal pose of The real time measure robot with five degrees of freedom is also sent into computer 1, control method with the present invention's design is controlled in real time to the terminal pose of robot with five degrees of freedom, realizes accurate track following and location; At last, robot with five degrees of freedom 2 locking, doctor's operation that under robot with five degrees of freedom 2 auxiliary, undergos surgery.

Five degree-of-freedom manipulator is made up of arm and wrist, has five joints, takes the PPRRR configuration of two Gliding joints and three cradle heads; Arm segment has three joints, is respectively I, II and III joint, and I joint 11-1 adopts Gliding joint, and the direction of motion is perpendicular to horizontal plane; II joint 11-2 adopts Gliding joint, and is "T"-shaped vertical with the I joint; III joint 11-3 adopts cradle head, and axis is parallel with I joint motions direction; Wrist partly has two joints, is respectively IV and V joint, and IV joint 11-4 adopts cradle head, and axis is parallel with the III joints axes; V joint 11-5 adopts cradle head, and axis is vertical with the IV joints axes; Terminal apparatus is installed in V joint at mechanical arm, and it is parallel with the IV joints axes that axis is installed.All there is independent driving mechanisms in each joint, is made up of motor, decelerator, and arthrodial driving mechanism also includes ball screw.

Mechanical arm controller 10 comprises programmed logic controller PLC and stepper motor driver, adopt three PLC and five stepper motor driver combinations, five joints of corresponding control five degree-of-freedom manipulator 11, mechanical arm controller 10 communicates by RS232C Serial Communication Component and computer 1, accept the joint position order of computer 1 input, 11 motions of control five degree-of-freedom manipulator arrive assigned address.

The robot pose measurement of closed loop with the real-time method of feedback control system is: the passive marker 5 that is installed in the robot with five degrees of freedom end by tracking by optictracking device 3 is measured the terminal pose of robot with five degrees of freedom 2, calculate controlled quentity controlled variable by computer 1 according to pose that measures and the pose that expectation robot with five degrees of freedom 2 reaches, control robot with five degrees of freedom 2 further moves to revise deviation; Communicate by the RS232/RS422 serial ports between optictracking device 3 and the computer 1; Communicate by the RS232C serial ports between robot with five degrees of freedom 2 and the computer 1.

Described celebral operating robot system, passive marker 5 satisfies at the robot with five degrees of freedom 2 terminal geometry sites of installing: the distance between any two passive markers 5 can not be less than 50mm, minimal spatial separation between any two line segments that are made of passive marker 5 lines can not be less than 5mm, any two line segments are not parallel, and the angle between any two line segments can not be lower than 0.5 degree.

Implementation method based on the celebral operating robot system of optical tracking closed loop control, carry out pose measurement by means of the optical tracking technology, comprising: the closed loop control method of the automatic assignment test method of the easy assignment test method of the 3 D medical model space and the conversion of optical measurement spatial mappings, robot with five degrees of freedom pedestal coordinate space and the conversion of optical measurement spatial mappings, the terminal pose of robot with five degrees of freedom.

The easy assignment test method of the 3 D medical model space and the conversion of optical measurement spatial mappings is:

A. paste four medical science markers 6 by the doctor in patient head, require these four markers not on same plane, and any three markers are not on same straight line;

B. patient head is carried out CT or the scanning of MRI medical image, gained scan-image input computer 1;

That c. determines four medical science markers is scanned into the coordinate of picture point in the 3 D medical model space;

D. allow patient lie on the operation table 8, head uses moulding pillow 7 and operation table 8 relative fixed, and four medical science markers 6 using optics registration tool 4 mensuration patient head are at the spatial coordinate of optical measurement, and measured value is by optictracking device 3 input computers 1;

E. calculate the 3 D medical model space and the spatial mapping transformation of optical measurement by computer 1.

The automatic assignment test method of robot with five degrees of freedom pedestal coordinate space and the conversion of optical measurement spatial mappings is:

A. four points in chosen in advance robot with five degrees of freedom 2 work spaces require these four points should be in the measuring range of optictracking device 3, and four somes coplane not, and any three points are conllinear not;

B. send the joint position order that arrives above-mentioned four points by computer 1 to robot with five degrees of freedom 2, control robot with five degrees of freedom 2 moves to above-mentioned four points successively;

C. note the coordinate of above-mentioned four points in robot with five degrees of freedom pedestal coordinate space by computer 1; Simultaneously, cooperate by optictracking device 3 and the passive marker 5 that is installed in the robot with five degrees of freedom end, the coordinate of its end in optical measurement space when sequentially determining robot with five degrees of freedom 2 moves to above-mentioned four points, and send into computer 1;

D. by computer 1 according to above-mentioned four points at robot with five degrees of freedom pedestal coordinate space and the spatial coordinate figure of optical measurement, calculate robot with five degrees of freedom 2 pedestal coordinate spaces and the spatial mapping transformation of optical measurement, thereby finish automatic mapping.

The closed loop control method of the terminal pose of robot with five degrees of freedom is:

A. will transform to the optical measurement space at the operation pathway that the 3 D medical model space is planned in advance by computer 1, and calculate 1 and go out the desirable pose that need reach at current path point robot with five degrees of freedom end;

B. by the desirable pose of computer 1 according to current path point, carry out by of the coordinate transform of optical measurement space to robot with five degrees of freedom pedestal coordinate space, and, obtain the ideal position in each joint of robot with five degrees of freedom by finding the solution the robot with five degrees of freedom inverse kinematics;

C. by the ideal position of computer 1 to robot with five degrees of freedom 2 each joints of input, control robot with five degrees of freedom 2 moves;

D. cooperate by optictracking device 3 and the passive marker 5 that is installed in the robot with five degrees of freedom end, the pose of The real time measure robot with five degrees of freedom end, and send into computer 1;

E. by computer 1 the desirable pose of the posture information of The real time measure with the current path point of planning in advance compared, obtain both pose deviations;

F. by computer 1 according to the pose deviation according to predetermined control law, Position Control adopts proportional-integral-differential to regulate; Attitude control employing ratio is regulated, and the calculation correction controlled quentity controlled variable is revised the desirable pose of current path point, and begins the motor control of a new round, thereby realizes accurate track following and location.

Claims (10)

1, a kind of based on optical tracking closed loop control celebral operating robot system, comprise the hardware components that computer (1), robot with five degrees of freedom (2), optictracking device (3), optics registration tool (4), passive marker (5), medical science marker (6), moulding pillow (7) and operation table equipment such as (8) are formed, with assisted surgery planning and guiding software (9) part, be the position that receives medical image information, mensuration and definite focus, the auxiliary celebral operating robot that undergos surgery and plan and carry out surgical guide; It is characterized in that, constitute the robot pose measurement and real-time feedback control system of a closed loop by said computer (1), robot with five degrees of freedom (2), optictracking device (3) and passive marker (5), passive marker (5) is installed in the end of robot with five degrees of freedom (2), is followed the tracks of by optictracking device (3) all the time; Said robot with five degrees of freedom (2) comprises five degree-of-freedom manipulator (11) and mechanical arm controller (10); Said assisted surgery planning comprises digitized video input and pretreatment module with guiding software (9), focus extracts and module is implemented in three-dimensionalreconstruction module, surgery planning module and operation;

Use the undergo surgery concrete operations step in stage of the present invention to be:

One. when before undergoing surgery, preparing: on patient's head, paste four medical science markers (6), brain is carried out medical image scanning, and will scan gained medical image information input computer (1), the planning of utilization assisted surgery is determined focus with guiding software (9), the 3 D medical model of reconstruct focus and head, the sign of target spot and medical science sign image position and planning operation pathway undergo surgery;

Two. registering timing signal: allow patient lie on the operation table (8), head uses moulding pillow (7) and operation table (8) relative fixed, measure the coordinate of four the medical science markers (6) on patient's head on the one hand with optics registration tool (4), measured value is provided by optictracking device (3), and send into computer (1), by the mapping transformation between computer (1) the calculating 3 D medical model space and the optical measurement space; With the assignment test method of the present invention's design, cooperate on the other hand, measure the mapping transformation between robot with five degrees of freedom pedestal coordinate space and the optical measurement space automatically by robot with five degrees of freedom (2), optictracking device (3) and computer (1);

Three. when undergoing surgery enforcement: at first, will transform to the optical measurement space at the operation pathway that the 3 D medical model space is planned in advance, and calculate the desirable pose that need reach at current path point robot with five degrees of freedom end by computer (1); Then, by the desirable pose of computer (1) according to current path point, carry out by of the coordinate transform of optical measurement space, and, obtain the ideal position in each joint of robot with five degrees of freedom (2) by finding the solution the robot with five degrees of freedom inverse kinematics to robot with five degrees of freedom pedestal coordinate space; Then, import the ideal position in each joint to robot with five degrees of freedom (2), control robot with five degrees of freedom (2) motion by computer (1); Simultaneously, cooperate by optictracking device (3) and the passive marker (5) that is installed in the robot with five degrees of freedom end, the terminal pose of The real time measure robot with five degrees of freedom is also sent into computer (1), control method with the present invention's design is controlled in real time to the terminal pose of robot with five degrees of freedom, realizes accurate track following and location; At last, robot with five degrees of freedom (2) locking, doctor's operation that under robot with five degrees of freedom (2) auxiliary, undergos surgery.

2. celebral operating robot system according to claim 1 is characterized in that, five degree-of-freedom manipulator (11) is made up of arm and wrist, has five joints, takes the PPRRR configuration of two Gliding joints and three cradle heads; Arm segment has three joints, is respectively I, II and III joint, and I joint 11-1 adopts Gliding joint, and the direction of motion is perpendicular to horizontal plane; II joint 11-2 adopts Gliding joint, and is "T"-shaped vertical with the I joint; III joint 11-3 adopts cradle head, and axis is parallel with I joint motions direction; Wrist partly has two joints, is respectively IV and V joint, and IV joint 11-4 adopts cradle head, and axis is parallel with the III joints axes; V joint 11-5 adopts cradle head, and axis is vertical with the IV joints axes; Terminal apparatus is installed in V joint at mechanical arm, and it is parallel with the IV joints axes that axis is installed; All there is independent driving mechanisms in each joint, is made up of motor, decelerator, and arthrodial driving mechanism also includes ball screw.

3. celebral operating robot system according to claim 1, it is characterized in that, mechanical arm controller (10) comprises programmed logic controller (PLC) and stepper motor driver, adopt three PLC and five stepper motor driver combinations, five joints of corresponding control five degree-of-freedom manipulator (11), mechanical arm controller (10) communicates by RS232C Serial Communication Component and computer (1), accept the joint position order of computer (1) input, control five degree-of-freedom manipulator (11) motion arrives assigned address.

4. celebral operating robot system according to claim 1, it is characterized in that, the robot pose measurement of closed loop with the method for real-time feedback control system is: be installed in the terminal pose that the wooden passive marker of holding (5) of robot with five degrees of freedom is measured robot with five degrees of freedom (2) by optictracking device (3) by tracking, calculate controlled quentity controlled variable by computer (1) according to the pose that pose that measures and expectation robot with five degrees of freedom (2) reach, control robot with five degrees of freedom (2) further moves to revise deviation; Communicate by the RS232/RS422 serial ports between optictracking device (3) and the computer (1); Communicate by the RS232C serial ports between robot with five degrees of freedom (2) and the computer (1).

5. celebral operating robot system according to claim 1 is characterized in that, the quantity of passive marker (5) is at least 3.

6. celebral operating robot system according to claim 1, it is characterized in that, passive marker (5) satisfies at the terminal geometry site of installing of robot with five degrees of freedom (2): the distance between any two passive markers (5) can not be less than 50mm, minimal spatial separation between any two line segments that are made of passive marker (5) line can not be less than 5mm, any two line segments are not parallel, and the angle between any two line segments can not be lower than 0.5 degree.

7. implementation method based on the celebral operating robot system of optical tracking closed loop control, it is characterized in that, carry out pose measurement by means of the optical tracking technology, comprising: the closed loop control method of the automatic assignment test method of the easy assignment test method of the 3 D medical model space and the conversion of optical measurement spatial mappings, robot with five degrees of freedom pedestal coordinate space and the conversion of optical measurement spatial mappings, the terminal pose of robot with five degrees of freedom.

8. according to the said implementation method of claim 7, it is characterized in that the easy assignment test method of the 3 D medical model space and the conversion of optical measurement spatial mappings is:

A. paste four medical science markers (6) by the doctor in patient head, require these four markers not on same plane, and any three markers are not on same straight line;

B. patient head is carried out CT or the scanning of MRI medical image, gained scan-image input computer (1);

That c. determines four medical science markers is scanned into the coordinate of picture point in the 3 D medical model space;

D. allow patient lie on the operation table (8), head uses moulding pillow (7) and operation table (8) relative fixed, four medical science markers (6) of measuring patient head with optics registration tool (4) are at the spatial coordinate of optical measurement, and measured value is by optictracking device (3) input computer (1);

E. calculate the 3 D medical model space and the spatial mapping transformation of optical measurement by computer (1).

9. according to the said method of claim 7, it is characterized in that the automatic assignment test method of robot with five degrees of freedom pedestal coordinate space and the conversion of optical measurement spatial mappings is:

A. four points in chosen in advance robot with five degrees of freedom (2) work space require these four points should be in the measuring range of optictracking device (3), and four somes coplane not, and any three points are conllinear not;

B. send the joint position order that arrives above-mentioned four points by computer (1) to robot with five degrees of freedom (2), control robot with five degrees of freedom (2) moves to above-mentioned four points successively;

C. note the coordinate of above-mentioned four points in robot with five degrees of freedom pedestal coordinate space by computer (1); Simultaneously, cooperate by optictracking device (3) and the passive marker (5) that is installed in the robot with five degrees of freedom end, the coordinate of its end in optical measurement space when sequentially determining robot with five degrees of freedom (2) moves to above-mentioned four points, and send into computer (1);

D. by computer (1) according to above-mentioned four points at robot with five degrees of freedom pedestal coordinate space and the spatial coordinate figure of optical measurement, calculate robot with five degrees of freedom (2) pedestal coordinate space and the spatial mapping transformation of optical measurement, thereby finish automatic mapping.

10. according to the said implementation method of claim 7, it is characterized in that the closed loop control method of robot with five degrees of freedom art end pose is:

A. will transform to the optical measurement space at the operation pathway that the 3 D medical model space is planned in advance by computer (1), and calculating (1) goes out the desirable pose that need reach at current path point robot with five degrees of freedom end;

B. by the desirable pose of computer (1) according to current path point, carry out by of the coordinate transform of optical measurement space to robot with five degrees of freedom pedestal coordinate space, and, obtain the ideal position in each joint of robot with five degrees of freedom by finding the solution the robot with five degrees of freedom inverse kinematics;

C. import the ideal position in each joint by computer (1) to robot with five degrees of freedom (2), control robot with five degrees of freedom (2) motion:

D. cooperate by optictracking device (3) and the passive marker (5) that is installed in the robot with five degrees of freedom end, the pose of The real time measure robot with five degrees of freedom end, and send into computer (1);

E. by computer (1) the desirable pose of the posture information of The real time measure with the current path point of planning in advance compared, obtain both pose deviations;

F. by computer (1) according to the pose deviation according to predetermined control law, Position Control adopts proportional-integral-differential to regulate; Attitude control employing ratio is regulated, and the calculation correction controlled quentity controlled variable is revised the desirable pose of current path point, and begins the motor control of a new round, thereby realizes accurate track following and location.

Images