ロボット全体でのカメラ姿勢の算出 カメラ部は胴体に乗っているため、胴体の姿勢(股関節姿勢)の影響を受ける。 カメラ姿勢を(ct(camera tilt), cp(camera pan), cr(camera roll))としたとき、 Rot(z,cp)・Rot(x,ct)・Rot(y,cr) として表す。 カメラ姿勢行列 [ccpccr-scpsctscr] [-scpcct] [ccpscr+scpsctccr] [scpccr+ccpsctscr] [ccpcct] [scpscr-ccpsctccr] [-cctscr] [sct] [cctccr] 水平面から見たカメラ姿勢は股関節姿勢の影響を受けるため、股関節姿勢を(t, p, r)、首関節角度を(j1(第一チルト),j2(第二チルト),j3(パン))としたとき、 Rot(z,r)・Rot(y,p)・Rot(x,t)・Rot(x,(j1+j2))・Rot(z,j3) となる。 胴体姿勢と頭部関節によるカメラ姿勢 [crcpcj3-srctc(j1+j2)sj3+crspstc(j1+j2)sj3+srsts(j1+j2)sj3+crspcts(j1+j2)sj3] [-crcpsj3-srctc(j1+j2)cj3+crspstc(j1+j2)cj3+srsts(j1+j2)cj3+crspcts(j1+j2)cj3] [srcts(j1+j2)-crspsts(j1+j2)+srstc(j1+j2)+crspctc(j1+j2)] [srcpcj3+crctc(j1+j2)sj3+srspstc(j1+j2)sj3-crsts(j1+j2)sj3+srspcts(j1+j2)sj3] [-srcpsj3+crctc(j1+j2)cj3+srspstc(j1+j2)cj3-crsts(j1+j2)cj3+srspcts(j1+j2)cj3] [-crcts(j1+j2)-srspsts(j1+j2)-crstc(j1+j2)+srspctc(j1+j2)] [-spcj3+cpstc(j1+j2)sj3+cpcts(j1+j2)sj3] [spsj3+cpstc(j1+j2)cj3+cpcts(j1+j2)cj3] [-cpsts(j1+j2)+cpctc(j1+j2)] これらを整理すると、カメラ姿勢(ct,cp,cr)は sin(ct) = [spsj3+cpstc(j1+j2)cj3+cpcts(j1+j2)cj3] tan(cp) = -[-scpcct]/[ccpcct] = -[-crcpsj3-srctc(j1+j2)cj3+crspstc(j1+j2)cj3+srsts(j1+j2)cj3+crspcts(j1+j2)cj3]/[-srcpsj3+crctc(j1+j2)cj3+srspstc(j1+j2)cj3-crsts(j1+j2)cj3+srspcts(j1+j2)cj3] tan(cr) = sin(cr)/cos(cr) = -[-cctscr]/[cctccr] = -[-spcj3+cpstc(j1+j2)sj3+cpcts(j1+j2)sj3]/[-cpsts(j1+j2)+cpctc(j1+j2)] 胴体姿勢角度 t,c,r = 0の時、sin項は0となり、cos項は1となるため、 tan(cr) = -[s(j1+j2)sj3]/[c(j1+j2)] sin(ct) = [s(j1+j2)cj3] tan(cp) =-[-sj3]/[c(j1+j2)cj3] となる。