Page - 214 - in Proceedings - OAGM & ARW Joint Workshop 2016 on "Computer Vision and Robotics“

with themodifiedoptimizationproblem{ ‖c‖=‖c1‖+‖c2‖→min c1= ( A˜ T 1 )+( y˜− A˜T2c2 ) } . (23) Thesolutionstructure is equivalent to c= ( A˜ T (q) )+( M˜(q)q¨+ C˜(q, q˙) q˙+Q˜(q, q˙) ) ︸ ︷︷ ︸ 1 +N A˜ T (q)c0︸ ︷︷ ︸ 3 , (24) withN A˜ T = Im−(A˜T) + A˜ T , butunlikebefore ( A˜ T )+ =    ( A˜ T 1 )+( In− A˜T2 ( Im−δ+BTB )−1BT(A˜T1)+)( Im−δ+BTB )−1BT(A˜T1)+    , B=(A˜T1)+A˜T2 (25) isnot the rightpseudoinverse. 4.2. AugmentedPDController Thesolutionof the inversedynamics isonlyacontrol schemewithout thefeedback termbyweighted joint errors. Therefore, sucha termhas tobeadded toEq. 20 c= ( AT (q) )+ W ( M(q) q¨di+C(q, q˙)q˙ d i+Q(q, q˙) )︸ ︷︷ ︸ 1 −(AT (q))+W(KPei+KDe˙i)︸ ︷︷ ︸ 2 +NAT,W(q)c 0︸ ︷︷ ︸ 3 , (26) for thecontrol torques inminimal formulationand c= ( A˜ T (q) )+( M˜(q) q¨d+ C˜(q, q˙)q˙d+Q˜(q, q˙) ) ︸ ︷︷ ︸ 1 − ( A˜ T (q) )+( K˜Pe+K˜De˙ ) ︸ ︷︷ ︸ 2 +N A˜ T (q)c0︸ ︷︷ ︸ 3 . (27) for the redundant formulation (Eq. 24). The superscriptd indicates thedesiredvalues. Thevariables ei=qi−qdi , respectivelye=q−qd are theerrorcoordinatesandKP,KD,K˜P,K˜D are theweighting matrices for thePDcontroller. 4.3. SimulationResults The simulations are realizedwith an augmented PD controller in both formulations. Furthermore, quantization effects of encoders are implemented. Theweightingmatrices of the PDcontroller are chosenby two issues. Firstly, the joint error shall be as small as possible. And secondly, the torque of themotors shallnotbe toohighandaggressive. Thepath is a simplepoint topointmotionwitha fewarbitrarypointsand theacceleration is realized by sin2profiles. Furthermore, the unknowndynamics parameters are foundwith aCADprogram. Table 1 shows an overview.J is themass inertiaaroundtheCoGandrelevantaxis,except theentryof theMotor/active link - combination. Instead it is along themotor axis.m is themass and ls is the distance between 214