一种基于六维力传感器的双臂机器人精密装配方法
阅读说明：本技术 一种基于六维力传感器的双臂机器人精密装配方法 (Double-arm robot precise assembly method based on six-dimensional force sensor ) 是由 张俊宁 李晓琪 杨帆 吕博瀚 刘书选 于 2021-06-28 设计创作，主要内容包括：本发明公开了一种基于六维力传感器的双臂机器人精密装配方法,该方法包括如下步骤：机器人控制器读取左机械臂各关节角度和右机械臂各关节角度；得到左右机械臂的笛卡尔空间位姿；根据左右机械臂笛卡尔空间位姿通过坐标转换获得左右臂末端销孔工件相对于机器人基坐标的笛卡尔空间位姿；运用基于快速势能探索随机树方法得到左臂末端销孔和右臂末端销孔规划路径；其中,规划路径为运动距离短、装配作业时间短、机械臂能耗低和避免干涉；根据左臂末端销孔和右臂末端销孔规划路径运用基于学习变阻抗控制系统的控制方法控制左机械臂和右机械臂柔顺装配。本发明能够规划出质量高的路径和具备柔顺装配能力。(The invention discloses a precision assembly method of a double-arm robot based on a six-dimensional force sensor, which comprises the following steps: the robot controller reads the joint angles of the left mechanical arm and the right mechanical arm; obtaining Cartesian space poses of the left mechanical arm and the right mechanical arm; obtaining Cartesian space poses of the pin hole workpieces at the tail ends of the left arm and the right arm relative to the base coordinates of the robot through coordinate transformation according to the Cartesian space poses of the left arm and the right arm; obtaining a left arm tail end pin hole and a right arm tail end pin hole planning path by using a random tree exploring method based on rapid potential energy; the planning path is short in movement distance, short in assembly operation time, low in energy consumption of the mechanical arm and capable of avoiding interference; and controlling the smooth assembly of the left mechanical arm and the right mechanical arm by using a control method based on a learning variable impedance control system according to the planned paths of the pin holes at the tail end of the left arm and the pin holes at the tail end of the right arm. The invention can plan a path with high quality and has compliant assembly capability.)
技术领域
本发明属于六维力传感器的双臂机器人技术领域，尤其涉及一种基于六维力传感器的双臂机器人精密装配方法。
背景技术
航天制造对精度、可靠性、规范流程等要求非常高，因此航天制造领域培养了一大批大国工匠。但是随着时间的推移，一些能工巧匠逐步到了退休年龄，而新一代的80后、90后劳动力人口对于从事简单重复、工作环境差的产业工作不再那么感兴趣，因此在培养新一代大国工匠的同时以机器人替代人工完成一部分工作也是一个不错的选择，由航天制造向航天智造转变已经成为大势所趋。
目前制造业应用的工业机器人装配主要是单机械臂装配或者是多个单独的机械臂在流水线上进行装配。这种装配方法各机械臂之间没有相互配合、装配动作都是提前编辑好的，导致机械臂装配精度差、装配效率低、适应能力差。
机器人装配属于非结构环境下的接触操作任务，由于任务复杂，接触环境多变且不可预测，很难建立系统的精确动力学模型，如何让机器人安全高效快速地执行新任务，精确地控制不同环境下的接触力，是机器人面临的新挑战。阻抗控制因良好的适应性及鲁棒性，被广泛应用于机器人交互控制任务中。由于力控制特性决定于机器人的阻抗参数，惯性、刚度和阻尼参数的选择高度依赖任务，通常难以先验推断，为了得到良好的控制性能，通常需要对控制器设计及其参数有深入的认识，仍需调整控制参数的好方法。所以，学习变阻抗控制能力是现代机器人安全快速地完成复杂操作任务的关键。
对于双机械臂路径规划，现有的方法有人工势场法、A*算法和RRT算法等。在自动化装配的过程中，为了保证零件不发生损坏，使零件之间能平稳地进行配合，需要对零件移动的速度进行约束，在保证零件能够平稳装配的情况下规划出一条效率高的合适路径。但是由于人工势场法容易陷入局部最优，A*算法难以在动态路网中发挥作用，RRT算法路径质量差等问题，导致在双机械臂协同装配作业方面难以规划出质量高的路径。
发明内容
本发明解决的技术问题是：克服现有技术的不足，提供了一种基于六维力传感器的双臂机器人精密装配方法，能够规划出质量高的路径和具备柔顺装配能力。
本发明目的通过以下技术方案予以实现：一种基于六维力传感器的双臂机器人精密装配方法，所述方法包括如下步骤：步骤一：机器人控制器读取左机械臂各关节角度和右机械臂各关节角度；步骤二：建立双机械臂机器人的运动学模型和动力学模型；根据步骤一中的左机械臂各关节角度和双机械臂机器人的运动学模型和动力学模型运用六自由度机械臂正运动学算法得到左机械臂的笛卡尔空间位姿；根据步骤一中的右机械臂各关节角度和双机械臂机器人的运动学模型和动力学模型运用六自由度机械臂正运动学算法得到右机械臂的笛卡尔空间位姿；步骤三：根据步骤二中的左机械臂笛卡尔空间位姿通过坐标转换获得左臂末端销孔工件相对于机器人基坐标的笛卡尔空间位姿；根据步骤二中的右机械臂笛卡尔空间位姿通过坐标转换获得右臂末端销孔工件相对于机器人基坐标的笛卡尔空间位姿；步骤四：运用基于快速势能探索随机树方法得到左臂末端销孔和右臂末端销孔规划路径；其中，规划路径为运动距离短、装配作业时间短、机械臂能耗低和避免干涉；步骤五：根据步骤四中的左臂末端销孔和右臂末端销孔规划路径运用基于学习变阻抗控制系统的控制方法控制左机械臂和右机械臂柔顺装配。
上述基于六维力传感器的双臂机器人精密装配方法中，在步骤二中，双机械臂机器人的运动学模型和动力学模型通过如下公式得到：
其中，是双机械臂机器人关节i-1与i的坐标转换矩阵，c代表cos，s代表sin，θi,αi-1,ai-1,di分别为双机械臂机器人关节i的D-H参数；为左机械臂末端销工件到左机械臂末端的转换矩阵，m为左机械臂末端销工件到左机械臂末端的距离；为右机械臂末端孔工件到右机械臂末端的转换矩阵，m为右机械臂末端销工件到右机械臂末端的距离；为左机械臂末端销工件到左机械基坐标的转换矩阵；为右机械臂末端孔工件到右机械臂基坐标的转换矩阵；τ为关节控制力矩，M(Θ)为机械臂的n*n的质量矩阵，为n*1的离心力和哥氏力矢量，G(Θ)为n*1的重力矢量，Θ为机械臂关节位置矢量，为机械臂关节速度矢量，为机械臂关节加速度矢量。
上述基于六维力传感器的双臂机器人精密装配方法中，在步骤四中，基于快速势能探索随机树方法包括如下步骤：
(41)设定初始参数，并进行初始采样；
(42)根据双机械臂机器人的运动学模型和动力学模型和步骤(41)中的初始参数计算随机树的树节点距离采样点的距离，找到最近的树节点作为父节点；
(43)根据父节点得到拓展子节点，判断拓展子节点是否碰撞，如果不碰撞则加入候选采样点；
(44)计算候选采样点势能；
(45)修改采样步长和采样概率；重复步骤(42)-(44)，直到候选采样点达到num；
(46)根据num个候选采样点势能的倒数进行轮盘赌选择，得到采样点，将采样点作为新的子节点；
(47)选择最短代价的连接方式将步骤(46)的新的子节点连接父节点，确定左臂末端销孔和右臂末端销孔规划路径。
上述基于六维力传感器的双臂机器人精密装配方法中，在步骤五中，基于学习变阻抗控制系统的控制方法包括如下步骤：
(51)将预设的初始化控制变量作用于机械臂的控制系统；
(52)根据预设的采样数据建立控制系统的高斯过程动态模型，作为系统的变换动力学模型；
(53)根据步骤(52)中的系统的变换动力学模型使用策略学习算法搜索最优阻抗控制策略；
(54)根据最优阻抗控制策略，应用于变阻抗控制器进行力控制，并采集数据；
(55)重复步骤(52)-(54)直到得到预设力跟踪效果。
上述基于六维力传感器的双臂机器人精密装配方法中，在步骤(41)中，初始参数包括：左机械臂的起始位姿left_start，目标位姿left_goal，右机械臂的起始位姿right_start，目标位姿right_goal，初始路径随机树tree，初始采样步长step_size，初始采样概率probability，采样上限upper_limit，单次候选采样点数num，重连接代价price，碰撞距离distance，最终位置势能误差h。
上述基于六维力传感器的双臂机器人精密装配方法中，在步骤(41)中，将左机械臂的目标位姿left_goal和右机械臂的目标位姿right_goal连一条直线，在直线上选取在left_goal左侧与left_goal距离为L的点定义为left_viapoint，在直线上选取在right_goal右侧与right_goal距离为L的点定义为right_viapoint；
从左机械臂的left_start点向left_viapoint点，从左机械臂的left_goal点向left_viapoint点，从左机械臂的left_viapoint点向left_start点，从左机械臂的left_viapoint点向left_goal点，按照采样公式进行初始化采样；
从右机械臂的right_start点向right_viapoint点，从右机械臂的right_goal点向right_viapoint点，从右机械臂的right_viapoint点向right_start点，从右机械臂的right_viapoint点向right_goal点，按照采样公式进行初始化采样。
上述基于六维力传感器的双臂机器人精密装配方法中，采样公式为：
其中，qsamk是关节k的采样角度，random是随机选取关节角度，qkmax是关节k的最大角度，qkmin是关节k的最小角度，probability是采样概率，qkgoal是goal位置关节k的角度。
上述基于六维力传感器的双臂机器人精密装配方法中，在步骤(43)中，根据父节点得到拓展子节点通过如下公式得到：
其中，qnearestk是随机树父节点的关节k角度，qk是拓展子节点的关节k角度，step_size为初始采样步长，qsamk是关节k的采样角度。
上述基于六维力传感器的双臂机器人精密装配方法中，在步骤(44)中，候选采样点势能通过如下公式得到：
U(j)＝Fgra(j)+Frepinter(gj)+Frepexter(j)+Freptree(gj)；
其中，Fgra(j)是节点j的引力势能，λ是引力比例系数，gj是节点j的机械臂末端位置，gvia是viapoint点的机械臂末端位置，gstart是start点的机械臂末端位置，ggoal是goal点的机械臂末端位置；Frepinter(gj)是节点j内部位姿产生的斥力势能，ε是斥力比例系数，gjk1是机械臂1节点j的关节k的位置，gjk2是机械臂2节点j的关节k的位置，drob是两机械臂之间的最小距离，dmin是最小碰撞距离，dinter是内部斥力边界；Frepexter(j)是节点j与外部产生的斥力势能，ε是斥力比例系数，gjk是节点j的机械臂位置，obs是障碍物的外包络中心，dobs是关节j距离障碍物的距离，dmin是最小碰撞距离，dexter是外部斥力边界；Freptree(gj)是节点j与同一机械臂其他随机树之间产生的斥力势能，ε是斥力比例系数，是随机树m1节点j的的关节k的位置，是随机树m2节点j的的关节k的位置，dtree是随机树之间的最小距离，dmin是最小碰撞距离，dtrees是树间斥力边界；U(j)是节点j的势能，Fgra(j)是节点j的引力势能，Frepinter(gj)是节点j内部位姿产生的斥力势能，Frepexter(j)是节点j与外部产生的斥力势能，Freptree(gj)是节点j与同一机械臂其他随机树之间产生的斥力势能。
上述基于六维力传感器的双臂机器人精密装配方法中，在步骤三中，左臂末端销孔工件是一个前端为钝头圆锥和后端为圆柱的结构。
本发明与现有技术相比具有如下有益效果：
(1)本发明在双臂机器人的末端安装了六维力传感器，方便机械臂在按照规划路径运动时能够根据接触力和接触力矩进行微调，保证销孔能够精密装配，装配精度<＝0.1mm。
(2)本发明以RRT算法为基础，在机械臂运动路径上引入途经点，并从起始点、途经点、目标点三点同时出发扩展生成四棵随机树，极大地加快了随机树对状态空间的探索速度。
(3)本发明在保证采样随机性的基础上，在势能函数中添加随机树之间的斥力势能，更好地引导随机树采样，同时在根据代价重新连接随机树的方法中加入加速度代价，使机械臂运动的路径更加平滑，这两点创新，提高了随机树的采样质量，优化了各节点之间的连接方式，保证了规划路径的高质量。
(4)本发明加入了自适应步长调节函数和自适应采样概率调节函数，这两个函数使随机树在空间有障碍物和无障碍物情况下分别自适应调节步长和采样概率，从而提高随机树的扩展速度。
(5)本发明引入了改进RRT*算法，保证了随机树在有障碍物的三维空间探索时能够生成最优路径，同时避免算法陷入局部最优。
附图说明
通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。而且在整个附图中，用相同的参考符号表示相同的部件。在附图中：
图1是本发明实施例提供的基于六维力传感器的双臂机器人精密装配方法的流程图；
图2是本发明实施例提供的左臂末端的结构示意图；
图3是本发明实施例提供的右臂末端的结构示意图。
具体实施方式
下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。
图1是本发明实施例提供的基于六维力传感器的双臂机器人精密装配方法的流程图。如图1所示，该基于六维力传感器的双臂机器人精密装配方法包括如下步骤：
步骤一：机器人控制器读取左机械臂各关节角度和右机械臂各关节角度；
步骤二：建立双机械臂机器人的运动学模型和动力学模型；根据步骤一中的左机械臂各关节角度和双机械臂机器人的运动学模型和动力学模型运用六自由度机械臂正运动学算法得到左机械臂的笛卡尔空间位姿；根据步骤一中的右机械臂各关节角度和双机械臂机器人的运动学模型和动力学模型运用六自由度机械臂正运动学算法得到右机械臂的笛卡尔空间位姿；
步骤三：根据步骤二中的左机械臂笛卡尔空间位姿通过坐标转换获得左臂末端销孔工件相对于机器人基坐标的笛卡尔空间位姿；根据步骤二中的右机械臂笛卡尔空间位姿通过坐标转换获得右臂末端销孔工件相对于机器人基坐标的笛卡尔空间位姿；
步骤四：运用基于快速势能探索随机树方法得到左臂末端销孔和右臂末端销孔规划路径；其中，规划路径为运动距离短、装配作业时间短、机械臂能耗低和避免干涉；
步骤五：根据步骤四中的左臂末端销孔和右臂末端销孔规划路径运用基于学习变阻抗控制系统的控制方法控制左机械臂和右机械臂柔顺装配。
在步骤四中，基于快速势能探索随机树方法包括如下步骤：(41)设定初始参数，并进行初始采样；(42)根据双机械臂机器人的运动学模型和动力学模型和步骤(41)中的初始参数计算随机树的树节点距离采样点的距离，找到最近的树节点作为父节点；(43)根据父节点得到拓展子节点，判断拓展子节点是否碰撞，如果不碰撞则加入候选采样点；(44)计算候选采样点势能；(45)修改采样步长和采样概率；重复步骤(42)-(44)，直到候选采样点达到num；(46)根据num个候选采样点势能的倒数进行轮盘赌选择，得到采样点，将采样点作为新的子节点；(47)选择最短代价的连接方式将步骤(46)的新的子节点连接父节点，确定左臂末端销孔和右臂末端销孔规划路径。
在步骤五中，基于学习变阻抗控制系统的控制方法包括如下步骤：(51)将预设的初始化控制变量作用于机械臂的控制系统；(52)根据预设的采样数据建立控制系统的高斯过程动态模型，作为系统的变换动力学模型；(53)根据步骤(52)中的系统的变换动力学模型使用策略学习算法搜索最优阻抗控制策略；(54)根据最优阻抗控制策略，应用于变阻抗控制器进行力控制，并采集数据；(55)重复步骤(52)-(54)直到得到预设力跟踪效果。
本实施例中的机器人，包括全方位移动平台、机器人躯干、两条六自由度机械臂、两个六维力传感器、末端装配工件。机器人躯干安装在移动平台上，两条六自由度机械臂对称安装在机器人躯干两侧，两个六维力传感器分别安装在机械臂末端法兰上，在左机械臂末端六维力传感器法兰上安装装配销工件，在右机械臂末端六维力传感器法兰上安装装配孔工件。
具体的，该方法包括如下步骤：
步骤1：建立双机械臂机器人的运动学模型和动力学模型：
式(1)中：是双机械臂机器人关节i-1与i的坐标转换矩阵，c代表cos，s代表sin，θi,αi-1,ai-1,di分别为双机械臂机器人关节i的D-H参数；式(2)中：为左机械臂末端销工件到左机械臂末端的转换矩阵，m为左机械臂末端销工件到左机械臂末端的距离；式(3)中：为右机械臂末端孔工件到右机械臂末端的转换矩阵，m为右机械臂末端销工件到右机械臂末端的距离；式(4)中：为左机械臂末端销工件到左机械基坐标的转换矩阵；式(5)中：为右机械臂末端孔工件到右机械臂基坐标的转换矩阵；式(6)中：τ为关节控制力矩，M(Θ)为机械臂的n*n的质量矩阵，为n*1的离心力和哥氏力矢量，G(Θ)为n*1的重力矢量，Θ为机械臂关节位置矢量，为机械臂关节速度矢量，为机械臂关节加速度矢量。
左臂机械末端销工件如图2所示，销工件是一个前端为钝头圆锥，后端为圆柱的结构，销工件与六维力传感器连接，然后六维力传感器再与左机械臂末端法兰连接。
右臂机械末端孔工件如图3所示，孔工件是一个前端为圆筒型，配有观察窗口，后端为圆盘的结构，孔工件与六维力传感器连接，然后六维力传感器再与右机械臂末端法兰连接。销孔之间安装间隙为0.1mm。
步骤2：设置初始参数，左机械臂的起始位姿left_start，目标位姿left_goal，右机械臂的起始位姿right_start，目标位姿right_goal，初始路径随机树tree，初始采样步长step_size，初始采样概率probability，采样上限upper_limit，单次候选采样点数num，重连接代价price，碰撞距离distance，最终位置势能误差h；
步骤3：将步骤2中左机械臂的目标位姿left_goal和右机械臂的目标位姿right_goal连一条直线，在直线上选取在left_goal左侧与left_goal距离为L的点定义为left_viapoint，在直线上选取在right_goal右侧与right_goal距离为L的点定义为right_viapoint。
步骤4：根据步骤2和步骤3，从左机械臂的left_start点向left_viapoint点，从左机械臂的left_goal点向left_viapoint点，从左机械臂的left_viapoint点向left_start点，从左机械臂的left_viapoint点向left_goal点，按照公式(7)进行初始化采样；从右机械臂的right_start点向right_viapoint点，从右机械臂的right_goal点向right_viapoint点，从右机械臂的right_viapoint点向right_start点，从右机械臂的right_viapoint点向right_goal点，按照公式(7)进行初始化采样：
式(7)中：qsamk是关节k的采样角度，random是随机选取关节角度，qkmax是关节k的最大角度，qkmin是关节k的最小角度，probability是采样概率，qkgoal是goal位置关节k的角度。
步骤5：根据步骤4的采样点，选取距离采样点最近的树节点q_nearest作为父节点。
步骤6：根据采样点利用公式(8)得到子结点，判断子节点是否碰撞，如果不发生碰撞则加入候选采样点，如果子节点碰撞，则舍弃。
式(8)中：qnearestk是随机树父节点的关节k角度，qk是得到的采样点的关节k角度。
步骤7：根据步骤6得到的候选采样点，利用公式(9)-(13)计算候选采样点的势能。
U(j)＝Fgra(j)+Frepinter(gj)+Frepexter(j)+Freptree(gj) (13)
式(9)中：Fgra(j)是节点j的引力势能，λ是引力比例系数，gj是节点j的机械臂末端位置，gvia是viapoint点的机械臂末端位置，gstart是start点的机械臂末端位置，ggoal是goal点的机械臂末端位置。式(10)中：Frepinter(gj)是节点j内部位姿产生的斥力势能，ε是斥力比例系数，gjk1是机械臂1节点j的关节k的位置，gjk2是机械臂2节点j的关节k的位置，drob是两机械臂之间的最小距离，dmin是最小碰撞距离，dinter是内部斥力边界。式(11)中：Frepexter(j)是节点j与外部产生的斥力势能，ε是斥力比例系数，gjk是节点j的机械臂位置，obs是障碍物的外包络中心，dobs是关节j距离障碍物的距离，dmin是最小碰撞距离，dexter是外部斥力边界。式(12)中：Freptree(gj)是节点j与同一机械臂其他随机树之间产生的斥力势能，ε是斥力比例系数，是随机树m1节点j的的关节k的位置，是随机树m2节点j的的关节k的位置，dtree是随机树之间的最小距离，dmin是最小碰撞距离，dtrees是树间斥力边界。式(13)中：U(j)是节点j的势能，Fgra(j)是节点j的引力势能，Frepinter(gj)是节点j内部位姿产生的斥力势能，Frepexter(j)是节点j与外部产生的斥力势能，Freptree(gj)是节点j与同一机械臂其他随机树之间产生的斥力势能。
步骤8：根据公式(14)修改采样概率，根据公式(15)修改采样步长：
式(14)中：α，β是步长调整系数，d为机械臂末端与障碍物外包络之间的距离。式(15)中：μ，σ是概率调整系数，d为机械臂末端与障碍物外包络之间的距离。
步骤9：重复采样过程，直到采样后选点数量达到num。
步骤10：根据势能U(j)的倒数进行轮盘赌选择，得到采样点q_new。
步骤11：当随机树扩展遇到障碍物时，以采样点q_nearest(xn,yn,zn)为球心，以2*step_size的半径re向外做球，然后在球面上以90度为间隔采样8个节点，节点坐标由公式(16)-(18)得到。得到8个节点后，分别对求得的每个节点进行碰撞检测，并记录所有不存在与障碍物碰撞的节点，分别计算每个节点到目标点(start点、goal点或viapoint点)的距离，最终选择出无碰撞且距离固定点最近的点作为下一步扩展方向，并向前扩展一个步长至节点q_newer。再以q_newer为球心重复上述过程，直到随机树摆脱障碍物。
ze＝zn+recosθ(18)
步骤12：根据公式(19)-(27)计算随机树上各节点到q_new的代价，把代价小于price的点加入重连接点中，计算q_new与各重连接点之间组成路径的代价，选取代价最小的连接方式，确定q_new的连接父节点。
W(gj,gk)＝τaWa(aj,ak)+τqWq(qj,qk)+τTWT(Tj,Tk)+τEWE(Ej,Ek) (19)
式(19)中：W(gj,gk)是总代价，Wa(aj,ak)是末端加速度代价，Wq(qj,qk)是关节角变化代价，WT(Tj,Tk)是末端速度代价，WE(Ej,Ek)是能耗代价，τ是权重。式(20)中，costaj,k是从第j个节点向第k个节点的加速度变化代价，是从第j个节点向第k个节点的加速度变化值。式(22)中，costqj,k是从第j个节点向第k个节点的关节角代价，Δqj,k是从第j个节点向第k个节点的加速度变化值。式(24)中，costTj,k是从第j个节点向第k个节点的速度变化代价，Δvj,k是从第j个节点向第k个节点的速度变化值。式(26)中，costEj,k是从第j个节点向第k个节点的能耗代价，c是关节电机转矩，kc是关节电机转矩与电流的比例系数。
步骤13：去除重连接点之间的连接关系，计算重连接点之间所有的连接方式所需要的路径代价，选择代价最低的连接方式重新确立节点的父子关系；
步骤14：如果q_new与q_goal(或q_start或q_via)的势能差小于h时，用q_goal(或q_start或q_via)代替q_new完成随机树。
步骤15：由以上步骤确定的父子节点关系，生成规划的路径。
步骤16：根据公式(28)和(29)得到六维力传感器测得的外力和外力矩，根据公式(28)-(29)对沿规划路径运动的机械臂运动过程进行微调。
式(28)中，Fe是六维力传感器受到的外力，F是六维力传感器测得的力，F0是六维力传感器的力零点值，G是六维力传感器的重力分量。式(29)中，Me是六维力传感器受到的外力矩，M是六维力传感器测得的力矩，M0是六维力传感器的力矩零点值，Mg是六维力传感器的重力矩分量。式(30)中，Δv是机械臂末端线速度变量，σf是机械臂末端受力运动灵敏度，M是机械臂末端负载质量，fT是六维力传感器测得的外力。式(31)中，Δω是机械臂末端角速度变量，σm是机械臂末端受力矩运动灵敏度，I是机械臂末端负载转动惯量，mT是六维力传感器测得的外力矩。
步骤17：双臂机器人销孔精密装配成功。
本发明在双臂机器人的末端安装了六维力传感器，方便机械臂在按照规划路径运动时能够根据接触力和接触力矩进行微调，保证销孔能够精密装配，装配精度<＝0.1mm；本发明以RRT算法为基础，在机械臂运动路径上引入途经点，并从起始点、途经点、目标点三点同时出发扩展生成四棵随机树，极大地加快了随机树对状态空间的探索速度；本发明在保证采样随机性的基础上，在势能函数中添加随机树之间的斥力势能，更好地引导随机树采样，同时在根据代价重新连接随机树的方法中加入加速度代价，使机械臂运动的路径更加平滑，这两点创新，提高了随机树的采样质量，优化了各节点之间的连接方式，保证了规划路径的高质量；本发明加入了自适应步长调节函数和自适应采样概率调节函数，这两个函数使随机树在空间有障碍物和无障碍物情况下分别自适应调节步长和采样概率，从而提高随机树的扩展速度；本发明引入了改进RRT*算法，保证了随机树在有障碍物的三维空间探索时能够生成最优路径，同时避免算法陷入局部最优。
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种可翻转立方体机器人及其建模方法和控制方法