本发明提供一种混凝土泵车臂架末端位姿的摄像测量方法。本发明的技术方案是在混凝土泵车臂架末端安装摄像机，构建摄像测量系统。系统包含一台或多台摄像机（可见光、红外摄像机等）和数据处理器（微机或DSP等）、数据传输线等。摄像机安装在泵车臂架末端，数据处理器可与摄像机集成安装在一起，也可单独安装在泵车其他部位，数据传输线用于传送摄像机图像和传送测量数据给泵车控制系统。摄像测量系统工作时，摄像机随泵车臂架末端运动，对视场内的景物实时成像，数据处理器实时检测处理视频图像上景物的变化，实时计算泵车臂架末端的位姿变化。计算测量结果实时传输给泵车控制系统，用于泵车臂架末端位姿控制或测试。本发明也可用于其它工程机械部件的位姿测量、测试或控制。
1.一种混凝土泵车臂架末端位置姿态的摄像测量方法，其特征在于，在混凝土泵车臂架末端安装摄像机系统，通过测量摄像机视频图像上景物的变化，实时计算泵车臂架末端的位姿变化，测量结果实时传输给泵车控制系统，用于泵车臂架末端位姿控制或测试，其实施步骤为：
1) 在泵车臂架末端处安装摄像机系统，包含一台或多台摄像机，在泵车上安装数据处理器和数据传输线，构成泵车臂架末端位姿变化摄像测量系统；
2) 标定摄像测量系统相关参数，建立摄像测量坐标系，标定摄像测量坐标系与泵车臂架控制坐标系之间的关系；
3) 摄像测量系统工作时，摄像机系统随泵车臂架末端运动连续拍摄，将视频图像传送给数据处理器；
4) 数据处理器实时检测视频图像中景物特征的变化，根据上述变化实时计算摄像机系统的运动，运动计算方法采用光流法、本质矩阵法或多目交会测量法；
上述摄像机系统的运动计算结果为相邻时刻间或相对初始测量时刻且在摄像测量坐标系的摄像机系统的位姿变化，由于摄像机固定安装在泵车臂架末端处，摄像机系统的位姿变化即为泵车臂架末端的位姿变化；
5) 根据步骤2)标定得到的摄像测量坐标系与泵车臂架控制坐标系之间的关系，将步骤4)计算得到的泵车臂架末端的位姿变化转换到泵车臂架控制坐标系内表示，得到泵车臂架末端位姿变化测量结果；
6)将步骤5)的测量结果实时传送给泵车控制系统，用于臂架末端位姿控制或测试；
首先在当前时刻 多台摄像机同步采集的图像上检测、匹配多个景物特征，多目交会测量得到这些特征在当前成像时刻摄像测量坐标系Ct2的坐标或方程表示，设在双摄像机同一时刻拍摄的图像中某特征点图像坐标分别为 和 ， 和 分别为已标定的双摄像机投影矩阵，包含摄像机内参数和摄像机在测量坐标系的位姿外参数，根据摄像测量原理，该特征点在摄像测量坐标系内的坐标 满足， (1)
对于线特征其他类型特征，利用类似方法，通过面面交会确定特征在摄像测量坐标系的方程表示，也可以首先确定特征上的关键点，然后利用交会测量确定点的坐标，最后建立特征的方程表示，然后在当前测量的特征中匹配查找上一成像时刻 测量的景物特征，计算当前成像时刻 与上一成像时刻 间共同特征的坐标或方程表示的差异，根据上述共同特征的表示差异以及摄像测量坐标系 与泵车臂架控制坐标系 间的关系计算泵车臂架末端在两个时刻间的位姿变化在泵车臂架控制坐标系的表示 和 ，设时刻 测量得到第 个特征点在摄像测量坐标系的三维坐标为 ， 时刻测量得到该第 个特征点在摄像测量坐标系中的三维坐标为 ， 时刻摄像测量坐标系原点在泵车臂架控制坐标系的坐标为 ，摄像测量坐标系与泵车臂架控制坐标系姿态转换矩阵为 ，和 通过标定摄像测量坐标系与泵车臂架控制坐标系间的位姿关系得到，则有， (2)由(2)可得
当测量多个特征点时，根据(3)可确定摄像测量坐标系在 和 两时刻间的位姿变化和 ，综合处理各时刻间的位姿变化，可得到初始测量时刻至当前测量时刻的泵车臂架末端位姿变化。
一种混凝土泵车臂架末端位置姿态的摄像测量方法
技术领域
[0001] 本发明涉及工程机械、计算机视觉技术领域，具体指在混凝土泵车臂架末端安装摄像测量系统，实时测量泵车臂架末端位置姿态的方法。
背景技术
[0002] 混凝土泵车臂架末端的稳定控制对于提高工程施工的安全性和效率具有十分重要的意义，臂架末端的位置姿态（位姿）实时测量是稳定控制的前提。目前混凝土泵车主要依靠测量各节臂间的油缸油压、倾角等信息得到各节臂的位置姿态，进而得到臂架末端的位置姿态。目前的测量方法用于臂架末端稳定控制，存在响应延迟大、臂架振动幅度大等问题。
[0003] 在计算机视觉领域，可以利用摄像机构建摄像测量系统，通过分析处理摄像机视场中景物的视频图像特征，计算摄像机的运动或摄像机视场中目标的运动。本发明利用计算机视觉领域相关技术，泵车臂架上安装摄像机，构建混凝土泵车位姿的摄像测量系统，实时测量泵车臂架末端的位置姿态变化，用于臂架末端的位姿控制或测试。
发明内容
[0004] 本发明提供一种混凝土泵车臂架末端位姿的摄像测量方法，具体指在混凝土泵车臂架末端安装摄像机，构建摄像测量系统。系统包含一台或多台摄像机（可见光、红外摄像机等）和数据处理器（微机或DSP等）、数据传输线等。摄像机安装在泵车臂架末端，数据处理器可与摄像机集成安装在一起，也可单独安装在泵车其他部位，数据传输线用于传送摄像机图像和传送测量数据给泵车控制系统。系统组成示意图如图1所示。
[0005] 摄像测量系统工作时，摄像机随泵车臂架末端运动，对视场内的景物实时成像，数据处理器实时检测处理视频图像上景物（如地面、建筑物、人工设定标志等）的变化，实时计算泵车臂架末端的位姿变化。计算测量结果实时传输给泵车控制系统，用于泵车臂架末端位姿控制或测试。具体实施过程为：
[0006] 1) 在泵车臂架末端处安装摄像机系统（包含一台或多台摄像机），在泵车上安装数据处理器（微机、DSP等）和数据传输线等，构成泵车臂架末端位姿变化摄像测量系统。
[0007] 2) 标定摄像测量系统相关参数（包括摄像机内参数、多台摄像机间的位姿关系等），建立摄像测量坐标系，标定摄像测量坐标系与泵车臂架控制坐标系之间的关系。上述标定可采用平面靶板标定法、控制点标定法、手眼关系标定法等摄像测量、计算机视觉领域的常用摄像系统标定方法。
[0008] 3) 摄像测量系统工作时，摄像机系统随泵车臂架末端运动并连续拍摄，将视频图像传送给数据处理器。
[0009] 4) 数据处理器实时检测视频图像中景物特征的变化，根据上述变化实时计算摄像机系统的运动。该运动计算方法可采用光流法、本质矩阵法、多目交会测量法等摄像测量、计算机视觉领域的运动估计方法。
[0010] 上述摄像机系统的运动计算结果主要指摄像机系统的位姿变化（相邻时刻间或相对初始测量时刻），一般为在摄像测量坐标系的表示。由于摄像机固定安装在泵车臂架末端处，摄像机系统的位姿变化即为泵车臂架末端的位姿变化。
[0011] 5) 根据步骤2)标定得到的摄像测量坐标系与泵车臂架控制坐标系之间的关系，将步骤4)计算得到的泵车臂架末端的位姿变化转换到泵车臂架控制坐标系内表示，得到泵车臂架末端位姿变化测量结果。
[0012] 6)将步骤5)的测量结果实时传送给泵车控制系统，用于臂架末端位姿控制或测试。
[0013] 本发明可以达到以下的技术效果：
[0014] 本发明通过在泵车臂架末端安装摄像机对景物成像，实时检测景物在图像上的变化，实时计算泵车臂架末端的位姿变化，测量方式自动化智能化程度高，适用范围广，测量结果可直接用于泵车臂架位姿控制或测试。
[0015] 本发明同样可用于其它工程机械部件的位姿测量、测试或控制。
附图说明
[0016] 图1泵车臂架末端位姿变化摄像测量系统组成。
[0017] 图2泵车臂架末端位姿变化摄像测量原理示意图。
具体实施方式
[0018] 如图1所示，泵车臂架末端位姿摄像测量系统由一台或多台摄像机（可见光、红外等）、数据处理器（微机、DSP等）和数据传输线构成。摄像机安装在泵车臂架末端，数据处理器可与摄像机安装在一起也可安装在泵车其他位置。摄像机用于对泵车作业附近区域景物成像，数据处理器用于图像分析处理和臂架末端位姿计算，数据传输线用于将视频图像传输给数据处理器和将泵车臂架末端位姿测量结果传送给泵车控制系统。
[0019] 泵车臂架末端位姿变化测量原理如图2所示，由于臂架末端的运动，不同时刻相同景物在摄像机图像上的位置不同，通过检测景物在图像上的变化可以计算得到泵车臂架末端的位置姿态变化情况。根据视频图像计算泵车臂架末端运动的方法可以采用计算机视觉领域的光流法、本质矩阵法、多目交会测量法等，其中多目交会测量法的实现方式具体描述如下。
[0020] 如图2所示，首先在当前时刻（t2）多台摄像机同步采集的图上检测、匹配多个景物特征，多目交会测量得到这些特征在当前成像时刻摄像测量坐标系（Ct2）的坐标或方程表示。设在双摄像机同一时刻拍摄的图像中某特征点图像坐标分别为(x1，y1)和(x2，y2)，M1和M2分别为已标定的双摄像机投影矩阵（包含摄像机内参数和摄像机在测量坐标系的位姿c T外参数），根据摄像测量原理，该点特征在摄像测量坐标系内的坐标X ＝[x,y,z] 满足[0021]
[0022] 根据公式(1)的两个等式可求解出该点特征坐标Xc＝[x,y,z]T。
[0023] 对于线特征等其他类型特征，利用类似方法可以通过面面交会等确定特征在摄像测量坐标系的方程表示。也可以首先确定特征上的关键点，然后利用交会测量确定点的坐标，最后建立特征的方程表示。
[0024] 然后在当前测量的特征中匹配查找上一成像时刻（t1)测量的景物特征，计算当前成像时刻与上一成像时刻间共同特征的坐标或方程表示的差异。根据上述共同特征的表示差异以及摄像测量坐标系（Ct1）与泵车臂架控制坐标系（W）间的关系计算泵车臂架末端在两个时刻间的位姿变化在泵车臂架控制坐标系的表示 。设时刻t11测量得到第i个特征点在t1时刻摄像测量坐标系的三维坐标为 ，t2时刻测量得到该特征点在在t2时刻摄像测量坐标系中的三维坐标为 。设t1时刻泵车臂架控制坐标系为W，设t1时刻摄像测量坐标系原点的坐标为 ，摄像测量坐标系与泵车臂架控制坐标系姿态转换矩阵为 ， 、 通过标定摄像测量坐标系与泵车臂架控制坐标系间的位姿关系得到。t2时刻摄像测量坐标系位置变化δXw，姿态变化 ，则有
[0025]
[0026] 由(2)可得
[0027]
[0028] 当测量多个特征点时，可根据(3)确定摄像测量坐标系在t1、t2两时刻间的位姿变w化δX 和 。
[0029] 综合处理各时刻间的位姿变化，可以得到某初始测量时刻至当前测量时刻的泵车臂架末端位姿变化。
