CN110039521B - 一种基于双rrrr构型的水下多功能协作型双机械臂 - Google Patents

Abstract

本发明公开了一种基于双RRRR构型的水下多功能协作型双机械臂，属于机器人水下作业领域，其各机械臂均包括肩关节、大臂关节、小臂关节、腕关节这四个运动关节和一个末端手爪，构成双RRRR构型水下作业机构，可根据不同工作环境，采用不同的捕获模式对水下动、静态目标物进行捕获回收。本发明的协作型双机械臂，双臂协同相较于单臂更加灵活，协作抓取动、静态目标物的能力更强；末端手爪通过丝杆、推杆进行传动，实现手爪开度的精确控制，同时减少手爪纵向运动行程，构型更为紧凑；可实现手爪直接抓取和环抱抓取双重作业功能，解决水下动态目标难以抓取回收的难题，且双臂的多种捕获方式扩展了可达工作区域，提高了水下作业能力和工作效率。

Description

一种基于双RRRR构型的水下多功能协作型双机械臂

技术领域

本发明属于机器人水下作业领域，更具体地，涉及一种基于双RRRR构型的水下多功能协作型双机械臂。

背景技术

无人水下潜航器是一类可以在水下执行特定作业任务的无人航行器，是人类开发海洋资源不可缺少的工具，具有广泛的应用前景和重要的研究价值。在绝大多数情况下，无人水下潜航器需要重复使用，这意味着它们在完成任务后必须能够被回收。因此，如何对无人水下潜航器实现快速、安全、可靠地回收已成为无人水下潜航器技术的一个研究重点。

一般而言，无人水下潜航器的回收方式可分为水面回收与水下回收两种。与水面回收方式相比，水下回收方式受海况影响小，速度快，更为隐蔽、灵活，已成为中小型无人水下潜航器回收技术的发展趋势。水下回收方式主要有牵引回收、机械臂回收、直接对接回收和附着母艇回收等四种。其中，牵引回收和机械臂回收均采用中介装置与无人水下潜航器对接，可避免无人水下潜航器与母艇发生碰撞，但是对中介装置的位姿控制要求很高。直接对接回收不需要中介装置，但是直接对接产生的碰撞可能损坏无人水下潜航器或者母艇。附着母艇回收可使无人水下潜航器直接降落并附着在母艇上，但这种回收方式需对母艇的外部结构做较大修改，相关技术的难度很高，目前尚处于构想阶段。

由于水下环境十分复杂，因此水下回收既要考虑水动力的作用又要考虑海流等不确定性因素的影响。此外，水下回收主要依赖的水声设备容易受到环境噪声的干扰，而图像处理设备则受到海水能见度低的限制而不具有通用性。这些问题导致了水下回收的难度大大增加，因此如何在复杂的水下环境中实现快速、安全、可靠地回收便成为迫切需要解决的难题。为此，在设计水下回收装置时一方面要具有良好的灵活性，以使水下机器人与母艇在保持足够距离的情况下实施安全回收，另一方面在面对不同的水下环境时，水下回收装置需要面对不同的捕获情况，因此需要多种捕获方式。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于双RRRR(关节式球面机器人)构型的水下多功能协作型双机械臂，通过双机械臂的机械、驱动、传动三个部分的结构设计，具备手爪直接抓取和环抱抓取双重作业功能，能够解决水下静、动态目标回收过程困难，灵活性较差，回收效率低下等问题。

为实现上述目的，按照本发明的一个方面，提供了一种基于双RRRR构型的水下多功能协作型双机械臂，用于搭载至水下潜航器械上执行环抱和抓取工作，每个机械臂均包括肩部分、大臂部分、小臂部分、腕部分和爪部分；

肩部分包括：底座、第一传动轴和肩关节电机；底座沿轴Z布置，第一传动轴沿X轴布置，且枢接于底座下部，第一传动轴一端与肩关节电机的输出轴对接；肩关节电机固定于底座上；底座用于与水下潜航器械固连；

大臂部分包括：短臂、第二传动轴、大臂关节电机和大臂；短臂沿Z轴布置，其上部与第一传动轴固连；第二传动轴沿Y轴布置，且枢接于短臂下部，第二传动轴与大臂关节电机的输出轴对接；大臂关节电机固定于短臂上；大臂沿Z轴布置，其上部与第二传动轴固连；

小臂部分包括：第三传动轴、小臂关节电机和小臂；第三传动轴沿Y轴布置，且枢接于大臂下部，第三传动轴与小臂关节电机的输出轴对接；小臂关节电机固定于大臂上；小臂沿Z轴布置，其上部与第三传动轴固连；

腕部分包括腕关节电机，腕关节电机沿Z轴布置，且固定于小臂内部；腕关节电机的输出轴伸出小臂用于固定爪部分，以带动爪部分旋转。

进一步地，腕部分还包括：轴承座、传动盖和箱体；轴承座固定于小臂下端；腕关节电机的输出轴下端穿过轴承座内部的轴承，且与传动盖固连；箱体固定于传动盖下表面,；

爪部分包括：支架、推杆、第一销轴、第二销轴、第三销轴、爪关节电机、丝杆、拨块、导套、爪部以及连杆；

支架沿Z轴固定于箱体下表面，其径向开设有腰型槽，轴向开设有连通腰型槽的通孔；导套设于支架的通孔内，固定推杆只能沿Z轴运动，推杆设于导套的通孔内，下端固连第一销轴，以带动第一销轴在腰型槽内上下移动；支架下端固连第二销轴，第二销轴与第一销轴平行；

丝杆平行于推杆且固定于箱体上，丝杆一端与爪关节电机的输出轴对接；爪关节电机固定于箱体上；拨块设于丝杆上作为动子，推杆上部与拨块固连；

两个爪部关于Z轴对称，均枢接于第二销轴上，每个爪部上关于Z轴对称有一个开孔，分别与两个第三销轴相连；两个连杆关于Z轴对称，一端共同枢接于第一销轴上，另一端分别枢接于两个第三销轴上，且与第二销轴的枢接位置错开。

进一步地，爪部分还包括滑套，滑套外表面与支架的通孔过盈配合，内表面与推杆滑动配合。

进一步地，丝杆为T型丝杆。

进一步地，肩关节电机、大臂关节电机、小臂关节电机、腕关节电机和爪关节电机均为水密电机。

进一步地，小臂部分还包括：隔垫、前端盖和第二法兰；前端盖连接小臂关节电机与第二法兰，第二法兰固定在小臂之上；隔垫位于第三传动轴与大臂中间。

进一步地，还包括控制器和加权机械臂关节运动距离最优程序模块；控制器分别连接肩关节电机、大臂关节电机、小臂关节电机和腕关节电机，以控制各关节电机的运动角度；设肩关节电机、大臂关节电机、小臂关节电机和腕关节电机的初始角度依次为θ₀₁、θ₀₂、θ₀₃、θ₀₄，角度权重依次为ω₁、ω₂、ω₃、ω₄，且ω₁>ω₂>ω₃>ω₄，使爪部分从初始位置运动到目标位置的各关节角度的运动学逆解有n组，记为[θ_k1，θ_k2，θ_k3，θ_k4]，k＝1～n，则所述控制器在调用所述加权机械臂关节运动距离最优程序模块时，执行如下步骤：

(1)计算第k组解的加权运动距离权和得：

(2)计算出所有的权和a_k后，再进行选小，即取mina_k为最优解的运动距离权和，其对应的四个关节角度，即为满足加权机械臂关节运动距离最优的解；

(3)控制器按照最优解对应的关节角度，控制各关节电机旋转，从而使爪部分从初始位置运动到目标位置。

总体而言，本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

1、现有技术水下一般为单臂抓取作业，本发明设计了协作型双机械臂，双臂协同相较于单臂更加灵活，协作抓取动、静态目标物的能力更强；本发明的协作型双机械臂具有双运动臂，每一单臂具有四个自由度，构成RRRR-RRRR型机构，可安装于水下无人潜航器，实现捕获、水下打捞等应用，双臂运动灵活，平衡性好、可自由旋转、运行可靠、省时省力。

2、本发明协作型双机械臂能够实现多种方式对于水下物体的捕获：当捕获静、动态小型目标物时，可采用双臂协同爪部捕获，控制电机旋转至某一角度，伸展双机械臂靠近目标物，闭合爪部从而对目标物进行捕获；或单臂抓取固定结构物作为支点，另外一臂完成复杂外界海流扰动下的精确捕获任务。当捕获静、动态大型目标物时，可采用双臂协同环抱捕获，控制电机旋转至某一角度，伸展机械臂环抱目标物，从而对目标物进行捕获。两种作业形态下的双臂协同抓取，解决了水下动态目标难以抓取回收的难题，提高了机械臂水下作业能力，且双臂的多种捕获方式扩展了其工作区域，提高了工作效率，从而为水下物体捕获工作提供了便捷和保障。

3、末端的爪部分通过丝杆、推杆、连杆、销轴部件，将电机的转动转化为推杆的平动，再通过连杆、销轴部件转化为爪部的转动，能够实现爪部开度的精确控制，同时也减少了手爪纵向运动行程，构型上更为紧凑。

4、本发明设计的双RRRR构型水下机械臂的机械构型独特，机械臂安装在水下无人潜航器(搭载本体)上可实现手爪直接抓取和环抱抓取双重作业功能，分别完成针对静态目标和动态目标这两种作业形态下的双臂协同抓取，从而解决水下动态目标难以抓取回收的难题，且双臂的多种捕获方式扩展了可达工作区域，提高了水下作业能力和工作效率。

附图说明

图1是协作型双机械臂结构示意图；

图2是本发明优选实施例的协同抓取工作示意图；

图3是本发明优选实施例的协同抱取工作示意图；

图4是本发明优选实施例的肩部分示意图；

图5是本发明本发明优选实施例的大臂部分示意图；

图6是本发明优选实施例的小臂部分示意图；

图7是本发明优选实施例的腕部电机示意图；

图8是本发明优选实施例的爪部及腕部组装示意图。

在所有附图中，相同的附图标记用来表示相同的元件或结构，其中：

1-爪部，2-第二销轴，3-第一销轴，4-第三销轴，5-连杆，6-支架，8-推杆，9-小拨块，10-导套，11-拨块，12-滑套，13-第一法兰，16-箱体，33-丝杆，34-传动盖，37-小盖，38-轴承座，40-前端盖，79-腕关节电机，82-第二法兰，83-小臂，84-大臂，85-隔垫，86-第三传动轴，115-肩关节电机，119-大臂关节电机，126-小臂关节电机，134-短臂，135-底座，144-电机盖，145-轴承盖，146-第一传动轴，147-垫板，149-第二传动轴。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，本发明优选实施例的一种基于RRRR-RRRR构型的水下多功能协作型双机械臂，每个单臂均包括机械部分、驱动部分和传动部分，所述机械部分、驱动部分、传动部分共同组成了协作型双机械臂的臂部。

机械部分由垫板147、底座135、短臂134、大臂84、小臂83、滑套12、箱体16、支架6、爪部1、电机盖144、轴承盖145、轴承座38组成，底座135通过第一传动轴146与短臂134相连，短臂134通过第二传动轴149连接大臂84的一端，大臂84通过第三传动轴86连接小臂83的一端，小臂83通过传动盖34连接箱体16的一端，箱体16另一端与支架6相连，支架6内部设置有推杆8，支架6另一端设置有转轴3与爪部1相连；

驱动部分由肩关节电机115、大臂关节电机119、小臂关节电机126、腕关节电机79、爪关节电机17组成，分别作用于第一传动轴146、第二传动轴149、第三传动轴86、传动盖34、T型丝杆33；

传动部分由第一传动轴146、第二传动轴149、第三传动轴86、传动盖34、拨块11、T型丝杆33、推杆8、转轴3、连杆5组成，分别作用于肩部、大臂部、小臂部、腕部、爪部。

通过控制各传动部分的实时状态，实现协作型双机械臂不同捕获模式的切换。

该协作型双机械臂的每个单臂可划分为肩部分、大臂部分、小臂部分、腕部分、爪部分五个部分。

如图1、4所示，所述肩部分包括：肩关节电机115、底座135、垫板147、轴承盖145、第一传动轴146、电机盖144、隔垫；垫板147一端与水下无人潜航器搭载物连接，另一端与底座135连接，轴承盖145、电机盖144安装在底座135两侧，肩关节电机115安装于底座135一侧，与第一传动轴146通过电机轴连接，传递电机转动。

如图1、5所示，所述大臂部分包括：大臂关节电机119、短臂134、轴承盖145、第二传动轴149、电机盖144、大臂84、隔垫；短臂134与第一传动轴146连接，大臂关节电机119和第二传动轴149安装在短臂134和大臂84交界圆处，通过电机轴连接，轴承盖145和电机盖144安装在短臂134两侧，隔垫安装于轴承和大臂84之间。

如图1、6所示，所述小臂部分包括：小臂关节电机126、小臂83、轴承盖145、第三传动轴86、电机盖144、隔垫；小臂关节电机126和第三传动轴86安装在小臂83与大臂84交界圆处，通过电机轴连接，轴承盖145和电机盖144安装在小臂83两侧，隔垫安装于轴承和小臂83之间。

如图1、7、8所示，所述腕部分包括：腕关节电机79、轴承座38、传动盖34、小盖37、箱体16；腕关节电机79安装在小臂83内部，与法兰相连接；传动盖34与腕关节电机79轴相连，传递扭矩；传动轴与轴承座38相连，同时轴承座38与箱体16相连；使得电机转动能够带动箱体16及爪部1一并发生转动；小盖37固定于箱体上，丝杆33穿过小盖37开孔，对丝杆33进行轴向固定。

如图1、7、8所示，所述爪部分包括：第一法兰13、爪关节电机17、滑套12、T型丝杆33、拨块11、小拨块9、推杆8、导套10、支架6、第一销轴3、连杆5、爪部1；第一法兰13连接爪关节电机17与箱体16，T型丝杆33与电机轴相连接，传递扭矩；爪关节电机17带动T型丝杆33旋转，带动拨块11沿螺纹向上下运动；小拨块9固定在拨块11一侧凹口处，另一侧凸出部分卡在推杆8凹口处，使得拨块11能够带动推杆8沿轴向运动；增加滑套12对推杆8进行限位，使其只能在支架6内部沿轴向进行运动；第一销轴3安装在推杆8另一侧，沿支架6开孔进行上下运动，带动其上连杆5转动；连杆5一端与爪部1相连，另一端同爪部1一角一同连接在第一销轴3上，连杆5发生运动时，自然带动爪部1产生张开和闭合运动。

本发明的协作型双机械臂通常搭载于本体上(例如水下无人潜航器)进行使用，以水下无人潜航器为例，通过垫板147将水下无人潜航器与底座135连接固定。

协作型双机械臂工作时，上位机控制各个电机工作；所述肩关节电机115连在底座135上，驱动底座135内传动轴旋转，带动短臂134及以下部分一同前后旋转；所述大臂关节电机119连接在短臂134和大臂84之间，驱动短臂134和大臂84之间传动轴旋转，带动大臂84及以下部分一同左右旋转；所述小臂关节电机126安装于小臂83和大臂84之间，驱动小臂83和大臂84之间传动轴旋转，带动小臂83及以下部分一同左右旋转；所述腕关节电机79安装于小臂83之内，驱动箱体16上传动盖34旋转，带动箱体16及以下部分发生旋转；所述爪关节电机17安装于箱体16上端一侧，驱动T型丝杆33产生向上运动，带动拨块11一同向上运动，拨块11通过小拨块9与推杆8凹槽处连接，推杆8向上运动，使得连杆5转动，爪部1闭合，对目标物进行捕获。

可根据被捕获目标物的实际情况，通过加权机械臂关节运动距离最优法选择控制各机械臂的运动轨迹，实现两种捕获方式的切换：

捕获方式1(协同抓取)：通过上位机控制协作型双机械臂转动至目标位置，通过双爪部的闭合对小型动态目标物进行捕获；

捕获方式2(协同抱取)：通过上位机控制肩、大臂、小臂、腕转动，爪部辅助锁紧，配合搭载本体对大型动态目标物环抱进行捕获。

其中，所述的加权机械臂关节运动距离最优法如下：

设肩关节电机115、大臂关节电机119、小臂关节电机126和腕关节电机79的初始角度依次为θ₀₁、θ₀₂、θ₀₃、θ₀₄，角度权重依次为ω₁、ω₂、ω₃、ω₄，且ω₁>ω₂>ω₃>ω₄，使爪部分从初始位置运动到目标位置的各关节角度的运动学逆解有n组，记为[θ_k1，θ_k2，θ_k3，θ_k4]，k＝1～n，则所述控制器在调用所述加权机械臂关节运动距离最优程序模块时，执行如下步骤：

(1)计算第k组解的加权运动距离权和得：

(2)计算出所有的权和a_k后，再进行选小，即取mina_k为最优解的运动距离权和，其对应的四个关节角度，即为满足加权机械臂关节运动距离最优的解；

(3)控制器按照最优解对应的关节角度，控制各关节电机旋转，从而使爪部分从初始位置运动到目标位置。

该方法按照关节运动的先后顺序设置权重大小，结合求权和并取小的方式，能够控制机械臂带动爪部分以最优的动作快速到达指定位置，从而提高工作效率、节约能源。

下面结合图2、图3对两种捕获方式进行介绍：

如图2所示，此新型协作型双机械臂捕获动态小型目标物时，可采用双臂协同爪部1捕获，双机械臂的肩关节电机115驱动大臂84前后转动一定角度，大臂关节电机119、小臂关节电机126驱动大臂84、小臂83处左右转动一定角度，腕关节电机79驱动机械爪绕自身轴心旋转一定角度，爪关节电机17带动T型丝杆33产生上下运动，T型丝杆33带动拨块11与推杆8沿支架6轴心发生上下运动，使得连杆5发生转动，带动机械爪分开或闭合，协作型双机械臂通过双爪部的闭合对动态目标物协同捕获；或单臂抓取固定结构物作为支点，另外一臂完成复杂外界海流扰动下的精确捕获任务。

如图3所示，此新型协作型双机械臂捕获动态大型目标物时，可采用双臂协同环抱捕获，双机械臂的肩关节电机115驱动大臂84前后转动一定角度，大臂关节电机119驱动大臂84左右正向转动一定角度，小臂关节电机126驱动小臂83左右反向转动一定角度，直至夹紧住目标物；腕关节电机79驱动机械爪转动直至固定住目标物难以滑脱，使得协作型双机械臂通过大臂84、小臂83转动紧抱动态目标物进行协同捕获；

综上所述，本发明专利真正实现了一种结构紧凑，设计合理而工作稳定的水下双机械臂，双RRRR构型使得此双机械臂运动灵活，自由度高；手爪开度可精准控制是的此双机械臂精准控制爪部抓取力度；多种捕获方式使得此双机械臂能够适应各种不同水下情况对不同静、动态目标物的捕获需求，可以提高对水下目标物的回收能力和捕获的效率。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于双RRRR构型的水下多功能协作型双机械臂，用于搭载至水下潜航器械上执行环抱和抓取工作，每个机械臂均包括肩部分、大臂部分、小臂部分、腕部分和爪部分，其特征在于：

肩部分包括：底座(135)、第一传动轴(146)和肩关节电机(115)；底座(135)沿轴Z布置，第一传动轴(146)沿X轴布置，且枢接于底座(135)下部，第一传动轴(146)一端与肩关节电机(115)的输出轴对接；肩关节电机(115)固定于底座(135)上；底座(135)用于与水下潜航器械固连；

大臂部分包括：短臂(134)、第二传动轴(149)、大臂关节电机(119)和大臂(84)；短臂(134)沿Z轴布置，其上部与第一传动轴(146)固连；第二传动轴(149)沿Y轴布置，且枢接于短臂(134)下部，第二传动轴(149)与大臂关节电机(119)的输出轴对接；大臂关节电机(119)固定于短臂(134)上；大臂(84)沿Z轴布置，其上部与第二传动轴(149)固连；

小臂部分包括：第三传动轴(86)、小臂关节电机(126)和小臂(83)；第三传动轴(86)沿Y轴布置，且枢接于大臂(84)下部，第三传动轴(86)与小臂关节电机(126)的输出轴对接；小臂关节电机(126)固定于大臂(84)上；小臂(83)沿Z轴布置，其上部与第三传动轴(86)固连；

腕部分包括腕关节电机(79)，腕关节电机(79)沿Z轴布置，且固定于小臂(83)内部；腕关节电机(79)的输出轴伸出小臂(83)用于固定爪部分，以带动爪部分旋转。

2.根据权利要求1所述的一种基于双RRRR构型的水下多功能协作型双机械臂，其特征在于，

腕部分还包括：轴承座(38)、传动盖(34)和箱体(16)；轴承座(38)固定于小臂(83)下端；腕关节电机(79)的输出轴下端穿过轴承座(38)内部的轴承，且与传动盖(34)固连；箱体(16)固定于传动盖(34)下表面,；

爪部分包括：支架(6)、推杆(8)、第一销轴(3)、第二销轴(2)、第三销轴(4)、爪关节电机(17)、丝杆(33)、拨块(11)、导套(10)、爪部(1)以及连杆(5)；

支架(6)沿Z轴固定于箱体(16)下表面，其径向开设有腰型槽，轴向开设有连通腰型槽的通孔；导套(10)设于支架(6)的通孔内，固定推杆(8)只能沿Z轴运动，推杆(8)设于导套(10)的通孔内，下端固连第一销轴(3)，以带动第一销轴(3)在腰型槽内上下移动；支架(6)下端固连第二销轴(2)，第二销轴(2)与第一销轴(3)平行；

丝杆(33)平行于推杆(8)且固定于箱体(16)上，丝杆(33)一端与爪关节电机(17)的输出轴对接；爪关节电机(17)固定于箱体(16)上；拨块(11)设于丝杆(33)上作为动子，推杆(8)上部与拨块(11)固连；

两个爪部(1)关于Z轴对称，均枢接于第二销轴(2)上，每个爪部(1)上关于Z轴对称有一个开孔，分别与两个第三销轴(4)相连；两个连杆(5)关于Z轴对称，一端共同枢接于第一销轴(3)上，另一端分别枢接于两个第三销轴(4)上，且与第二销轴(2)的枢接位置错开。

3.根据权利要求2所述的一种基于双RRRR构型的水下多功能协作型双机械臂，其特征在于，爪部分还包括滑套(12)，滑套(12)外表面与支架(6)的通孔过盈配合，内表面与推杆(8)滑动配合。

4.根据权利要求2所述的一种基于双RRRR构型的水下多功能协作型双机械臂，其特征在于，丝杆(33)为T型丝杆。

5.根据权利要求2～4任意一项所述的一种基于双RRRR构型的水下多功能协作型双机械臂，其特征在于，肩关节电机(115)、大臂关节电机(119)、小臂关节电机(126)、腕关节电机(79)和爪关节电机(17)均为水密电机。

6.根据权利要求1所述的一种基于双RRRR构型的水下多功能协作型双机械臂，其特征在于，小臂部分还包括：隔垫(85)、前端盖(40)和第二法兰(82)；前端盖(40)连接小臂关节电机(126)与第二法兰(82)，第二法兰(82)固定在小臂(83)之上；隔垫(85)位于第三传动轴(86)与大臂(84)中间。

7.根据权利要求1所述的一种基于双RRRR构型的水下多功能协作型双机械臂，其特征在于，还包括控制器和加权机械臂关节运动距离最优程序模块；控制器分别连接肩关节电机(115)、大臂关节电机(119)、小臂关节电机(126)和腕关节电机(79)，以控制各关节电机的运动角度；设肩关节电机(115)、大臂关节电机(119)、小臂关节电机(126)和腕关节电机(79)的初始角度依次为θ₀₁、θ₀₂、θ₀₃、θ₀₄，角度权重依次为ω₁、ω₂、ω₃、ω₄，且ω₁>ω₂>ω₃>ω₄，使爪部分从初始位置运动到目标位置的各关节角度的运动学逆解有n组，记为[θ_k1，θ_k2，θ_k3，θ_k4]，k＝1～n，则所述控制器在调用所述加权机械臂关节运动距离最优程序模块时，执行如下步骤：

(1)计算第k组解的加权运动距离权和得：

(2)计算出所有的权和a_k后，再进行选小，即取mina_k为最优解的运动距离权和，其对应的四个关节角度，即为满足加权机械臂关节运动距离最优的解；

(3)控制器按照最优解对应的关节角度，控制各关节电机旋转，从而使爪部分从初始位置运动到目标位置。

Images