专利名称：一种机械铰接臂的控制方法及控制装置的制作方法
技术领域：
本发明涉机械铰接臂定位领域，特别是涉及一种机械铰接臂的控制方法控制及控 制装置。
背景技术：
众所周知，机械臂系统的传统控制方式是采用人工操作的手动模式，通过操作手 操作不同的手柄以实现机械臂的多个关节协调运动，以达到目标位置，但该方式存在多种 的缺陷。例如，人工操作时，特别是在需要多节机械臂联动协调的情况下，人工操作的效率 与精准性往往比较低；人工操作对操作人员的操作的熟练程度要求比较高，且在操作过程 中要求操作人员时刻关注铰接臂的移动情况，稍有不慎就有将施工人员甩下楼层的危险， 因此存在安全性差，劳动强度大的缺陷。随着社会与科学日新月异的进步，如何为劳动者提供更好的工作环境，如何最大 程度的减轻劳动者的劳动强度，一直是机械臂系统研发工作的焦点与热点。1993年普茨 迈斯特公司在US5640996A专利中提出通过遥控器手柄的单个调节实现多节机械臂的协调 运动，使各机械臂能互不影响、相互独立的进行伸缩、旋转和升降运动，并在1994年申请的 US5823218专利首次揭示了末端软管随动功能，即通过由操作人员引导末端软管移动至混 凝土浇注点，实现机械臂随动功能。尽管国内外一直都在尝试实现机械臂的机器人化，然而截至目前为止，其应用效 果仍然未能充分满足工程化的要求，其实现的难点主要体现在控制的精准性方面。参见图1，示出两节铰接臂简图，由图可知，二轴机械臂可绕关节O1和O2旋转，已 知Op O2长度为I1, O2A长度为12，由几何分析法，根据各关节转角θ ρ θ 2可建立机械臂的 运动方程χ = I1Cos θ ^l2COS ( θ j+ θ 2) 式 1y = I1Sin θ Jl2Sin ( θ 广 θ 2) 式 2其中，(X，y)为末端A的坐标。进一步，在机械臂控制及轨迹规划中，往往需要在已知点要达到的空间位置的情 况下，求出各关节运动量，以驱动各关节的运动，使端点的位置得到满足。将式1、式2化简，得X2+^2 =Z12+ Il + H1I2[cosθλ COSp1 +^2)+ sinθχ sin(^ + B1)]= Z12 + Il + 2/, /2 cos 6>2式 3
(χ2+γ2_12_12λΘ = arccos -———-——-式 4
2I 2/Λ J工、
^ γλΓΧ2+ 2+/2_/2^θλ 二 arctan — - arccos - , 1 -式 5
UJ L Vx2+/这样，在要到达的位置Α(Χ，Y)已知情况下，可得到所需的各关节转角θ。θ2，驱动液压油缸控制机械臂转动，使得机械臂末端到达目标位置A(X，Y)。但是，机械臂运动因 其自身的柔性可分解为刚性运动与柔性运动，上述计算方式忽略了机械架的柔性，则将导 致最终的计算与规划结果与实际情况存在较大的偏差，影响控制精准性。
发明内容
本发明所要解决的技术问题是提供一种机械铰接臂的控制方法，该方法增强对铰 接臂姿态的可测性与可控性，提高控制精度。本发明一种机械铰接臂控制方法，在铰接臂的不同位置上至少 设置两个倾角传感 器，该方法包括标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接臂弹性变形 前、后末端位置分别为P点和P’点，在铰接臂上选取一 R点；利用倾角传感器检测弹性变形 前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量△ θ，结合 OR和RP’的长度值，计算铰接臂变形后的长度参数La和角度参数θ a;根据长度参数La和 角度参数θ a得到P’点的位置参数X'ρ，依据位置参数X'ρ以及控制目 标控制铰接臂动作。优选的，在铰接臂不同位置上至少设置两个倾角传感器为在每节铰接臂的首末 两端各设有一个倾角传感器。优选的，在铰接臂上选取一 R点为将铰接臂弹性变形后弧线的中间位置设置为R
点ο优选的，在铰接臂上选取一 R点为将铰接臂弹性变形后距离等效刚性铰接臂最 远点。优选的，利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得 铰接臂因弹性变形产生的角度偏移量△ θ包括以下步骤设铰接臂弹性变形前首、末端的 倾角传感器检测的角度值分别为Θ』和θ Μ，因倾角传感器安装位置不与铰接臂轴线方向 平行导致的角度差为Δ θ ^，则Δ θ 0 = θ b0- θ a0设铰接臂首端位置为0点，利用倾角传感器分别检测0点的角度值与P’点的角度 值，分别计作9al与θ ω，计算角度偏移量Δ θ Δ θ = Θω-Θ3 -Δ θ0Ο优选的，计算铰接臂弹性变形后长度参数La和角度参数θ a包括以下步骤设Z R0P，和Z RP，0的角度值分别为Z 1和Z 2，则有；Δ θ =Z 1+ Z 2 ；根据三角定理得到
sin Zl _ sin Z2 _ sin(^ - Αθ)将 Δ θ =Ζ1+Ζ2 代入得Sin(幼-Z2) = ]^SinZ2 由此获得铰接臂的角度e a与长度La参数为 优选的，根据长度参数！^和角度参数ea得到p’点的位置参数X' 为
具体
Xp=Lacosda本发明还提供一种机械铰接臂的控制装置，该装置增强对铰接臂姿态的可测性与 可控性，提高控制精度。本发明一种机械铰接臂控制装置，包括铰接臂，还包括设置在铰接臂不同位置上 的至少两个倾角传感器，与所述倾角传感器连接的控制器；所述控制器包括计算单元和驱 动单元所述计算单元，用于标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接 臂弹性变形前、后末端位置分别为P点和P’点，在铰接臂上选取一 R点；利用倾角传感器检 测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移 量A 0，结合OR和RP’的长度值，计算铰接臂变形后的长度参数La和角度参数ea;根据长 度参数La和角度参数ea得到P’点的位置参数x'p，依据位置参数X'p 计算驱动量所述驱动单元，用于依据驱动量控制铰接臂动作。优选的，在铰接臂不同位置上至少设置两个倾角传感器为在每节铰接臂的首末 两端各设有一个倾角传感器。远点。
优选的，在铰接臂上选取一 R点为将铰接臂弹性变形后弧线的中间位置设置为R 优选的，在铰接臂上选取一 R点为将铰接臂弹性变形后距离等效刚性铰接臂最
与现有技术相比，本发明具有以下优点
本发明充分考虑铰接臂在运动过程中的弹性变形，利用机械臂变形前后的角度变 化值进行推理与运算，实现对铰接臂姿态的更加准确可靠的判断，增强对铰接臂的姿态的 可测性与可控性，提高控制精度。
图1为现有两节铰接臂示意图2为本发明机械铰接臂控制方法流程图3为本发明铰接臂弹性变形后第一示意图4为本发明铰接臂弹性变形后第二示意图5为本发明铰接臂弹性变形后第三示意图6为本发明五节铰接臂弹性变形后示意图7为本发明铰接臂控制装置示意图。
具体实施例方式为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式
对本发明作进一步详细的说明。本发明旨在提供一种通过在一个机械臂的不同位置(如首端与末端)安装两个 或两个以上的倾角传感器，然后利用倾角传感器测量出的角度值来获取机械臂的弹性变形 后姿态参数，通过数学运算获取弹性变形后的末端坐标位置参数，再根据该末端位置参数 控制机械动作。参见图2，示出本发明机械铰接臂控制方法，具体步骤如下。步骤S201、在机械铰接臂的首末两端各安装一个倾角传感器。当然，也可以根据机 械铰接臂的长度及材料特性确定倾角传感器的安装位置，例如在铰接臂的L/8，L/4，L/2处。步骤S202、标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接臂弹性 变形前、后末端位置分别为P点和P’点，在铰接臂上选取一 R点。零点位置是指将铰接臂 至于一个具备完备支撑结构的支架上，确保铰接臂处于没有弹性变形的姿态时铰接臂的位 置，该支撑结构可以是水平的，也可以是沿着铰接臂伸展方向倾斜的，只需要取保铰接臂可 以被完备支撑，没有发生弹性变形即可。步骤S203、利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获 得铰接臂因弹性变形产生的角度偏移量△ θ，结合OR和RP’的长度值，计算铰接臂变形后 的长度参数La和角度参数ea;步骤S204、根据长度参数La和角度参数θ a得到P’点的位置参数X'ρ。步骤S205、依据P’点的位置参数X'ρ控制铰接臂动作。本发明充分考虑铰接臂在运动过程中的弹性变形，利用机械臂变形前后的角度变 化值进行推理与运算，实现对铰接臂姿态的更加准确可靠的判断，增强对铰接臂的姿态的 可测性与可控性，提高控制精度。下面以一节铰接臂为例，详细说明本发明计算过程。如图3所示，假设铰接臂不变 形的方向为OP向量方向，铰接臂变形后则沿ORP弧线方向。设定R点为铰接臂变形后的弧 线的中点位置(该中点位置到0点与P点的直线距离相等)。当然在铰接臂变形分布情况 已知或可测的情况下，也可将R点近似设定为铰接臂上距离变形后的等效刚性铰接臂(0Ρ’ 段)距离最远点(如下图4所示)。在R点人为确定的情况下，则OR与RP的长度也是可测 的，使得折线ORP可以更加近似的描述变形后的沿ORP弧线伸展的铰接臂的姿态。更进一步，为了更好的近似铰接臂的变形情况，可以在铰接臂的不同位置如L/8， L/4, L/2处各设置一个采样点M点，N点，R点，则折线OMNRP可以更加近似的描述铰接臂OP 的变形情况，其实质是一种折线近似曲线的办法，其采样点数越多，精确程度也会越大。实 际应用时，采样点的个数的选择一方面需要考虑采样器件的成本因素，另一方面也需要考 虑采样器件的采样精度问题，综合以上两方面的因素，根据应用精度需求而选择适合自己 的方案。由图3可知，变形后的铰接臂ORP可等效刚性铰接臂OP'，若可求出OP'的长度 与角度参数，则可将柔性铰接臂的问题转化为刚性铰接臂的正运动与逆运动的求解问题。 如下介绍如何利用在机械铰接臂的至少两个不同的位置，安装至少两个倾角传感器所测量 到的角度差值，计算铰接臂变形后的精确坐标参数。
在铰接臂仅存在微小变形时，倾角传感器的优选安装位置为铰接臂的首端与末 端，如图3中的0位置与B位置(铰接臂不发生变形时的位置)或B’位置(铰接臂发生弹 性变形后的位置)，其中的倾角传感器所在的B位置坐标已知，该坐标参数可以用0P的长 度(即铰接臂的长度参数)及0P的角度参数加以描述，忽略铰接臂铰接点的变形，可近似 认为0P的角度参数即为A点倾角传感器A测量出的角度值e a(l (见图5)。若将铰接臂0P置于一个具备完备支撑结构的支架上，使得铰接臂处于未发生弹 性变形的姿态，则可认为0P的角度参数同样可以用B点的倾角传感器B测量出的角度值描 述eb(l，此时，当倾角传感器A与倾角传感器B的安装位置与铰接臂轴线方向完全平衡时， 则存在如下关系 θaO ≠ θbo
式6 由于实际应用时倾角传感器A的安装位置与倾角传感器B的安装位置，可能存在 不完全平行于铰接臂的轴线方向，由此可能导致两个倾角传感器的测量值存在有起始偏 差，即 θa0 ≠ θb0 式 7将铰接臂未发生弹性变形的情况下倾角传感器A和倾角传感器B的测量值记为零 点位置值，分别记忆为9令△ e0 = 0 b0- 0 a0即 0b0 = 0a0+△ eo
式8如图4所示，铰接臂发生变形后，倾角传感器A测得其相对零点位置的角度值为 0 al，倾角传感器B测得的其相对零点的角度值为ebl，铰接臂变形后的偏移角度值为A 0， 综上所述有如下关系成立。A e = 0bl-0al-A e0 式 9由图4可知，在R点选择满足OR可近似为铰接臂弹性变形前方向的条件下，对于 A0RP'有如下关系成立△ 0 =Z 1+ Z 2 式 10忽略铰接臂变形时铰接臂的长度参数的变化，假设已知铰接臂长|0P| =L，则有 0R| + |RP，| = L 式 11 由三角定理可知道
式12 即
将△θ =z 1+ Z 2代入上式可得
展开后 令 则有 又 综上可得 由此可以获得铰接臂的角度(e a)与长度(La)参数为 铰接臂形变后末端的位置为 这样，就可以利用铰接臂形变后末端位置控制铰接臂动作，该控制方式充分考虑 了铰接臂的弹性变形，提高控制的精确性。存在两节铰接臂或者两节以上铰接臂时，只须将与前面一节铰接臂的铰接点看作 是上述坐标的0点，则其求解方法可依照如上所述式7到式18的方法执行。如图6所示，以五节铰接臂的求解为例子，参照如上方法，可依次得出第二节铰接 臂2#，第三节铰接臂3#，第四节铰接臂4#，第五节铰接臂5#的角度和长度参数eb，e。，ed， G e，Lb LCJ Ldj Leo由此计算出的铰接臂变形后末端的坐标参数为 基于上述机械铰接臂控制方法，本发明还提供一种机械铰接臂控制装置，见图7， 包括铰接臂11，还包括设置在铰接臂11上的倾角传感器12和倾角传感器13，与倾角传感 器12和倾角传感器13的控制器14，控制器14包括计算单元141和驱动单元142。
计算单元141所述计算单元，用于标定铰接臂未发生弹性变形时倾角传感器的零 点位置，设铰接臂弹性变形前、后末端位置分别为P点和P’点，在铰接臂上选取一 R点；利 用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形 产生的角度偏移量Δ θ，结合OR和RP’的长度值，计算铰接臂变形后的长度参数La和角度 参数θ a;根据长度参数La和角度参数θ a得到P’点的位置参数X'ρ，依据位置参 数X' ρ计算驱动量；驱动单元142依据位置参数X'ρ控制铰接臂11动作。以上对本发明所提供的机械铰接臂的控制方法和控制装置，进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于 帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思 想，在具体实施方式
及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对 本发明的限制。
权利要求
一种机械铰接臂控制方法，其特征在于，在铰接臂的不同位置上至少设置两个倾角传感器，该方法包括标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接臂弹性变形前、后末端位置分别为P点和P’点，在铰接臂上选取一R点；利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量Δθ，结合OR和RP’的长度值，计算铰接臂变形后的长度参数La和角度参数θa；根据长度参数La和角度参数θa得到P’点的位置参数X′p和Y′p，依据位置参数X′p和Y′p控制铰接臂动作。
2.如权利要求1所述的方法，其特征在于，在铰接臂不同位置上至少设置两个倾角传 感器为在每节铰接臂的首末两端各设有一个倾角传感器。
3.如权利要求2所述的方法，其特征在于，在铰接臂上选取一R点为 将铰接臂弹性变形后弧线的中间位置设置为R点。
4.如权利要求2所述的方法，其特征在于，在铰接臂上选取一R点为 将铰接臂弹性变形后距离等效刚性铰接臂最远点。
5.如权利要求2、3或4所述的方法，其特征在于，利用倾角传感器检测弹性变形前、后 铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量△ e包括以下 步骤设铰接臂弹性变形前首、末端的倾角传感器检测的角度值分别为0a。和0b。，因倾角传 感器安装位置不与铰接臂轴线方向平行导致的角度差为a e^，则A 9 0 = 9 bo" 9 aO设铰接臂首端位置为0点，利用倾角传感器分别检测0点的角度值与P’点的角度值， 分别计作eal与0bl，计算角度偏移量A e A 0 = 0bl-0al-A e0。
6.如权利要求5所述的方法，其特征在于，计算铰接臂变形后的长度参数La和角度参 数9 a包括以下步骤设Z R0P，和Z RP，0的角度值分别为Z 1和Z 2，则有； A 6 =Z 1+ Z 2 ；根据三角定理得到 将 由此获得铰接臂的角度ea与长度La参数为
7.如权利要求6所述的方法，其特征在于，根据长度参数La和角度参数03得到？’点的位置参数X'p具体为
8.一种机械铰接臂控制装置，包括铰接臂，其特征在于，还包括设置在铰接臂不同位置 上的至少两个倾角传感器，与所述倾角传感器连接的控制器；所述控制器包括计算单元和 驱动单元所述计算单元，用于标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接臂 弹性变形前、后末端位置分别为P点和P’点，在铰接臂上选取一 R点；利用倾角传感器检测 弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量 A e，结合OR和rp’的长度值，计算铰接臂变形后的长度参数La和角度参数e a ；根据长度 参数La和角度参数ea得到P’点的位置参数x'p，依据位置参数X'5计算驱动量；所述驱动单元，用于依据驱动量控制铰接臂动作。
9.如权利要求8所述的装置，其特征在于，在铰接臂不同位置上至少设置两个倾角传 感器为在每节铰接臂的首末两端各设有一个倾角传感器。
10.如权利要求8或9所述的装置，其特征在于，在铰接臂上选取一R点为将铰接臂弹性变形后弧线的中间位置设置为R点。
11.如权利要求8或9所述的装置，其特征在于，在铰接臂上选取一R点为将铰接臂弹性变形后距离等效刚性铰接臂最远点。
全文摘要
本发明涉及一种机械铰接臂控制方法，在铰接臂的不同位置上至少设置两个倾角传感器，该方法包括标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接臂弹性变形前、后末端位置分别为P点和P’点，在铰接臂上选取一R点；利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量Δθ，结合OR和RP’的长度值，计算铰接臂变形后的长度参数La和角度参数θa；根据长度参数La和角度参数θa到P’点的位置参数X′p和Y′p，依据位置参数X′p和Y′p控制铰接臂动作。本发明还涉及一种机械铰接臂控制装置。本发明实现对铰接臂姿态的更加准确可靠的判断，增强对铰接臂的姿态的可测性与可控性，提高控制精度。
文档编号B25J13/00GK101870110SQ201010219928
公开日2010年10月27日 申请日期2010年7月1日 优先权日2010年7月1日
发明者周翔, 邓秋连, 陈安涛 申请人:三一重工股份有限公司