| JP2007152445 | SELF-PROPELLED ROBOT |
| JP2984218 | WORK SCHEDULING METHOD FOR PLURAL ROBOTS |
| JP2004330206 | ROBOT CONTROLLER |
易小刚 (中国湖南省长沙经济技术开发区三一工业城, Hunan 0, 410100, CN)
ZHOU, Xiang (Sany Industry Town, Economic and Technological Development ZoneChangsha, Hunan 0, 410100, CN)
周翔 (中国湖南省长沙经济技术开发区三一工业城, Hunan 0, 410100, CN)
湖南三一智能控制设备有限公司 (中国湖南省长沙经济技术开发区三一工业城, Hunan 0, 410100, CN)
SANY HEAVY INDUSTRY CO., LTD (Sany Industry Town, Economic and Technological Development ZoneChangsha, Hunan 0, 410100, CN)
三一重工股份有限公司 (中国湖南省长沙经济技术开发区三一工业城, Hunan 0, 410100, CN)
YI, Xiaogang (Sany Industry Town, Economic and Technological Development ZoneChangsha, Hunan 0, 410100, CN)
易小刚 (中国湖南省长沙经济技术开发区三一工业城, Hunan 0, 410100, CN)
| 权 利 要 求 1、 一种机械铰接臂控制方法， 其特征在于， 在铰接臂的不同位置上至 少设置两个倾角传感器， 该方法包括： 标定铰接臂未发生弹性变形时倾角传感器的零点位置,设铰接臂弹性 变形前、 后末端位置分别为 P点和 P，点， 在铰接臂上选取一 R点； 利用倾角传感器检测弹性变形前、 后铰接臂两个不同位置的角度值， 获得铰接臂因弹性变形产生的角度偏移量 , 结合 OR和 RP，的长度值， 计算铰接臂变形后的长度参数 和角度参数 ； 根据长度参数 和角度参数 得到 P'点的位置参数 χρ 和 ， 依据位 置参数 和 控制铰接臂动作。 2、 如权利要求 1所述的方法， 其特征在于， 在铰接臂不同位置上至少 设置两个倾角传感器为： 在每节铰接臂的首末两端各设有一个倾角传感器。 3、 如权利要求 2所述的方法， 其特征在于， 在铰接臂上选取一 R点 为： 将铰接臂弹性变形后弧线的中间位置设置为 R点。 4、 如权利要求 2所述的方法， 其特征在于， 在铰接臂上选取一 R点 为： 将铰接臂弹性变形后距离等效刚性铰接臂最远点。 5、 如权利要求 2、 3或 4所述的方法， 其特征在于， 利用倾角传感器 检测弹性变形前、 后铰接臂两个不同位置的角度值， 获得铰接臂因弹性变 形产生的角度偏移量△ 包括以下步骤： 设铰接臂弹性变形前首、 末端的倾角传感器检测的角度值分别为 。和 。， 因倾角传感器安装位置不与铰接臂轴线方向平行导致的角度差为 Δ ， 则： Δ(¾ = ο _ 。 设铰接臂首端位置为 0点，利用倾角传感器分别检测 0点的角度值与 P，点的角度值， 分别计作 与 , 计算角度偏移量 △ = H △ 。 6、 如权利要求 5所述的方法， 其特征在于， 计算铰接臂变形后的长度 数^。和角度参数 包括以下步骤： 设 ZROP，和 Z RP' O的角度值分别为 Z i和 Z2, 则有; ΑΘ = Ζ Ι + Ζ 2; 根据三角定理得到： θη = θ„、 - Zl RP sinA(9 L. OP 由此获得铰接臂的角度 与长度 sin Z2 7、 如权利要求 6所述的方法， 其特征在于， 根据长度参数 L。和角度参 数 得到 P，点的位置参数 Xp 和 具体为： [Yp = La sm 0a 8、 一种机械铰接臂控制装置， 包括铰接臂， 其特征在于， 还包括设置 在铰接臂不同位置上的至少两个倾角传感器， 与所述倾角传感器连接的控 制器； 所述控制器包括计算单元和驱动单元： 所述计算单元， 用于标定铰接臂未发生弹性变形时倾角传感器的零点 位置,设铰接臂弹性变形前、后末端位置分别为 P点和 P，点， 在铰接臂上选 取一 R点； 利用倾角传感器检测弹性变形前、 后铰接臂两个不同位置的角 度值， 获得铰接臂因弹性变形产生的角度偏移量 Δ 结合 OR和 RP，的长 度值， 计算铰接臂变形后的长度参数 和角度参数 ； 根据长度参数 和 角度参数 得到 P，点的位置参数 XP 和 依据位置参数 XP 和 计算驱 动量； 所述驱动单元， 用于依据驱动量控制铰接臂动作。 9、 如权利要求 8所述的装置， 其特征在于， 在铰接臂不同位置上至少 设置两个倾角传感器为： 在每节铰接臂的首末两端各设有一个倾角传感器。 10、 如权利要求 8或 9所述的装置， 其特征在于， 在铰接臂上选取一 R点为： 将铰接臂弹性变形后弧线的中间位置设置为 R点。 11、 如权利要求 8或 9所述的装置， 其特征在于， 在铰接臂上选取一 R点为： 将铰接臂弹性变形后距离等效刚性铰接臂最远点。 |
技术领域 本发明涉机械铰接臂定位领域， 特别是涉及一种机械铰接臂的控制方 法控制及控制装置。 背景技术
众所周知， 机械臂系统的传统控制方式是采用人工操作的 手动模式， 通过操作手操作不同的手柄以实现机械臂的多 个关节协调运动， 以达到目 标位置， 但该方式存在多种的缺陷。 例如， 人工操作时， 特别是在需要多 节机械臂联动协调的情况下， 人工操作的效率与精准性往往比较低； 人工 操作对操作人员的操作的熟练程度要求比较高 ， 且在操作过程中要求操作 人员时刻关注铰接臂的移动情况， 稍有不慎就有将施工人员甩下楼层的危 险， 因此存在安全性差， 劳动强度大的缺陷。 随着社会与科学日新月异的进步，如何为劳动 者提供更好的工作环境， 如何最大程度的减轻劳动者的劳动强度， 一直是机械臂系统研发工作的焦 点与热点。 1993年普茨迈斯特公司在 US5640996A专利中提出通过遥控器 手柄的单个调节实现多节机械臂的协调运动， 使各机械臂能互不影响、 相 互独立的进行伸缩、 旋转和升降运动， 并在 1994年申请的 US5823218专 利首次揭示了末端软管随动功能， 即通过由操作人员引导末端软管移动至 混凝土浇注点， 实现机械臂随动功能。 尽管国内外一直都在尝试实现机械臂的机器人 化，然而截至目前为止， 其应用效果仍然未能充分满足工程化的要求， 其实现的难点主要体现在控 制的精准性方面。 参见图 1 , 示出两节铰接臂筒图， 由图可知， 二轴机械臂可绕关节 01 和 02旋转， 已知 01、 02长度为 11 , 02 A长度为 12 , 由几何分析法， 根据各关节转角 、 可建立机械臂的运动方程：
JC = cos (¾ + / 2 cos((¾ + θ 2 ) 式 1
_ = sin θ ι + 1 2 ήη(θ ι + θ 2 ) 式 2 其中， （x, y )为末端 A的坐标。
进一步， 在机械臂控制及轨迹规划中， 往往需要在已知点要达^ 间位置的情况下， 求出各关节运动量， 以驱动各关节的运动， 使端点的位 置得到满足。 将式 1、 式 2化筒， 得: x 2 + y 2 = ll + 1\ + 21J 2 [cos θ 1 cos(0 1 + θ 2 )+ sin θ 1 ήη(θ 1 + θ 2 )]
■■Κ + l: + 2LL cos (9， 式 3
Θ Ί = arccos
2l i 」 式 4
θ、 = 式 5
这样， 在要到达的位置 Α(Χ,Υ) 已知情况下， 可得到所需的各关节转 角 、 θ 2 , 驱动液压油缸控制机械臂转动， 使得机械臂末端到达目标位置 Α(Χ,Υ)。但是，机械臂运动因其自身的柔性可 分解为刚性运动与柔性运动， 上述计算方式忽略了机械架的柔性， 则将导致最终的计算与规划结果与实 际情况存在较大的偏差， 影响控制精准性。 发明内容 本发明所要解决的技术问题是提供一种机械铰 接臂的控制方法， 该方 法增强对铰接臂姿态的可测性与可控性， 提高控制精度。 本发明一种机械铰接臂控制方法， 在铰接臂的不同位置上至少设置两 个倾角传感器， 该方法包括： 标定铰接臂未发生弹性变形时倾角传感器的 零点位置,设铰接臂弹性变形前、后末端位置 别为 P点和 P，点，在铰接臂 上选取一 R点； 利用倾角传感器检测弹性变形前、 后铰接臂两个不同位置 的角度值， 获得铰接臂因弹性变形产生的角度偏移量 Δ 结合 OR和 RP， 的长度值，计算铰接臂变形后的长度参数 1 ^和角度参数 ；根据长度参数 和角度参数 得到 P，点的位置参数 X P 和 依据位置参数 X P 和 以及 控制目标控制铰接臂动作。 优选的， 在铰接臂不同位置上至少设置两个倾角传感器 为： 在每节铰 接臂的首末两端各设有一个倾角传感器。 优选的， 在铰接臂上选取一 R点为： 将铰接臂弹性变形后弧线的中间 位置设置为 R点。 优选的， 在铰接臂上选取一 R点为： 将铰接臂弹性变形后距离等效刚 性铰接臂最远点。 优选的， 利用倾角传感器检测弹性变形前、 后铰接臂两个不同位置的 角度值， 获得铰接臂因弹性变形产生的角度偏移量 包括以下步骤： 设铰 接臂弹性变形前首、 末端的倾角传感器检测的角度值分别为 和 , 因倾 角传感器安装位置不与铰接臂轴线方向平行导 致的角度差为 Δ ， 贝^
Δ ( ¾ = 。 - 。 设铰接臂首端位置为 0点，利用倾角传感器分别检测 0点的角度值与 P'点的角度值， 分别计作 与 ^， 计算角度偏移量
Δ 6 * = ι _ ι _ Δ( ¾。 优选的， 计算铰接臂弹性变形后长度参数 L 。和角度参数 包括以下步 骤： 设 ZROP，和 ZRP'O的角度值分别为 Z1和 Z2, 则有； ΑΘ = Ζ\+Ζ2; 根据三角定理得到：
θ η = θ„、 - Zl
RP sinA(9
L. OP
由此获得铰接臂的角度 与长度 sin 2 优选的， 根据长度参数 和角度参数 得到 P'点的位置参数 和 ^ 具体为： 的控制装置， 该装置增强对铰接臂姿态 的可测性与可控性， 提高控制精度。 本发明一种机械铰接臂控制装置， 包括铰接臂， 还包括设置在铰接臂 不同位置上的至少两个倾角传感器， 与所述倾角传感器连接的控制器； 所 述控制器包括计算单元和驱动单元： 所述计算单元， 用于标定铰接臂未发 生弹性变形时倾角传感器的零点位置,设铰接 弹性变形前、后末端位置分 别为 P点和 P，点， 在铰接臂上选取一 R点； 利用倾角传感器检测弹性变 ¾ 前、 后铰接臂两个不同位置的角度值， 获得铰接臂因弹性变形产生的角度 偏移量 Δ 结合 OR和 RP'的长度值， 计算铰接臂变形后的长度参数 ^和 角度参数 ；根据长度参数 和角度参数 得到 P'点的位置参数 χ ρ 和 1 ^ , 依据位置参数 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， 的长度值， 计算铰接臂变形后的长度参数 和角度参数 ；
步骤 S204、根据长度参数 和角度参数 得到 P'点的位置参数 X P 和 。 步骤 S205、 依据 P，点的位置参数 χ ρ 和 控制铰接臂动作。 本发明充分考虑铰接臂在运动过程中的弹性变 形， 利用机械臂变形前 后的角度变化值进行推理与运算， 实现对铰接臂姿态的更加准确可靠的判 断， 增强对铰接臂的姿态的可测性与可控性， 提高控制精度。
下面以一节铰接臂为例， 详细说明本发明计算过程。 如图 3所示， 假 设铰接臂不变形的方向为 向量方向， 铰接臂变形后则沿 ORP弧线方向。 设定 W点为铰接臂变形后的弧线的中点位置（该中 位置到 0点与 P点的 直线距离相等）。 当然在铰接臂变形分布情况已知或可测的情况 下，也可将 R点近似设定为铰接臂上距离变形后的等效刚 铰接臂（0 P，段）距离最 远点（如下图 4所示）。 在 R点人为确定的情况下， 则 OR与 RP的长度也 是可测的， 使得折线 ORP可以更加近似的描述变形后的沿 ORP弧线伸展 的铰接臂的姿态。 更进一步， 为了更好的近似铰接臂的变形情况， 可以在铰接臂的不同 位置如: L/8, L/4, L/2处各设置一个采样点 M点 ,Ν点, R点， 则折线 OMNRP 可以更加近似的描述铰接臂 ΟΡ的变形情况， 其实质是一种折线近似曲线 的办法， 其采样点数越多， 精确程度也会越大。 实际应用时， 采样点的个 数的选择一方面需要考虑采样器件的成本因素 ， 另一方面也需要考虑采样 器件的采样精度问题， 综合以上两方面的因素， 根据应用精度需求而选择 适合自己的方案。
由图 3可知，变形后的铰接臂 可等效刚性铰接臂 若可求出 的长度与角度参数， 则可将柔性铰接臂的问题转化为刚性铰接臂的 正运动 与逆运动的求解问题。 如下介绍如何利用在机械铰接臂的至少两个不 同的 位置， 安装至少两个倾角传感器所测量到的角度差值 ， 计算铰接臂变形后 的精确坐标参数。 在铰接臂仅存在微小变形时， 倾角传感器的优选安装位置为铰接臂的 首端与末端， 如图 3中的 0位置与 B位置（铰接臂不发生变形时的位置） 或 B，位置（铰接臂发生弹性变形后的位置）， 其中的倾角传感器所在的 B 位置坐标已知， 该坐标参数可以用 OP的长度（即铰接臂的长度参数）及 OP的角度参数加以描述， 忽略铰接臂铰接点的变形， 可近似认为 OP的角 度参数即为 A点倾角传感器 A测量出的角度值 。 （见图 5 )。 若将铰接臂 OP置于一个具备完备支撑结构的支架上， 使得铰接臂处 于未发生弹性变形的姿态， 则可认为 OP的角度参数同样可以用 B点的倾 角传感器 B测量出的角度值描述 。， 此时， 当倾角传感器 A与倾角传感 器 B的安装位置与铰接臂轴线方向完全平衡时， 则存在如下关系：
。 = e bo 式 6 由于实际应用时倾角传感器 A的安装位置与倾角传感器 B 的安装位 置， 可能存在不完全平行于铰接臂的轴线方向， 由此可能导致两个倾角传 感器的测量值存在有起始偏差， 即 o≠ ^bo 式 7 将铰接臂未发生弹性变形的情况下倾角传感器 A和倾角传感器 B的测 量值记为零点位置值， 分别记忆为 。, 。 令： ¾0 = θ α0 + Δ( ¾ 式 8 如图 4所示， 铰接臂发生变形后， 倾角传感器 Α测得其相对零点位置 的角度值为 倾角传感器 B测得的其相对零点的角度值为 1 , 铰接臂变 形后的偏移角度值为△ 综上所述有如下关系成立。
Θ = Θ -Θ Λ -Ι Θ。 式 9 由图 4可知， 在 R点选择满足 OR可近似为铰接臂弹性变形前方向的 条件下， 对于 Δ ^Ρ'有如下关系成立
ΑΘ = ΖΙ + Ζ2 式 10 忽略铰接臂变形时铰接臂的长度参数的变化， 假设已知铰接臂长 \° P \ = L , 则有：
OR + RP =L
式 11 由三角定理可知道
式 12
式 13
式 14 sin 2
展 开后： 式 15 A(9) sin 2/sinA(9
式 16 则有： cos 2 = csin 2 式 17 cos Z2 + sin Z2
Ζ1 = ΑΘ-Ζ2
RP sinA(9
OP =
综上可得: sin Z2 式 18 由此可以获得铰接臂的角度 ( ）与长度 ( L 。 )参数为 θ η =θ„、 -Zl
RP sinA(9
L. OP
sin Z2
铰接臂形变后末端的位置为：
[Υ ρ =L a sm0 a
这样， 就可以利用铰接臂形变后末端位置控制铰接臂 动作， 该控 制方式充分考虑了铰接臂的弹性变形， 提高控制的精确性。 存在两节铰接臂或者两节以上铰接臂时， 只须将与前面一节铰接 臂的铰接点看作是上述坐标的 0点， 则其求解方法可依照如上所述式
7到式 18的方法执行。 由此计算出的铰接臂变形后末端的坐标参数为 ：
θ 6
基于上述机械铰接臂控制方法， 本发明还提供一种机械铰接臂控制装 置， 见图 7, 包括铰接臂 11 , 还包括设置在铰接臂 11上的倾角传感器 12 和倾角传感器 13 ,与倾角传感器 12和倾角传感器 13的控制器 14,控制器 14包括计算单元 141和驱动单元 142。 计算单元 141所述计算单元， 用于标定铰接臂未发生弹性变形时倾角 传感器的零点位置,设铰接臂弹性变形前、 后末端位置分别为 Ρ点和 Ρ，点， 在铰接臂上选取一 R点； 利用倾角传感器检测弹性变形前、 后铰接臂两个 不同位置的角度值， 获得铰接臂因弹性变形产生的角度偏移量△ 结合
OR和 RP，的长度值，计算铰接臂变形后的长度参数 和角度参数 ； 根据 长度参数 和角度参数 得到 P，点的位置参数 χ ρ 和 ， 依据位置参数 χ ρ 和 计算驱动量； 驱动单元 142依据位置参数 χ ρ 和 控制铰接臂 11动作。 以上对本发明所提供的机械铰接臂的控制方法 和控制装置， 进行了详 以上实施例的说明只是用于帮助理解本发明的 方法及其核心思想； 同时， 对于本领域的一般技术人员， 依据本发明的思想， 在具体实施方式及应用 范围上均会有改变之处， 综上所述， 本说明书内容不应理解为对本发明的 限制
Next Patent: COMPOSITE STRUCTURE AND METHOD OF PREPARING THE SAME