CN112936264B - 一种面向臂手假肢的拟人运动规划方法及系统 - Google Patents
一种面向臂手假肢的拟人运动规划方法及系统 Download PDFInfo
- Publication number
- CN112936264B CN112936264B CN202110116130.1A CN202110116130A CN112936264B CN 112936264 B CN112936264 B CN 112936264B CN 202110116130 A CN202110116130 A CN 202110116130A CN 112936264 B CN112936264 B CN 112936264B
- Authority
- CN
- China
- Prior art keywords
- track
- arm
- prosthesis
- motion
- trajectory
- 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.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Prostheses (AREA)
- Manipulator (AREA)
Abstract
本发明属于康复机器人领域,并具体公开了一种面向臂手假肢的拟人运动规划方法及系统,其包括如下步骤:S1、采集人体上肢运动数据和空间障碍物信息,并根据人体上肢运动数据得到模板轨迹;S2、构建臂手假肢运动链模型以模拟臂手假肢运动,并预设一条臂手假肢运动的初始轨迹;S3、构建臂手假肢运动轨迹优化的目标函数,其包括示教项、避障项、光滑项;S4、根据目标函数对初始轨迹进行优化,得到臂手假肢的最终运动轨迹,完成臂手假肢的拟人运动规划。本发明方法无需对轨迹做后期光滑处理,同时保证轨迹具有拟人性,也无需对特定动作进行单独设计,满足了臂手假肢领域需求。
Description
技术领域
本发明属于康复机器人领域,更具体地,涉及一种面向臂手假肢的拟人运动规划方法及系统。
背景技术
现阶段运动规划的方法主要有基于随机采样的方法和基于优化的方法。
基于随机采样的运动规划算法在复杂环境中也能找到一条路径,因此广泛用于移动机器人的路径规划中,但是在机械臂领域,由于机械臂的自由度一般比较高,比如拟人机械臂一般有7个自由度,在高纬度空间中,随机采样算法需要搜寻的空间过大,效率低,随着维度的增加,搜索空间成指数上升,带来了维度灾难。
基于优化的运动规划方法的主要思路是将运动规划问题转化为一个优化问题,将轨迹序列作为泛函输入,将约束条件作为优化问题的代价项,相比较随机采样的方法,基于优化的运动规划方法效率更高,轨迹规划的耗时短。但是该方法中也存在一些问题,如约束条件如何设置以获得最优的运动轨迹,以及针对特定行为,需要手动设计比较合适的目标函数,比如机器人模仿人解绳子这一个特定动作,这样一个轨迹规划目标不易找到合适的目标函数。
发明内容
针对现有技术的以上缺陷或改进需求,本发明提供了一种面向臂手假肢的拟人运动规划方法及系统,其目的在于,通过构建臂手假肢运动链模型,以及包含示教项、避障项、光滑项的目标函数实现机械臂的拟人运动规划方法,从而保证规划轨迹的光滑,无需对轨迹做后期光滑处理,同时保证得到的轨迹具有拟人性,满足臂手假肢领域需求。
为实现上述目的,按照本发明的一方面,提出了一种面向臂手假肢的拟人运动规划方法,包括如下步骤:
S1、采集人体上肢运动数据和空间障碍物信息,并根据人体上肢运动数据得到模板轨迹;
S2、构建臂手假肢运动链模型以模拟臂手假肢运动,并预设臂手假肢运动的初始轨迹;
其中,ξ为待优化轨迹;为示教项,其根据待优化轨迹与模板轨迹间的距离确定;为避障项,其根据臂手假肢按待优化轨迹运动时,臂手假肢上点扫过的区域与空间中障碍物间的距离确定;为光滑项,其根据待优化轨迹的总体速度确定;a、b、c分别为示教项、避障项、光滑项的权值参数;
S4、根据目标函数对初始轨迹进行优化,得到臂手假肢的最终运动轨迹,完成臂手假肢的拟人运动规划。
作为进一步优选的,确定模板轨迹的具体步骤如下:采集人体上肢进行不同功能性动作的运动数据,从而得到不同功能性动作的多条均值轨迹;根据拟人规划的目标点位置,计算该目标点与各条均值轨迹的马氏距离,以马氏距离最小的一条均值轨迹作为模板轨迹。
作为进一步优选的,所述步骤S4中,根据目标函数对初始轨迹进行优化,具体包括如下步骤:
S41、以初始轨迹作为待优化轨迹,代入目标函数得到代价值,判断代价值是否小于预设阈值,若否,则转入步骤S42;若是,则此时的轨迹即为最终运动轨迹;
S42、通过梯度增量对待优化轨迹进行更新;
S43、以更新后的轨迹作为待优化轨迹,代入目标函数得到代价值,判断代价值是否小于预设阈值,若否,则重复步骤S42;若是,则此时的轨迹即为最终运动轨迹,完成臂手假肢的拟人运动规划。
其中,将臂手假肢看作一连串球的集合,B表示臂手假肢处于当前构型下的球集合;x表示前向运动学,即将处于构型空间中t时刻状态下的臂手假肢上的点u映射到工作空间中;c表示工作空间中的代价函数。
其中,Kv为待优化轨迹的一阶微分矩阵。
作为进一步优选的,示教项、避障项、光滑项的权值参数分别为:a=0.1,b=0.5,c=0.4。
作为进一步优选的,所述臂手假肢运动链模型包括多个关节,根据各关节的起点、终点,通过多次插值得到臂手假肢运动的初始轨迹。
按照本发明的另一方面,提供了一种面向臂手假肢的拟人运动规划系统,包括存储器和规划器,其中,所述存储器上存储有计算机程序,该计算机程序被所述规划器执行时实现上述规划方法。
总体而言,通过本发明所构思的以上技术方案与现有技术相比,主要具备以下的技术优点:
1.本发明通过构建臂手假肢运动链模型,以及包含示教项、避障项、光滑项的目标函数实现了机械臂的拟人运动规划,相比较随机采样的运动规划算法,本发明方法效率更高,轨迹规划的耗时短,保证了规划轨迹的光滑,无需对轨迹做后期光滑处理,可以直接发送给机器人控制器执行,同时保证得到的轨迹具有拟人性,满足臂手假肢领域需求。
2.本发明通过采集人体上肢进行不同功能性动作的运动数据,得到多条均值轨迹,进而根据拟人规划的目标点位置,从各均值轨迹中确定模板轨迹,即本发明可以根据规划目标快速确定模板轨迹,进而完成对初始轨迹的优化,无需对特定动作进行单独设计。
3.本发明为了方便计算机械臂与障碍物之间的距离,对机械臂做了简化处理,将其看作一连串球的集合,以便于避障项的确定;此外,确定了示教项、避障项、光滑项的权值参数,以在保证轨迹避障、光滑的情况下,使输出轨迹的拟人性最好。
附图说明
图1为本发明实施例机械臂模型示意图;
图2为本发明实施例面向臂手假肢的拟人运动规划方法流程图;
图3为本发明实施例机器人避障简化模型;
图4为本发明实施例规划器工作流程图;
图5中(a)~(i)为本发明实施例按照时间顺序排列的拟人运动规划轨迹序列。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。此外,下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
本发明实施例提供的一种面向臂手假肢的拟人运动规划方法,如图2和图4所示,包括如下步骤:
S1、采集人体上肢运动数据,并得到模板轨迹,具体包括如下步骤:
S11、采集人体上肢运动数据:1)标定识别摄像机组初始位置,以及设置原点;2)在受试者身上贴标记点;3)对受试者做静态标定;4)对每位受试者分别采集摸头、摸嘴、摸右耳、摸肚子、摸右肩等日常生活生活中动作,每一组动作采集三次;
S12、根据采集到的人体上肢运动数据,计算得到不同动作对应的均值轨迹及协方差矩阵,每条轨迹由98个轨迹点组成,每一个轨迹点有一个7*7协方差矩阵来描述该点能够泛化程度;
S13、根据拟人规划的目标点位置,分别计算该目标点与各条均值轨迹的马氏距离,以马氏距离最小的一条均值轨迹作为模板轨迹。
S2、构建臂手假肢运动链模型以模拟臂手假肢运动,并得到臂手假肢运动的初始轨迹,具体包括如下步骤:
S21、构建臂手假肢运动链模型:如图1所示,整个运动链有七个自由度,包括三个肩部关节、三个腕部关节和一个肘部关节,其中,肩部三个关节的转动轴线Z1、Z2、Z3交于一点,腕部三个关节的转动轴线Z5、Z6、Z7交于一点。
具体的,连杆关节模型通过通用机器人描述文件(urdf)来描述,文件定义了每个连杆坐标系的位置和姿态,以及惯性矩阵。对于关节,urdf文件指明了关节类型,比如是旋转关节还是移动关节;关节描述文件指定了关节副相连接的两个连杆,其次urdf还指定了每个关节的运动学、动力学限制范围。
机器人通用描述文件具体的建立方式为:首先在三维建模软件Solidworks中绘制机器人各个零件,建立装配关系;然后利用SoildWorks自带的开源插件sw2urdf导出机器人模型,保存格式为urdf格式,在这个基础上根据实际机器人运动学参数,修改连杆的属性,比如质量、颜色等;根据机械臂每一个关节的运动范围确定关节角的上下极限。
S22、确定初始轨迹:在关节空间中,根据臂手假肢运动链模型中各关节的起点、终点,通过多次插值得到一条待优化的初始轨迹,具体为线性插值、三次多项式插值或五次多项式插值。
其中,ξ为待优化轨迹;为示教项,其根据待优化轨迹与模板轨迹间的距离确定;为避障项,其根据臂手假肢按待优化轨迹运动时,臂手假肢上点扫过的区域与空间中障碍物间的距离确定;为光滑项,其根据待优化轨迹的总体速度确定;a、b、c分别为示教项、避障项、光滑项的权值参数,进一步的,本发明中选取a=0.1,b=0.5,c=0.4,使得输出轨迹拟人性最好。
具体的,所述示教项、避障项、光滑项计算方式如下:
(2)为了方便计算臂手假肢与障碍物之间的距离,对臂手假肢做了简化处理,如图3所示,t=0表示轨迹起点,t=1表示机器人处于轨迹终点;将臂手假肢看作是一连串球的集合,u=0表示第一个球,u=1表示最后一个球,通过球心坐标,以及球半径就能确定球在空间中的位置,当机械臂处于不同构型下,球心的位置根据机器人前向运动学来确定,臂手假肢分解球半径大小根据臂手假肢实际尺寸,选择合适的包络半径。
其中,B表示臂手假肢处于当前构型下的球集合;x表示前向运动学,即将处于构型空间中t时刻状态下的臂手假肢上的点u映射到工作空间中;c表示臂手假肢操作工作空间中的代价函数,用于惩罚到在障碍物中的点;即避障项表达式的含义为对轨迹序列上所有中间状态上机器人所有采样点计算其到障碍物的距离乘以速度;
代价函数c的具体表达式如下:
其中,∈表示常量缩放因子,这里采用0.8;d(xu)表示简化模型中球到最近障碍物的距离,当臂手假肢处于障碍物内部该距离为负数,当臂手假肢处于障碍物边界该距离等于0,当臂手假肢处于障碍物外部该距离大于0。
S4、根据目标函数对初始轨迹进行优化,得到臂手假肢的最终运动轨迹,具体包括如下步骤:
S41、以初始轨迹作为待优化轨迹,代入目标函数得到代价值,判断代价值是否小于预设阈值,若否,则转入步骤S42;若是,则此时的轨迹即为最终运动轨迹;
S42、通过梯度增量对待优化轨迹进行更新;
S43、以更新后的轨迹作为待优化轨迹,代入目标函数得到代价值,判断代价值是否小于预设阈值,若否,则重复步骤S42;若是,则此时的轨迹即为最终运动轨迹,完成臂手假肢的拟人运动规划。
其中,B表示臂手假肢处于当前构型下的简化球集合,为模板轨迹,为模板轨迹点对应的协方差矩阵的逆,t为时刻,I为单位矩阵,k为轨迹曲率向量,x′为轨迹运动速度,为归一化速度向量,c表示工作空间中的代价函数,J为臂手假肢的雅各比矩阵。
此外,也可根据实际情况,通过更新后的轨迹与障碍物是否存在碰撞来判断是否停止迭代更新。即以初始轨迹作为待优化轨迹的初始值,通过梯度增量对待优化轨迹进行更新;然后判断更新后的轨迹与障碍物是否存在碰撞,若不存在,则此时的轨迹即为最终运动轨迹,若存在,则通过梯度增量对待优化轨迹再次进行更新,直至轨迹与障碍物不存在碰撞时停止。
上述步骤S1~S4在规划器中进行,规划器的具体参数通过配置文件.yaml(YetAnother Markup Language)来描述。规划的时间限制为1000秒,最大迭代次数为200次,若达到限制时间或最大迭代次数仍未达阈值,则停止迭代,轨迹优化失败。规划器规划得到最终优化轨迹后,规划器将该轨迹输出给控制器,控制器控制臂手假肢按该轨迹运动。规划效果如图5所示,运动规划起点为臂手自然下垂状态,终点在人左耳附近,圆球为环境中障碍物,大小为0.05m,在空间全局坐标系下,球心位置为{0.6,0,0.1},整个轨迹序列按照时间顺序排列,整个轨迹在避开障碍物同时,保证了连续,同时与正常人做这一动作类似,很好地说明了所得轨迹具有拟人性。
本领域的技术人员容易理解,以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,均应包含在本发明的保护范围之内。
Claims (8)
1.一种面向臂手假肢的拟人运动规划方法,其特征在于,包括如下步骤:
S1、采集人体上肢运动数据和空间障碍物信息,并根据人体上肢运动数据得到模板轨迹;
确定模板轨迹的具体步骤如下:采集人体上肢进行不同功能性动作的运动数据,从而得到不同功能性动作的多条均值轨迹;根据拟人规划的目标点位置,计算该目标点与各条均值轨迹的马氏距离,以马氏距离最小的一条均值轨迹作为模板轨迹;
S2、构建臂手假肢运动链模型以模拟臂手假肢运动,并预设臂手假肢运动的初始轨迹;
其中,ξ为待优化轨迹;为示教项,其根据待优化轨迹与模板轨迹间的距离确定;为避障项,其根据臂手假肢按待优化轨迹运动时,臂手假肢上点扫过的区域与空间中障碍物间的距离确定;为光滑项,其根据待优化轨迹的总体速度确定;a、b、c分别为示教项、避障项、光滑项的权值参数;
S4、根据目标函数对初始轨迹进行优化,得到臂手假肢的最终运动轨迹,完成臂手假肢的拟人运动规划;
根据目标函数对初始轨迹进行优化,具体包括如下步骤:
S41、以初始轨迹作为待优化轨迹,代入目标函数得到代价值,判断代价值是否小于预设阈值,若否,则转入步骤S42;若是,则此时的轨迹即为最终运动轨迹;
S42、通过梯度增量对待优化轨迹进行更新;
S43、以更新后的轨迹作为待优化轨迹,代入目标函数得到代价值,判断代价值是否小于预设阈值,若否,则重复步骤S42;若是,则此时的轨迹即为最终运动轨迹,完成臂手假肢的拟人运动规划。
6.如权利要求1所述的面向臂手假肢的拟人运动规划方法,其特征在于,示教项、避障项、光滑项的权值参数分别为:a=0.1,b=0.5,c=0.4。
7.如权利要求1~6任一项所述的面向臂手假肢的拟人运动规划方法,其特征在于,所述臂手假肢运动链模型包括多个关节,根据各关节的起点、终点,通过多次插值得到臂手假肢运动的初始轨迹。
8.一种面向臂手假肢的拟人运动规划系统,其特征在于,包括存储器和规划器,其中,所述存储器上存储有计算机程序,该计算机程序被所述规划器执行时实现如权利要求1~7任一项所述的规划方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110116130.1A CN112936264B (zh) | 2021-01-28 | 2021-01-28 | 一种面向臂手假肢的拟人运动规划方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110116130.1A CN112936264B (zh) | 2021-01-28 | 2021-01-28 | 一种面向臂手假肢的拟人运动规划方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112936264A CN112936264A (zh) | 2021-06-11 |
CN112936264B true CN112936264B (zh) | 2022-08-05 |
Family
ID=76238378
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110116130.1A Active CN112936264B (zh) | 2021-01-28 | 2021-01-28 | 一种面向臂手假肢的拟人运动规划方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112936264B (zh) |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109291046A (zh) * | 2017-07-25 | 2019-02-01 | 中国科学院沈阳自动化研究所 | 一种七自由度拟人构型机械臂逆运动学规划方法 |
CN107538493B (zh) * | 2017-10-20 | 2021-05-07 | 湘潭大学 | 一种避免线缆干涉约束的工业机器人轨迹规划方法 |
CN108721058A (zh) * | 2018-06-28 | 2018-11-02 | 广州康医疗设备实业有限公司 | 上肢康复训练装置及康复训练方法 |
CN109998855B (zh) * | 2019-04-09 | 2021-09-24 | 上海电气集团股份有限公司 | 基于康复机器人的康复运动控制系统 |
JP7222803B2 (ja) * | 2019-04-25 | 2023-02-15 | 株式会社日立製作所 | 軌道計画装置、軌道計画方法及びプログラム |
-
2021
- 2021-01-28 CN CN202110116130.1A patent/CN112936264B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN112936264A (zh) | 2021-06-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111538949B (zh) | 冗余机器人逆运动学求解方法、装置和冗余机器人 | |
Aristidou et al. | Inverse kinematics: a review of existing techniques and introduction of a new fast iterative solver | |
Park et al. | Toward memory-based human motion simulation: development and validation of a motion modification algorithm | |
Qu et al. | Human-like coordination motion learning for a redundant dual-arm robot | |
Wang et al. | A geometric algorithm to predict the arm reach posture for computer‐aided ergonomic evaluation | |
CN109702740B (zh) | 机器人柔顺性控制方法、装置、设备及存储介质 | |
US20050001842A1 (en) | Method, system and computer program product for predicting an output motion from a database of motion data | |
Zheng et al. | A 3D dynamic model for continuum robots inspired by an octopus arm | |
CN115469576B (zh) | 一种基于人-机械臂异构运动空间混合映射的遥操作系统 | |
JP4267508B2 (ja) | 仮想ダミーの人間工学的動きの最適化 | |
Yang et al. | Humanoid motion planning of robotic arm based on human arm action feature and reinforcement learning | |
CN115018963B (zh) | 一种基于物理仿真的人型智能体姿态生成方法 | |
Fang et al. | Vision-based posture-consistent teleoperation of robotic arm using multi-stage deep neural network | |
Rosado et al. | Reproduction of human arm movements using Kinect-based motion capture data | |
Jha et al. | Robot programming by demonstration using teleoperation through imitation | |
Rigotti et al. | Modeling and driving a reduced human mannequin through motion captured data: A neural network approach | |
CN112959330B (zh) | 基于主从动态运动基元的机器人双臂运动人机对应装置及方法 | |
Li et al. | An efficient motion generation method for redundant humanoid robot arm based on the intrinsic principles of human arm motion | |
CN112936264B (zh) | 一种面向臂手假肢的拟人运动规划方法及系统 | |
Tarokh et al. | Classification and characterization of inverse kinematics solutions for anthropomorphic manipulators | |
Ji et al. | Motion trajectory of human arms based on the dual quaternion with motion tracker | |
CN115781666A (zh) | 一种用于机器人全身模仿系统的控制方法 | |
CN112936282A (zh) | 一种提高工业机器人体感控制精确度的方法及系统 | |
Monforte et al. | Multifunctional principal component analysis for human-like grasping | |
Zhu | Robot Learning Assembly Tasks from Human Demonstrations |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |