CN107729637A - 基于臂形角区间的冗余自由度机械臂运动规划及评价方法 - Google Patents
基于臂形角区间的冗余自由度机械臂运动规划及评价方法 Download PDFInfo
- Publication number
- CN107729637A CN107729637A CN201710927923.5A CN201710927923A CN107729637A CN 107729637 A CN107729637 A CN 107729637A CN 201710927923 A CN201710927923 A CN 201710927923A CN 107729637 A CN107729637 A CN 107729637A
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- mtd
- mtr
- joint
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/10—Geometric CAD
- G06F30/17—Mechanical parametric or variational design
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/30—Circuit design
- G06F30/36—Circuit design at the analogue level
- G06F30/367—Design verification, e.g. using simulation, simulation program with integrated circuit emphasis [SPICE], direct methods or relaxation methods
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Computer Hardware Design (AREA)
- Theoretical Computer Science (AREA)
- Geometry (AREA)
- General Physics & Mathematics (AREA)
- General Engineering & Computer Science (AREA)
- Evolutionary Computation (AREA)
- Microelectronics & Electronic Packaging (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法，该方法采用臂形角参数来描述冗余自由度机械臂的自运动，在关节位置级上显式地应用自运动特性。通过求避关节极限的臂形角空间和避障碍物的臂形角空间，并求两空间交集。若交集不为空集，将此交集中的任意值带入运动学逆解模型，得到在给定机械臂末端位姿下避关节极限且避障碍物的所有可行的机械臂运动学逆解。在所规划的工作空间进行采样，并对采样点进行评价，以提高运动规划的效率和精度，使优化规划的路径更符合人的手臂运动形态。本发明通过先求解避关节极限和避障碍极限交集，得出给定末端执行器位姿下避关节极限并且避障碍物的所有可行逆解，再进行运动规划的方法，排除了大量冗余解，方法简单、高效率、高精度，可实现在线实时运动规划。
Description
技术领域
本发明属于机械臂运动规划领域，具体涉及一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法。
背景技术
机械臂运动规划的重要研究内容是规划出一条满足各类指标的最优路径。冗余自由度机械臂关节空间具有自运动特性，逆解具有多解性，可通过满足避关节极限或避障碍物指标求得冗余自由度机械臂逆解空间。
针对避关节极限问题，目前多数方法是基于雅可比矩阵零空间的关节速度向量进行求解，用关节极限值构造出排斥因子或用关节极限平均值构造出吸引因子来约束关节转角。该类方法仅能得到唯一可行解，没有充分利用避关节极限区间，并且在奇异值附近求解广义逆运算量大，存在积分漂移误差，易出现有解但无法求出的问题。
针对避障碍物问题，多数方法是在机械臂上选取关键点，用关键点到障碍物的距离构建势场排斥机械臂的位置。该类方法存在机械臂运动过程中关键点不容易选取，复杂情况下易陷入局部极值问题。另外也有文献通过引入神经网络算法求解基于人工势场建立的模型，研究冗余自由度机械臂的避关节极限和避障碍物问题，该类方法过于复杂，无法实时在线进行运动规划。
发明内容
为解决现有冗余自由度机械臂运动规划运算量大、存在积分漂移误差和易陷入局部极值的问题，本发明提出一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法，该方法引入臂形角约束，在关节位置级上显式地应用自运动特性，求解满足避关节极限和避障碍物的所有可行的运动学逆解；在所规划的工作空间进行采样，并对采样点进行评价，以提高运动规划的效率和精度，使优化规划的路径更符合人的手臂运动形态。
本发明为达到上述目的，采取如下技术方案：
1.一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法，该方法具体内容包括如下步骤：
步骤1：采用D-H法建立坐标系，将7R机械臂基座的前三个关节坐标进行旋转，重合于以第三个关节坐标系原点为球心的球铰，前三个关节等效为肩关节S；同样，将7R机械臂的末端最后三个关节坐标进行旋转，重合于以第六个坐标系原点为球心的球铰，后三个关节等效成腕关节W；第四个关节等效成肘关节E；末端执行器等效成手掌T；
步骤2：由给定的机械臂末端位姿，给定肩关节S、肘关节E和腕关节W的控制量，逆解求机械臂各关节角度；如果有解，设定为目标状态；如果无解，则无法对机械臂进行运动规划；
步骤3：求避关节极限的臂形角空间和避障碍物的臂形角空间，并求两空间交集；若交集不为空集，将此交集上的任意值带入运动学逆解模型，得到在给定机械臂末端位姿下避关节极限且避障碍物的所有可行的机械臂运动学逆解；从而针对末端执行器位姿，对关节工作空间运动轨迹进行规划；
步骤4：对所规划的关节工作空间进行采样，采用“成本收益比”指标对样点进行评价，提高运动规划效率，优化规划的路径更符合人的手臂运动形态。
进一步，在步骤3中，所述求避关节极限的臂形角空间和避障碍物的臂形角空间交集，其具体求解方法如下：
对于给定的末端位姿，输入末端位姿的6个约束，并输入3个控制参数arm，elbow，wrist，求解避关节极限的臂形角区间和避障碍物的臂形角区间；
(1)避关节极限
将臂形角Ψ映射到关节转角，输入给定的末端位姿约束和每个关节转角的极限值，计算满足关节转角极限的臂形角Ψ的取值区间；
设前三个关节转角分别为q1,q2,q3，当前三个关节角都为0时，记为Σo
记坐标系Σarm为ΣD
记肩膀S变换矩阵为RS
由
得
Rot(x,Ψ)＝I3+sinΨ[x×]+(1-cosΨ)[x×]2………………………(5)
式中：I3是3×3单位矩阵，[x×]是单位向量x构成的反对称阵；展开式(5)得到关于Ψ的变换函数，带入式(2)得到ΣD关于Ψ的直观表达式，将ΣD带入式(3)得
RS(q1,q2,q3)＝AssinΨ+BscosΨ+Cs………………………(6)
式中：记As(i,j)为asij；
记Bs(i,j)为bsij；
Cs＝x×xT×Σo T，记Cs(i,j)为csij。
从球铰变换公式有
式中：Si指sin(qi)，Ci指cos(qi)，i＝1,2,3，*表示忽略该项；
将系数矩阵As、Bs和Cs求出代入式(6)，并与式(7)对比[3,3]位置的值得：
cos q2＝as33 sinΨ+bs33 cosΨ+cs33………………………(8)
从q2的范围就能够推导出cos q2的范围，进而计算出满足q2极限范围Ψ的取值范围；同理，分别求出满足q1、q3、q5、q6和q7极限范围Ψ的取值范围；
对于七个关节，除去第四个关节，求交集得避关节极限的Ψ区间为
Ψ＝{Ψ1}∩{Ψ2}∩{Ψ3}∩{Ψ5}∩{Ψ6}∩{Ψ7}………………………(9)
式中，Ψi(i＝1,2,3,,5,6,7)是避关节qi极限的区间；
(2)避障碍物
将臂形角Ψ在取值范围上离散，输入末端位姿约束、臂形角Ψ和控制参数elbow，得到肘关节E的位置；则连杆SE和连杆EW位置已知，运用碰撞检测算法对每一个Ψ的取值，测试连杆SE和连杆EW是否与障碍物发生碰撞；从所有测试结果中，解算出没有发生碰撞的臂形角Ψ的若干个连续区间的并集；
给定末端位姿Td将末端位置沿着Z7相反方向移动dWT即得到W点位置；记W点位置为pW，则
对给定末端位姿Td，肩关节S和腕关节W在空间的位置已经固定，过点E在直线SW上的垂足为Oe，求出设为H。，以Oe为坐标系原点，以为X轴正方向，以OeE为Y轴正方向，建立坐标系Oe-XeYeZe，
向量
向量
已知Oe-XeYeZe的齐次变换矩阵Te
在中位置取为Ψ的起始线，E在Oe-XeYeZe中的坐标如式(14)
求出E在O-XYZ中的坐标E：
则S、E、W、T四个点的位置确定，从而确定连杆SE、连杆EW和连杆WT位置，利用碰撞检测算法判断连杆是否与障碍物发生碰撞；
综上所述，将避关节极限子空间和避障碍物子空间的臂形角求交集，得到在给定机械臂末端位姿下避关节极限且避障碍物的所有可行的机械臂运动学逆解。
进一步，在步骤4中，所述对所规划的关节工作空间进行采样，采用“成本收益比”指标对采样点进行评价，其具体实现方法如下：
对机械臂的腕关节位置空间进行采样，对每一个采样点xnew判断腕关节到机械臂末端之间的连杆是否碰撞，如果碰撞则放弃该采样点；如果没有碰撞则求解该位置对应的避关节极限形角区间和避障碍物臂形角区间的交集，若交集不为空集，则该采样点成为一个Xfree中的采样点，并记录每个采样点的臂形角区间；
对所加入的每一个采样点，采用“成本收益比ratio”进行评价：
式中，成本costPath是根节点到该采样点的欧氏距离路程；收益proProject是从根节点到该采样点在根节点到目标点连线上的投影；Xs的ratio为0，x2的路程成本为d1+d2，X2的收益为p2。
由于采用上述技术方案，本发明与现有技术相比具有这样的有益效果：
因冗余自由度关节空间具有自运动特性，冗余自由度机械臂运动规划的逆解具有多解性，需依据条件进一步后处理选择和优化运动学逆解，才能得到光滑运动规划路径。解算优化运算量大、计算复杂。本发明通过引入臂形角参数显式地描述了冗余自由度机械臂的自运动特性，通过先求解避关节极限和避障碍极限交集，得出给定末端执行器位姿下避关节极限并且避障碍物的所有可行逆解，再进行运动规划的方法，排除了大量冗余解，方法简单、高效率、高精度，可实现在线实时运动规划。本发明不但有效解决了隐式求解算法中存在积分漂移误差和寻优易陷入局部极值的问题，所求交集亦可作为后续动力学优化的完备取值区间。运动规划评价方法采用在工作空间进行采样，并对采样点进行评价，可提高运动规划的效率和精度，使优化规划的路径更符合人的手臂运动形态。
附图说明
图1是本发明实施机械臂运动规划方法的解算流程图；
图2是本发明臂形角定义示意图；
图3中的(a)是本发明7R机械臂坐标系示意图，(b)是等效为手臂形态后示意图；
图4是本发明关节空间逆解的多解性示意图；
图5中的(a)是本发明避障碍物肘部坐标系示意图，(b)是肘部E位置示意图；
图6是本发明对工作空间采样点评价方法示意图。
具体实施方式
下面结合附图以及具体实施方式，对本发明做进一步描述：
本发明的一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法，图1所示是本发明实施机械臂运动规划方法的解算流程图，该方法的实现过程如下：
步骤1：采用D-H法建立坐标系，如图3中的(a)所示。将7R机械臂基座的前三个关节坐标进行旋转，重合于以第三个关节坐标系原点为球心的球铰，前三个关节等效为肩关节S；同样，将7R机械臂的末端最后三个关节坐标进行旋转，重合于以第六个坐标系原点为球心的球铰，后三个关节等效成腕关节W；第四个关节等效成肘关节E；末端执行器等效成手掌T，如图3中的(b)所示。
步骤2：由给定的机械臂末端位姿，给定肩关节S、肘关节E和腕关节W的控制量，逆解求机械臂各关节角度；如果有解，设定为目标状态；如果无解，则无法对机械臂进行运动规划。
步骤3：求避关节极限的臂形角空间和避障碍物的臂形角空间，图2所示是本发明臂形角定义示意图，并求两空间交集。若交集不为空集，将此交集上的任意值带入运动学逆解模型，得到在给定机械臂末端位姿下避关节极限且避障碍物的所有可行的机械臂运动学逆解。从而针对末端执行器位姿，对关节工作空间运动轨迹进行规划。
在步骤3中，所述求避关节极限和避障碍物的臂形角空间交集，其具体求解方法如下：
对于给定的末端位姿，输入末端位姿的6个约束，并输入3个控制参数arm，elbow，wrist，求解避关节极限的臂形角区间和避障碍物的臂形角区间；
(1)避关节极限
将臂形角Ψ映射到关节转角，输入给定的末端位姿约束和每个关节转角的极限值，计算满足关节转角极限的臂形角Ψ的取值区间；
设前三个关节转角分别为q1,q2,q3，当前三个关节角都为0时，记为Σo
记坐标系Σarm为ΣD
记肩膀S变换矩阵为RS
由
得
Rot(x,Ψ)＝I3+sinΨ[x×]+(1-cosΨ)[x×]2………………………(5)
式中：I3是3×3单位矩阵，[x×]是单位向量x构成的反对称阵；展开式(5)得到关于Ψ的变换函数，带入式(2)得到ΣD关于Ψ的直观表达式，将ΣD带入式(3)得
RS(q1,q2,q3)＝As sinΨ+Bs cosΨ+Cs………………………(6)
式中：记As(i,j)为asij；
记Bs(i,j)为bsij；
Cs＝(x×xT)×Σo T，记Cs(i,j)为csij。
从球铰变换公式有
式中：Si指sin(qi)，Ci指cos(qi)，i＝1,2,3，*表示忽略该项；
将系数矩阵As、Bs和Cs求出代入式(6)，并与式(7)对比[3,3]位置的值得：
cos q2＝as33 sinΨ+bs33 cosΨ+cs33………………………(8)
从q2的范围就能够推导出cos q2的范围，进而计算出满足q2极限范围Ψ的取值范围；同理，分别求出满足q1、q3、q5、q6和q7极限范围Ψ的取值范围；
对于七个关节，除去第四个关节，求交集得避关节极限的Ψ区间为
Ψ＝{Ψ1}∩{Ψ2}∩{Ψ3}∩{Ψ5}∩{Ψ6}∩{Ψ7}………………………(9)
式中，Ψi(i＝1,2,3,,5,6,7)是避关节qi极限的区间；
(2)避障碍物
将臂形角Ψ在取值范围上离散，输入末端位姿约束、臂形角Ψ和控制参数elbow，得到肘关节E的位置；则连杆SE和连杆EW位置已知，运用碰撞检测算法对每一个Ψ的取值，测试连杆SE和连杆EW是否与障碍物发生碰撞；从所有测试结果中，解算出没有发生碰撞的臂形角Ψ的若干个连续区间的并集；
给定末端位姿Td将末端位置沿着Z7相反方向移动dWT即可得到W点位置；记W点位置为pW，则
对给定末端位姿Td，肩关节S和腕关节W在空间的位置已经固定，过点E在直线SW上的垂足为Oe，求出设为H。，以Oe为坐标系原点，以为X轴正方向，以OeE为Y轴正方向，建立坐标系Oe-XeYeZe，如图5(a)所示，
向量
向量
已知Oe-XeYeZe的齐次变换矩阵Te
E在Oe-XeYeZe中位置如图5(b)所示，取为Ψ的起始线，E在Oe-XeYeZe中的坐标如式(14)
求出E在O-XYZ中的坐标E：
则S、E、W、T四个点的位置确定，从而确定连杆SE、连杆EW和连杆WT位置，利用碰撞检测算法可判断连杆是否与障碍物发生碰撞。
综上所述，将避关节极限子空间和避障碍物子空间的臂形角求交集，得到在给定机械臂末端位姿下避关节极限且避障碍物的所有可行的机械臂运动学逆解。图4所示为关节空间逆解的多解性示意图。
步骤4：对所规划的关节工作空间进行采样，采用“成本收益比”指标对样点进行评价，提高运动规划效率，优化规划的路径更符合人的手臂运动形态。
在步骤4中，所述对所规划的关节工作空间进行采样，采用“成本收益比”指标对采样点进行评价，其具体内容如下：对机械臂的腕关节位置空间进行采样，对每一个采样点xnew判断腕关节到机械臂末端之间的连杆是否碰撞，如果碰撞则放弃该采样点；如果没有碰撞则求解该位置对应的避关节极限和避障碍物臂形角区间的交集，若交集不为空集，则该采样点成为一个Xfree中的采样点，并记录每个采样点的臂形角区间；
对所加入的每一个采样点，采用“成本收益比ratio”进行评价；
式中，成本costPath是根节点到该采样点的欧氏距离路程；收益proProject是从根节点到该采样点在根节点到目标点连线上的投影；如图6所示，Xs的ratio为0，x2的路程成本为(d1+d2)，X2的收益为p2。
Claims (3)
1.一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法，其特征在于：该方法具体内容包括如下步骤：
步骤1：采用D-H法建立坐标系，将7R机械臂基座的前三个关节坐标进行旋转，重合于以第三个关节坐标系原点为球心的球铰，前三个关节等效为肩关节S；同样，将7R机械臂的末端最后三个关节坐标进行旋转，重合于以第六个坐标系原点为球心的球铰，后三个关节等效成腕关节W；第四个关节等效成肘关节E；末端执行器等效成手掌T；
步骤2：由给定的机械臂末端位姿，给定肩关节S、肘关节E和腕关节W的控制量，逆解求机械臂各关节角度；如果有解，设定为目标状态；如果无解，则无法对机械臂进行运动规划；
步骤3：求避关节极限的臂形角空间和避障碍物的臂形角空间，并求两空间交集；若交集不为空集，将此交集上的任意值带入运动学逆解模型，得到在给定机械臂末端位姿下避关节极限且避障碍物的所有可行的机械臂运动学逆解；从而针对末端执行器位姿，对关节工作空间运动轨迹进行规划；
步骤4：对所规划的关节工作空间进行采样，采用“成本收益比”指标对样点进行评价，提高运动规划效率，优化规划的路径更符合人的手臂运动形态。
2.根据权利要求1所述的一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法，其特征在于：在步骤3中，所述求避关节极限的臂形角空间和避障碍物的臂形角空间交集，其具体求解方法如下：
对于给定的末端位姿，输入末端位姿的6个约束，并输入3个控制参数arm，elbow，wrist，求解避关节极限的臂形角区间和避障碍物的臂形角区间；
(1)避关节极限
将臂形角Ψ映射到关节转角，输入给定的末端位姿约束和每个关节转角的极限值，计算满足关节转角极限的臂形角Ψ的取值区间；
设前三个关节转角分别为q1,q2,q3，当前三个关节角都为0时，记为Σo
<mrow>
<msub>
<mi>&Sigma;</mi>
<mi>o</mi>
</msub>
<mo>=</mo>
<mo>&lsqb;</mo>
<mtable>
<mtr>
<mtd>
<msup>
<mi>X</mi>
<msub>
<mi>&Sigma;</mi>
<mi>o</mi>
</msub>
</msup>
</mtd>
<mtd>
<msup>
<mi>Y</mi>
<msub>
<mi>&Sigma;</mi>
<mi>o</mi>
</msub>
</msup>
</mtd>
<mtd>
<msup>
<mi>Z</mi>
<msub>
<mi>&Sigma;</mi>
<mi>o</mi>
</msub>
</msup>
</mtd>
</mtr>
</mtable>
<mo>&rsqb;</mo>
<mn>...</mn>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
记坐标系Σarm为ΣD
<mrow>
<msub>
<mi>&Sigma;</mi>
<mi>D</mi>
</msub>
<mo>=</mo>
<msubsup>
<mi>&Sigma;</mi>
<mi>D</mi>
<mrow>
<mi>&Psi;</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
</msubsup>
<mo>&times;</mo>
<mi>R</mi>
<mi>o</mi>
<mi>t</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>,</mo>
<mi>&Psi;</mi>
<mo>)</mo>
</mrow>
<mn>...</mn>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
记肩膀S变换矩阵为RS
<mrow>
<msub>
<mi>R</mi>
<mi>S</mi>
</msub>
<mo>=</mo>
<msub>
<mi>&Sigma;</mi>
<mi>D</mi>
</msub>
<mo>&times;</mo>
<msup>
<msub>
<mi>&Sigma;</mi>
<mi>o</mi>
</msub>
<mi>T</mi>
</msup>
<mo>=</mo>
<msubsup>
<mi>&Sigma;</mi>
<mi>D</mi>
<mrow>
<mi>&Psi;</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
</msubsup>
<mo>&times;</mo>
<mi>R</mi>
<mi>o</mi>
<mi>t</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>,</mo>
<mi>&Psi;</mi>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<msup>
<msub>
<mi>&Sigma;</mi>
<mi>o</mi>
</msub>
<mi>T</mi>
</msup>
<mn>...</mn>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
由
<mrow>
<mi>R</mi>
<mi>o</mi>
<mi>t</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>,</mo>
<mi>&Psi;</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi>&Psi;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi>&Psi;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi>&Psi;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi>&Psi;</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mn>...</mn>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>)</mo>
</mrow>
</mrow>
得
Rot(x,Ψ)＝I3+sinΨ[x×]+(1-cosΨ)[x×]2………………………(5)
式中：I3是3×3单位矩阵，[x×]是单位向量x构成的反对称阵；展开式(5)得到关于Ψ的变换函数，带入式(2)得到ΣD关于Ψ的直观表达式，将ΣD带入式(3)得
RS(q1,q2,q3)＝AssinΨ+BscosΨ+Cs………………………(6)
式中：记As(i,j)为asij；
记Bs(i,j)为bsij；
Cs＝(x×xT)×Σo T，记Cs(i,j)为csij。
从球铰变换公式有
<mrow>
<msub>
<mi>R</mi>
<mi>s</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>,</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>,</mo>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mo>*</mo>
</mtd>
<mtd>
<mo>*</mo>
</mtd>
<mtd>
<mrow>
<msub>
<mi>C</mi>
<mn>1</mn>
</msub>
<msub>
<mi>S</mi>
<mn>2</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>*</mo>
</mtd>
<mtd>
<mo>*</mo>
</mtd>
<mtd>
<mrow>
<msub>
<mi>S</mi>
<mn>1</mn>
</msub>
<msub>
<mi>S</mi>
<mn>2</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>C</mi>
<mn>3</mn>
</msub>
<msub>
<mi>S</mi>
<mn>2</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>S</mi>
<mn>3</mn>
</msub>
<msub>
<mi>S</mi>
<mn>2</mn>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mn>...</mn>
<mrow>
<mo>(</mo>
<mn>7</mn>
<mo>)</mo>
</mrow>
</mrow>
式中：Si指sin(qi)，Ci指cos(qi)，i＝1,2,3，*表示忽略该项；
将系数矩阵As、Bs和Cs求出代入式(6)，并与式(7)对比[3,3]位置的值得：
cosq2＝as33sinΨ+bs33cosΨ+cs33………………………(8)
从q2的范围就能够推导出cosq2的范围，进而计算出满足q2极限范围Ψ的取值范围；同理，分别求出满足q1、q3、q5、q6和q7极限范围Ψ的取值范围；
对于七个关节，除去第四个关节，求交集得避关节极限的Ψ区间为
Ψ＝{Ψ1}∩{Ψ2}∩{Ψ3}∩{Ψ5}∩{Ψ6}∩{Ψ7}………………………(9)
式中，Ψi(i＝1,2,3,,5,6,7)是避关节qi极限的区间；
(2)避障碍物
将臂形角Ψ在取值范围上离散，输入末端位姿约束、臂形角Ψ和控制参数elbow，得到肘关节E的位置；则连杆SE和连杆EW位置已知，运用碰撞检测算法对每一个Ψ的取值，测试连杆SE和连杆EW是否与障碍物发生碰撞；从所有测试结果中，解算出没有发生碰撞的臂形角Ψ的若干个连续区间的并集；
给定末端位姿Td将末端位置沿着Z7相反方向移动dWT即得到W点位置；记W点位置为pw，则
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>p</mi>
<mi>W</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<msub>
<mi>T</mi>
<mi>d</mi>
</msub>
<mo>*</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>-</mo>
<msub>
<mi>d</mi>
<mrow>
<mi>W</mi>
<mi>T</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mn>...</mn>
<mrow>
<mo>(</mo>
<mn>10</mn>
<mo>)</mo>
</mrow>
</mrow>
对给定末端位姿Td，肩关节S和腕关节W在空间的位置已经固定，过点E在直线SW上的垂足为Oe，求出设为H。，以Oe为坐标系原点，以为X轴正方向，以OeE为Y轴正方向，建立坐标系Oe-XeYeZe，
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>X</mi>
<mi>e</mi>
</msub>
<mo>=</mo>
<mover>
<mrow>
<mi>S</mi>
<mi>W</mi>
</mrow>
<mo>&RightArrow;</mo>
</mover>
<mo>/</mo>
<mo>|</mo>
<mo>|</mo>
<mover>
<mrow>
<mi>S</mi>
<mi>W</mi>
</mrow>
<mo>&RightArrow;</mo>
</mover>
<mo>|</mo>
<mo>|</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>Y</mi>
<mi>e</mi>
</msub>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mi>Z</mi>
<mo>&times;</mo>
<msub>
<mi>X</mi>
<mi>e</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>/</mo>
<mo>|</mo>
<mo>|</mo>
<mi>Z</mi>
<mo>&times;</mo>
<msub>
<mi>X</mi>
<mi>e</mi>
</msub>
<mo>|</mo>
<mo>|</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>Z</mi>
<mi>e</mi>
</msub>
<mo>=</mo>
<msub>
<mi>X</mi>
<mi>e</mi>
</msub>
<mo>&times;</mo>
<msub>
<mi>Y</mi>
<mi>e</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mn>...</mn>
<mrow>
<mo>(</mo>
<mn>11</mn>
<mo>)</mo>
</mrow>
</mrow>
向量
向量
已知Oe-XeYeZe的齐次变换矩阵Te
在中位置取为Ψ的起始线，E在Oe-XeYeZe中的坐标如式(14)
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msup>
<mi>E</mi>
<msub>
<mi>Z</mi>
<mi>e</mi>
</msub>
</msup>
<mo>=</mo>
<mi>H</mi>
<mi> </mi>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&Psi;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<mi>E</mi>
<msub>
<mi>Y</mi>
<mi>e</mi>
</msub>
</msup>
<mo>=</mo>
<mo>-</mo>
<mi>H</mi>
<mi> </mi>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&Psi;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<mi>E</mi>
<msub>
<mi>X</mi>
<mi>e</mi>
</msub>
</msup>
<mo>=</mo>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mn>...</mn>
<mrow>
<mo>(</mo>
<mn>15</mn>
<mo>)</mo>
</mrow>
</mrow>
求出E在O-XYZ中的坐标E：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>E</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<msub>
<mi>T</mi>
<mi>e</mi>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msup>
<mi>E</mi>
<msub>
<mi>X</mi>
<mi>e</mi>
</msub>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mi>E</mi>
<msub>
<mi>Y</mi>
<mi>e</mi>
</msub>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mi>E</mi>
<msub>
<mi>z</mi>
<mi>e</mi>
</msub>
</msup>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mn>...</mn>
<mrow>
<mo>(</mo>
<mn>16</mn>
<mo>)</mo>
</mrow>
</mrow>
则S、E、W、T四个点的位置确定，从而确定连杆SE、连杆EW和连杆WT位置，利用碰撞检测算法判断连杆是否与障碍物发生碰撞；
综上所述，将避关节极限子空间和避障碍物子空间的臂形角求交集，得到在给定机械臂末端位姿下避关节极限且避障碍物的所有可行的机械臂运动学逆解。
3.根据权利要求1所述的一种基于臂形角区间的冗余自由度机械臂运动规划及评价方法，其特征在于：在步骤4中，所述对所规划的关节工作空间进行采样，采用“成本收益比”指标对采样点进行评价，其具体实现方法如下：
对机械臂的腕关节位置空间进行采样，对每一个采样点xnew判断腕关节到机械臂末端之间的连杆是否碰撞，如果碰撞则放弃该采样点；如果没有碰撞则求解该位置对应的避关节极限形角区间和避障碍物臂形角区间的交集，若交集不为空集，则该采样点成为一个Xfree中的采样点，并记录每个采样点的臂形角区间；
对所加入的每一个采样点，采用“成本收益比ratio”进行评价：
<mrow>
<mi>r</mi>
<mi>a</mi>
<mi>t</mi>
<mi>i</mi>
<mi>o</mi>
<mo>=</mo>
<mi>ln</mi>
<mrow>
<mo>(</mo>
<mo>|</mo>
<mfrac>
<mrow>
<mi>cos</mi>
<mi>t</mi>
<mi>P</mi>
<mi>a</mi>
<mi>t</mi>
<mi>h</mi>
</mrow>
<mrow>
<mi>p</mi>
<mi>r</mi>
<mi>o</mi>
<mi>P</mi>
<mi>r</mi>
<mi>o</mi>
<mi>j</mi>
<mi>e</mi>
<mi>c</mi>
<mi>t</mi>
</mrow>
</mfrac>
<mo>|</mo>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>l</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mo>|</mo>
<mfrac>
<mrow>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>X</mi>
<mi>s</mi>
</msub>
<msub>
<mi>X</mi>
<mi>g</mi>
</msub>
<mo>|</mo>
<mo>|</mo>
<mo>-</mo>
<mi>p</mi>
<mi>r</mi>
<mi>o</mi>
<mi>P</mi>
<mi>r</mi>
<mi>o</mi>
<mi>j</mi>
<mi>e</mi>
<mi>c</mi>
<mi>t</mi>
</mrow>
<mrow>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>X</mi>
<mi>s</mi>
</msub>
<msub>
<mi>X</mi>
<mi>g</mi>
</msub>
<mo>|</mo>
<mo>|</mo>
</mrow>
</mfrac>
<mo>|</mo>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
式中，成本costPath是根节点到该采样点的欧氏距离路程；收益proProject是从根节点到该采样点在根节点到目标点连线上的投影；Xs的ratio为0，x2的路程成本为(d1+d2)，X2的收益为p2。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710927923.5A CN107729637A (zh) | 2017-10-09 | 2017-10-09 | 基于臂形角区间的冗余自由度机械臂运动规划及评价方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710927923.5A CN107729637A (zh) | 2017-10-09 | 2017-10-09 | 基于臂形角区间的冗余自由度机械臂运动规划及评价方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107729637A true CN107729637A (zh) | 2018-02-23 |
Family
ID=61209359
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710927923.5A Pending CN107729637A (zh) | 2017-10-09 | 2017-10-09 | 基于臂形角区间的冗余自由度机械臂运动规划及评价方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107729637A (zh) |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108481324A (zh) * | 2018-04-25 | 2018-09-04 | 中国科学院合肥物质科学研究院 | 一种八轴多功能机械臂的逆解工程及其碰撞检测算法 |
| CN108537404A (zh) * | 2018-03-06 | 2018-09-14 | 中国人民解放军63920部队 | 一种地外天体探测采样区可采性评估方法、介质及设备 |
| CN108789406A (zh) * | 2018-06-05 | 2018-11-13 | 上海大学 | 一种用于冗余机械臂可达性分析的逆解求解方法 |
| CN108839025A (zh) * | 2018-07-12 | 2018-11-20 | 杭州电子科技大学 | 一种移动机械臂的运动规划方法和装置 |
| CN109033688A (zh) * | 2018-08-16 | 2018-12-18 | 居鹤华 | 基于轴不变量的通用7r机械臂逆解建模与解算方法 |
| CN112530000A (zh) * | 2020-11-20 | 2021-03-19 | 西安科技大学 | 一种任务约束下虚拟人臂运动复现的方法 |
| CN113084791A (zh) * | 2019-12-23 | 2021-07-09 | 深圳市优必选科技股份有限公司 | 机械臂控制方法、机械臂控制装置及终端设备 |
| CN113119131A (zh) * | 2021-05-08 | 2021-07-16 | 北京爱康宜诚医疗器材有限公司 | 机器人的控制方法、装置、计算机可读存储介质及处理器 |
| CN113119104A (zh) * | 2019-12-31 | 2021-07-16 | 深圳市优必选科技股份有限公司 | 机械臂控制方法、机械臂控制装置、计算设备及系统 |
| CN115464650A (zh) * | 2022-09-19 | 2022-12-13 | 哈尔滨工业大学 | 一种针对动态障碍物的冗余度机械臂避障模型的构建方法 |
| CN116985147A (zh) * | 2023-09-27 | 2023-11-03 | 睿尔曼智能科技（北京）有限公司 | 机械臂逆运动学求解方法及装置 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110106302A1 (en) * | 2009-10-30 | 2011-05-05 | Hong Fu Jin Precision Industry (Shenzhen) Co., Ltd. | Robot arm assembly and industrial robot using the same |
| CN102509025A (zh) * | 2011-11-25 | 2012-06-20 | 苏州大学 | 一种六自由度仿人灵巧臂逆运动学的快速求解方法 |
| CN102646148A (zh) * | 2012-04-25 | 2012-08-22 | 浙江大学 | 仿人机器人机械臂避碰运动轨迹规划方法 |
| CN104635762A (zh) * | 2015-01-13 | 2015-05-20 | 北京航空航天大学 | 一种面向srs仿人手臂的自运动角计算方法 |
| CN106228260A (zh) * | 2016-01-26 | 2016-12-14 | 西北工业大学 | 一种平面三自由度空间机器人逆运动学求解方法 |
-
2017
- 2017-10-09 CN CN201710927923.5A patent/CN107729637A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110106302A1 (en) * | 2009-10-30 | 2011-05-05 | Hong Fu Jin Precision Industry (Shenzhen) Co., Ltd. | Robot arm assembly and industrial robot using the same |
| CN102509025A (zh) * | 2011-11-25 | 2012-06-20 | 苏州大学 | 一种六自由度仿人灵巧臂逆运动学的快速求解方法 |
| CN102646148A (zh) * | 2012-04-25 | 2012-08-22 | 浙江大学 | 仿人机器人机械臂避碰运动轨迹规划方法 |
| CN104635762A (zh) * | 2015-01-13 | 2015-05-20 | 北京航空航天大学 | 一种面向srs仿人手臂的自运动角计算方法 |
| CN106228260A (zh) * | 2016-01-26 | 2016-12-14 | 西北工业大学 | 一种平面三自由度空间机器人逆运动学求解方法 |
Non-Patent Citations (1)
| Title |
|---|
| 王美玲: "面向救援任务的双臂机器人协作运动规划与控制方法研究", 《中国博士学位论文全文数据库 信息科技辑》 * |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108537404A (zh) * | 2018-03-06 | 2018-09-14 | 中国人民解放军63920部队 | 一种地外天体探测采样区可采性评估方法、介质及设备 |
| CN108537404B (zh) * | 2018-03-06 | 2021-10-22 | 中国人民解放军63920部队 | 一种地外天体探测采样区可采性评估方法、介质及设备 |
| CN108481324A (zh) * | 2018-04-25 | 2018-09-04 | 中国科学院合肥物质科学研究院 | 一种八轴多功能机械臂的逆解工程及其碰撞检测算法 |
| CN108481324B (zh) * | 2018-04-25 | 2021-04-20 | 中国科学院合肥物质科学研究院 | 一种八轴多功能机械臂的逆解工程及其碰撞检测算法 |
| CN108789406A (zh) * | 2018-06-05 | 2018-11-13 | 上海大学 | 一种用于冗余机械臂可达性分析的逆解求解方法 |
| CN108839025A (zh) * | 2018-07-12 | 2018-11-20 | 杭州电子科技大学 | 一种移动机械臂的运动规划方法和装置 |
| CN109033688A (zh) * | 2018-08-16 | 2018-12-18 | 居鹤华 | 基于轴不变量的通用7r机械臂逆解建模与解算方法 |
| CN109033688B (zh) * | 2018-08-16 | 2020-03-31 | 居鹤华 | 基于轴不变量的通用7r机械臂逆解建模与解算方法 |
| CN113084791A (zh) * | 2019-12-23 | 2021-07-09 | 深圳市优必选科技股份有限公司 | 机械臂控制方法、机械臂控制装置及终端设备 |
| CN113119104A (zh) * | 2019-12-31 | 2021-07-16 | 深圳市优必选科技股份有限公司 | 机械臂控制方法、机械臂控制装置、计算设备及系统 |
| CN113119104B (zh) * | 2019-12-31 | 2022-08-05 | 深圳市优必选科技股份有限公司 | 机械臂控制方法、机械臂控制装置、计算设备及系统 |
| CN112530000A (zh) * | 2020-11-20 | 2021-03-19 | 西安科技大学 | 一种任务约束下虚拟人臂运动复现的方法 |
| CN113119131A (zh) * | 2021-05-08 | 2021-07-16 | 北京爱康宜诚医疗器材有限公司 | 机器人的控制方法、装置、计算机可读存储介质及处理器 |
| CN113119131B (zh) * | 2021-05-08 | 2022-08-16 | 北京壹点灵动科技有限公司 | 机器人的控制方法、装置、计算机可读存储介质及处理器 |
| CN115464650A (zh) * | 2022-09-19 | 2022-12-13 | 哈尔滨工业大学 | 一种针对动态障碍物的冗余度机械臂避障模型的构建方法 |
| CN116985147A (zh) * | 2023-09-27 | 2023-11-03 | 睿尔曼智能科技（北京）有限公司 | 机械臂逆运动学求解方法及装置 |
| CN116985147B (zh) * | 2023-09-27 | 2023-12-22 | 睿尔曼智能科技（北京）有限公司 | 机械臂逆运动学求解方法及装置 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107729637A (zh) | 基于臂形角区间的冗余自由度机械臂运动规划及评价方法 | |
| CN110228069B (zh) | 一种机械臂在线避障运动规划方法 | |
| CN111168675B (zh) | 一种家用服务机器人的机械臂动态避障运动规划方法 | |
| CN107214701B (zh) | 一种基于运动基元库的带电作业机械臂自主避障路径规划方法 | |
| CN104965517B (zh) | 一种机器人笛卡尔空间轨迹的规划方法 | |
| CN102646148B (zh) | 仿人机器人机械臂避碰运动轨迹规划方法 | |
| CN108673509A (zh) | 一种六自由度手腕偏置型串联机械臂的运动控制方法 | |
| CN111761582B (zh) | 一种基于随机采样的移动机械臂避障规划方法 | |
| CN106584461A (zh) | 多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法 | |
| CN110682286B (zh) | 一种协作机器人实时避障方法 | |
| CN103692440B (zh) | 一种连续型机器人空间路径跟踪方法 | |
| CN105184019A (zh) | 机器人抓取方法及系统 | |
| CN108972549A (zh) | 基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统 | |
| CN106064377A (zh) | 一种空间机器人动力学参数辨识的激励轨迹优化方法 | |
| CN106475999A (zh) | 刚性条件下基于阻抗模型的双臂协调的加速度控制方法 | |
| CN105537824B (zh) | 一种基于机械臂手眼协调自主焊接控制方法 | |
| CN107160401B (zh) | 一种解决冗余度机械臂关节角偏移问题的方法 | |
| CN109344477B (zh) | 一种6自由度机械臂逆运动学求解方法 | |
| CN104858852B (zh) | 类人机器人实时模仿人体上肢动作的优化与约束方法 | |
| CN108897321A (zh) | 基于领航跟随法的机器人队形可变编队控制方法与控制器 | |
| CN113664829A (zh) | 一种空间机械臂避障路径规划系统、方法、计算机设备及存储介质 | |
| CN115213898A (zh) | 一种基于逆解多目标优化的焊接机器人笛卡尔空间轨迹规划方法 | |
| CN110450165A (zh) | 一种基于零力控制的机器人标定方法 | |
| CN112045664A (zh) | 一种基于ros系统的通用机械臂控制器 | |
| CN109866224A (zh) | 一种机器人雅可比矩阵计算方法、装置及存储介质 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20180223 |
|
| WD01 | Invention patent application deemed withdrawn after publication |