专利名称：一种冗余度机械臂运动规划的初始化方法
技术领域：
本发明属于一种冗余度机械臂运动规划的初始化方法。
背景技术：
冗余度机械臂是一种自由度大于任务空间所需最少自由度的末端能动机械装置， 其运动任务包括焊接、油漆、组装、挖掘和绘图等，广泛应用于装备制造、产品加工、机器作 业等国民经济生产活动中。冗余度机械臂在空间不同位置执行任务时，当结束一个任务后， 往往需要从当前位置移动到下一个任务的执行位置。一般而言，冗余度机械臂从当前位置 移动到下一个任务的执行位置，都是通过慢慢移动机械臂并不断调整来实现。然而，这种做 法是相当繁琐且存在着不足的每次改变任务操作位置时都有可能需要多次度量各个关节 角以使机械臂准确到达任务执行的位置；并且任务执行位置测量(或调整)的不准确也会 影响到机械臂正常完成任务。
发明内容
本发明的目的在于克服一般技术的不足，提供一种计算量小、操作方便、工作量 少、作业规范的冗余度机械臂运动规划初始化方法。为了实现上述发明目的，采用的技术方案如下一种冗余度机械臂运动规划的初始化方法，包括如下步骤1)启动冗余度机械臂控制器，将冗余度机械臂调节到极限位置；2)将步骤1)的极限位置作为起始值，并设定机械臂任务执行位置作为终止值，从 而把机械臂的控制转化为二次型规划问题；3)将步骤2)的二次型规划问题利用二次型规划求解器进行求解；4)将步骤3)的求解结果用于驱动各关节电机使机械臂由极限位置转动到任务执 行位置。上述技术方案中，所述步骤1)将冗余度机械臂调节到极限位置，具体操作是依次 驱动各个关节的电机使其最终达到各自的极限位置。所述步骤2)把机械臂的控制转化为二次型规划问题，其性能指标为 々^f々/2 + zM，约束条件为Θ-彡θ ( θ+，欠幺汐幺一，其中…为机械臂关节角速度，W 为合适维数的单位阵，ζ = λ ( θ - θ f)表示关节位移幅值偏差，λ > 0是用来控制关节位 移幅值的设计参数，θ ,为任务执行初始位置，θ ±、^±分别表示机械臂的关节角度极限、关 节角速度极限。所述步骤3)利用二次型规划求解器来求解二次型规划问题，具体是将该二次型 规划问题变换为分段线性投影方程，从而构造二次型规划求解器进行求解。所述二次型规划求解器采用基于线性变分不等式的原对偶神经网络求解器或二 次规划数值方法。所述步骤4)将二次型规划求解器的结果转化为电机驱动所需要的脉冲频率，从而驱动各关节电机使机械臂由极限位置转动到任务执行位置。本发明能有效克服现有技术的不足，提供一种计算量小、操作方便、工作量少、作 业规范的冗余度机械臂运动规划初始化方法。
图1为本发明的流程图；图2为实现本发明的机械臂关节极限状态俯视图；图3为实现本发明的机械臂关节极限状态主视图；图4为实现本发明的机械臂任务初始关节位置俯视图；图5为实现本发明的机械臂任务初始关节位置主视具体实施例方式
下面结合附图对本发明做进一步的说明。图1所示的一种冗余度机械臂运动规划的初始化方法主要由机械臂调节到极限 位置1、转为二次型规划问题2、二次规划型求解器3、驱动机械臂转到任务执行位置4组成。图2、图3、图4和图5所示的冗余度机械臂为一个平面六连杆冗余度机械臂，主要 由关节5、关节6、关节7、关节8、关节9和关节10组成。首先启动冗余度机械臂控制器，将冗余度机械臂调节到极限位置；再将该极限位 置作为初始化方法的起始值，并设定机械臂任务执行位置作为初始化方法的终止值，从而 把机械臂的控制问题转化为相应的二次型规划问题；然后构造相应的二次型规划求解器将 该问题求解；最后把求解结果用于驱动机械臂的各关节电机使其由极限位置转动到任务执 行位置。基于最小化机械臂关节当前位置与任务执行位置之间位移量的思想，机械臂运动 规划的初始化方法可设计为最小化&炉々/2 + /汐(1)约束条件θ _彡θ彡θ +，(2)0- 10 么 θ+。(3)其中，W为合适维数的单位阵，ζ = λ ( θ - θ f) e Rn, λ > 0是用来控制关节位移 幅值的设计参数，θ 任务执行初始位置，θ i、炉分别表示关节角度极限、关节角速度极 限。考虑到上述优化问题是在速度层上求解，因此，将机械臂的关节角度约束(2)和关节角 速度约束(3)合并，便可得到基于角速度力的双端不等式约束ξ~ <θ<ξ+,其中，ξ_和ξ+中的第i个元素可分别定义为《,-=Π^χ{^,// -代)}，
6+=min0+,//(^+-《)丨。常数y是用来调节由关节角速度决定的可行域。用χ表示机 械臂的关节角速度力，带物理约束的机械臂运动规划初始化方法(1)_(3)便可描述为如下 的二次型规划方案最小化xTWx/2+zTx，(4)约束条件ξ _彡χ彡ξ +，(5)
其又等价于如下的分段线性投影方程P Ω (χ- (ffx+z)) -χ = 0。(6)其中，集合0 = {太|广<太<€+}〔灭"，投影算子？￡2(·)是从铲到Ω上的，它的 每一个元素函数可以定义如下
《,-, if Xi < ξ~-x,, if ξ； < Χ, <ξ；, = 1,2,.. °
ifX对于上述分段线性投影方程(6)和二次规划问题(4)_ (5)，可采用如下算法求解。首先，定义计算误差e(x) = χ-ΡΩ (x-(Wx+z))。(7)显然，当误差为零时，对应的χ值便为二次规划问题(4)_(5)的解。然后，给定初 始值x° e Rn，通过如下的迭代方法来得到χ更新值使得误差(7)达到预设的精度xk+1 = xk-p (xk) d (Xk)，(8)其中，k = 0，1，2，…，d(xk)和P (xk)分别定义为d(xk) = (ffT+I) e (xk) ,(9)厂(？)。(10)
W) Il2、…通过算法不断迭代，便可得到分段线性投影方程(6)的解，从而得到二次型优化 问题(4)-(5)的最优解。通过上位机的二次型规划求解器得到该二次型规划问题的解后，再将结果转化为 电机驱动所需要的脉冲频率，并通过该冗余度机械臂的控制器来驱动机械臂各个关节电机 使其由极限位置转动到任务执行位置，从而完成冗余度机械臂运动规划的初始化工作。
权利要求
一种冗余度机械臂运动规划的初始化方法，其特征在于包括如下步骤1)启动冗余度机械臂控制器，将冗余度机械臂调节到极限位置；2)将步骤1)的极限位置作为起始值，并设定机械臂任务执行位置作为终止值，从而把机械臂的控制转化为二次型规划问题；3)将步骤2)的二次型规划问题利用二次型规划求解器进行求解；4)将步骤3)的求解结果用于驱动各关节电机使机械臂由极限位置转动到任务执行位置。
2.根据权利要求1所述的冗余度机械臂运动规划方法，其特征在于所述步骤1)将冗余 度机械臂调节到极限位置，具体操作是依次驱动各个关节的电机使其最终达到各自的极限 位置。
3.根据权利要求1或2所述的冗余度机械臂运动规划方法，其特征在于所述步骤 2)把机械臂的控制转化为二次型规划问题，其性能指标为俨妒6/2+ 76，约束条件为 Θ —≤θ≤θ+，力-<々<尔，其中j为机械臂关节角速度，W为合适维数的单位阵，ζ = λ ( θ - θ f)表示关节位移幅值偏差，λ > 0是用来控制关节位移幅值的设计参数，θ f为任 务执行初始位置，θ +、力i分别表示机械臂的关节角度极限、关节角速度极限。
4.根据权利要求3所述的冗余度机械臂运动规划方法，其特征在于所述步骤3)利用二 次型规划求解器来求解二次型规划问题，具体是将该二次型规划问题变换为分段线性投影 方程，从而构造二次型规划求解器进行求解。
5.根据权利要求4所述的冗余度机械臂运动规划方法，其特征在于所述二次型规划求 解器采用基于线性变分不等式的原对偶神经网络求解器或二次规划数值方法。
6.根据权利要求5所述的冗余度机械臂运动规划方法，其特征在于所述步骤4)将二次 型规划求解器的结果转化为电机驱动所需要的脉冲频率，从而驱动各关节电机使机械臂由 极限位置转动到任务执行位置。
全文摘要
本发明提供了一种冗余度机械臂运动规划的初始化方法，包括如下步骤1)启动冗余度机械臂控制器，将冗余度机械臂调节到极限位置；2)将步骤1)的极限位置作为本初始化方法的起始值，并设定机械臂任务执行位置作为初始化方法的终止值，从而把机械臂的控制问题转化为相应的二次型规划问题；3)将步骤2)的二次型规划问题利用二次型规划求解器进行求解；4)将步骤3)的求解结果用于驱动各关节电机使机械臂由极限位置转动到任务执行位置。本发明基于二次型规划的初始化方法，避免了每次任务操作时都需要度量各个关节角度的繁琐过程，同时根据任务要求规范了每次作业时的起始位置。
文档编号B25J13/00GK101890718SQ201010193200
公开日2010年11月24日 申请日期2010年6月1日 优先权日2010年6月1日
发明者张智军, 张雨浓, 郭东生 申请人:中山大学