六自由度运动平台正解(几何法)
1. 对上平台（运动平台）进行扩展，示意如下：
Pic 1 上平台示意图
由于确定一个平面状态只需要三个点，因此获得C1,C2,C3坐标，即可确定平面状态。

如图，h1,h2均为已知量，设L h k /1=，212*h h L +=，),,(i i i i z y x C =。

设下平台各点坐标为),,(i i i i s n m B =，设各轴长为i i i l B A =。

于是问题简化为：已知：L k l B i i ,,,，求解i C 。

2. 建立方程组 2.1 i l 相关
对于1l ，分析如下：
Pic 2 单轴示意图
由图可知：向量3111111111*C C k C B A C C B A B +=+=，
即，1111111131313),,(),,(l s z n y m x z z y y x x k
=---+---
所以：
)1......(0])1([])1([])1([21211321132113=---++--++--+l s z k kz n y k ky m x k kx
同理有：
)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([262631263126312525322532253224242324232423232321232123212
2221222122212=---++--++--+=---++--++--+=---++--++--+=---++--++--+=---++--++--+l s z k kz n y k ky m x k kx l s z k kz n y k ky m x k kx l s z k kz n y k ky m x k kx l s z k kz n y k ky m x k kx l s z k kz n y k ky m x k kx
2.2 L 相关
)
9......(0)()()()8......(0)()()()7......(0)()()(222322322322312312312221221221=--+-+-=--+-+-=--+-+-L z z y y x x L z z y y x x L z z y y x x 3. 求解
3.1 联立方程组（1）-（9），牛顿迭代法解方程组，即可求的i C ，
取0>i z ，可得唯一解。

3.2 由i C 求出平台姿态
根据实际情况，建立坐标系如下
Pic 3
取中心向C1方向为X 正方向，逆时针90度为Y 正方向，则XY 方向在静坐标系中的方向向量为：
),,()5.05.0,5.05.0,5.05.0(131313321321321z z y y x x VectorY z z z y y y x x x VectorX ---=------=
显然平台中心点为3/)(321C C C O ++=
)/arctan()/arctan()/arctan(3
/)(3/)(3/)(321032103210VectorXx VectorXy VectorXx VectorXz VectorYy VectorYz z z z z y y y y x x x x -=-==++=++=++=γβα。