摘要 |
A method for locating a tool in a work space via ultrasound, comprising:
associating at least one ultrasound transmitter (T) to the aforesaid tool (U) and calculating a position of the tool (U) on the basis of determination of the position (P z ) of the aforesaid transmitter (T);
providing, at least three ultrasound receivers (R 1 , R 2 , R 3 ), set in fixed positions (P 1 , P 2 , P 3 ) relative to the aforesaid work space (WS), which are configured for receiving ultrasound signals emitted by the aforesaid ultrasound transmitter (T); and
measuring, in a first reference system (IRS), distances (r 1 , r 2 , r 3 ) between the aforesaid ultrasound transmitter (T) and the aforesaid at least three ultrasound receivers (R 1 , R 2 , R 3 ) on the basis of propagation times associated to the aforesaid ultrasound signals received,
the aforesaid calculation of the position (P z ) of the transmitter comprising using a trilateration procedure (100) that comprises applying trilateration formulas as a function of the aforesaid measured distances (r 1 , r 2 , r 3 ) and the aforesaid fixed positions (P 1 , P 2 , P 3 ),
the aforesaid trilateration procedure (100) comprising:
carrying out (110) a change of reference of the fixed positions (P 1 , P 2 , P 3 ) of the aforesaid at least three receivers to reference them in a second simplified reference system (TRS) in which the aforesaid fixed positions (P 1 , P 2 , P 3 ) lie in a plane associated to a zero value of position coordinate;
applying (120) the aforesaid trilateration formulas to obtain the position of an unknown point (P z ) in the aforesaid second simplified reference system (TRS) as a function of the aforesaid measured distances (r 1 , r 2 , r 3 ) and of the aforesaid fixed positions (P 1 , P 2 , P 3 ) expressed in the aforesaid second simplified reference system (TRS);
carrying out (130) a rototranslation (R) of the position (P z ) of the unknown point obtained from the second simplified reference system (TRS) into the first reference system, a corresponding rototranslation matrix (R) being calculated as a function of the fixed positions (P 1 , P 2 , P 3 ) in the first reference system (IRS) and in the simplified reference system (TRS); and
using a position of the aforesaid unknown point obtained following upon the above rototranslation as position of the aforesaid transmitter (T). |