CN111702766A - 一种基于力觉引导的机械臂自适应开门旋拧方法 - Google Patents

一种基于力觉引导的机械臂自适应开门旋拧方法 Download PDF

Info

Publication number
CN111702766A
CN111702766A CN202010646146.9A CN202010646146A CN111702766A CN 111702766 A CN111702766 A CN 111702766A CN 202010646146 A CN202010646146 A CN 202010646146A CN 111702766 A CN111702766 A CN 111702766A
Authority
CN
China
Prior art keywords
mechanical arm
force
axis direction
moment
axis
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.)
Granted
Application number
CN202010646146.9A
Other languages
English (en)
Other versions
CN111702766B (zh
Inventor
刘满禄
张静
蒋元成
张华�
李新茂
王姮
刘宏伟
周建
宋宇
曾睿
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southwest University of Science and Technology
Original Assignee
Southwest University of Science and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Southwest University of Science and Technology filed Critical Southwest University of Science and Technology
Priority to CN202010646146.9A priority Critical patent/CN111702766B/zh
Publication of CN111702766A publication Critical patent/CN111702766A/zh
Application granted granted Critical
Publication of CN111702766B publication Critical patent/CN111702766B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/1607—Calculation of inertia, jacobian matrixes and inverses
    • 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/1679—Programme controls characterised by the tasks executed
    • 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/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明公开了一种基于力觉引导的机械臂自适应开门旋拧方法，其通过机械臂末端的六维力传感器获得力和力矩信息，将实际力或力矩与期望力或力矩之间的差值，作为深度确定性策略梯度算法的状态输入，同时输出动作，利用机械臂末端所受两个方向力的函数关系，设置基础奖励函数，通过机械臂的期望运动方向，设置引导性奖励函数，使机械臂自动适应力与力矩的变化，完成旋拧门把手任务。

Description

一种基于力觉引导的机械臂自适应开门旋拧方法
技术领域
本发明涉及人工智能领域，具体涉及一种基于力觉引导的机械臂自适应开门旋拧方法。
背景技术
因核辐射对人体会造成损伤，以核应急、核运维、核退役为代表的非结构化环境需要机器人代替人执行相关作业、处置任务。传统结构化环境机器人任务执行算法难以适应以上复杂的任务和环境，需要通过改进算法，进一步提升机器人系统的环境和任务适应性。以核退役与应急中典型的开门任务为例，因任务对象的不确定，任务过程多变，无法做到每次开门能够规范化操作。且现有机械臂无法根据旋拧门的力反馈对开门过程进行调节，容易造成门锁或机械臂损伤。
发明内容
针对现有技术中的上述不足，本发明提供的一种基于力觉引导的机械臂自适应开门旋拧方法通过自适应的强化学习算法，提高了机器人适应环境的能力，减小了门锁或机械臂损伤概率。
为了达到上述发明目的，本发明采用的技术方案为：
提供一种基于力觉引导的机械臂自适应开门旋拧方法，其包括以下步骤：
S1、在机械臂的夹持端设置力传感器，并根据旋量理论获取机械臂末端的当前位姿；
S2、根据马尔科夫决策过程对机械臂从当前状态变化到下一状态的过程进行建模，得到机械臂运动模型；机械臂运动模型包括状态集合、动作集合、状态转移概率和奖励回报；
S3、在机械臂运动模型中，采集并根据机械臂在开门旋拧过程中力传感器的数据获取机械臂在X轴的力矩误差、Y轴和Z轴方向的力误差；获取机械臂在开门旋拧过程中在X轴、Y轴和Z轴方向的位移；其中Y轴为水平轴，Z轴为竖直轴，X轴与门锁同轴；
S4、根据力矩误差和力误差建立引导性奖励；根据位移结果和实时力大小建立惩罚值；
S5、将机械臂运动模型中Y方向的力误差的绝对值取负为奖惩依据，获取机械臂在开门旋拧过程中的基础奖励；
S6、基于机械臂运动模型，根据步骤S3至步骤S5获取的数据，采用深度确定性策略梯度算法对机械臂开门旋拧路径进行规划，完成基于力觉引导的机械臂自适应开门。
进一步地，步骤S1中根据旋量理论获取机械臂末端的当前位姿的具体方法为：
根据旋量理论的指数积公式对机械臂建立数学模型，设定机械臂的每个关节的方向、位置和末端初始位姿，进而得到包含每个关节转轴单位矢量、每个关节旋转轴的位置矢量、机械臂末端初始位姿、每个关节的运动旋量和机械臂正运动学POE模型。
进一步地，机械臂为UR5机械臂。
进一步地，步骤S3的具体方法为：
将门把手旋转角度作为机械臂的理想旋转角度，获取机械臂在开门旋拧过程中在X轴、Y轴和Z轴方向的位移；将机械臂末端旋拧门把手沿着圆弧轨迹运行时在Y轴和Z轴方向所受反作用力作为机械臂的期望力，根据公式：
ΔFy＝Fye-Fyd
ΔFz＝Fze-Fzd
获取机械臂在Y轴方向的力误差ΔFy和在Z轴方向的力误差ΔFz；根据公式：
ΔTx＝Txe-Txd
获取机械臂在X轴的力矩误差ΔTx；其中Fye为Y轴方向的实际力；Fyd为Y轴方向的期望力；Fze为Z轴方向的实际力；Fzd为Z轴方向的期望力；Txe为X轴方向的实际力矩；Txd为X轴方向的期望力矩。
进一步地，步骤S4中根据力矩误差和力误差建立引导性奖励的具体方法为：
根据公式：
Figure BDA0002573181180000031
建立引导性奖励rg1；其中ΔFy为机械臂在Y轴方向的力误差；ΔFz为机械臂在Z轴方向的力误差；ΔTx为机械臂在X轴的力矩误差。
进一步地，步骤S4中根据位移结果和实时力大小建立惩罚值的具体方法为：
根据公式：
Figure BDA0002573181180000032
建立惩罚值rg2；其中Δystep为机械臂末端沿Y轴方向移动的步长；Δzstep为机械臂末端沿Z轴方向移动的步长；Fye为机械臂Y轴方向的实际力；Fze为机械臂Z轴方向的实际力；Txe为机械臂在X轴的实际力矩。
进一步地，步骤S5的具体方法为：
根据公式：
rb＝-λ|ΔFy|
获取机械臂在开门旋拧过程中的基础奖励rb；其中λ为常数；ΔFy为机械臂在Y轴方向的力误差。
进一步地，步骤S6的具体方法包括以下子步骤：
S6-1、初始化深度确定性策略梯度算法中actor的评估网络的网络参数
Figure BDA0002573181180000041
actor的目标网络的网络参数
Figure BDA0002573181180000042
critic的评估网络的网络参数
Figure BDA0002573181180000043
和critic的目标网络的网络参数
Figure BDA0002573181180000044
S6-2、获取当前时刻机械臂在Y轴、Z轴的力误差和X轴方向的力矩误差
Figure BDA0002573181180000045
判断当前时刻是否为初始时刻，若是则随机生成一个下一时刻的力误差
Figure BDA0002573181180000046
并进入步骤S6-3；否则从经验回放池中随机选取数据作为下一个时刻的力误差
Figure BDA0002573181180000047
并进入步骤S6-3；其中
Figure BDA0002573181180000048
和
Figure BDA0002573181180000049
分别为当前时刻机械臂在Y轴、Z轴的力误差和X轴方向的力矩误差；
Figure BDA00025731811800000410
和
Figure BDA00025731811800000411
分别为随机生成的下一时刻时机械臂在Y轴、Z轴的力误差和X轴方向的力矩误差；i＝0表示当前时刻为初始时刻；
S6-3、将力误差si输入actor的评估网络，得到动作值ai＝μ(si)；其中μ(si)表示当前时刻actor的评估网络的输出；将力误差si+1输入actor的目标网络，得到下一时刻时的动作值ai+1＝μ'(si+1)；其中μ'(si+1)为下一时刻时actor的目标网络的输出；将ai和si输入critic的评估网络，得到当前时刻critic的评估网络输出的状态-动作值
Figure BDA00025731811800000412
将ai+1和si+1输入critic的目标网络，得到下一时刻时critic的目标网络输出的状态-动作值
Figure BDA00025731811800000413
S6-4、根据公式：
Figure BDA00025731811800000414
以loss最小化为目标，采用均方根误差定义critic的评估网络的损失函数，对损失函数采用梯度下降的方法更新当前时刻下critic的评估网络的网络参数，得到下一时刻critic的评估网络的网络参数
Figure BDA0002573181180000051
其中ri为当前时刻引导性奖励、惩罚值和基础奖励的总和，γ为软更新系数；N为常数；
S6-5、根据公式：
Figure BDA0002573181180000052
得到下一时刻critic的目标网络的网络参数
Figure BDA0002573181180000053
S6-6、根据公式：
Figure BDA0002573181180000054
得到下一时刻actor的评估网络的网络参数
Figure BDA0002573181180000055
其中
Figure BDA0002573181180000056
表示当前时刻的critic的评估网络的输出的状态-动作值
Figure BDA0002573181180000057
对动作值ai的梯度；
Figure BDA0002573181180000058
表示当前时刻的actor的评估网络输出的动作值ai对当前时刻的actor的评估网络的网络参数
Figure BDA0002573181180000059
的梯度；
S6-7、根据公式：
Figure BDA00025731811800000510
得到下一时刻actor的目标网络的网络参数
Figure BDA00025731811800000511
S6-8、将生成的力误差、实际得到的力误差、每个时刻下引导性奖励、惩罚值和基础奖励的总和，以及生成的动作存储至经验回放池中；
S6-9、判断是否完成了旋拧开门，若是则结束路径规划，否则返回步骤S6-2。
进一步地，软更新系数γ的值为0.001。
本发明的有益效果为：本发明通过机械臂末端的六维力传感器获得力和力矩信息，将实际力或力矩与期望力或力矩之间的差值，作为深度确定性策略梯度算法的状态输入，同时输出动作，利用机械臂末端所受两个方向力的函数关系，设置基础奖励函数，通过机械臂的期望运动方向，设置引导性奖励函数，使机械臂自动适应力与力矩的变化，完成旋拧门把手任务。
附图说明
图1为本发明的流程示意图；
图2为UR5机械臂模型示意图；
图3为机械臂末端受力分析示意图；
图4为深度确定性策略梯度算法的使用框图；
图5为引导性奖励对路径收敛的影响示意图；
图6为实施例中机械臂末端轨迹示意图；
图7为实施例中Y轴方向的力跟踪示意图；
图8为实施例中Z轴方向的力跟踪示意图；
图9为实施例中X轴的转矩跟踪示意图；
图10为实施例中机械臂末端变化曲线示意图。
具体实施方式
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
如图1所示，该基于力觉引导的机械臂自适应开门旋拧方法包括以下步骤：
S1、在机械臂的夹持端设置力传感器，并根据旋量理论获取机械臂末端的当前位姿；
S2、根据马尔科夫决策过程对机械臂从当前状态变化到下一状态的过程进行建模，得到机械臂运动模型；机械臂运动模型包括状态集合、动作集合、状态转移概率和奖励回报；
S3、在机械臂运动模型中，采集并根据机械臂在开门旋拧过程中力传感器的数据获取机械臂在X轴的力矩误差、Y轴和Z轴方向的力误差；获取机械臂在开门旋拧过程中在X轴、Y轴和Z轴方向的位移；其中Y轴为水平轴，Z轴为竖直轴，X轴与门锁同轴；
S4、根据力矩误差和力误差建立引导性奖励；根据位移结果和实时力大小建立惩罚值；
S5、将机械臂运动模型中Y方向的力误差的绝对值取负为奖惩依据，获取机械臂在开门旋拧过程中的基础奖励；
S6、基于机械臂运动模型，根据步骤S3至步骤S5获取的数据，采用深度确定性策略梯度算法对机械臂开门旋拧路径进行规划，完成基于力觉引导的机械臂自适应开门。
步骤S1中根据旋量理论获取机械臂末端的当前位姿的具体方法为：根据旋量理论的指数积公式对机械臂建立数学模型，设定机械臂的每个关节的方向、位置和末端初始位姿，进而得到包含每个关节转轴单位矢量、每个关节旋转轴的位置矢量、机械臂末端初始位姿、每个关节的运动旋量和机械臂正运动学POE模型。
如图3所示，步骤S3的具体方法为：将门把手旋转角度作为机械臂的理想旋转角度，获取机械臂在开门旋拧过程中在X轴、Y轴和Z轴方向的位移；将机械臂末端旋拧门把手沿着圆弧轨迹运行时在Y轴和Z轴方向所受反作用力作为机械臂的期望力，根据公式：
ΔFy＝Fye-Fyd
ΔFz＝Fze-Fzd
获取机械臂在Y轴方向的力误差ΔFy和在Z轴方向的力误差ΔFz；根据公式：
ΔTx＝Txe-Txd
获取机械臂在X轴的力矩误差ΔTx；其中Fye为Y轴方向的实际力；Fyd为Y轴方向的期望力；Fze为Z轴方向的实际力；Fzd为Z轴方向的期望力；Txe为X轴方向的实际力矩；Txd为X轴方向的期望力矩。
步骤S4中根据力矩误差和力误差建立引导性奖励的具体方法为：根据公式：
Figure BDA0002573181180000081
建立引导性奖励rg1；其中ΔFy为机械臂在Y轴方向的力误差；ΔFz为机械臂在Z轴方向的力误差；ΔTx为机械臂在X轴的力矩误差。
步骤S4中根据位移结果和实时力大小建立惩罚值的具体方法为：根据公式：
Figure BDA0002573181180000082
建立惩罚值rg2；其中Δystep为机械臂末端沿Y轴方向移动的步长；Δzstep为机械臂末端沿Z轴方向移动的步长；Fye为机械臂Y轴方向的实际力；Fze为机械臂Z轴方向的实际力；Txe为机械臂在X轴的实际力矩。
步骤S5的具体方法为：根据公式：
rb＝-λ|ΔFy|
获取机械臂在开门旋拧过程中的基础奖励rb；其中λ为常数；ΔFy为机械臂在Y轴方向的力误差。
参考图4，步骤S6的具体方法包括以下子步骤：
S6-1、初始化深度确定性策略梯度算法中actor的评估网络的网络参数
Figure BDA0002573181180000083
actor的目标网络的网络参数
Figure BDA0002573181180000084
critic的评估网络的网络参数
Figure BDA0002573181180000085
和critic的目标网络的网络参数
Figure BDA0002573181180000091
S6-2、获取当前时刻机械臂在Y轴、Z轴的力误差和X轴方向的力矩误差
Figure BDA0002573181180000092
判断当前时刻是否为初始时刻，若是则随机生成一个下一时刻的力误差
Figure BDA0002573181180000093
并进入步骤S6-3；否则从经验回放池中随机选取数据作为下一个时刻的力误差
Figure BDA0002573181180000094
并进入步骤S6-3；其中
Figure BDA0002573181180000095
和
Figure BDA0002573181180000096
分别为当前时刻机械臂在Y轴、Z轴的力误差和X轴方向的力矩误差；
Figure BDA0002573181180000097
和
Figure BDA0002573181180000098
分别为随机生成的下一时刻时机械臂在Y轴、Z轴的力误差和X轴方向的力矩误差；i＝0表示当前时刻为初始时刻；
S6-3、将力误差si输入actor的评估网络，得到动作值ai＝μ(si)；其中μ(si)表示当前时刻actor的评估网络的输出；将力误差si+1输入actor的目标网络，得到下一时刻时的动作值ai+1＝μ'(si+1)；其中μ'(si+1)为下一时刻时actor的目标网络的输出；将ai和si输入critic的评估网络，得到当前时刻critic的评估网络输出的状态-动作值
Figure BDA0002573181180000099
将ai+1和si+1输入critic的目标网络，得到下一时刻时critic的目标网络输出的状态-动作值
Figure BDA00025731811800000910
S6-4、根据公式：
Figure BDA00025731811800000911
以loss最小化为目标，采用均方根误差定义critic的评估网络的损失函数，对损失函数采用梯度下降的方法更新当前时刻下critic的评估网络的网络参数，得到下一时刻critic的评估网络的网络参数
Figure BDA00025731811800000912
其中ri为当前时刻引导性奖励、惩罚值和基础奖励的总和，γ为软更新系数，值为0.001；N为常数；
S6-5、根据公式：
Figure BDA00025731811800000913
得到下一时刻critic的目标网络的网络参数
Figure BDA00025731811800000914
S6-6、根据公式：
Figure BDA0002573181180000101
得到下一时刻actor的评估网络的网络参数
Figure BDA0002573181180000102
其中
Figure BDA0002573181180000103
表示当前时刻的critic的评估网络的输出的状态-动作值
Figure BDA0002573181180000104
对动作值ai的梯度；
Figure BDA0002573181180000105
表示当前时刻的actor的评估网络输出的动作值ai对当前时刻的actor的评估网络的网络参数
Figure BDA0002573181180000106
的梯度；
S6-7、根据公式：
Figure BDA0002573181180000107
得到下一时刻actor的目标网络的网络参数
Figure BDA0002573181180000108
S6-8、将生成的力误差、实际得到的力误差、每个时刻下引导性奖励、惩罚值和基础奖励的总和，以及生成的动作存储至经验回放池中；
S6-9、判断是否完成了旋拧开门，若是则结束路径规划，否则返回步骤S6-2。
在本发明的一个实施例中，机械臂为UR5机械臂，其模型如图2所示，因此UR5机械臂当前状态包括6个关节转轴单位矢量：
Figure BDA0002573181180000109
6个关节旋转轴的位置矢量：
Figure BDA00025731811800001010
机械臂末端初始位姿：
Figure BDA0002573181180000111
第i个关节的运动旋量：
Figure BDA0002573181180000112
正运动学POE模型：
Figure BDA0002573181180000113
其中τ1为第一个关节的转轴单位矢量；τ2为第二个关节的转轴单位矢量；τ3为第三个关节的转轴单位矢量；τ4为第四个关节的转轴单位矢量；τ5为第五个关节的转轴单位矢量；τ6为第六个关节的转轴单位矢量；p1为第一个关节旋转轴的位置矢量；p2为第二个关节旋转轴的位置矢量；p3为第三个关节旋转轴的位置矢量；p4为第四个关节旋转轴的位置矢量；p5为第五个关节旋转轴的位置矢量；p6为第六个关节旋转轴的位置矢量；L1为第一个关节的纵向长度；L5为第五个关节的纵向长度；L6为第六个关节的横向长度；a2为第二个关节的纵向长度；a3为第三个关节的纵向长度；a4为第四个关节的横向长度；τi为第i个关节的转轴单位矢量；pi为第i个关节旋转轴的位置矢量。
在具体实施过程中，深度确定性策略梯度算法(DDPG)的训练过程中，Optimizer优化器采用的随机梯度下降学习率取10-4，奖励衰减率取0.9，batch size取35，回合数取1000，经验回放池容量取6000，评估网络和目标网络权值为随机初始化，偏置b初始化为10-3，探索策略选用ε-greedy策略。在设置奖励的权重时，为了提高基础奖励的重要性，将λ设置为1.5。训练前，机械臂和门把手之间有间隔；仿真开始，机械臂末端根据V-REP内在的逆运动学模块进行路径规划，运动至门把手，并夹握门把手上距离旋转副轴心70mm的位置；训练开始，机械臂夹握门把手运动，六维力传感器通过采集机械臂末端所受到的力，来获取当前力误差状态，输入到DDPG算法中，根据actor动作策略以及critic的评判获取动作，进行下一次机械臂与门把手的交互循环，直至旋拧门把手的角度为90°，即任务完成。
为了避免在探索过程中，机械臂沿着不理想的方向运动时间过长，导致收敛速度变慢，设置了-1500的奖励下限，当累计奖励R<-1500时，判定任务失败。如下图5所示(横坐标为回合数)，图5(a)为机械臂在未设置引导性奖励的情况下，算法训练所获取的奖励值。可以看出算法在第612个回合时，机械臂第一次完成任务，但是由于机械臂能够运动的路径非常多，探索过程非常缓慢，在训练1000回合之后，算法无法收敛；由图5(b)可以看出，在有引导性奖励的情况下，机械臂在386回合以后，算法逐渐开始收敛，并在650回合之后，逐渐稳定在-500左右。由此可以看出，在有引导性奖励的情况下，算法收敛速度更快，而且更加稳定。
测试阶段，将训练阶段获得的半径为70mm的旋拧模型用于旋拧半径为80mm的门把手，如图6所示(横坐标为Y轴，纵坐标为Z轴)，图6(a)为圆心(-0.05，0.5914)，半径为80mm，圆心角π/4的一段圆弧，图6(b)为机械臂夹握门把手半径80mm处旋拧的轨迹，可以看出其与理想的圆弧轨迹有一定的偏差，但是总体偏差不大。该算法并未进行位置跟踪，主要通过控制力来适应环境，自动规划轨迹，目的是尽可能使机械臂所受到的力最小。
机械臂在能完成任务的情况下，所受到的力越小越好，保证机械臂不受到损伤，本方法中设置期望力的目的是让实际力有一个收敛的方向，并且不能超过15N。如图7所示(横坐标为运行步长，纵坐标为力)，机械臂末端Y轴方向的实际力都是逐渐收敛于期望力，从图7(a)可以看出，训练模型在半径70mm的门把手位置测试时，力变化更加平滑，任意一点的力前后变化范围在5N内。从图7(b)可以看出，将模型运用在半径80mm的门把手位置测试时，力的变化幅度会大一些，但仍处于可控范围内。
在图8中(横坐标为运行步长，纵坐标为力)，在Z轴方向上，训练模型在两个半径的门把手位置测试时，实际力逐渐收敛于期望力，同时在旋拧半径80mm的门把手上，力未超出最大值，收敛效果较好。图9中(横坐标为运行步长，纵坐标为转矩)力矩跟踪的目的是为了设置引导性奖励，其次将其限制在(-2，0)的范围，避免力矩过大，使机械臂第六个关节角瞬时变化过大，损坏机械臂。在图10中(横坐标为运行步长，纵坐标为角度)，第六个关节角变化曲线平滑，结束状态，说明机械臂末端的位姿是跟随门把手角度的变化而变化的，从而验证了算法具有位姿跟踪效果，并且跟踪效果较好。
综上所述，本发明通过机械臂末端的六维力传感器获得力和力矩信息，将实际力或力矩与期望力或力矩之间的差值，作为深度确定性策略梯度算法的状态输入，同时输出动作，利用机械臂末端所受两个方向力的函数关系，设置基础奖励函数，通过机械臂的期望运动方向，设置引导性奖励函数，使机械臂自动适应力与力矩的变化，完成旋拧门把手任务。仿真数据结果表明，在有引导性奖励的情况下，基于力觉引导的机械臂自适应旋拧方法能够在更短的时间内达到收敛，完成机械臂旋拧门把手的任务。

Claims (9)

1.一种基于力觉引导的机械臂自适应开门旋拧方法，其特征在于，包括以下步骤：
S1、在机械臂的夹持端设置力传感器，并根据旋量理论获取机械臂末端的当前位姿；
S2、根据马尔科夫决策过程对机械臂从当前状态变化到下一状态的过程进行建模，得到机械臂运动模型；机械臂运动模型包括状态集合、动作集合、状态转移概率和奖励回报；
S3、在机械臂运动模型中，采集并根据机械臂在开门旋拧过程中力传感器的数据获取机械臂在X轴的力矩误差、Y轴和Z轴方向的力误差；获取机械臂在开门旋拧过程中在X轴、Y轴和Z轴方向的位移；其中Y轴为水平轴，Z轴为竖直轴，X轴与门锁同轴；
S4、根据力矩误差和力误差建立引导性奖励；根据位移结果和实时力大小建立惩罚值；
S5、将机械臂运动模型中Y方向的力误差的绝对值取负为奖惩依据，获取机械臂在开门旋拧过程中的基础奖励；
S6、基于机械臂运动模型，根据步骤S3至步骤S5获取的数据，采用深度确定性策略梯度算法对机械臂开门旋拧路径进行规划，完成基于力觉引导的机械臂自适应开门。
2.根据权利要求1所述的基于力觉引导的机械臂自适应开门旋拧方法，其特征在于，所述步骤S1中根据旋量理论获取机械臂末端的当前位姿的具体方法为：
根据旋量理论的指数积公式对机械臂建立数学模型，设定机械臂的每个关节的方向、位置和末端初始位姿，进而得到包含每个关节转轴单位矢量、每个关节旋转轴的位置矢量、机械臂末端初始位姿、每个关节的运动旋量和机械臂正运动学POE模型。
3.根据权利要求1所述的基于力觉引导的机械臂自适应开门旋拧方法，其特征在于，所述机械臂为UR5机械臂。
4.根据权利要求1所述的基于力觉引导的机械臂自适应开门旋拧方法，其特征在于，所述步骤S3的具体方法为：
将门把手旋转角度作为机械臂的理想旋转角度，获取机械臂在开门旋拧过程中在X轴、Y轴和Z轴方向的位移；将机械臂末端旋拧门把手沿着圆弧轨迹运行时在Y轴和Z轴方向所受反作用力作为机械臂的期望力，根据公式：
ΔFy＝Fye-Fyd
ΔFz＝Fze-Fzd
获取机械臂在Y轴方向的力误差ΔFy和在Z轴方向的力误差ΔFz；根据公式：
ΔTx＝Txe-Txd
获取机械臂在X轴的力矩误差ΔTx；其中Fye为Y轴方向的实际力；Fyd为Y轴方向的期望力；Fze为Z轴方向的实际力；Fzd为Z轴方向的期望力；Txe为X轴方向的实际力矩；Txd为X轴方向的期望力矩。
5.根据权利要求1所述的基于力觉引导的机械臂自适应开门旋拧方法，其特征在于，所述步骤S4中根据力矩误差和力误差建立引导性奖励的具体方法为：
根据公式：
Figure FDA0002573181170000021
建立引导性奖励rg1；其中ΔFy为机械臂在Y轴方向的力误差；ΔFz为机械臂在Z轴方向的力误差；ΔTx为机械臂在X轴的力矩误差。
6.根据权利要求1所述的基于力觉引导的机械臂自适应开门旋拧方法，其特征在于，所述步骤S4中根据位移结果和实时力大小建立惩罚值的具体方法为：
根据公式：
Figure FDA0002573181170000031
建立惩罚值rg2；其中Δystep为机械臂末端沿Y轴方向移动的步长；Δzstep为机械臂末端沿Z轴方向移动的步长；Fye为机械臂Y轴方向的实际力；Fze为机械臂Z轴方向的实际力；Txe为机械臂在X轴的实际力矩。
7.根据权利要求1所述的基于力觉引导的机械臂自适应开门旋拧方法，其特征在于，所述步骤S5的具体方法为：
根据公式：
rb＝-λ|ΔFy|
获取机械臂在开门旋拧过程中的基础奖励rb；其中λ为常数；ΔFy为机械臂在Y轴方向的力误差。
8.根据权利要求1所述的基于力觉引导的机械臂自适应开门旋拧方法，其特征在于，所述步骤S6的具体方法包括以下子步骤：
S6-1、初始化深度确定性策略梯度算法中actor的评估网络的网络参数
Figure FDA0002573181170000032
actor的目标网络的网络参数
Figure FDA0002573181170000033
critic的评估网络的网络参数
Figure FDA0002573181170000034
和critic的目标网络的网络参数
Figure FDA0002573181170000035
S6-2、获取当前时刻机械臂在Y轴、Z轴的力误差和X轴方向的力矩误差
Figure FDA0002573181170000036
判断当前时刻是否为初始时刻，若是则随机生成一个下一时刻的力误差
Figure FDA0002573181170000037
并进入步骤S6-3；否则从经验回放池中随机选取数据作为下一个时刻的力误差
Figure FDA0002573181170000038
并进入步骤S6-3；其中
Figure FDA0002573181170000039
和
Figure FDA00025731811700000310
分别为当前时刻机械臂在Y轴、Z轴的力误差和X轴方向的力矩误差；
Figure FDA0002573181170000041
和
Figure FDA0002573181170000042
分别为随机生成的下一时刻时机械臂在Y轴、Z轴的力误差和X轴方向的力矩误差；i＝0表示当前时刻为初始时刻；
S6-3、将力误差si输入actor的评估网络，得到动作值ai＝μ(si)；其中μ(si)表示当前时刻actor的评估网络的输出；将力误差si+1输入actor的目标网络，得到下一时刻时的动作值ai+1＝μ'(si+1)；其中μ'(si+1)为下一时刻时actor的目标网络的输出；将ai和si输入critic的评估网络，得到当前时刻critic的评估网络输出的状态-动作值
Figure FDA0002573181170000043
将ai+1和si+1输入critic的目标网络，得到下一时刻时critic的目标网络输出的状态-动作值
Figure FDA0002573181170000044
S6-4、根据公式：
Figure FDA0002573181170000045
以loss最小化为目标，采用均方根误差定义critic的评估网络的损失函数，对损失函数采用梯度下降的方法更新当前时刻下critic的评估网络的网络参数，得到下一时刻critic的评估网络的网络参数
Figure FDA0002573181170000046
其中ri为当前时刻引导性奖励、惩罚值和基础奖励的总和，γ为软更新系数；N为常数；
S6-5、根据公式：
Figure FDA0002573181170000047
得到下一时刻critic的目标网络的网络参数
Figure FDA0002573181170000048
S6-6、根据公式：
Figure FDA0002573181170000049
得到下一时刻actor的评估网络的网络参数
Figure FDA00025731811700000410
其中
Figure FDA00025731811700000411
表示当前时刻的critic的评估网络的输出的状态-动作值
Figure FDA00025731811700000412
对动作值ai的梯度；
Figure FDA00025731811700000413
表示当前时刻的actor的评估网络输出的动作值ai对当前时刻的actor的评估网络的网络参数
Figure FDA0002573181170000051
的梯度；
S6-7、根据公式：
Figure FDA0002573181170000052
得到下一时刻actor的目标网络的网络参数
Figure FDA0002573181170000053
S6-8、将生成的力误差、实际得到的力误差、每个时刻下引导性奖励、惩罚值和基础奖励的总和，以及生成的动作存储至经验回放池中；
S6-9、判断是否完成了旋拧开门，若是则结束路径规划，否则返回步骤S6-2。
9.根据权利要求8所述的基于力觉引导的机械臂自适应开门旋拧方法，其特征在于，所述软更新系数γ的值为0.001。
CN202010646146.9A 2020-07-07 2020-07-07 一种基于力觉引导的机械臂自适应开门旋拧方法 Active CN111702766B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010646146.9A CN111702766B (zh) 2020-07-07 2020-07-07 一种基于力觉引导的机械臂自适应开门旋拧方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010646146.9A CN111702766B (zh) 2020-07-07 2020-07-07 一种基于力觉引导的机械臂自适应开门旋拧方法

Publications (2)

Publication Number Publication Date
CN111702766A true CN111702766A (zh) 2020-09-25
CN111702766B CN111702766B (zh) 2021-10-22

Family

ID=72545300

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010646146.9A Active CN111702766B (zh) 2020-07-07 2020-07-07 一种基于力觉引导的机械臂自适应开门旋拧方法

Country Status (1)

Country Link
CN (1) CN111702766B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112192614A (zh) * 2020-10-09 2021-01-08 西南科技大学 一种基于人机合作的核运维机器人轴孔装配方法
CN112894808A (zh) * 2021-01-15 2021-06-04 山东大学 一种基于深度强化学习的机器人旋拧阀门系统及方法
CN114235374A (zh) * 2021-12-22 2022-03-25 上海无线电设备研究所 一种飞机舱门机构耐久性试验装置及其试验方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04108553A (ja) * 1990-07-20 1992-04-09 Kobe Steel Ltd 自動車の塗装システム
CN1788859A (zh) * 2004-09-30 2006-06-21 Abb股份有限公司 与工业机器人一起使用的开门器装置
CN203783263U (zh) * 2014-01-26 2014-08-20 莲都区梅山中学 一种开锁用机械臂
CN106313114A (zh) * 2016-08-30 2017-01-11 四川超影科技有限公司 室内巡检机器人过门系统及方法
CN108575788A (zh) * 2018-03-22 2018-09-28 苏州科技大学 一种基于强化学习的宠物自动投食控制系统和方法
CN109476022A (zh) * 2016-04-08 2019-03-15 巴滕伯格机器人两合公司 用于检测关闭部件的参数的方法
CN109989625A (zh) * 2019-04-12 2019-07-09 深圳芯邦科技股份有限公司 一种开锁控制方法以及锁具
CN110549338A (zh) * 2019-09-10 2019-12-10 哈尔滨工业大学 一种圆-长方形复合孔类零件的机器人自动装配方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04108553A (ja) * 1990-07-20 1992-04-09 Kobe Steel Ltd 自動車の塗装システム
CN1788859A (zh) * 2004-09-30 2006-06-21 Abb股份有限公司 与工业机器人一起使用的开门器装置
CN203783263U (zh) * 2014-01-26 2014-08-20 莲都区梅山中学 一种开锁用机械臂
CN109476022A (zh) * 2016-04-08 2019-03-15 巴滕伯格机器人两合公司 用于检测关闭部件的参数的方法
CN106313114A (zh) * 2016-08-30 2017-01-11 四川超影科技有限公司 室内巡检机器人过门系统及方法
CN108575788A (zh) * 2018-03-22 2018-09-28 苏州科技大学 一种基于强化学习的宠物自动投食控制系统和方法
CN109989625A (zh) * 2019-04-12 2019-07-09 深圳芯邦科技股份有限公司 一种开锁控制方法以及锁具
CN110549338A (zh) * 2019-09-10 2019-12-10 哈尔滨工业大学 一种圆-长方形复合孔类零件的机器人自动装配方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112192614A (zh) * 2020-10-09 2021-01-08 西南科技大学 一种基于人机合作的核运维机器人轴孔装配方法
CN112894808A (zh) * 2021-01-15 2021-06-04 山东大学 一种基于深度强化学习的机器人旋拧阀门系统及方法
CN114235374A (zh) * 2021-12-22 2022-03-25 上海无线电设备研究所 一种飞机舱门机构耐久性试验装置及其试验方法

Also Published As

Publication number Publication date
CN111702766B (zh) 2021-10-22

Similar Documents

Publication Publication Date Title
CN111702766B (zh) 一种基于力觉引导的机械臂自适应开门旋拧方法
CN111496792B (zh) 一种机械臂输入饱和固定时间轨迹跟踪控制方法及系统
Wen et al. Elman fuzzy adaptive control for obstacle avoidance of mobile robots using hybrid force/position incorporation
CN107505947B (zh) 一种空间机器人捕获目标后消旋及协调控制方法
CN109343350A (zh) 一种基于模型预测控制的水下机器人路径跟踪控制方法
CN112847235B (zh) 基于深度强化学习的机器人分阶力引导装配方法及系统
CN113093526B (zh) 一种基于强化学习的无超调pid控制器参数整定方法
CN110598285A (zh) 机械手轨迹逆运动学求解方法、装置及存储介质
CN116533249A (zh) 基于深度强化学习的机械臂控制方法
Ji et al. Time-energy optimal trajectory planning for variable stiffness actuated robot
Liu et al. Novel method of obstacle avoidance planning for redundant sliding manipulators
CN112000116A (zh) 一种基于改进萤火虫pid方法的自主水下航行器航向角控制方法
Al Homsi et al. A hierarchical approach to minimum-time control of industrial robots
Kang et al. Kinematic path‐tracking of mobile robot using iterative learning control
JP3465236B2 (ja) ロバスト強化学習方式
CN116540721A (zh) 基于改进遗传粒子群算法的空间机器人最优轨迹规划方法
Tu et al. Moving object flexible grasping based on deep reinforcement learning
CN114967459A (zh) 一种机械臂时间收敛性的控制方法及其7 dof机械臂
Roveda et al. Cartesian tasks oriented friction compensation through a reinforcement learning approach
Benhellal et al. Decoupled adaptive neuro-interval type-2 fuzzy sliding mode control applied in a 3Dcrane system
Heyu et al. Impedance control method with reinforcement learning for dual-arm robot installing slabstone
CN117140527B (zh) 一种基于深度强化学习算法的机械臂控制方法及系统
Liang et al. Multiobjective trajectory optimization and adaptive backstepping control for rubber unstacking robot based on RFWNN method
Liu et al. Adaptive regulation of rigid-link electrically driven robots with uncertain kinematics
Zhou et al. Intelligent Control of Manipulator Based on Deep Reinforcement Learning

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