一种一体化双7自由度机械臂全向移动机器人系统与控制方法与流程

本发明属于机器人技术领域，更具体地，涉及一种一体化双7自由度机械臂全向移动机器人系统与控制方法。

背景技术：

随着人们对工业、医疗、服务水平要求的不断提高，迫切希望机器人能够更加的像人，具有双臂，并且能够和人协同作业。单个机械臂易受制于环境约束，一些任务，例如柔性物体的操作，单个机械臂难以完成，而采用2个独立的机械臂组成的机器人系统，由于不在同一个物理坐标系和控制系统空间下，协调难度较大，安装校准工作也非常耗时。同时，对于一些单个机械臂可以完成的工作，例如搬运形状不规则的刚性物体，则搬运难度较高，或者对机械臂末端的夹抓结构或负载能力要求较高，如果由双臂机器人完成，则可以降低搬运难度，并且效率更高。

一体化双臂机器人的左右臂不仅可以独立工作，对多目标进行操作与控制；也可以互相配合，例如螺帽与螺钉的装配，左臂抓取螺钉，右臂操作螺帽，左臂和右臂配合操作完成。因此，一体化双臂机器人更加灵巧，对环境的适应能力更强，也适用于人机协同作业，具有更好的应用价值。

同时，常见的机械臂具有6个自由度，在越来越多的人机协同作业场合下，无法满足更高的柔性和灵活性要求；具体地：6个自由度是具有完成空间定位(3个位置量、3个姿态角)能力的最小自由度数，而增加自由度可改善机器人相关的运动学和动力学特性。对于运动学特性方面，6个自由度机械臂可能存在奇异构型、关节位移超限以及工作环境中存在障碍三大问题，使得机械臂的运动受到较大的限制；对于动力学特性方面，6个自由度机械臂存在关节力矩的分配可能很不合理的情况；在容错性方面，若6个自由度机械臂中有一关节失效，则机械臂便无法继续完成工作。

另一方面，机械臂也需要结合全向移动运动底盘，进行任意方向的移动，以充分保证机械臂自主操作时的移动灵活性，提高人机协同作业环境的适应性。

技术实现要素：

针对现有技术的缺陷，本发明的目的在于提供一种一体化双7自由度机械臂全向移动机器人系统与控制方法，旨在解决现有技术中单个6自由度机械臂无法满足更高的柔性和灵活性的要求、无法保证具有良好环境适应性的技术问题。

本发明提供了一种一体化双7自由度机械臂全向移动机器人系统，包括：全向移动运动底盘、升降柱、双7自由度机械臂和控制模块；所述全向移动运动底盘用于实现一体化双7自由度机械臂全向移动机器人的任意方向的运动以及原地任意角度旋转；所述升降柱用于通过调节机器人高度来增大机械臂的运动空间；所述双7自由度机械臂包括：7自由度机械左臂和7自由度机械右臂，用于改善机器人运动学和动力学特性满足更高的柔性和灵活性要求；所述控制模块包括：第一控制单元和第二控制单元，所述第二控制单元用于实现所述全向移动运动底盘、所述升降柱和所述双7自由度机械臂的实时控制；所述第一控制单元用于实现所述全向移动运动底盘的路径规划以及所述双7自由度机械臂的路径规划的实时计算。

更进一步地，双7自由度机械臂全向移动机器人系统还包括：用于为全向移动运动底盘、升降柱、双7自由度机械臂和控制模块提供电源的电源模块。

更进一步地，所述全向移动运动底盘包括：4个无刷直流电机驱动器、4个无刷直流电机、4套无刷直流电机传动机构、4个麦克纳姆轮和1个底盘；所述无刷直流电机驱动器与无刷直流电机通过u相、v相、w相三线进行连接，无刷直流电机与无刷直流电机传动机构通过梅花联轴器进行连接，无刷直流电机传动机构与麦克纳姆轮之间采用法兰盘连接，直流电机传动机构使用电机支架，通过紧固螺钉固定在底盘上。

更进一步地，每套无刷直流电机传传动机构包括：梅花联轴器、轴承、连接轴、电机支架以及法兰盘部件；电机支架与底盘连接，麦克纳姆轮安装在法兰盘上，法兰盘与连接轴相连，连接轴通过轴承与电机支架固定，并与梅花联轴器的一端相连，梅花联轴器的另一端与无刷直流电机的轴相连，无刷直流电机通过紧固螺钉连接在电机支架上。

更进一步地，所述全向移动运动底盘长度为500mm～700mm，宽度为350mm～450mm；全向移动运动底盘轮子选用的是轮径为130mm～170mm的麦克纳姆轮。其中，麦克纳姆轮具有结构紧凑、承受负载能力强以及运动灵活等特点，可以保证该全向移动运动底盘具有较大的负载能力，又可以满足一体化双7自由度机械臂全向移动机器人系统灵活运动的要求。具有高度可变的机构，利用升降柱来调节机器人高度。进一步地，升降柱由步进电机驱动器、步进电机和滚珠丝杠机构组成。升降柱固定部分高450mm，可升降部分在滚珠丝杠的最低点时，升降柱高度为600mm，可升降行程为400mm，可升降部分在滚珠丝杠的最高点时，升降柱高度为1000mm。

更进一步地，7自由度机械左臂和所述7自由度机械右臂结构相同，均包括：夹爪、大臂连杆、小臂连杆、设置在机器人肩部的第一关节电机、第二关节电机和第三关节电机，设置在机器人肘部的第四关节电机和第五关节电机，以及设置在机器人腕部的第六关节电机和第七关节电机；所述第一关节电机的轴线垂直于地面，所述第二关节电机垂直于所述第一关节电机安装，且安装在所述第一关节电机的末端；所述第三关节电机连接于所述第二关节电机的末端，且垂直于所述第二关节电机安装，；所述第三关节电机连接有大臂连杆，大臂连杆轴与所述第三关节电机轴共线；大臂连杆末端连接所述第四关节电机，且保证所述第四关节电机的轴线与所述第三关节电机的轴线垂直；所述第五关节电机连接于所述第四关节电机的末端，轴线垂直于所述第四关节电机的轴线，小臂连杆连接于所述第五关节电机末端，且保证两者轴线共线；小臂连杆末端连接所述第六关节电机，且保证所述第六关节电机的轴线与所述第五关节电机的轴线垂直；所述第七关节电机连接于所述第六关节电机的末端，轴线垂直于所述第六关节电机的轴线。

更进一步地，第一关节电机的轴线与所述第三关节电机的轴线之间的距离是所述第二关节电机的减速器长度及与所述第三关节电机连接法兰厚度之和；所述第五关节电机的轴线与所述第三关节电机的轴线之间的距离是所述第四关节电机的减速器长度及与所述第五关节电机连接法兰厚度之和；所述第七关节电机轴线与所述第五关节电机轴线之间的距离是所述第六关节电机的减速器长度及与所述第七关节电机连接法兰厚度之和。

更进一步地，第一关节电机、所述第二关节电机和所述第三关节电机的连续扭矩均大于等于20n·m，所述第四关节电机和所述第五关节电机的连续扭矩范围均为5n·m～25n·m，所述第六关节电机和第七关节电机的连续扭矩均小于等于5n·m。

本发明还提供了一种基于上述的一体化双7自由度机械臂全向移动机器人系统的控制方法，包括下述步骤：

(1)通过7自由度机械臂逆运动学求解器并列运行kdl-rr算法和sqp-ss算法，将kdl-rr算法和sqp-ss算法中最快得到的结果作为求解器的结果；

(2)根据求解器的结果对机械臂路径进行规划：

采用基于采样的rrt算法在关节状态空间下，利用7自由度机械臂的冗余自由度，生成从机械臂起始位姿到目标位姿之间的可灵活避障的一系列中间过程路径点；

使用最小二乘法平滑多个路径点，再进行路径点之间的插补计算得到多个插补点；

结合关节电机的最大速度约束和最大加速度约束进行轨迹生成，得到与每个插补点对应的轨迹点，每个轨迹点包含轨迹时间和7个关节的角度、速度及加速度；

(3)根据规划的路径进行实时控制：

接收轨迹动作指令并同步对每个关节电机进行轨迹跟踪，进行双7自由度机械臂的实时运动控制，使得机械臂的运动过程更平滑且连续；

所述轨迹动作指令包含轨迹起始时间、一系列轨迹点以及容差值；容差值分为路径位置容差值、终点位置容差值以及终点时间容差值。

其中，在步骤(1)中，所述kdl-rr算法的循环中止条件为最大循环时间，当kdl-rr算法的循环时间小于最大循环时间时，利用rand函数生成关节向量的随机种子后继续运行kdl-rr算法；

所述sqp-ss算法的循环中止条件为最大循环次数，sqp-ss算法以起始点的位姿和目标点的位姿之间的笛卡尔误差向量的平方和最小为目标函数。

本发明中，7自由度机械臂相比6自由度机械臂，多了1个冗余自由度，可改善机器人的运动学和动力学特性。同时，7自由度机械臂对空间的某个点可以得到无穷多个解，因此具有更好的运动柔性，可以灵活避障，甚至可以对动态障碍物如运动的工作人员肢体等进行避让，以达到人机协同作业的目的，提高了环境适应性。

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

(1)本发明从系统机械结构、硬件组成、控制方法进行统一设计，具有同一个物理坐标系和控制系统空间下的可以移动和升降的双臂结构，每个机械臂具有冗余自由度，运动柔性好，控制方法采用分层控制策略，对全向移动运动底盘、升降柱、双7自由度机械臂进行控制，通过将上层(第一控制单元)路径规划与底层(第二控制单元)运动控制分布在两个平台上进行，实现了双7自由度机械臂全向移动机器人运动控制的实时性与准确性。同时结合全向移动运动底盘，可以进行任意方向的移动，保证了机器人自主操作的灵活性，以及人机协同作业环境的适应性。

(2)本发明的一体化双7自由度机械臂全向移动机器人系统机械结构按照负载与灵活运动要求，将双臂固定升降柱两侧，升降柱安装在全向移动运动底盘上，升降柱的高度可调，扩大了双7自由度机械臂的操作空间，系统具有结构紧凑、承受负载能力强、可全向移动以及运动灵活等特点，可以保证具有较大的负载能力，以及灵活运动的要求。

(3)本发明的一体化双臂机器人的左右臂不仅可以独立工作，对多目标进行操作与控制，也可以互相配合，使系统更加灵巧，对环境的适应能力更强，具有更好的应用价值。

(4)本发明的7自由度机械臂具有冗余自由度，相比6自由度机械臂具有更多的解空间，可以实现工作空间范围内的任意位置均可达且有无穷解，因此可以保证人机协同作业时，实现避障及人体安全防护等功能，具有更好的运动柔性、灵活性和适应性。

(5)本发明的7自由度机械臂的关节电机布局形式相对人类手臂结构进行了优化，将组成人类腕部的1个自由度关节电机调整到肘部，即将人类腕部的3个自由度关节电机调整为2个自由度关节电机，从而解决了腕部关节电机自重造成的机械臂末端负载能力降低的问题，保证了机械臂的负载及自重的分布均衡化，既可以满足7自由度机械臂的灵活运动要求，又可以保证其末端具有较大的负载能力。

(6)本发明的7自由度机械臂采用基于数值解的逆运动学求解方法。逆运动学求解包括两个解算线程，一个线程运行增强kdl算法即kdl-rr算法，另一个线程运行优化sqp即sqp-ss算法求逆算法，一旦其中的一个解算线程计算完成，两个线程立即同时停止，并返回已完成的那个解算线程的结果作为逆运动学求解的计算结果，保证了逆运动学求解的实时性。

(7)本发明的7自由度机械臂采用基于采样的rrt算法进行路径规划，得到从机械臂的起始点到目标点的一系列中间过程路径点，然后使用最小二乘法平滑多个路径点，进行插补计算得到轨迹点。每个轨迹点包含轨迹时间和各关节的角度、速度及加速度等运动参数信息，使得机械臂的运动过程更平滑且连续。

附图说明

图1为本发明实施例提供的一种一体化双7自由度机械臂全向移动机器人系统结构图；

图2为本发明实施例提供的全向移动运动底盘的闭环控制图；

图3为本发明实施例提供的全向移动运动底盘的运动模型图；

图4为本发明实施例提供的一种一体化双7自由度机械臂全向移动机器人系统外形示意图；

图5为本发明实施例提供的7自由度机械臂结构示意图；

图6为本发明实施例提供的7自由度机械臂控制方法流程图；

图7为本发明实施例提供的7自由度机械臂逆运动学求解算法流程图；

图8为本发明实施例提供的7自由度机械臂路径规划算法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明提供了一种一体化双7自由度机械臂全向移动机器人系统与控制方法，主要应用于分拣机器人、服务机器人、协作机器人等双7自由度机械臂全向移动机器人应用领域。

本发明中一体化双7自由度机械臂全向移动机器人系统与控制方法具有同一个物理坐标系和控制系统空间下的双臂结构，且每个机械臂具有冗余自由度，运动柔性好，同时结合全向移动运动底盘，可以进行任意方向的移动，保证了机器人自主操作的灵活性，以及人机协同作业环境的适应性。本发明从系统机械结构、硬件组成、控制方法进行全面设计，由此解决为实现上述目的，按照本发明的一个方面，提供了一种一体化双7自由度机械臂全向移动机器人系统与控制方法，其特征在于，系统包含：全向移动运动底盘、升降柱、双7自由度机械臂、控制模块、电源模块。全向移动运动底盘由4个无刷直流电机驱动器、4个无刷直流电机、4套无刷直流电机传动机构、4个麦克纳姆轮和1个底盘组成，其中，每套无刷直流电机传传动机构具体包括1个梅花联轴器、1个轴承、1根连接轴、1个电机支架以及1个法兰盘部件；升降柱由步进电机驱动器、步进电机和滚珠丝杠机构组成；双7自由度(dof)机械臂由左右两个机械臂组成，每个机械臂由7个关节、夹爪以及大臂连杆和小臂连杆组成，具体包括3个关节组成的肩部、2个关节组成的肘部、2个关节组成的腕部。

一体化双7自由度机械臂全向移动机器人控制方法采用分层结构，控制模块包括第二控制单元和第一控制单元。第二控制单元可以采用基于stm32f407微控制器的stm32控制板，进行全向移动运动底盘、升降柱、双7自由度机械臂的实时控制。stm32控制板与全向移动运动底盘的4个无刷直流电机驱动器连接，通信接口方式为can。stm32控制板与升降柱的步进电机驱动器连接，控制接口方式为io口。stm32控制板与双7自由度机械臂内部的各个关节驱动控制板连接，通信接口方式为ethercat或can或rs485。全向移动运动底盘的运动控制采用基于矩阵变换的逆运动学求解方法，根据逆运动学变换方程，将全向移动运动底盘的给定速度求解出4个麦克纳姆轮所需的转速，进而求解所需的4个无刷直流电机输出控制量，然后通过读取4个无刷直流电机转速，进行运动学变换，获得全向移动运动底盘实际速度，与给定速度进行比较，进行速度闭环控制。升降柱控制方法采用基于脉冲计数的控制方法，根据升降柱的目标位置求解出步进电机所需的转动圈数，进而求解所需的步进电机输出脉冲个数。双7自由度机械臂通过stm32控制板，接收来自第一控制单元的轨迹动作指令，轨迹动作指令包含轨迹起始时间、一系列轨迹点以及容差值。其中，每个轨迹点包含运动到该点的时间、各关节的位置、速度以及加速度，容差值分为路径位置容差值、终点位置容差值以及终点时间容差值。

第一控制单元进行全向移动运动底盘路径规划、双7自由度机械臂路径规划的实时计算。第一控制单元与第二控制单元的stm32控制板的通信接口方式为以太网或rs485或rs232，采用机器人操作系统(ros)的rosserial通信协议。首先第一控制单元与stm32控制板协商，确定需要发送和接收的topic消息，完成初始化；在通信过程中，第一控制单元与stm32控制板进行topic消息双向发送和接收。

具体地，第一控制单元通过rosserial通信协议将全向移动运动底盘控制指令、升降柱控制指令、以及双7自由度机械臂轨迹消息发送至stm32控制板；stm32控制板采集全向移动运动底盘、升降柱、双7自由度机械臂的位置和运动等状态参数，通过rosserial通信协议反馈给第一控制单元。

双7自由度机械臂控制方法包括基于数值解的逆运动学求解方法和基于rrt的路径规划算法。逆运动学求解方法使用trac-ik求解器，其组合了基于雅可比矩阵的优化迭代求逆算法和基于牛顿迭代法的优化sqp求逆算法两种解算器，通过选取时间上优先完成的解算器的解作为逆运动学的解；然后进行基于rrt算法的路径规划，生成从机械臂的起始点到目标点的一系列中间过程路径点，再进行路径点之间的插补计算，进行轨迹生成，对每个关节电机进行轨迹跟踪，实时控制双7自由度机械臂运动。

在本发明实施例中，全向移动运动底盘的机械结构按照负载与灵活运动要求，来设计全向移动运动底盘尺寸及4个轮子的选型，全向移动运动底盘长度为500mm～700mm，宽度为350mm～450mm。全向移动运动底盘轮子选用的是轮径为130mm～170mm的麦克纳姆轮。麦克纳姆轮具有结构紧凑、承受负载能力强以及运动灵活等特点，可以保证该全向移动运动底盘具有较大的负载能力，又可以满足一体化双7自由度机械臂全向移动机器人系统灵活运动的要求。

在本发明实施例中，全向移动运动底盘由4个无刷直流电机驱动器、4个无刷直流电机、4套无刷直流电机传动机构、4个麦克纳姆轮和1个底盘组成，其中，每套无刷直流电机传传动机构具体包括1个梅花联轴器、1个轴承、1根连接轴、1个电机支架以及1个法兰盘部件。具体地，电机支架与底盘连接，麦克纳姆轮安装在法兰盘上，法兰盘与连接轴相连，连接轴通过轴承与电机支架固定，并与梅花联轴器的一端相连，梅花联轴器的另一端与无刷直流电机的轴相连，无刷直流电机通过紧固螺钉连接在电机支架上。

在本发明实施例中，一体化双7自由度机械臂全向移动机器人系统具有高度可变的机构，利用升降柱来调节机器人高度。进一步地，升降柱由步进电机驱动器、步进电机和滚珠丝杠机构组成。升降柱固定部分高450mm，可升降部分在滚珠丝杠的最低点时，升降柱高度为600mm，可升降行程为400mm，可升降部分在滚珠丝杠的最高点时，升降柱高度为1000mm。

在本发明实施例中，双7自由度机械臂由左右两个机械臂组成，其机械结构通过参照正常成人手臂，来设计机械臂的大臂和小臂长度尺寸及各关节选型，每个机械臂包括夹爪、大臂连杆、小臂连杆和7个关节，每个机械臂在不包含夹爪时的总长度为600mm～700mm，在不包含夹爪时的总重量为6kg～7kg。机械臂各关节电机的布局及选型拟人化，将组成人类腕部的1个自由度关节电机调整到肘部，将人类腕部的3个自由度关节电机调整为2个自由度关节电机，选用较小扭矩的电机；肘部包含2个自由度关节电机，选用扭矩较大或中等的电机，提高机械臂末端的负载能力；肩部由3个关节电机组成，选用扭矩较大的电机。

在本发明实施例中，所述肩部关节电机的连续扭矩大于等于20n·m，所述肘部关节电机的连续扭矩范围为5n·m～25n·m，所述腕部关节电机的连续扭矩小于等于5n·m。

在本发明实施例中，单个7dof机械臂的相邻两关节电机轴均互相垂直，7个关节包括肩部第一关节电机1、肩部第二关节电机2、肩部第三关节电机3、第四关节电机4、第五关节电机5、第六关节电机6和第七六关节电机7，相邻两关节电机轴线垂直相交，其中，肩部第一关节电机1的轴线垂直于地面，肩部第二关节电机2垂直于肩部第一关节电机1安装，且安装在肩部第一关节电机1的末端；肩部第三关节电机3连接于肩部第二关节电机2的末端，垂直于肩部第二关节电机2安装，且保证肩部第一关节电机1的轴线与肩部第三关节电机3的轴线之间的距离是肩部第二关节电机2的减速器长度及与肩部第三关节电机3连接法兰厚度之和；肩部第三关节电机3连接有大臂连杆，大臂连杆轴与肩部第三关节电机3轴共线；大臂连杆末端连接第四关节电机4，且保证第四关节电机4的轴线与肩部第三关节电机3的轴线垂直；第五关节电机5连接于第四关节电机4的末端，轴线垂直于第四关节电机4的轴线，且保证第五关节电机5的轴线与肩部第三关节电机3的轴线之间的距离是第四关节电机4的减速器长度及与第五关节电机5连接法兰厚度之和；小臂连杆连接于第五关节电机5末端，且保证两者轴线共线；小臂连杆末端连接第六关节电机6，且保证第六关节电机6的轴线与第五关节电机5的轴线垂直；第七六关节电机7连接于第六关节电机6的末端，轴线垂直于第六关节电机6的轴线，且保证第七六关节电机7轴线与第五关节电机5轴线之间的距离是第六关节电机6的减速器长度及与第七六关节电机7连接法兰厚度之和。

在本发明实施例中，一体化双7自由度机械臂全向移动机器人控制方法采用分层结构，控制模块包括第二控制单元和第一控制单元。第二控制单元采用基于stm32f407微控制器的stm32控制板，进行全向移动运动底盘、升降柱、双7自由度机械臂的实时控制。stm32控制板与全向移动运动底盘的4个无刷直流电机驱动器连接，通信接口方式为can。stm32控制板与升降柱的步进电机驱动器连接，控制接口方式为io口。stm32控制板与双7自由度机械臂内部的各个关节驱动控制板连接，通信接口方式为ethercat、can或rs485。

其中，第一控制单元进行全向移动运动底盘路径规划、双7自由度机械臂路径规划的实时计算。第一控制单元与第二控制单元的stm32控制板的通信接口方式为以太网、rs485或rs232，采用机器人操作系统(ros)的rosserial通信协议。首先第一控制单元与stm32控制板协商，确定需要发送和接收的topic消息，完成初始化；在通信过程中，第一控制单元与stm32控制板进行topic消息双向发送和接收。具体地，第一控制单元通过rosserial通信协议将全向移动运动底盘控制指令、升降柱控制指令、以及双7自由度机械臂轨迹消息发送至stm32控制板；stm32控制板采集全向移动运动底盘、升降柱、双7自由度机械臂的位置和运动等状态参数，通过rosserial通信协议反馈给第一控制单元。

作为本发明的一个实施例，全向移动运动底盘的运动控制采用基于矩阵变换的逆运动学求解方法。具体地，通过全向移动运动底盘的运动学模型对底盘运动与各个轮子运动之间的关系进行分析，求解出全向移动运动底盘的运动学方程。利用矩阵变换方法将运动学方程转化为逆运动学方程。根据逆运动学变换方程，将全向移动运动底盘的给定速度求解出4个麦克纳姆轮所需的转速，进而求解所需的4个无刷直流电机输出控制量，然后通过读取4个无刷直流电机转速，进行运动学变换，获得全向移动运动底盘实际速度，与给定速度进行比较，进行速度闭环控制。

作为本发明的一个实施例升降柱控制方法采用基于脉冲计数的控制方法。具体地，利用步进电机位置与转动圈数换算公式，将升降柱的目标位置换算为步进电机所需的转动圈数，进而求解所需的步进电机输出脉冲个数。

作为本发明的一个实施例本发明中7自由度机械臂结构拟人化，不符合pieper准则，无法得到封闭解，因此采用基于数值解的逆运动学求解方法。具体地，第一控制单元根据机械臂左臂或右臂末端的起始点和目标点，采用基于数值解的逆运动学求解方法，使用trac-ik求解器，其组合了基于雅可比矩阵的优化迭代求逆算法和基于牛顿迭代法的优化sqp求逆算法两种解算器，通过选取时间上优先完成的解算器的解作为逆运动学的解，得到对应起始位姿和目标位姿下的机械臂的7个关节角度。

作为本发明的一个实施例trac-ik求解器包括并列运行的kdl-rr算法和sqp-ss算法，将kdl-rr算法和sqp-ss算法中最快得到的结果作为trac-ik求解器的结果。

其中，kdl-rr算法和sqp-ss算法都是基于数值解的逆运动学求解算法。

其中，kdl-rr算法的循环中止条件为最大循环时间，当kdl-rr算法的循环时间小于最大循环时间时，利用rand函数生成关节向量的随机种子后继续运行kdl-rr算法。

其中，sqp-ss算法的循环中止条件为最大循环次数，sqp-ss算法以起始点的位姿和目标点的位姿之间的笛卡尔误差向量的平方和最小为目标函数。

其中，机械臂路径规划算法采用基于采样的rrt算法，在关节状态空间下，利用7自由度机械臂的冗余自由度，生成从机械臂起始位姿到目标位姿之间的可灵活避障的一系列中间过程路径点，每个路径点包含7个关节角度；再进行路径点之间的插补计算，得到多个插补点，并结合关节电机的最大速度、最大加速度等约束，进行轨迹生成，得到与每个插补点对应的轨迹点，每个轨迹点包含轨迹时间和7个关节的角度、速度及加速度。

在本发明实施例中，第二控制单元接收来自第一控制单元的轨迹动作指令，同步对每个关节电机进行轨迹跟踪，进行双7自由度机械臂的实时运动控制，使得机械臂的运动过程更平滑且连续。其中，轨迹动作指令包含轨迹起始时间、一系列轨迹点以及容差值；每个轨迹点包含运动到该点的时间、各关节的位置、速度以及加速度，容差值分为路径位置容差值、终点位置容差值以及终点时间容差值。

如图1所示，一种一体化双7自由度机械臂全向移动机器人系统，包含：全向移动运动底盘、升降柱、双7自由度机械臂、控制模块、电源模块。其中，全向移动运动底盘可进行任意方向的运动、原地任意角度旋转，无转弯半径，可实现一体化双7自由度机械臂全向移动机器人的移动功能；

升降柱是具有高度可变的机构，可用来调节机器人高度，可增大机械臂的运动空间；双7自由度机械臂包含左、右2个7自由度机械臂，每个7自由度机械臂具有冗余自由度，可改善机器人运动学和动力学特性，满足更高的柔性和灵活性要求；控制模块，由第一控制单元和第二控制单元组成，第二控制单元进行全向移动运动底盘、升降柱、双7自由度机械臂的实时控制；第一控制单元进行全向移动运动底盘路径规划、双7自由度机械臂路径规划的实时计算。电源模块为全向移动运动底盘、升降柱、双7自由度机械臂、控制模块提供电源。

全向移动运动底盘由4个无刷直流电机驱动器、4个无刷直流电机、4套无刷直流电机传动机构、4个麦克纳姆轮和1个底盘组成，无刷直流电机驱动器与无刷直流电机通过u相、v相、w相三线进行连接，无刷直流电机与无刷直流电机传动机构通过梅花联轴器进行连接，无刷直流电机传动机构与麦克纳姆轮之间采用法兰盘连接，直流电机传动机构使用电机支架，通过紧固螺钉固定在底盘上。

其中，每套无刷直流电机传传动机构具体包括1个梅花联轴器、1个轴承、1根连接轴、1个电机支架以及1个法兰盘部件；电机支架与底盘连接，麦克纳姆轮安装在法兰盘上，法兰盘与连接轴相连，连接轴通过轴承与电机支架固定，并与梅花联轴器的一端相连，梅花联轴器的另一端与无刷直流电机的轴相连，无刷直流电机通过紧固螺钉连接在电机支架上。

升降柱由步进电机驱动器、步进电机和滚珠丝杠机构组成；步进电机驱动器与步进电机采用2相4线方式进行连接，步进电机与滚珠丝杆机构通过梅花联轴器进行连接。

双7自由度(dof)机械臂由左右两个机械臂组成，7dof机械臂的相邻两关节电机轴均互相垂直，夹爪与腕部第2个关节连接，腕部第1个关节与肘部第2个关节之间通过小臂连杆连接，肘部第1个关节与肩部第3个关节通过大臂连杆进行连接。

每个机械臂由7个关节、夹爪以及大臂连杆和小臂连杆组成，具体包括3个关节组成的肩部、2个关节组成的肘部、2个关节组成的腕部。

一体化双7自由度机械臂全向移动机器人控制方法采用分层结构，控制模块包括第二控制单元和第一控制单元。第二控制单元采用基于stm32f407微控制器的stm32控制板，进行全向移动运动底盘、升降柱、双7自由度机械臂的实时控制。stm32控制板与全向移动运动底盘的4个无刷直流电机驱动器连接，通信接口方式为can。stm32控制板与升降柱的步进电机驱动器连接，控制接口方式为io口。stm32控制板与双7自由度机械臂内部的各个关节驱动控制板连接，通信接口方式为ethercat、can或rs485。

控制模块由第一控制单元和第二控制单元组成，第二控制单元进行全向移动运动底盘、升降柱、双7自由度机械臂的实时控制；第一控制单元进行全向移动运动底盘路径规划、双7自由度机械臂路径规划的实时计算。

第二控制单元包括：全向移动运动底盘实时控制子模块、升降柱实时控制子模块、7自由度机械臂实时控制子模块；第一控制单元包括：全向移动运动底盘路径规划与实时通信子模块、升降柱实时通信子模块、7自由度机械臂路径规划与实时通信子模块。

具体地，全向移动运动底盘的运动控制采用基于矩阵变换的逆运动学求解方法，根据逆运动学变换方程，将全向移动运动底盘的给定速度求解出4个麦克纳姆轮所需的转速，进而求解所需的4个无刷直流电机输出控制量，然后通过读取4个无刷直流电机转速，进行运动学变换，获得全向移动运动底盘实际速度，与给定速度进行比较，进行速度闭环控制。

进一步地，图2为全向移动运动底盘的闭环控制图，给定速度包括全向移动运动底盘的三个速度变量：vrx，vry，wr(其中，vrx表示全向移动运动底盘的横向移动速度分量，vry表示全向移动运动底盘的纵向移动速度分量，wr表示全向移动运动底盘的转动角速度)，由第一控制单元发送给stm32控制板，stm32控制板接收后，根据逆运动学变换方程，得到4个无刷直流电机转速，然后经过速度调节器，得到4个无刷直流电机电流环的给定值，再经过电流调节器，闭环控制无刷直流电机。

进一步地，通过can总线通信接口，stm32控制板读取4个无刷直流电机转速，进行运动学变换，获得全向移动运动底盘实际的三个速度变量：vrx，vry，wr，与给定速度进行比较，进行速度闭环控制。

进一步地，图3为全向移动运动底盘的运动模型图，bi代表麦克纳姆轮，i＝{1，2，3，4}。每个麦克纳姆轮的轮缘上斜向分布着若干辊子，故麦克纳姆轮可以横向滑移，是一种全向运动轮，可以灵活方便的实现全向移动。全向移动运动底盘的运动速度用(vrx，vry，wr)表示，wbi表示麦克纳姆轮的角速度，wri表示麦克纳姆轮的辊子绕其轴线旋转的角速度，wzi表示麦克纳姆轮绕与垂直于地面的轴线的旋转角速度。αi表示辊子轴线与麦克纳姆轮的轴线之间的夹角，βi表示轮子中心与全向移动运动底盘中心的连线与x轴的夹角，la、lb分别表示轮子中心与全向移动运动底盘中心的横向距离和纵向距离，r和r分别代表麦克纳姆轮半径和辊子半径。

进一步地，麦克纳姆轮的αi均为45°，通过对麦克纳姆轮的运动学分析可以得到，全向移动运动底盘的运动方程如公式(1)所示：

进一步地，根据公式(1)，运动向量[vrx，vry，wr]仅与wbi有关，通过控制wbi即可控制全向移动运动底盘运动。

进一步地，全向移动运动底盘运动状态分为以下几种：

(1)纵向移动状态：当wb1＝wb2＝wb3＝wb4≠0时，vrx＝wr＝0，vry＝wbi·r，全向移动运动底盘纵向移动。

(2)横向移动状态：当wb1＝-wb2＝-wb3＝wb4≠0时，vrx＝wbi·r，vry＝wr＝0，全向移动运动底盘横向移动。

(3)旋转运动状态：当-wb1＝wb2＝-wb3＝wb4≠0时，vrx＝vry＝0，全向移动运动底盘绕中心点旋转。

(4)斜向移动状态：当wb1+wb3＝wb2+wb4＝w≠0，且wb1-wb2＝wb4-wb3＝δ≠0时，vrx≠0，vry≠0，wr＝0，全向移动运动底盘将沿斜向做直线运动，方向与y轴正方向夹角为

进一步地，通过矩阵变换，可以得到全向移动运动底盘的逆运动学变换方程，如公式(2)所示，通过全向移动运动底盘的给定速度，可以求解出4个轮子所需要的转速，即所需的电机输出控制量。

进一步地，stm32控制板与全向移动运动底盘的4个无刷直流电机驱动器通过can总线通信接口连接，通信波特率为1mbps。

进一步地，stm32控制板向无刷直流电机驱动器发送电机输出控制量，无刷直流电机驱动器向stm32控制板反馈无刷直流电机转速，实现闭环控制。

进一步地，4个无刷直流电机驱动器通过can报文，向stm32控制板反馈无刷直流电机转速数据，当stm32控制板的can控制器接收到转速数据时，触发can接收中断。stm32控制板进入中断后，利用接收到的can报文中的无刷直流电机转速，实现闭环控制。

具体地，升降柱控制方法采用基于脉冲计数的控制方法，根据升降柱的目标位置求解出步进电机所需的转动圈数，进而求解所需的步进电机输出脉冲个数。

双7自由度机械臂通过stm32控制板，接收来自第一控制单元的轨迹动作指令，轨迹动作指令包含轨迹起始时间、一系列轨迹点以及容差值。其中，每个轨迹点包含运动到该点的时间、各关节的位置、速度以及加速度，容差值分为路径位置容差值、终点位置容差值以及终点时间容差值。

第一控制单元进行全向移动运动底盘路径规划、双7自由度机械臂路径规划的实时计算。第一控制单元与第二控制单元的stm32控制板的通信接口方式为以太网、rs485或rs232，采用机器人操作系统(ros)的rosserial通信协议。

第一控制单元通过rosserial通信协议将全向移动运动底盘控制指令、升降柱控制指令、以及双7自由度机械臂轨迹消息发送至stm32控制板；stm32控制板采集全向移动运动底盘、升降柱、双7自由度机械臂的位置和运动等状态参数，通过rosserial通信协议反馈给第一控制单元。

在接收到第一控制单元的轨迹动作指令后，stm32控制板开始执行轨迹动作，并向第一控制单元不断反馈当前的轨迹动作指令的执行状态。反馈状态包含当前指令状态、实际关节位置、目标关节位置、关节位置误差。

具体地，在轨迹动作的执行过程中，第一控制单元可以向stm32控制板发送动作取消指令，以中止当前轨迹动作的执行。

具体地，在轨迹动作执行结束后，stm32控制板将向第一控制单元报告轨迹动作的最终执行结果，包括是否执行成功、执行耗时、终点位置误差。

具体地，轨迹动作的执行状态分为以下几种：

(1)空闲状态：尚未收到轨迹动作指令，机械臂保持原位，当收到轨迹动作指令后，切换到等待开始状态。

(2)等待开始状态：给定轨迹起始时间未到，机械臂保持原位，当到达轨迹起始时间时，切换到正在执行状态。

(3)正在执行状态：计算当前时间与轨迹开始时间之间的时间差，寻找一系列轨迹点中，与之对应的前后两个轨迹点，使用直线插值算法(或梯形等其它插值算法)，计算当前时刻对应的各关节的位置、速度与加速度，并将位置数据作为目标关节位置发送至各关节电机。此外，还将检查实际关节位置与目标关节位置之间的误差。若误差大于路径位置容差值，则立即停止机械臂运动，终止轨迹动作的执行，发送轨迹动作的最终执行结果并切换到空闲状态。若当前时刻晚于最后一个轨迹点，则切换到等待结束状态。

(4)等待结束状态：不断向各关节电机发送最后一个轨迹点对应的目标关节位置，并检查实际关节位置与目标关节位置之间的误差，若误差小于终点位置容差值，则记为轨迹动作执行成功结束，发送轨迹动作的最终执行结果并切换到空闲状态。若误差大于终点位置容差值并且当前时刻晚于终点时间容差值，则记为轨迹动作执行失败，同样发送轨迹动作的最终执行结果并切换到空闲状态。

(5)急停状态：当物理急停按钮被按下或软件急停标志被置位时，以最高优先级切换至急停状态，该状态下，将不断向所有关节电机发送停机指令，强制禁用力矩输出，当物理急停按钮弹起且软件急停标志被清除后，切换到空闲状态。

图4为本发明实施例提供的一种一体化双7自由度机械臂全向移动机器人系统外形示意图，全向移动运动底盘长度为600mm，宽度为400mm。全向移动运动底盘轮子选用的是轮径为152mm的麦克纳姆轮。麦克纳姆轮具有结构紧凑、承受负载能力强以及运动灵活等特点，可以保证该全向移动运动底盘具有较大的负载能力，又可以满足一体化双7自由度机械臂全向移动机器人系统灵活运动的要求。

具体地，全向移动运动底盘由4个无刷直流电机驱动器、4个无刷直流电机、4套无刷直流电机传动机构、4个麦克纳姆轮和1个底盘组成，其中，每套无刷直流电机传传动机构具体包括1个梅花联轴器、1个轴承、1根连接轴、1个电机支架以及1个法兰盘部件。具体地，电机支架与底盘连接，麦克纳姆轮安装在法兰盘上，法兰盘与连接轴相连，连接轴通过轴承与电机支架固定，并与梅花联轴器的一端相连，梅花联轴器的另一端与无刷直流电机的轴相连，无刷直流电机通过紧固螺钉连接在电机支架上。

具体地，升降柱由步进电机驱动器、步进电机和滚珠丝杠机构组成。升降柱固定部分高450mm，可升降部分在滚珠丝杠的最低点时，升降柱高度为600mm，可升降行程为400mm，可升降部分在滚珠丝杠的最高点时，升降柱高度为1000mm。

图5为7自由度机械臂结构示意图，图中虚线用于示意机械臂各关节的轴线，关节电机编号为1～7。其中，相邻两关节轴线垂直相交，如直线1与直线2等均垂直相交，但相邻三个关节轴未满足相交于一点或相互平行的要求。因此无法满足pieper准则，无法得到封闭解。

具体地，本发明的单个7dof机械臂的相邻两关节电机轴均互相垂直，7个关节包括肩部第一关节电机1、肩部第二关节电机2、肩部第三关节电机3、第四关节电机4、第五关节电机5、第六关节电机6和第七六关节电机7，相邻两关节电机轴线垂直相交，

所述肩部第一关节电机1的轴线垂直于地面，肩部第二关节电机2垂直于肩部第一关节电机1安装，且安装在肩部第一关节电机1的末端；肩部第三关节电机3连接于肩部第二关节电机2的末端，垂直于肩部第二关节电机2安装，且保证肩部第一关节电机1的轴线与肩部第三关节电机3的轴线之间的距离是肩部第二关节电机2的减速器长度及与肩部第三关节电机3连接法兰厚度之和；肩部第三关节电机3连接有大臂连杆，大臂连杆轴与肩部第三关节电机3轴共线；大臂连杆末端连接第四关节电机4，且保证第四关节电机4的轴线与肩部第三关节电机3的轴线垂直；第五关节电机5连接于第四关节电机4的末端，轴线垂直于第四关节电机4的轴线，且保证第五关节电机5的轴线与肩部第三关节电机3的轴线之间的距离是第四关节电机4的减速器长度及与第五关节电机5连接法兰厚度之和；小臂连杆连接于第五关节电机5末端，且保证两者轴线共线；小臂连杆末端连接第六关节电机6，且保证第六关节电机6的轴线与第五关节电机5的轴线垂直；第七六关节电机7连接于第六关节电机6的末端，轴线垂直于第六关节电机6的轴线，且保证第七六关节电机7轴线与第五关节电机5轴线之间的距离是第六关节电机6的减速器长度及与第七六关节电机7连接法兰厚度之和。

相邻两关节轴线垂直相交，但相邻三个关节轴未满足相交于一点或相互平行的要求。因此无法满足pieper准则，无法得到封闭解。

7自由度机械臂结构通过参照正常成人手臂，来设计机械臂的大臂和小臂长度尺寸及各关节选型，机械臂包括夹爪、大臂连杆、小臂连杆和7个关节，7个关节包括3个关节组成的肩部、2个关节组成的肘部、2个关节组成的腕部，关节电机布局形式相对人类手臂结构进行优化，将组成人类腕部的1个自由度关节电机调整到肘部，将人类腕部的3个自由度关节电机调整为2个自由度关节电机，肘部包含2个自由度关节电机，提高机械臂末端的负载能力；所述7自由度机械臂在不包含夹爪时的总长度为600mm～700mm，所述7自由度机械臂在不包含夹爪时的总重量为6kg～7kg。

具体地，本发明的单个7自由度机械臂总长700mm(不含夹爪)，总重量为7kg。其中，大臂长度为350mm，小臂长度为350mm。大臂是指关节轴2到关节轴4的距离，小臂是指关节轴4到关节轴6的距离。

具体地，关节7末端有法兰，法兰上面有定位孔，夹爪通过定位孔与关节7固定。

所述肩部的关节电机的连续扭矩大于等于20n·m，所述肘部的关节电机的连续扭矩范围为5n·m～25n·m，所述腕部的关节电机的连续扭矩小于等于5n·m。

如图5所示，各关节电机选型方案一：关节1/2/3/4选扭矩较大的电机，关节5选扭矩中等的电机，关节6/7选扭矩较小的电机。各电机参数如下：关节1/2/3/4的电机额定功率为70w，连续扭矩(指经减速器之后的连续扭矩，下同)为21.9n·m；关节5的电机额定功率为30w，连续扭矩为8n·m；关节6/7的电机额定功率为15w，连续扭矩为3n·m。

如图5所示，各关节电机选型方案二：关节1/2/3选扭矩较大的电机，关节4/5选扭矩中等的电机，关节6/7选扭矩较小的电机。各电机参数如下：关节1/2/3的电机额定功率为70w，连续扭矩为21.9n·m；关节4/5的电机额定功率为30w，连续扭矩为8n·m；关节6/7的电机额定功率为15w，连续扭矩为3n·m。7dof机械臂的相邻两关节电机轴均互相垂直，夹爪与腕部第2个关节连接，腕部第1个关节与肘部第2个关节之间通过小臂连杆连接，肘部第1个关节与肩部第3个关节通过大臂连杆进行连接。

这2种电机选型方案都可以保证7自由度机械臂末端的负载能力适合用于人机协作的场合。

图6所示为7自由度机械臂控制方法流程图，包括：

设置7自由度机械臂的起始点和目标点，采集7自由度机械臂在起始点的状态，采用trac-ik求解器，得到对应起始位姿和目标位姿下的7自由度机械臂的7个关节角度；在关节状态空间下，通过rrt算法进行路径规划得到起始位姿到目标位姿之间的一系列路径点，每个路径点包含7个关节角度；进行路径点之间的插补计算，得到多个插补点，并结合关节电机的最大速度、最大加速度等约束，进行轨迹生成，得到与每个插补点对应的轨迹点，每个轨迹点包含轨迹时间和7个关节的角度、速度及加速度。

利用一系列轨迹点，第二控制单元对每个关节电机进行轨迹跟踪，实时控制7自由度机械臂运动。

具体地，首先设置机械臂末端起始点的位姿(xs，ys，zs，rxs，rys，rzs)和目标点的位姿(xe，ye，ze，rxe，rye，rze)，通过trac-ik求解器，分别得到对应起始位姿和目标位姿下的7自由度机械臂的7个关节角度(a1s，a2s，a3s，a4s，a5s，a6s，a7s)和(a1e，a2e，a3e，a4e，a5e，a6e，a7e)；在关节空间下，通过rrt算法进行路径规划，得到从起始位姿到目标位姿之间的一系列路径点；再对这一系列路径点采用插补算法进行插补，并结合机械臂关节电机的最大速度、最大加速度等约束，进行轨迹生成，得到包含轨迹时间和7个关节的角度、速度及加速度的轨迹点。在第二控制单元利用路径规划生成的一系列的轨迹点，对每个关节电机进行轨迹跟踪，实时控制7自由度机械臂运动，使机械臂末端运动过程平滑且连续。

进一步地，xs，ys，zs代表机械臂末端起始点的空间位置，rxs，rys，rzs代表机械臂末端起始点的空间姿态；xe，ye，ze代表机械臂末端目标点的空间位置，rxe，rye，rze代表机械臂末端目标点的空间姿态。

图7所示为7自由度机械臂逆运动学求解算法流程图，本发明采用的逆运动学求解算法通过trac-ik(逆运动学，ik：inversekinematics)求解器，提高了计算的实时性。trac-ik求解器会创建两个解算线程，一个线程运行基于雅可比矩阵的优化迭代求逆算法：kdl-rr(kdlwithrandomrestarts)，一种增强kdl(机器人操作系统ros中的运动学和动力学库，kdl：kinematicsanddynamicslibrary)算法；另一个线程运行基于牛顿迭代法的优化序列二次规划算法：sqp-ss(sqpwithsumofsquares)，一种优化后的sqp(序列二次规划，sqp：sequentialquadraticprogramming)求逆算法。一旦其中的一个解算线程计算完成，两个线程会同时立即停止，并返回已完成的那个解算线程的结果作为trac-ik求解器的计算结果，因而求解效率更高。例如机械臂某个姿态下，利用增强kdl算法求解需要5ms，利用优化sqp求逆算法求解需要8ms，则将增强kdl算法求解的结果作为机械臂逆运动学求解结果。本发明的基于数值计算的方法，通过不断迭代，找寻最合适的解，具有运算快，稳定性好的特点。

进一步地，trac-ik求解器包含2种逆运动学求解算法，也可以解决当某种求解算法失败时，造成逆运动学无解的缺点，提高了求解成功率。

kdl-rr算法是对经典kdl算法进行优化，得到的增强型运动学求逆算法。经典kdl算法的主要公式如公式(3)所示：

qnext＝qprev+j^-1perr(3)

其中，qnext、qprev是后一状态、前一状态的关节向量，perr是机械臂当前位姿和目标位姿之间的笛卡尔误差向量，j^-1是雅可比矩阵的逆。

在kdl-rr算法中，主要对经典kdl进行了两点改进：1)解算器的循环中止条件将最大迭代次数改为最大循环时间；2)增加关节向量的随机种子，以防止出现局部最优解的情况。

如图7中的kdl-rr算法模块所示，其中初始化包括：1)初始的笛卡尔坐标系下的位姿；2)初始位姿和目标位姿之间的笛卡尔误差向量perr；3)雅可比矩阵的逆j^-1；初始化完成后开始循环迭代计算qnext；若机械臂各关节角度误差||dθ||小于设定误差阈值eps1，则将随机种子作为qnext，继续进行循环计算，以防止出现局部最优解的情况。

进一步地，为了使算法可控，设定循环时间上限，如果循环时间t超过最大循环时间tset，则中止循环，此时的qnext作为kdl的计算结果。

sqp算法是非线性约束优化问题中常用的方法，这种算法可以解决kdl系列算法在存在关节约束条件下求解失败的缺点，可提供更高的求解成功率。基本sqp算法的最优化问题如公式(4)所示：

其中，qseed是关节的n维初始值，不等式约束fi(q)是关节限制、欧几里德距离误差和角距离误差。

sqp-ss相比于基本sqp算法优化了sqp中的最小化目标函数，即将最小化平方和作为目标函数，如公式(5)所示：

如图7中的sqp-ss算法模块所示，初始化完成初始关节向量qseed及不等式约束fi(q)；再通过序列二次规划不断迭代循环；若机械臂末端位姿误差||perr||小于设定误差阈值eps2，则结束循环。

进一步地，为了使算法可控，设定循环次数上限，如果循环次数c达到设定值cset，则结束循环。

本实施例的sqp-ss算法相比基本sqp算法具有更好的运动学求逆效果。

图8所示为7自由度机械臂路径规划算法流程图。rrt(快速遍历随机树，或快速扩展随机树，rrt：rapidly-exploringrandomtree)是一种树形数据存储结构和算法，通过递增的方法建立，并快速减小随机选择点同树的距离。本发明采用基于采样的rrt路径规划算法，能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径。搜索时，通过对状态空间的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题，适合于解决多自由度机器人在复杂环境下和动态环境中的路径规划。

本发明的路径规划算法具体流程如图8所示，初始化过程包括：随机树t的初始化、起始点qorigin、目标点qgoal，初始化的随机数t只包含一个节点：根节点qinit。首先从状态空间中随机选择一个采样点qtarget；然后在随机树t中选择距离qtarget最近的节点qnearest；再通过从qnearest向qtarget扩展一段距离，得到一个新的节点qnew；如果qnew与障碍物发生碰撞，则放弃此次生长，等待下次迭代循环，否则将新节点qnew加入到随机树中；再判断随机树t中的子节点qnearest与目标点qgoal的距离，若距离小于设定的阈值，则退出循环，返回计算结果，否则继续循环。

进一步的，在上述算法中，当随机树的叶子节点进入了目标区域时，便可以在随机树中找到一条从根节点到叶子节点的路径，该路径中每个节点都包含关节空间下的每个关节电机对应的角度，然后在相邻节点之间进行插补计算，得到多个插补点，并结合关节电机的最大速度、最大加速度等约束，进行轨迹生成，得到与每个插补点对应的轨迹点，每个轨迹点包含轨迹时间和7个关节的角度、速度及加速度。

进一步地，为了使算法可控，设定搜索次数上限，如果在限制次数内无法到达目标点，则算法返回失败。

进一步地，在路径规划算法中，由于随机采样的缘故，这些路径点构成的线段可能并不平滑，容易造成机械臂在运动的过程中出现不稳定的情况，因此，使用最小二乘法平滑多个路径点。具体方法是：使用最小二乘多项式拟合的方式对原始路径进行平滑处理，即使用多项式函数代替原数据，并保证拟合数据与原始数据的偏差的平方和最小。

设n对原始数据点(xi，yi)，(i＝1，2，...，n)，以及m次多项式(m＜n)：φ(x)＝a0+a1x+a2x²+…+amx^m。如果限定多项式的最高次数是固定的情况下，求系数ai(i＝1，2，...，m)，使得多项式拟合数据与原始数据的偏差的平方和q最小，即满足最小，则该多项式就是要寻找的拟合曲线。

进一步地，本发明选取最小二乘多项式的最高次数为3次。多项式的最高次数要选取恰当，太小会欠拟合，平滑效果不好；太大会过拟合，也会增加计算量。

本发明所述一种一体化双7自由度机械臂全向移动机器人系统与控制方法从系统结构、硬件组成、控制方法进行统一设计，具有同一个物理坐标系和控制系统空间下的可以移动和升降的双臂结构，对全向移动运动底盘、升降柱、双7自由度机械臂的选型及布局配置做出了详细的设计说明；结合本发明中的系统结构选择了底层stm32单片机、上层高性能pc机的分层控制策略，对全向移动运动底盘、升降柱、双7自由度机械臂进行控制，通过将上层(第一控制单元)路径规划与底层(第二控制单元)运动控制分布在两个平台上进行，实现了双7自由度机械臂全向移动机器人运动控制的实时性与准确性；同时结合全向移动运动底盘，双7自由度机械臂可以进行任意方向的移动，保证了机器人自主操作的灵活性，以及人机协同作业环境的适应性。

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