本发明涉及系统建模领域,尤其涉及一种移动平台-机械臂系统建模方法。
背景技术
冗余度机械臂是指自由度数量多于完成任务所必须的自由度数量的机械臂。由于具有更多的自由度,该种类型的机械臂在完成末端执行器的各种任务的同时,还可以完成诸如障碍物躲避、关节角极限约束、机械臂奇异状态等额外工作。利用这样一种冗余特性,越来越多的冗余度机械臂被应用在工业制造和生产生活的各个方面。随着机械臂制造水平的不断提高和新兴医疗产业的不断涌现,近年来,搭载有冗余度机械臂的移动平台逐渐出现在人们的视野当中。一些新颖的、高度智能化的移动平台-机械臂系统应运而生,这些系统逐渐朝着集成化、一体化的方向发展,形成了诸如残疾人康复系统等的现代智能化医疗及康复系统。上述移动平台-机械臂系统可以对患有残疾或瘫痪等疾病的病人提供医疗帮助,并协助其处理和完成日常生活所需的各种操作。随着对该产业投入的不断增大以及市场需求的逐渐提高,对移动平台-机械臂系统各方面的要求也在不断上升,这就需要我们透彻地分析移动平台-机械臂系统的物理模型特性,以便更好地在该系统模型的架构和基础上设计和分析各种性质,进而完成人们期望的目标。
技术实现要素:
本发明的目的在于提供一种移动平台-机械臂系统建模方法。本发明为基于目标系统的模型构建方法,在充分考虑了目标模型系统的物理、数学特征及其参数的情况下,运用物理模型建立与数学模型代入相结合的方法,求解得到该目标系统的运动规划并完成该系统的建模,具有建模准确、思路清晰、逻辑性强、方法简明等特点。
本发明的目的能够通过以下技术方案实现:
一种移动平台-机械臂系统建模方法,具体步骤包括:
(1)根据末端机械臂执行器的运动学模型,构造二次规划问题的速度层运动学方程式;
(2)根据移动平台-机械臂系统的物理特性及模型参数,定义与移动平台相关的坐标系以及末端机械臂执行器的齐次位置表达式;
(3)根据步骤(2)中定义的齐次位置表达式,结合机械臂的d-h模型,构造机械臂的正运动学方程式;
(4)根据步骤(2)中定义的坐标系和步骤(3)中的机械臂正运动学方程式,构造移动平台-机械臂系统的总运动学方程式;
(5)根据步骤(4)中的移动平台-机械臂系统的总运动学方程式,求解系统的齐次变换矩阵;
(6)根据步骤(4)中的总运动学方程式和步骤(5)得到的系统的齐次变换矩阵,求解系统的雅可比矩阵;
(7)将步骤(6)求解得到的雅可比矩阵代入步骤(1)中的二次规划问题运动学方程式,得到末端机械臂执行器的运动规划,完成移动平台-机械臂系统的建模。
具体地,所述步骤(1)中,对于含冗余度的末端机械臂执行器,其期望的目标适用于如下二次规划问题的速度层运动学方程式:
其中,
在本发明中,所述控制矢量由移动平台左右驱动轮的2个旋转角速度以及冗余度机械臂的6个关节角速度所构成。
具体地,在所述步骤(2)中,根据移动平台-机械臂系统的物理特性及模型参数,定义与移动平台相关的坐标系分别为:
(ⅰ)定义移动平台-机械臂系统的工作空间坐标系,即世界坐标系
(ⅱ)定义原点固结在移动平台轮轴中po点的坐标系
(ⅲ)定义原点固结在移动平台上pc点的坐标系
(ⅳ)定义原点固结在末端机械臂执行器上的坐标系
其中,po为移动平台对称轴与两驱动轮轴线的交点;pc为机械臂在移动平台上的安装位置所在的点。
具体地,在所述步骤(2)中,依据实际移动平台-机械臂系统的物理特征,可以定义末端机械臂执行器的齐次位置表达式
具体地,在所述步骤(3)中,根据步骤(2)中末端机械臂执行器的齐次位置表达式,结合机械臂的d-h模型,得到机械臂的正运动学方程式,表示方式如下:
其中,
具体地,在所述步骤(4)中,根据各坐标系的关系和机械臂的正运动学方程即公式(2),可以得到移动平台-机械臂系统的总运动学方程式,表示为:
其中,
具体地,在所述步骤(5)中,对在移动平台轮轴中点坐标系
其中,wr表示移动平台驱动轮的半径;
对在移动平台轮轴中点坐标系
其中,
rc,sinα,cosα以及xoc,yoc,zoc的几何关系具体表示为:
rccosα=xoc(7)
rcsinα=yoc(8)
zoc在xoyo平面上为零。
其中,(xoc,yoc,zoc)为pc点在移动平台坐标系
将公式(7)和(8)代入公式式(6),并依照式(4)和式(5)中所陈述的关系,可以得到如下关系式:
通过左乘旋转变换矩阵,将在移动平台轮轴中点坐标系
其中,
通过对
通过以上各流程及工作,可以得到移动平台的运动学模型,即如下的齐次变换矩阵
其中,(xwc,ywc,zwc)表示点pc在世界坐标系
具体地,在所述步骤(6)中,结合总运动学方程wpend和齐次变换矩阵
其中,齐次变换矩阵
进一步地,在移动平台的运动学方程和机械臂的运动学方程的解析形式都已得到的情况下,对总运动学方程求解关于控制矢量θ的偏导数,即可求得末端机械臂执行器的雅可比矩阵,计算公式为:
具体地,在所述步骤(7)中,将求解得到的末端机械臂执行器的雅可比矩阵代入二次规划问题的运动学方程式中,即可得到末端机械臂执行器的运动规划并完成移动平台-机械臂系统的建模。
本发明相较于现有技术,具有以下的有益效果:
1、本发明为基于目标系统的模型构建方法,在充分考虑了目标模型系统的物理、数学特征及其参数的情况下,运用物理模型建立与数学模型代入相结合的方法,求解得到目标系统的运动规划并完成系统的建模。本发明的方法具有建模准确、思路清晰、逻辑性强以及方法简单明了等优点。
附图说明
图1为本实例中的移动平台-机械臂系统建模方法的流程图;
图2-(a)为本实例中机械臂的示意图;
图2-(b)为本实施例中移动平台-机械臂系统示意图;
图3为本实例中的移动平台的运动学模型示意图;
图4为本实施例中机械臂在移动平台上的安装位置所在点pc在移动轮椅平台轮轴中点po的坐标系
其中,1-冗余度机械臂、2-冗余度机械臂的末端执行器、3-移动平台-机械臂系统中冗余度机械臂部分、4-移动平台-机械臂系统中移动平台部分。
具体实施方式
下面结合实施例及附图对本发明作进一步详细的描述,但本发明的实施方式不限于此。
实施例
如图1所示为一种移动平台-机械臂系统建模方法的具体流程图,所述方法的具体步骤包括:
(1)根据末端机械臂执行器的运动学模型,构造二次规划问题的速度层运动学方程式;
对于含冗余度的末端机械臂执行器,其期望的目标适用于如下二次规划问题的速度层运动学方程式:
其中,
在本实施例中,移动平台-机械臂系统的工作空间为三维坐标空间,即m=3;控制变量为8个,即n=8。
(2)根据移动平台-机械臂系统的物理特性及模型参数,定义与移动平台相关的坐标系以及末端机械臂执行器的齐次位置表达式;
如图2所示为本发明所述的机械臂及移动平台-机械臂系统的示意图。其中,图2-(a)为机械臂示意图,该冗余度机械臂共包含6个关节,如图中标注“1”所示;该冗余度机械臂的末端执行器如图中标注“2”所示。图2-(b)为移动平台-机械臂系统示意图,该系统包含两个主要部分,冗余度机械臂部分如图中标注“3”所示;移动平台部分如图中标注“4”所示。该移动平台共包含有2个主动轮和4个从动轮。移动平台左右驱动轮的2个旋转角速度以及冗余度机械臂的6个关节角速度构成了该移动平台-机械臂系统的8个控制变量。
如图3所示为移动平台的运动学模型示意图。根据移动平台-机械臂系统的物理特性及模型参数,定义与移动平台相关的坐标系分别为:
(i)定义移动平台-机械臂系统的工作空间坐标系,即世界坐标系
(ii)定义原点固结在移动平台轮轴中点po的坐标系
(iii)定义原点固结在移动平台上pc点的坐标系
(iv)定义原点固结在末端机械臂执行器上的坐标系
其中,po为移动平台对称轴与两驱动轮轴线的交点;pc为机械臂在移动平台上的安装位置所在的点。
依据实际移动平台-机械臂系统的物理特征,可以定义末端机械臂执行器的齐次位置表达式
(3)根据步骤(2)中定义的齐次位置表达式,结合机械臂的d-h模型,构造机械臂的正运动学方程式;
根据步骤(2)中末端机械臂执行器的齐次位置表达式,结合机械臂的d-h模型,得到机械臂的正运动学方程式,表示方式如下:
其中,
(4)根据步骤(2)中定义的坐标系和步骤(3)中的机械臂正运动学方程式,构造移动平台-机械臂系统的总运动学方程式;
具体地,在所述步骤(4)中,根据各坐标系的关系和机械臂的正运动学方程即公式(2),可以得到移动平台-机械臂系统的总运动学方程式,表示为:
其中,
(5)根据步骤(4)中的移动平台-机械臂系统的总运动学方程式,求解系统的齐次变换矩阵;
具体地,在所述步骤(5)中,对在移动平台轮轴中点坐标系
其中,wr表示移动平台驱动轮的半径;
对在移动平台轮轴中点坐标系
其中,
rc,sinα,cosα以及xoc,yoc,zoc的几何关系具体表示为:
rccosα=xoc(7)
rcsinα=yoc(8)
其中,(xoc,yoc,zoc)为点pc在移动平台坐标系
将公式(7)和(8)代入公式式(6),并依照式(4)和式(5)中所陈述的关系,可以得到如下关系式:
通过左乘旋转变换矩阵,将在移动平台轮轴中点坐标系
其中,
通过对
通过以上各流程及工作,可以得到移动平台的运动学模型,即如下的齐次变换矩阵
其中,(xwc,ywc,zwc)表示点pc在世界坐标系
(6)根据步骤(4)中的总运动学方程式和步骤(5)得到的系统的齐次变换矩阵,求解系统的雅可比矩阵;
具体地,在所述步骤(6)中,结合总运动学方程wpend和齐次变换矩阵
其中,齐次变换矩阵
进一步地,在移动平台的运动学方程和机械臂的运动学方程的解析形式都已得到的情况下,对总运动学方程求解关于控制矢量θ的偏导数,即可求得末端机械臂执行器的雅可比矩阵,计算公式为:
(7)将步骤(6)求解得到的雅可比矩阵代入步骤(1)中的二次规划问题运动学方程式,得到末端机械臂执行器的运动规划,完成移动平台-机械臂系统的建模。
具体地,在所述步骤(7)中,将求解得到的末端机械臂执行器的雅可比矩阵代入二次规划问题的运动学方程式中,即可得到末端机械臂执行器的运动规划并完成移动平台-机械臂系统的建模。
上述实施例为本发明较佳的实施方式,但本发明的实施方式并不受上述实施例的限制,其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化,均应为等效的置换方式,都包含在本发明的保护范围之内。