5.6 System Calibration of Concentric Tube Robots using Collisions
5.6.3 Calibration of Two Concentric Tube Robots Using Collisions
We will start by looking at the parameters we want to calibrate and the output data we assume we are able to measure with our system. In the work presented here, we perform calibration in simulation on a two arm, two tube per arm concentric tube robot system, similar to the system used in [205]. Table 5.3 lists the parameters we seek to calibrate and the system measurements we can take, which in our case are the arc lengthss1ands2along
Figure 5.16: (a) An illustration of two concentric tube robots colliding is shown. We refer to the left robot as robot 1 and the right robot as robot 2 throughout. The parameters we seek to estimate [Tx,Ty,Tz,θx,θy,θz,k1r1,k2r1,k1r2,k2r2]in our simulation are shown here, as are the available output measurements[s1,s2]of the system. (b) An example of a two arm concentric tube robot system colliding is shown.
the robot backbones to the point of a collision. We note that we are only using two scalar output measurements to calibrate the desired parameters, and that the curved portions of the tubes are circular. See Figure 5.16 for a drawing of a collided configuration of the system with the desired calibration parameters and the output measurements shown. We now look to the model (5.1) that describes the robot backbone positions as a function of the parametersφφφ and known inputsuuu, and allows us to determine the arc length collision point along each robot.
Concentric Tube Robot Model
The states of an unloaded concentric-tube robot vary along the backbone arc lengthsand are the twist angles of the tubesψψψ(s)and their rate of changeψψψ(s)0, the arc-length positions of the tubeσσσ(s) relative to the tubes’ proximal ends, and the backbone position ppp(s)and
Parameters Sought
Tx Translation in thexdirection between the two robot origins Ty Translation in theydirection between the two robot origins Tz Translation in thezdirection between the two robot origins θx Rotation angle of robot 2 origin around thexaxis
θy Rotation angle of robot 2 origin around theyaxis θz Rotation angle of robot 2 origin around thezaxis k1r1 Curvature of tube 1 for robot 1
k2r1 Curvature of tube 2 for robot 1 k1r2 Curvature of tube 1 for robot 2 k2r2 Curvature of tube 2 for robot 2
Available Measurements
s1 Arc length along robot 1 backbone to collision point s2 Arc length along robot 2 backbone to collision point
Table 5.3: A description of the parameters we seek to estimate and the available output measure- ments is provided.
orientationRRR(s). The states are governed by the differential equation:
iψ(s)00 =−ggg(s)>iKKK(s)(∂RRR(iψ(s))ikkkˆiii (5.6)
iσ(s)0 =1 (5.7)
p
pp(s)0 =RRR(s)kkkˆ (5.8)
RR
R(s)0 =RRR(s)SSS(ggg(s)) (5.9)
where0 denotes the derivative with respect to arc length,ggg(s)is the frame-curvature, the stiffnesses of tubeiare iniKKK(s), the matrix∂RRR(iψ(s) =∂RRR(iψ(s))/∂iψ(s)whereRRR(iψ(s)) is the z-axis rotation matrix, ikkk is the precurvatuve of tube i and ˆiii is a unit vector in the xdirection, ˆkkk is a unit vector in thez direction, andSSS(ggg(s))is the skew-symmetric cross- product matrix. The formulation of, and further details on, this model can be found in [181]
and [103].
The inputs and boundary conditions for robot 1 are:
ψ ψ
ψ(0) =ααα1, σσσ(0) =γγγ1, ppp(0) =000, RRR(0) =III, ψψψ(l1)0=000 (5.10) whereααα1andγγγ1are the actuation-unit rotation angles and translations for robot 1, respec- tively, and l1 is the length of robot 1. The inputs and boundary conditions for robot 2 are:
ψ
ψψ(0) =ααα2, σσσ(0) =γγγ2, ppp(0) =TTT, RRR(0) =RRR(θz,θy,θx), ψψψ(l2)0=000 (5.11) whereααα2andγγγ2are the actuation-unit rotation angles and translations for robot 2, respec- tively, TTT is the translation between the base frame of robot 1 and robot 2,RRR(θz,θy,θx) is the fixed-angle rotation matrix around thez,y,xaxes byθz,θy,θx respectively, andl2is the length of robot 2. We assume that when a tube is not physically present at an arc-length, it has infinite torsional stiffness and zero bending stiffness.
The differential equation (5.6)-(5.9) with boundary constraints, (5.10) and (5.11), for robot 1 and 2, respectively, can be solved using standard two-point boundary-value meth- ods. The result is the backbone position and orientation as a function of arc lengths, for robot 1 and 2, denoted asppp1(s)andppp2(s)andRRR1(s)andRRR2(s), respectively. The nonlinear model in the form of (5.1), that we are calibrating, is given by:
yyy=
s1 s2
, s.t. ppp1(s1) =ppp2(s2) (5.12) where s1 and s2 are the arc lengths along the robot backbones to the collision point for robot 1 and 2, respectively.
Parameter Jacobian
In order to see how changes in the parameters affect the output measurements, we need to
calculate the JacobianJJJl that relates individual changes in our parametersφφφ to changes in s1ands2. A central finite difference method is used to obtainJJJl, which is define as:
JJJl=
δs1 δTx
δs1 δTy
δs1 δTz
δs1 δ θx
δs1 δ θy
δs1 δ θz
δs1
δk1r1 δs1
δk2r1 δs1
δk1r2 δs1 δk2r2 δs2
δTx
δs2 δTy
δs2 δTz
δs2 δ θx
δs2 δ θy
δs2 δ θz
δs2
δk1r1 δs2
δk2r1 δs2
δk1r2 δs2 δk2r2
SinceJJJl relates changes in the parameters we seek to calibrate (see Table 5.3) to the output measurements s1 and s2, we have a short and wide matrix. This fact means that our system is never observable (i.e., there always exist changes to the parameters we are estimating that produce no change in the measured arc lengths to the collision point). In order to overcome this challenge, we obtain data from multiple collisions using different robot poses and stack the data in the same form as (5.3), whereAAA is our regressor matrix that is comprised of P Jacobian matrices of the formJJJl, where P indicates the number of collision measurements. Because we are seeking to calibrate parameters with different units, it is also important to scale the parameters in order to improve the conditioning of the regressorAAA, enable comparison of the singular values, and help avoid numerical problems.