[发明专利]一种测量运动物体六维位姿的设备和方法

权利要求书

1.一种测量运动物体六维位姿的方法，其特征在于：通过自动跟踪控 制步骤控制经纬仪的方位角，使经纬仪的激光发射器始终指向接收器的投 射面板，激光发射器发射激光线条或者激光平面投射在投射面板上，产生 激光光斑或者激光条纹；再通过对经纬仪位姿的标定获得经纬仪相对于固 定在地面上的全局坐标系的位姿，进一步通过经纬仪的旋转变换矩阵获得 激光发射器发射的激光线条或者激光平面在全局坐标系中的数学方程；通 过成像单元对投射面板上的激光光斑或者激光条纹进行拍摄和图像处理， 获得激光光斑或者激光条纹在成像单元中的映像的像素坐标；通过对成像 单元的标定方法获得成像单元上每一个像素坐标相对于接收器坐标系的位 置，利用像素坐标与接收器坐标系位置的一一对应关系获得激光光斑或者 激光条纹相对于接收器坐标系的局部坐标值；通过设定接收器坐标系相对 于全局坐标系的六维位姿获得成像单元标定得到的激光光斑或者激光条纹 相对于全局坐标系中的全局坐标值；然后通过同步触发测量步骤建立激光 结构光生成的直线或者平面在全局坐标系中的数学方程和成像单元标定得 到的激光光斑或者激光条纹相对于全局坐标系的全局坐标值之间的约束关 系，求解约束关系得出接收器坐标系相对于全局坐标系的位姿；其中：接 收器坐标系为O′-X′Y′Z′，固定在接收器上，全局坐标系为O-XYZ，固定在 地面上。

2.按权利要求1所述的方法，其特征在于：所述对成像单元的标定是 通过外部设备标定投射面板和成像单元相对于接收器坐标系的位置和姿 态。

3.按权利要求1所述的方法，其特征在于：所述经纬仪位姿的标定采 用外部标定或是自行标定两种方式；外部标定方法借助外部设备进行标定； 自行标定方法通过将接收器固定、让经纬仪的转动的方式获得经纬仪相对 于地面固定全局坐标系的位姿；具体为：将接收器固定，使经纬仪的水平 转角和俯仰转角选取一组以上的不同数组，对于选取的每组数值均执行如 下步骤：固定经纬仪的转角，经纬仪将水平转角和俯仰角数据发送给计算 处理单元，计算处理单元根据经纬仪相对于全局坐标系的位姿以及水平转 角和俯仰角数据计算出激光线条或者激光平面的数学方程；同时接收器将 拍摄获得的激光光斑相对于接收器坐标系的局部坐标值发送给计算处理单 元；计算处理单元以接收器坐标系相对于全局坐标系的六维位姿为变量， 根据位姿坐标变换获得激光光斑在全局坐标系中的函数表达式；计算处理 单元根据激光光斑位于激光线条或者结构光平面之上的约束条件，将激光 光斑在全局坐标系中的函数表达式代入激光线条或者结构光平面的数学方 程，建立以经纬仪六维位姿为未知变量的多个方程，每次执行上述步骤获 得若干个方程，将所有转角数组对应的方程合成一个方程组，求解该方程 组获得所有经纬仪相对于地面固定全局坐标系的位姿。

4.按权利要求1所述的方法，其特征在于：所述同步触发测量的具体 步骤为：计算处理单元发送触发信号分别给经纬仪和接收器，经纬仪将触 发时刻的水平转角和俯仰角数据发送给计算处理单元，计算处理单元根据 经纬仪相对于全局坐标系的位姿以及水平转角和俯仰角数据计算出激光线 条或者结构光平面的数学方程；同时接收器将触发时刻计算获得的激光光 斑相对于接收器坐标系的局部坐标值发送给计算处理单元；计算处理单元 以接收器坐标系相对于全局坐标系的六维位姿为变量，根据坐标变换获得 激光光斑在全局坐标系中的函数表达式；计算处理单元根据激光光斑位于 激光线条或者结构光平面之上的约束条件，将激光光斑在全局坐标系中的 函数表达式代入激光线条或者结构光平面的数学方程，建立以接收器坐标 系六维位姿为未知变量的方程组，求解该方程组获得接收器坐标系相对于 全局坐标系的三维位置和三维姿态。

5.按权利要求1所述的方法，其特征在于：所述自动跟踪控制的实现 步骤具体为：由计算处理单元根据接收器的当前位姿和之前位姿计算接收 器的运动速度，根据接收器的运动速度推测下一采样时刻接收器将到达的 位姿，计算出经纬仪需要到达的新转角位置，并将新转角指令发送给相应 的经纬仪，经纬仪根据新转角指令控制其水平转角和俯仰转角至需要到达 的新转角位置。