六自由度运动平台正解(几何法)

六自由度运动平台正解(几何法)1.对上平台（运动平台）进行扩展，示意如下：Pic1上平台示意图由于确定一个平面状态只需要三个点，因此获得C1,C2,C3坐标，即可确定平面状态。如图，h1,h2均为已知量，设Lhk/1，212*hhL，),,(iiiizyxC。设下平台各点坐标为),,(iiiisnmB，设各轴长为iiilBA。于是问题简化为：已知：LklBii,,,，求解iC。2.建立方程组2.1il相关对于1l，分析如下：Pic2单轴示意图由图可知：向量3111111111*CCkCBACCBAB，即，1111111131313),,(),,(lsznymxzzyyxxk所以：)1......(0])1([])1([])1([21211321132113lszkkznykkymxkkx同理有：)6......(0])1([])1([])1([)5......(0])1([])1([])1([)4......(0])1([])1([])1([)3......(0])1([])1([])1([)2......(0])1([])1([])1([2626312631263125253225322532242423242324232323212321232122221222122212lszkkznykkymxkkxlszkkznykkymxkkxlszkkznykkymxkkxlszkkznykkymxkkxlszkkznykkymxkkx2.2L相关)9......(0)()()()8......(0)()()()7......(0)()()(222322322322312312312221221221LzzyyxxLzzyyxxLzzyyxx3.求解3.1联立方程组（1）-（9），牛顿迭代法解方程组，即可求的iC，取0iz，可得唯一解。3.2由iC求出平台姿态根据实际情况，建立坐标系如下Pic3取中心向C1方向为X正方向，逆时针90度为Y正方向，则XY方向在静坐标系中的方向向量为：),,()5.05.0,5.05.0,5.05.0(131313321321321zzyyxxVectorYzzzyyyxxxVectorX显然平台中心点为3/)(321CCCO)/arctan()/arctan()/arctan(3/)(3/)(3/)(321032103210VectorXxVectorXyVectorXxVectorXzVectorYyVectorYzzzzzyyyyxxxx