一种高自由度机器人逆运动学的快速求解方法及系统与流程

文档序号:12668763阅读:来源:国知局

技术特征:

1.一种高自由度机器人逆运动学的快速求解方法,其特征在于,包括:

步骤1,将关节变量θ带入机器人运动学方程中,获得雅克比矩阵J,将所述雅克比矩阵J进行转置,获得雅克比转置矩阵JT

步骤2,生成一组投机值,为每个投机值计算相应的关节变量更新值,将每个关节变量更新值带入机器人正运动学方程中,获得相应的位姿Pk,为每个位姿Pk计算其与目标位姿P的位姿偏差Δek,及位姿偏差Δek的模errork

步骤3,在模errork的集合中选取最小值errormin,及其对应的位姿偏差Δemin与关节变量更新值Δθmin,并更新位姿偏差为Δe=Δemin,更新关节变量θ=θ+Δθmin

步骤4,判断errormin是否满足errormin<Threshold,其中Threshold为预设的errormin阈值,如果是,则输出关节变量θ并结束,否则,返至所述步骤1,继续执行。

2.如权利要求1所述的高自由度机器人逆运动学的快速求解方法,其特征在于,所述步骤1之前还包括随机生成一组初始值θinit,并且令θ=θinit

将关节变量θ带入机器人正运动学方程中,获得相应的位姿Pinit

计算位姿Pinit与目标位姿P的位姿偏差Δe=P-Pk,及位姿偏差Δe的模error;

判断模error是否满足error<Threshold,如果是,则输出关节变量θ并结束,否则,执行所述步骤1。

3.如权利要求1所述的高自由度机器人逆运动学的快速求解方法,其特征在于,所述步骤2中每个投机值大于0,小于1。

4.如权利要求1所述的高自由度机器人逆运动学的快速求解方法,其特征在于,所述步骤2中为每个投机值计算相应的关节变量更新值的公式为:

Δθk=αk JTΔe

其中Δθk为关节变量更新值,αk为投机值,JT为雅克比转置矩阵,Δe为位姿偏差。

5.如权利要求1所述的高自由度机器人逆运动学的快速求解方法,其特征在于,所述开启m个计算线程,每个线程生成一个投机值,其中第k个线程生成投机值αk,计算公式为:

<mrow> <msub> <mi>&alpha;</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mi>k</mi> <mi>m</mi> </mfrac> <mo>&CenterDot;</mo> <mfrac> <mrow> <mi>&Delta;</mi> <mi>e</mi> <mo>&CenterDot;</mo> <msup> <mi>JJ</mi> <mi>T</mi> </msup> <mi>&Delta;</mi> <mi>e</mi> </mrow> <mrow> <msup> <mi>JJ</mi> <mi>T</mi> </msup> <mi>&Delta;</mi> <mi>e</mi> <mo>&CenterDot;</mo> <msup> <mi>JJ</mi> <mi>T</mi> </msup> <mi>&Delta;</mi> <mi>e</mi> </mrow> </mfrac> </mrow>

其中αk为投机值,Δe为位姿偏差,JT为雅克比转置矩阵,雅克比矩阵J。

6.一种高自由度机器人逆运动学的快速求解系统,其特征在于,包括:

获得雅克比转置矩阵模块,用于将关节变量θ带入机器人运动学方程中,获得雅克比矩阵J,将所述雅克比矩阵J进行转置,获得雅克比转置矩阵JT

获得位姿偏差模块,用于生成一组投机值,为每个投机值计算相应的关节变量更新值,将每个关节变量更新值带入机器人正运动学方程中,获得相应的位姿Pk,为每个位姿Pk计算其与目标位姿P的位姿偏差Δek,及位姿偏差Δek的模errork

更新关节变量模块,用于在模errork的集合中选取最小值errormin,及其对应的位姿偏差Δemin与关节变量更新值Δθmin,并更新位姿偏差为Δe=Δemin,更新关节变量θ=θ+Δθmin

判断模块,用于判断errormin是否满足errormin<Threshold,其中Threshold为预设的errormin阈值,如果是,则输出关节变量θ并结束,否则,返至所述获得雅克比转置矩阵模块,继续执行。

7.如权利要求6所述的高自由度机器人逆运动学的快速求解系统,其特征在于,所述获得雅克比转置矩阵模块之前还包括随机生成一组初始值θinit,并且令θ=θinit

将关节变量θ带入机器人正运动学方程中,获得相应的位姿Pinit

计算位姿Pinit与目标位姿P的位姿偏差Δe=P-Pk,及位姿偏差Δe的模error;

判断模error是否满足error<Threshold,如果是,则输出关节变量θ并结束,否则,执行所述步骤1。

8.如权利要求6所述的高自由度机器人逆运动学的快速求解系统,其特征在于,所述获得位姿偏差模块中每个投机值大于0,小于1。

9.如权利要求6所述的高自由度机器人逆运动学的快速求解系统,其特征在于,所述获得位姿偏差模块中为每个投机值计算相应的关节变量更新值的公式为:

Δθk=αk JTΔe

其中Δθk为关节变量更新值,αk为投机值,JT为雅克比转置矩阵,Δe为位姿偏差。

10.如权利要求6所述的高自由度机器人逆运动学的快速求解系统,其特征在于,所述开启m个计算线程,每个线程生成一个投机值,其中第k个线程生成投机值αk,计算公式为:

<mrow> <msub> <mi>&alpha;</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mi>k</mi> <mi>m</mi> </mfrac> <mo>&CenterDot;</mo> <mfrac> <mrow> <mi>&Delta;</mi> <mi>e</mi> <mo>&CenterDot;</mo> <msup> <mi>JJ</mi> <mi>T</mi> </msup> <mi>&Delta;</mi> <mi>e</mi> </mrow> <mrow> <msup> <mi>JJ</mi> <mi>T</mi> </msup> <mi>&Delta;</mi> <mi>e</mi> <mo>&CenterDot;</mo> <msup> <mi>JJ</mi> <mi>T</mi> </msup> <mi>&Delta;</mi> <mi>e</mi> </mrow> </mfrac> </mrow>

其中αk为投机值,Δe为位姿偏差,JT为雅克比转置矩阵,雅克比矩阵J。

当前第2页1 2 3 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1