专利名称：机械臂三维姿态的检测方法和系统、及控制其运行的系统的制作方法
技术领域：
本发明涉及工程机械技术领域，特别涉及一种机械臂三维姿态的检测方法和检测系统。本发明还涉及一种根据机械臂三维姿态控制机械臂运行的系统。
背景技术：
工程机械设备上，机械臂为较为常见且较为关键的部件，比如混凝土泵车的布料臂架、起重机的伸缩式臂架、大型机械手等。具有机械臂的工程机械的正常工作，有赖于机械臂的精准操作，以使机械臂达到预定位置、实施预定动作。为了保证工程机械设备正常完成施工工作，必须对机械臂实现精确的控制。操作人员控制机械臂操作或控制器自动控制时，均需要根据机械臂的实际姿态并结合操作目标控制机械臂的运行。因此，机械臂的实际运行姿态为机械臂精准控制的重要参考因素。目前对机械臂姿态的获取限于二维空间和静态测量，获得的机械臂姿态存在不精确的问题， 导致机械臂的控制难以满足实际需求。有鉴于此，如何精确地获取机械臂的姿态信息，是本领域技术人员需要解决的技术问题。
发明内容
本发明的核心为提供一种机械臂三维姿态的检测方法和系统，该方法和系统通过检测机械臂的角速度和加速度，即机械臂动态和静态测量结合，实现动静相互补偿，从而提高不同工况下机械臂姿态的测量精度。本发明的另一目的是提供一种根据机械臂三维姿态控制机械臂运行的系统。为达到上述目的，本发明提供一种机械臂三维姿态的检测方法，包括下述步骤A)通过加速度检测元件和角速度检测元件实时检测机械臂的加速度和角速度；B)对加速度和角速度进行数据处理；C)通过处理后的加速度和角速度计算获得机械臂的三维姿态。优选地，步骤A)中还实时检测加速度检测元件和角速度检测元件的温度；且步骤
B)中，通过检测的温度对加速度和角速度进行校正补偿。优选地，步骤B)中，采用二次曲线拟合的方式校正补偿检测的加速度和角速度。优选地，步骤B)中，校正补偿之前，对检测的加速度、角速度、温度进行滤波处理。优选地，步骤B)中，通过卡尔曼滤波算法对检测的加速度、角速度、温度进行滤波处理。优选地，步骤A)中，建立g = f(t) Xgx函数关系式，由实时检测的角速度获取不同时刻的旋转角度，由旋转角度和矩阵函数f(t)的对应关系，获取f(t)，根据f(t)和&获取g ;步骤C)中，按照下述公式V (k+1) = V (k) +T [g (k+1) +g (k) ] /2 ；
P (k+1) = P (k) +T[v (k+1) +v (k) ]/2 ;计算获得机械臂的空间位置，以获取机械臂的三维姿态；其中，g为实际加速度、gx为检测加速度、f (t)为随时间更新的空间位置的转换矩阵函数、T为相邻两次检测的间隔时间、V (k+1)和v(k)为相邻两次检测的机械臂的速度、 P (k+1)和p(k)为相邻两次检测的机械臂的空间位置、g(k+l)和g(k)为相邻两次检测机械臂的加速度。本发明还提供一种机械臂三维姿态的检测系统，包括加速度检测元件，用以实时检测机械臂的加速度；角速度检测元件，用以实时检测机械臂的角速度；处理单元，用于对加速度和角速度进行数据处理；计算单元，通过处理后的加速度和角速度计算获得机械臂的三维姿态。优选地，所述角速度检测元件为三轴陀螺仪。优选地，还包括实时检测加速度检测元件和角速度检测元件温度的温度传感器； 处理单元通过检测的温度对加速度和角速度进行校正补偿。优选地，处理单元采用二次曲线拟合的方式校正补偿检测的加速度和角速度。优选地，处理单元在校正补偿之前，对检测的加速度、角速度、温度进行滤波处理。优选地，处理单元通过卡尔曼滤波算法对检测的加速度、角速度、温度进行滤波处理。优选地，计算单元建立g = f (t) Xgx函数关系式，并根据三轴陀螺仪检测的角速度获取不同时刻的旋转角度，由旋转角度和矩阵函数f(t)的对应关系，获取f(t)，根据 f(t)和gx获取g ;并按照下述公式 V (k+1) = v(k) +T [g (k+1) +g (k) ] /2 ；p (k+1) = p (k) +T[v (k+1) +v (k) ]/2 ;计算获得机械臂的空间位置，以获取机械臂的三维姿态；其中，g为实际加速度、gx为检测加速度、f(t)为实时更新的矩阵函数、T为相邻两次检测的间隔时间、v(k+l)和v(k)为相邻两次检测的机械臂的速度、P (k+1)和p(k)为相邻两次检测的机械臂的空间位置、g(k+l)和g(k)为相邻两次检测机械臂的加速度。该发明提供的机械臂三维姿态的检测方法和检测系统，通过检测机械臂的加速度和角速度，即机械臂动态和静态测量结合，实现动静相互补偿，从而提高不同工况下机械臂三维姿态的检测精度。而且该种方法和系统不仅反映出机械臂线性位移对机械臂空间姿态的影响，同时考虑了旋转运动对空间姿态的影响，因此，可以较为准确地反映机械臂的三维姿态。本发明还提供一种控制机械臂运行的系统，包括机械臂三维姿态的检测系统，该检测系统为上述任一项所述的检测系统，还包括人机界面和/或控制机械臂运行的控制器，检测系统将检测的机械臂三维姿态信息发送至人机界面和/或控制机械臂运行的控制器。由于上述检测系统具有上述技术效果，具有该检测系统的控制机械臂运行的系统也具有相同的技术效果。
图I为本发明所提供机械臂三维姿态检测方法一种具体实施方式
的流程图；图2为本发明所提供机械臂三维姿态检测系统一种具体实施方式
的结构原理图；图3为本发明所提供控制机械臂运行的系统一种具体实施方式
的结构原理图。
具体实施例方式本发明的核心为提供一种机械臂三维姿态的检测方法和系统，该方法和系统通过检测机械臂的角速度和加速度，即机械臂动态和静态测量结合，实现动静相互补偿，从而提高不同工况下机械臂姿态的测量精度。本发明的另一核心是提供一种根据机械臂三维姿态控制机械臂运行的系统。为了使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施例对本发明作进一步的详细说明。为便于理解和简洁描述，下述内容结合机械臂三维姿态的检测方法和系统进行说明，有益效果不再重复论述。请参考图I和图2，图I为本发明所提供机械臂三维姿态检测方法一种具体实施方式
的流程图；图2为本发明所提供机械臂三维姿态检测系统一种具体实施方式
的结构原理图。该具体实施方式
中机械臂三维姿态的检测方法，包括下述步骤SI)通过加速度检测元件和角速度检测元件分别实时检测机械臂的加速度和角速度；加速度检测元件可以是常用的加速度传感器，角速度检测元件可以是三轴陀螺仪。在实际应用中，将加速度传感器和三轴陀螺仪固定于机械臂上，当应用于具有多级臂架场合时，可以在各级臂架上均安装检测加速度和角速度的检测装置，即可静态测量出机械臂的加速度、动态测量出机械臂的角速度。S2)对加速度和角速度进行数据处理；设置处理单元，处理单元与上述加速度检测元件和角速度元件连接，比如与加速度传感器和三轴陀螺仪连接，则处理单元可以将采集的加速度和角速度模拟信号转换为数据信号，以便于后续处理。S3)通过处理后的加速度和角速度计算获得机械臂的三维姿态。设置与处理单元连接的计算单元，计算单元可以对处理后的加速度和角速度进行计算，根据加速度和位移的计算公式，可以计算出机械臂的实时线性位移；根据旋转角度和角速度的计算公式，可以获得机械臂的实时旋转角度，进而获得机械臂的姿态角。机械臂的实时线性位移和姿态角完整地反映出了机械臂的三维姿态。该实施例中，通过检测机械臂的加速度和角速度，即机械臂动态和静态测量结合， 实现动静相互补偿，从而提高不同工况下机械臂三维姿态的检测精度。而且该种方法不仅反映出机械臂线性位移对机械臂空间姿态的影响，同时考虑了旋转运动对空间姿态的影响，因此，可以较为准确地反映机械臂的三维姿态。具体地，步骤SI)中，还可以实时检测加速度检测元件和角速度检测元件的温度； 且步骤S2)中，根据实时检测的温度对加速度和角速度进行温度校正补偿。由于加速度传感器和三轴陀螺仪等检测元件的精度可能受到温度的影响，故可以设置温度传感器，实时检测加速度和角速度检测装置的温度，获取不同时刻下检测的加速度和角速度对应的温度值。在步骤S2)中，处理单元对加速度和角速度信号进行处理时,还考虑温度对检测值的影响。比如，通过二次曲线拟合算法对加速度和角速度进行温度校正补偿，即将温度和加速度(或角速度)的离散检测值，模拟为曲线，根据曲线对应关系，获取检测加速度值和实际加速度值的关系，以获取更为精确的加速度值，消除温度变化对加速度检测的影响。针对上述方法，步骤S2)中，对加速度和角速度进行温度校正补偿之前，处理单元可以先对检测的加速度、角速度、温度进行滤波处理。检测加速度和角速度的检测装置、温度传感器在检测过程中可能出现较大偏差，滤波步骤可以有效滤除偏差较大的值，以保证处理数据的精确度，滤波可以采用卡尔曼滤波算法，或其他较为常用的滤波算法。具体地，由于加速度传感器为静态测量，则随机械臂的旋转运动和时间因素，检测的加速度与实际加速度可能具有一定偏差。故在步骤S3)中还计算获得机械臂的旋转角度和旋转加速度；建立函数关系式g = f(t) Xgx ；其中，g为实际加速度，f(t)为随时间更新的矩阵函数；gx为加速度检测值；加速度检测值gx受离心加速度的影响，与实际加速度g存在偏差，根据现有的旋转理论，该偏差可以通过转换矩阵函数f(t)消除，而f(t)函数与机械臂的旋转角度有关， 根据三轴陀螺仪的现有算法，不同时刻的机械臂的旋转角度之间可以通过f(t)函数相互转换，另外，根据检测的角速度，也可以计算出机械臂的不同时刻的旋转角度，故可以由已知的角速度推算出不同时段对应的f(t)函数。则根据f(t)和&可以获取实际加速度g;步骤C)中，按照下述公式V (k+1) = V (k) +T [g (k+1) +g (k) ] /2 ；p (k+1) = p (k) +T [v (k+1) +v (k) ] /2 ；可以计算获得机械臂的空间位置，以获取机械臂的三维姿态；其中，T为相邻两次检测的间隔时间、v(k+l)和v(k)为相邻两次检测的机械臂的速度、P(k+1)和p(k)为相邻两次检测的机械臂的空间位置、g(k+l)和g(k)为相邻两次检测机械臂的加速度。该方式中机械臂的空间位置由修正后的加速度获得，S卩加速度根据机械臂的旋转和时间两类因素进行了修正，弥补了静态测量的不足，以获取实际加速度，从而使得空间位置的获得更为精确，符合实际工况。除了上述机械臂三维姿态的检测方法和系统，本发明还提供一种控制机械臂运行的系统，请参考图3，图3为本发明所提供机械臂运行控制系统一种具体实施方式
的结构原理图。该实施方式中的机械臂运行控制系统包括机械臂三维姿态的检测系统，该检测系统为上述任一实施例所述的检测系统，还包括人机界面和/或控制机械臂运行的控制器， 检测系统将检测的机械臂三维姿态信息(包括机械臂的线性位移、姿态角，以及速度等) 发送至人机界面和/或控制机械臂运行的控制器，以便操作人员根据机械臂的三维姿态控制机械臂运行，或由控制器根据机械臂的三维姿态直接控制机械臂按照预定目标运行，图3 中检测系统与人机界面连接。由于上述机械臂三维姿态检测系统具有上述技术效果，具有该检测系统的机械臂运行控制系统也具有相同的技术效果，在此不赘述。以上对本发明所提供的一种检测机械臂三维姿态的方法和系统，以及控制机械臂运行的系统进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。
权利要求
1.一种机械臂三维姿态的检测方法，其特征在于，包括下述步骤A)通过加速度检测元件和角速度检测元件实时检测机械臂的加速度和角速度；B)对加速度和角速度进行数据处理；C)通过处理后的加速度和角速度计算获得机械臂的三维姿态。
2.根据权利要求I所述的机械臂三维姿态的检测方法，其特征在于，步骤A)中还实时检测加速度检测元件和角速度检测元件的温度；且步骤B)中，通过检测的温度对加速度和角速度进行校正补偿。
3.根据权利要求2所述的机械臂三维姿态的检测方法，其特征在于，步骤B)中，采用二次曲线拟合的方式校正补偿检测的加速度和角速度。
4.根据权利要求2或3所述的机械臂三维姿态的检测方法，其特征在于，步骤B)中，校正补偿之前，对检测的加速度、角速度、温度进行滤波处理。
5.根据权利要求4所述的机械臂三维姿态的检测方法，其特征在于，步骤B)中，通过卡尔曼滤波算法对检测的加速度、角速度、温度进行滤波处理。
6.根据权利要求I至3任一项所述的机械臂三维姿态的检测方法，其特征在于，步骤A)中，建立g = f (t) Xgx函数关系式，由实时检测的角速度获取不同时刻的旋转角度，由旋转角度和矩阵函数f(t)的对应关系，获取f(t)，根据f(t)和&获取8;步骤C)中，按照下述公式V(k+1) =v (k) +T [g (k+1) +g (k) ] /2 ；p(k+l) =p(k)+T[v(k+l)+v(k)]/2 ;计算获得机械臂的空间位置，以获取机械臂的三维姿态；其中，g为实际加速度、gx为检测加速度、f (t)为随时间更新的空间位置的转换矩阵函数、T为相邻两次检测的间隔时间、v(k+l)和v(k)为相邻两次检测的机械臂的速度、P (k+1) 和P(k)为相邻两次检测的机械臂的空间位置、g(k+l)和g(k)为相邻两次检测机械臂的加速度。
7.一种机械臂三维姿态的检测系统，其特征在于，包括加速度检测元件，用以实时检测机械臂的加速度；角速度检测元件，用以实时检测机械臂的角速度；处理单元，用于对加速度和角速度进行数据处理；计算单元，通过处理后的加速度和角速度计算获得机械臂的三维姿态。
8.根据权利要求7所述的机械臂三维姿态的检测系统，其特征在于，所述角速度检测元件为三轴陀螺仪。
9.根据权利要求8所述的机械臂三维姿态的检测系统，其特征在于，还包括实时检测加速度检测元件和角速度检测元件温度的温度传感器；处理单元通过检测的温度对加速度和角速度进行校正补偿。
10.根据权利要求9所述的机械臂三维姿态的检测系统，其特征在于，处理单元采用二次曲线拟合的方式校正补偿检测的加速度和角速度。
11.根据权利要求9或10所述的机械臂三维姿态的检测系统，其特征在于，处理单元在校正补偿之前，对检测的加速度、角速度、温度进行滤波处理。
12.根据权利要求11所述的机械臂三维姿态的检测系统，其特征在于，处理单元通过卡尔曼滤波算法对检测的加速度、角速度、温度进行滤波处理。
13.根据权利要求8至10任一项所述的机械臂三维姿态的检测系统，其特征在于，计算单元建立g = f(t) Xgx函数关系式，并根据三轴陀螺仪检测的角速度获取不同时刻的旋转角度，由旋转角度和矩阵函数f(t)的对应关系，获取f(t)，根据f(t)和&获取8;并按照下述公式V(k+1) =v (k) +T [g (k+1) +g (k) ] /2 ；P (k+1) =p(k)+T[v(k+l)+v(k)]/2 ;计算获得机械臂的空间位置，以获取机械臂的三维姿态；其中，g为实际加速度、gx为检测加速度、f(t)为实时更新的矩阵函数、T为相邻两次检测的间隔时间、v(k+l)和v(k)为相邻两次检测的机械臂的速度、p(k+l)和p(k)为相邻两次检测的机械臂的空间位置、g(k+l)和g(k)为相邻两次检测机械臂的加速度。
14.一种控制机械臂运行的系统，其特征在于，包括机械臂三维姿态的检测系统，该检测系统为权利要求7至13任一项所述的检测系统，还包括人机界面和/或控制机械臂运行的控制器，检测系统将检测的机械臂三维姿态信息发送至人机界面和/或控制机械臂运行的控制器。
全文摘要
本发明公开一种机械臂三维姿态的检测方法和检测系统，公开的检测方法包括下述步骤A)实时检测机械臂的加速度和角速度；B)对加速度和角速度进行数据处理；C)通过处理后的加速度和角速度计算获得机械臂的三维姿态。该检测方法通过检测机械臂的加速度和角速度，即机械臂动态和静态测量结合，实现动静相互补偿，从而提高不同工况下机械臂三维姿态的检测精度。而且该种方法和系统不仅反映出机械臂线性位移对机械臂空间姿态的影响，同时考虑了旋转运动对空间姿态的影响，因此，可以较为准确地反映机械臂的三维姿态。本发明还公开一种控制机械臂运行的系统。
文档编号G01P15/00GK102608351SQ201210032210
公开日2012年7月25日 申请日期2012年2月14日 优先权日2012年2月14日
发明者李明, 杨栋, 罗家豪 申请人:三一重工股份有限公司