CN112720462B - 一种机器人的轨迹规划系统和方法 - Google Patents
一种机器人的轨迹规划系统和方法 Download PDFInfo
- Publication number
- CN112720462B CN112720462B CN202011426513.0A CN202011426513A CN112720462B CN 112720462 B CN112720462 B CN 112720462B CN 202011426513 A CN202011426513 A CN 202011426513A CN 112720462 B CN112720462 B CN 112720462B
- Authority
- CN
- China
- Prior art keywords
- robot
- trajectory
- track
- foot
- planner
- 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
- 238000000034 method Methods 0.000 title claims abstract description 36
- 230000005021 gait Effects 0.000 claims abstract description 69
- 206010034701 Peroneal nerve palsy Diseases 0.000 claims abstract description 14
- 238000012952 Resampling Methods 0.000 claims abstract description 7
- 238000013528 artificial neural network Methods 0.000 claims description 31
- 230000002787 reinforcement Effects 0.000 claims description 16
- 230000001133 acceleration Effects 0.000 claims description 12
- 230000008878 coupling Effects 0.000 claims description 9
- 238000010168 coupling process Methods 0.000 claims description 9
- 238000005859 coupling reaction Methods 0.000 claims description 9
- 230000003993 interaction Effects 0.000 claims description 7
- 238000013507 mapping Methods 0.000 claims description 7
- 230000007613 environmental effect Effects 0.000 claims description 6
- 230000006870 function Effects 0.000 claims description 5
- 238000005070 sampling Methods 0.000 claims description 5
- 230000004913 activation Effects 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 230000004927 fusion Effects 0.000 claims 2
- 238000007781 pre-processing Methods 0.000 claims 1
- 230000033001 locomotion Effects 0.000 abstract description 18
- 230000009897 systematic effect Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 8
- 210000003141 lower extremity Anatomy 0.000 description 6
- 230000008569 process Effects 0.000 description 4
- 239000013598 vector Substances 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000005290 field theory Methods 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 206010033892 Paraplegia Diseases 0.000 description 2
- 230000009194 climbing Effects 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 210000001364 upper extremity Anatomy 0.000 description 2
- 206010019468 Hemiplegia Diseases 0.000 description 1
- 206010037714 Quadriplegia Diseases 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 239000003795 chemical substances by application Substances 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 210000003414 extremity Anatomy 0.000 description 1
- 230000037230 mobility Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
- 238000012549 training Methods 0.000 description 1
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/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)
- Manipulator (AREA)
Abstract
本发明提供一种机器人的轨迹规划系统和方法，路径规划器生成避障路径与步态周期，构成落脚点序列；步态规划器输出机器人质心轨迹、左脚轨迹、右脚轨迹以及经过重采样后的落脚点序列；关节轨迹规划器计算得到驱动机器人的关节轨迹。首次提出了针对外骨骼机器人的系统性的端到端全自主轨迹规划算法，将穿戴者的粗略运动意图指令直接换为具体的关节轨迹指令进而驱动外骨骼机器人实现载人自平衡行走，提高了外骨骼机器人的智能性、易用性与适用范围。
Description
技术领域
本发明属于机器人领域，特别涉及一种机器人的轨迹规划系统和方法，尤其是一种面向自平衡下肢外骨骼机器人的多层次轨迹规划系统和方法。
背景技术
在过去的几十年里，双足机器人由于其类人的结构使它们具有很强的机动性，能够完成各种复杂或重复性高的任务，如爬楼梯、搬运物品等，因此近年来研究人员将重点放在双足机器人的路径规划和运动规划上。自平衡外骨骼机器人虽然与双足机器人在技术上有一定的共同性，但是外骨骼机器人还需要考虑穿戴者的影响，这导致自平衡外骨骼机器人在运动的安全性、模型复杂性、人机交互性方面所带来的困难都大于一般的双足机器人。
在端到端全自主轨迹规划方面，目前国内还未能找到与自平衡外骨骼机器人直接相关的，从技术共性上来看，目前国内现有的双足机器人路径规划专利中，发明专利CN111830986A提出一种双足机器人的三维路径规划方法，考虑了双足机器人和障碍物信息，设计节点的代价，并进行代价评估，在存在多种类型物体的复杂环境中提前计算一条代价最小的三维路径；该方法规划得到的路径，能够引导机器人在相应的环境中执行跨越、绕行、上台阶的运动。
自平衡下肢外骨骼机器人可以为四肢瘫痪、高位截瘫、高位偏瘫等丧失完全四肢运动能力的患者提供无拐杖自平衡行走辅助，但这些患者已经失去了通过上肢精确控制外骨骼的能力，比如用手操纵控制器或摇杆来控制外骨骼等。因此，为了识别这些穿戴患者的运动意图，一般通过脑电设备、眼动跟踪仪等高层次人机接口来将使用者的意图识别出来，进而驱动外骨骼机器人。然而这些高层次人机接口设备的信号带宽十分有限，它们所能提供的只能是十分粗略的指令信号，例如通过脑电设备提供患者的启停行走意图，通过眼动仪提供患者目标位置指令。但想要驱动自平衡下肢外骨骼机器人来实现患者所要求的具体运动，这些信号远远不够，因为机器人最直接需要的是各个关节的运动轨迹指令。因而，如何将这些粗略的上层指令转换为具体的关节轨迹指令来驱动外骨骼，使得自平衡外骨骼机器人在保证行走平衡的同时还能完成穿戴者所想要实现的运动成为了一个极具挑战的问题。
在双足机器人稳定步行控制研究中，发明专利CN111546349A提出一种基于深度强化学习的步态规划方法，利用改进的DQN算法解决双足机器人的步行控制问题，将强化学习用于双足机器人实现在快速行走条件下的长距离稳定步态控制，包括建立仿人双足机器人模型、预训练控制参数和双足机器人步行运动。发明专利CN111674486A提出了一种基于模型预测控制的双足机器人稳定步行控制方法，使用双足机器人的简化刚体动力学模型，构造质心状态预测模型，根据质心状态预测模型将所述模型预测控制方法中的优化方程组简化成二次规划方程组，求解方程组，获得优化地面反作用力，从而获得稳定步行的控制参数，最后根据稳定步行控制参数控制所述双足机器人。目前还未有针对外骨骼机器人尤其是自平衡外骨骼机器人的端到端全自主轨迹规划算法框架，用于将粗略的高层次运动意图指令转换为具体的关节轨迹指令进而驱动外骨骼机器人实现载人自平衡行走。
发明内容
本说明书实施例的目的是提供一种机器人的轨迹规划系统和方法，可以将高层次的粗略运动意图指令实时在线的转换为具体的关节运动指令，进而实现机器人的载人行走。
为解决上述技术问题，本说明书实施例通过以下方式实现的：
第一方面，提供一种机器人的轨迹规划系统，包括：路径规划器、步态规划器和关节轨迹规划器，
所述路径规划器生成避障路径与步态周期，构成落脚点序列；
所述步态规划器接收到来自所述路径规划器的落脚点序列和步态周期，输出机器人质心轨迹、左脚轨迹、右脚轨迹以及经过重采样后的落脚点序列；
所述关节轨迹规划器根据机器人质心轨迹、左脚轨迹、右脚轨迹和经过重采样后的落脚点序列计算得到驱动机器人的关节轨迹。
第二方面，提供一种机器人的轨迹规划方法，包括如下步骤：
步骤100、信息输入，利用传感器从环境中获取环境点云图以及通过人机交互接口获取机器人目的地信息；
步骤200、路径规划，机器人目的地信息和环境点云图信息被预处理并导入到路径规划器中训练好的深度强化学习智能体中，生成从机器人当前位置到目的位置的避障路径与步态周期，构成落脚点序列；
步骤300、步态规划，接收到来自落脚点序列和步态周期，输出机器人质心轨迹、左脚轨迹、右脚轨迹以及经过重采样后的落脚点序列；
步骤400、关节轨迹规划，根据机器人质心轨迹、左脚轨迹、右脚轨迹和落脚点序列计算得到驱动机器人的关节轨迹。
由以上本说明书实施例提供的技术方案可见，
(1)本发明提出一种多层次轨迹规划算法，可以实时在线的为外骨骼机器人规划出落脚点路径、满足双足动态稳定要求的步态轨迹以及带动力学误差补偿的关节轨迹，最终可以将输出的关节轨迹作为驱动外骨骼机器人的运动指令来实现外骨骼机器人按穿戴者的意图行走；
(2)本发明首次提出了针对外骨骼机器人的系统性的端到端全自主轨迹规划算法，将粗略的高层次运动意图指令转换为具体的关节轨迹指令进而驱动外骨骼机器人实现载人自平衡行走，这可以提高外骨骼机器人的智能性、易用性与适用范围，可以让外骨骼机器人的应用范围扩展到高位截瘫、高位偏瘫等失去上下肢运动能力的患者；
(3)本发明的路径规划器为基于深度强化学习算法的在线实时路径规划算法，该层规划器的算法中还融入了外骨骼运动学约束和人工势场理论，该算法框架适用于动态环境，可以在线实时的输出驱动外骨骼机器人的关节运动轨迹指令，可用于在动态环境中生成满足步行安全稳定要求、符合人类行走思维、且能顺利避障的机器人双足落脚点路径；
(4)本发明的步态规划器中，模型预观测控制(MPC)理论和零力矩点理论(ZMP)的构成了在线双足步态轨迹生成算法，可用于生成满足双足步行稳定要求的机器人质心(COM)轨迹和左右脚轨迹；
(5)本发明的关节轨迹规划器中，融合了动力学过滤器及神经网络的机器人关节轨迹规划算法，可用于补偿步态规划器中的动力学建模误差带来的行走稳定性影响；建立了人机耦合11杆动力学模型，并利用该模型计算的零力矩点理论(ZMP)位置误差与模型预观测理论相结合用作动力学过滤器作用，用于生成步态轨迹中质心位置的补偿量，进而补偿步态规划层中采用小车-桌子简化模型所带来的建模误差影响；建立了“质心位置—腰部中心位置”的映射神经网络，用于将步态规划层输出的质心位置映射为机器人运动学模型所需要的腰部中心位置，解决了难以利用传统建模手段获得从机器人质心位置求得机器人腰部中心位置的难题。
附图说明
为了更清楚地说明本说明书实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本说明书中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本说明书实施例一提供的的多层次轨迹规划流程图；
图2为本说明书实施例一提供的路径规划器原理图
图3为本说明书实施例一提供的步态规划器原理图；
图4a为本说明书实施例提供的外骨骼机器人简化的小车-桌子动力学模型；
图4b为本说明书实施例提供的小车-桌子动力学模型侧视图；
图5为本说明书实施例一提供的关节轨迹规划器原理图；
图6a为本说明书实施例提供的神经网络(CWNN)的输入与输出原理图；
图6b为本说明书实施例提供的神经网络(CWNN)结构图；
图7a为本说明书实施例的两个障碍物环境中多层次轨迹规划算法实验结果的路径规划层输出的落脚点路径图；
图7b为本说明书实施例的两个障碍物环境中多层次轨迹规划算法实验结果的步态规划层输出的步态轨迹以及经关节轨迹规划层校正后的步态轨迹图；
图7c为本说明书实施例的两个障碍物环境中多层次轨迹规划算法实验结果的关节轨迹规划层输出的关节位置轨图。
具体实施方式
为了使本技术领域的人员更好地理解本说明书中的技术方案，下面将结合本说明书实施例中的附图，对本说明书实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本说明书一部分实施例，而不是全部的实施例。基于本说明书中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都应当属于本说明书保护的范围。
实施例一
参照图1所示，本说明书实施例一提供的一种机器人的轨迹规划系统的结构示意图，优选的为一种面向自平衡下肢外骨骼机器人的多层次轨迹规划系统，所述轨迹规划系统采用三层规划器，分别为：路径规划器、步态规划器和关节轨迹规划器。
其中，所述轨迹规划系统所面向的使用环境为穿戴者自平衡外骨骼机器人的患者行走在包含静态障碍物和动态障碍物的日常环境，输入为利用传感器从环境中获取环境点云图以及通过人机交互接口获取机器人目的地信息；
其中，所述路径规划器获取机器人目的地信息和环境点云图信息，机器人目的地信息和环境点云图信息被预处理并导入到所述路径规划器中训练好的深度强化学习智能体中，生成从机器人当前位置到目的位置的避障路径与步态周期，从而构成区分左右脚的离散落脚点序列；
所述路径规划器采用基于深度强化学习算法的在线实时路径规划算法，所述路径规划器融入了外骨骼运动学约束和人工势场理论，可以获得快速收敛的深度强化学习策略网络，用于在动态环境中生成满足步行安全稳定要求、符合人类行走思维、且能顺利避障的机器人双足落脚点路径。
其中，所述步态规划器接收到来自所述路径规划器的落脚点序列和步态周期，落脚点序列被所述步态规划器看作未来若干个步态周期的零力矩点(ZMP)参考值，根据零力矩点(ZMP)理论和预观测控制理论(MPC)生成机器人质心(COM)轨迹。同时，在每个落脚点之间利用多次样条曲线拟合出左脚轨迹和右脚轨迹。所述步态规划器的最终输出为机器人质心(COM)、左脚轨迹、右脚轨迹以及经过重采样后的落脚点序列；
所述步态规划器采用基于模型预观测控制(MPC)理论和零力矩点(ZMP)理论的在线双足步态轨迹生成算法，用于生成满足双足步行稳定要求的机器人质心(COM)轨迹和左右脚轨迹；
其中，所述关节轨迹规划器接收来自步态规划器的机器人质心(COM)轨迹、左脚轨迹、右脚轨迹和落脚点序列。关节轨迹规划器利用“质心位置—腰部中心位置”的神经网络(CWNN)，将机器人质心(COM)轨迹、左脚轨迹、右脚轨迹转换为机器人腰部中心位置，利用模型预观测控制(MPC)理论和人机耦合11杆动力学模型求得补偿动力学建模误差后的修正机器人质心(COM)轨迹，利用神经网络(CWNN)和机器人运动学逆解模型求得驱动机器人所需要的关节轨迹。
所述关节轨迹规划器融合了动力学过滤器及神经网络(CWNN)的机器人关节轨迹规划算法，用于补偿步态规划器中的动力学建模误差带来的行走稳定性影响，并利用神经网络(CWNN)将机器人质心(COM)轨迹映射为机器人腰部轨迹，结合机器人运动学模型求得驱动机器人所需要的关节轨迹；
进一步地，所述轨迹规划系统输入为利用传感器从环境中获取环境点云图M(如公式5.2所示)以及通过人机交互接口获取机器人目的地信息Ptar(如公
式5.1所示)。
其中，Ptarx、Ptary为目标位置在地图坐标中的X、Y位置，θtar为机器人相对当前位置的偏转方向。
其中
是环境点云图M坐标中的第(i,j)元素的X,Y,Z位置。
进一步地，参照图2所示，所述路径规划器获取到机器人目的地信息Ptar和环境点云图信息M，这些信息被预处理并导入到路径规划器中训练好的深度强化学习智能体中，生成从机器人当前位置到目的位置的避障路径Pnzp与步态周期Tgait，避障路径Pnzp为区分左右脚的离散落脚点序列；
进一步地，参照图3所示，所述步态规划器的输入为来自路径规划器的落脚点序列Pnzp和步态周期Tgait，落脚点序列被所述步态规划器看作未来若干个步态周期的零力矩点(ZMP)参考值。采样周期Δt由控制系统的伺服周期决定。所述步态规划器的最终输出为机器人质心(COM)Pcom(如式5.3所示)、左脚轨迹Plf(如式5.4所示)、右脚轨迹Prf(如式5.5所示)以及经过重采样后的落脚点序列Pnzp。
参照图4a-4b所示，当机器人以单腿站立在第i个规划的落脚点
时，即第i个ZMP参考位置
它的动力学模型简化为空间三维小车-桌子模型，即机器人所有重量集中在小车中心处，假定机器人的机器人质心(COM)的高度为HC，则质心位置Pcomk＝(xk,yk)为k时刻的质心位置向量，则Nt个时刻的质心位置Pcomk组成的质心位置序列组成机器人质心(COM)Pcom，质心位置Pcomk与规划落脚点位置(即ZMP位置)Pk＝(Pxk,Pyk)的关系可以用式5.6所计算。
C＝[1,0,-Hc/g]
其中，
其中，rxk和ryk分别为k时刻质心在x方向和y方向的位置、速度、加速度组成的三维向量，
g为重力常数。
所述路径规划器的落脚点序列Pnzp被以采样周期Δt重采样为
NL个规划落脚点位置
被作为未来参考ZMP点位置输入到一个先进先出缓存器里，利用预观测控制器理论可以求得sxk和syk，其中，
为质心在x方向的加速度的导数，
为质心在y方向的加速度的导数，如式5.7所示。
此式中，
Ke，Kr，KPj为设定的增益系数。因而通过式5.6和式5.7来求得质心位置Pcomk。
如果第i个规划的落脚点
为为左脚的落脚点，则
和
均为左脚的落脚点，而
和
均为右脚的落脚点。
和
之间的左脚轨迹用式5.8来拟合，同理
和
之间的右脚轨迹用式5.9来拟合。
上两式中，A0→A5为多项式系数矩阵。
所述关节轨迹规划器接收来自步态规划器的机器人质心(COM)Pcom、左脚轨迹Plf、右脚轨迹Prf和落脚点序列Pnzp。关节轨迹规划器利用一个“质心位置—腰部中心位置”的神经网络(CWNN)，将Pcom、Plf、Prf转换为机器人腰部中心位置Pw，然后利用模型预观测控制(MPC)理论和人机耦合11杆动力学模型求得补偿动力学建模误差后的修正的机器人质心(COM)轨迹
最后再利用神经网络(CWNN)和机器人运动学逆解模型求得驱动机器人所需要的关节轨迹θjoint。
进一步地，参照图5所示，所述关节轨迹规划器的输入为来自步态规划器的机器人质心(COM)轨迹Pcom、左脚轨迹Plf、右脚轨迹Prf以及经过重采样后的落脚点序列Pnzp。所述关节轨迹规划器的输出为外骨骼机器人的关节轨迹指令θjoint，如式5.10所示。
θjoint＝[θ1,…,θj,…,θ10]T (5.10)
其中，输入的机器人质心(COM)Pcom、左脚轨迹Plf、右脚轨迹Prf一方面被存入先入先出缓存器A中，参照图6a所示，另一方面机器人质心(COM)Pcom、左脚轨迹Plf、右脚轨迹Prf被输入预先训练好的“质心位置—腰部中心位置”映射神经网络(CWNN)中，求得腰部中心位置轨迹Pw、左脚轨迹Plf、右脚轨迹Prf，利用机器人运动学逆解模型，求得规划的关节位置序列θjoint，利用数值解法求得关节速度序列
和加速度序列
通过机器人运动学正解模型求得机器人各个连杆的位置序列Plinks，速度序列Vlinks以及加速度序列Alinks，利用建立的人机耦合11杆动力学模型，求得各个连杆的动量序列Mo和角动量序列L，求得连杆的动量导数序列
和角动量导数序列
得到这些变量值后，真实的ZMP值
通过式5.11求得。真实的ZMP值
与规划的ZMP值Pnzp之间的差值ΔPzp通过式5.12求得，这些ZMP差值存入先进先出缓存器B中。利用缓存器B中的NL个ZMP差值
利用模型预观测控制器理论求得用于补偿动力学建模误差的机器人质心(COM)轨迹修正值
修正后的机器人质心(COM)
通过式5.13求得。将修正后的机器人质心(COM)轨迹值
与存入先入先出缓存器A的左脚轨迹
与右脚轨迹
输入至神经网络(CWNN)中，求得修正后的腰部位置值
将
输入值机器人运动学逆解模型中，求得用于驱动机器人的关节轨迹指令θjoint。
此式中，mall为人机系统的总质量，Pnzpz为落脚点的离地间隙高度。
参照图6a所示，上述过程中所提到的“质心位置—腰部中心位置”神经网络(CWNN)的输入为机器人质心(COM)位置与左脚位置的差值Pcl、机器人质心(COM)位置与右脚位置的差值Pcr、左脚位置与右脚位置的差值Plr，神经网络(CWNN)的输出为质心位置与腰部中心位置的差值Pwc。参照图6b所示，神经网络(CWNN)的网络结构含有三个隐藏层，每个隐藏层的节点均为128个，两层之间使用Relu激活函数。
实施例二
本说明书实施例二提供的一种机器人的轨迹规划方法，优选的为一种面向自平衡下肢外骨骼机器人的多层次轨迹规划方法，包括如下步骤：
步骤100、信息输入，轨迹规划所面向的使用环境为穿戴者自平衡外骨骼机器人的患者行走在包含静态障碍物和动态障碍物的日常环境，输入为利用传感器从环境中获取环境点云图以及通过人机交互接口获取机器人目的地信息。
步骤200、路径规划，机器人目的地信息和环境点云图信息被预处理并导入到路径规划器中训练好的深度强化学习智能体中，生成从机器人当前位置到目的位置的避障路径与步态周期，从而构成区分左右脚的离散落脚点序列；
路径规划采用基于深度强化学习算法的在线实时路径规划算法，路径规划融入了外骨骼运动学约束和人工势场理论，可以获得快速收敛的深度强化学习策略网络，用于在动态环境中生成满足步行安全稳定要求、符合人类行走思维、且能顺利避障的机器人双足落脚点路径。
步骤300、步态规划，接收到来自落脚点序列和步态周期，落脚点序列看作未来若干个步态周期的零力矩点(ZMP)参考值，根据零力矩点(ZMP)理论和预观测控制理论(MPC)生成机器人质心(COM)轨迹。同时，在每个落脚点之间利用多次样条曲线拟合出左脚轨迹和右脚轨迹，最终输出为机器人质心(COM)、左脚轨迹、右脚轨迹以及经过重采样后的落脚点序列；
步态规划采用基于模型预观测控制(MPC)理论和零力矩点(ZMP)理论的在线双足步态轨迹生成算法，用于生成满足双足步行稳定要求的机器人质心(COM)轨迹和左右脚轨迹。
步骤400、关节轨迹规划，接收来自步态规划的机器人质心(COM)、左脚轨迹、右脚轨迹和落脚点序列，利用“质心位置—腰部中心位置”的神经网络(CWNN)，将机器人质心(COM)、左脚轨迹、右脚轨迹转换为机器人腰部中心位置，利用模型预观测控制(MPC)理论和人机耦合11杆动力学模型求得补偿动力学建模误差后的修正机器人质心(COM)，利用神经网络(CWNN)和机器人运动学逆解模型求得驱动机器人所需要的关节轨迹。
关节轨迹规划融合了动力学过滤器及神经网络(CWNN)的机器人关节轨迹规划算法，用于补偿步态规划器中的动力学建模误差带来的行走稳定性影响，并利用神经网络(CWNN)将机器人质心(COM)轨迹映射为机器人腰部轨迹，结合机器人运动学模型求得驱动机器人所需要的关节轨迹；
进一步地，所述步骤100具体为：利用传感器从环境中获取环境点云图M(如公式5.2所示)以及通过人机交互接口获取机器人目的地信息Ptar(如公式5.1所示)。
其中，Ptarx、Ptary为目标位置在地图坐标中的X、Y位置，θtar为机器人相对当前位置的偏转方向。
其中
是环境点云图M坐标中的第(i,j)元素的X,Y,Z位置。
进一步地，所述步骤200具体为：
机器人目的地信息Ptar和环境点云图信息M被预处理并导入到路径规划器中训练好的深度强化学习智能体中，生成从机器人当前位置到目的位置的避障路径Pnzp与步态周期Tgait，避障路径Pnzp为区分左右脚的离散落脚点序列；
进一步地，所述步骤300具体为：
步骤310、输入为来自路径规划器的落脚点序列Pnzp和步态周期Tgait，落脚点序列Pnzp被看作未来若干个步态周期的零力矩点(ZMP)参考值；
步骤320、控制系统伺服周期决定采样周期Δt；
步骤330、输出为机器人质心(COM)Pcom(如式5.3所示)、左脚轨迹Plf(如
式5.4所示)、右脚轨迹Prf(如式5.5所示)以及经过重采样后的落脚点序列
其中，所述步骤330中的机器人质心(COM)Pcom求解具体为：
参照图4a-4b所示，当机器人以单腿站立在第i个规划的落脚点
时，即第i个ZMP参考位置
它的动力学模型简化为空间三维小车-桌子模型，即机器人所有重量集中在小车中心处，假定机器人的机器人质心(COM)的高度为HC，则质心位置Pcomk＝(xk,yk)为k时刻的质心位置向量，则Nt个时刻的质心位置Pcomk组成的质心位置序列组成机器人质心(COM)Pcom，质心位置Pcomk与规划落脚点位置(即ZMP位置)Pk＝(Pxk,Pyk)的关系可以用式5.6所计算。
C＝[1,0,-Hc/g]
其中，
其中，rxk和ryk分别为k时刻质心在x方向和y方向的位置、速度、加速度组成的三维向量，
g为重力常数。
所述路径规划器的落脚点序列Pnzp被以采样周期Δt重采样为
NL个规划落脚点位置
被作为未来参考ZMP点位置输入到一个先进先出缓存器里，利用预观测控制器理论可以求得sxk和syk，其中，
为质心在x方向的加速度的导数，
为质心在y方向的加速度的导数，如式5.7所示。
此式中，
Ke，Kr，KPj为设定的增益系数。因而通过式5.6和式5.7来求得质心位置Pcomk。
其中，所述步骤330中的左脚轨迹Plf和右脚轨迹Prf的求解具体为：
如果第i个规划的落脚点
为为左脚的落脚点，则
和
均为左脚的落脚点，而
和
均为右脚的落脚点。
和
之间的左脚轨迹用式5.8来拟合，同理
和
之间的右脚轨迹用式5.9来拟合。
上两式中，A0→A5为多项式系数矩阵。
进一步地，所述步骤400具体为：
步骤401、参照图5所示，输入的机器人质心(COM)Pcom、左脚轨迹Plf、右脚轨迹Prf一方面被存入先入先出缓存器A中，另一方面机器人质心(COM)Pcom、左脚轨迹Plf、右脚轨迹Prf被输入预先训练好的“质心位置—腰部中心位置”映射神经网络(CWNN)中，求得腰部中心位置轨迹Pw、左脚轨迹Plf、右脚轨迹Prf；
步骤402、利用机器人运动学逆解模型，求得规划的关节位置序列θjoint；
步骤403、利用数值解法求得关节速度序列
和加速度序列
步骤404、通过机器人运动学正解模型求得机器人各个连杆的位置序列Plinks、速度序列Vlinks以及加速度序列Alinks；
步骤405、利用建立的人机耦合11杆动力学模型，求得各个连杆的动量序列Mo和角动量序列L，求得连杆的动量导数序列
和角动量导数序列
步骤406、通过式5.11求得真实的零力矩点(ZMP)值
通过式5.12求得真实的零力矩点(ZMP)值
与规划的零力矩点(ZMP)值Pnzp之间的差值ΔPzp；
式中，mall为人机系统的总质量，Pnzpz为落脚点的离地间隙高度；
步骤407、差值ΔPzp存入先进先出缓存器B中；
步骤408、利用缓存器B中的NL个零力矩点(ZMP)差值
根据模型预观测控制器理论求得用于补偿动力学建模误差的机器人质心(COM)轨迹修正值
修正后的机器人质心(COM)
通过式5.13求得；
步骤409、将修正后的机器人质心(COM)值
与存入先入先出缓存器A的左脚轨迹
与右脚轨迹
输入至神经网络(CWNN)中，求得修正后的腰部位置值
步骤410、将
输入值机器人运动学逆解模型中，求得用于驱动机器人的关节轨迹指令θjoint。
其中，所述步骤400中，参照图6a所示，的“质心位置—腰部中心位置”神经网络(CWNN)的输入为机器人质心(COM)位置与左脚位置的差值Pcl、机器人质心(COM)位置与右脚位置的差值Pcr、左脚位置与右脚位置的差值Plr，神经网络(CWNN)的输出为质心位置与腰部中心位置的差值Pwc。参照图6b所示，神经网络(CWNN)的网络结构含有三个隐藏层，每个隐藏层的节点均为128个，两层之间使用Relu激活函数。
此外，参照图7a所示的多层轨迹规划算法的路径规划层输出的落脚点路径，结果表明该路径可以顺利抵达目的地区域(图7a中的圆圈)，并且以自然的曲度绕开了所有障碍物；参照图7b所示的步态规划层输出的步态轨迹以及经关节轨迹规划层校正后的步态轨迹，结果表明经关节轨迹规划层校正后，质心与腰部位置轨迹曲线变得更加光滑；参照图7c所示的关节轨迹规划层输出的关节位置轨迹，该结果表明关节轨迹曲线足够光滑，能保证机器人运动的顺滑性。
上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、笔记本电脑、行动电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
Claims (12)
1.一种机器人的轨迹规划系统，包括：路径规划器、步态规划器和关节轨迹规划器，
所述路径规划器利用环境点云图以及机器人目的地信息及采用基于深度强化学习算法的在线实时路径规划算法，生成避障路径与步态周期，构成落脚点序列；
所述步态规划器接收到来自所述路径规划器的落脚点序列和步态周期，根据零力矩点(ZMP)理论和预观测控制理论(MPC)生成机器人质心(COM)轨迹，利用多次样条曲线拟合出左脚轨迹和右脚轨迹，输出机器人质心轨迹、左脚轨迹、右脚轨迹以及经过重采样后的落脚点序列；
所述关节轨迹规划器根据机器人质心轨迹、左脚轨迹、右脚轨迹和经过重采样后的落脚点序列，利用模型预观测控制(MPC)理论和人机耦合11杆动力学模型，及融合动力学过滤器及神经网络(CWNN)的机器人关节轨迹规划算法，计算得到驱动机器人的关节轨迹。
2.根据权利要求1所述的一种机器人的轨迹规划系统，其特征在于：所述路径规划器获取到机器人目的地信息和环境点云图信息用于生成从机器人当前位置到目的位置的避障路径与步态周期。
3.根据权利要求1所述的一种机器人的轨迹规划系统，其特征在于：所述步态规划器中，落脚点序列被看作未来若干个步态周期的零力矩点参考值。
4.根据权利要求3所述的一种机器人的轨迹规划系统，其特征在于：建立机器人质心位置和规划落脚点位置的关系，求得机器人质心位置。
5.根据权利要求3所述的一种机器人的轨迹规划系统，其特征在于：通过轨迹拟合求得左脚轨迹、右脚轨迹。
6.根据权利要求1所述的一种机器人的轨迹规划系统，其特征在于：所述关节轨迹规划器利用神经网络将机器人质心轨迹、左脚轨迹、右脚轨迹转换为机器人腰部中心位置，利用人机耦合11杆动力学模型求得补偿动力学建模误差后的修正机器人质心轨迹，利用神经网络和机器人运动学逆解模型求得驱动机器人所需要的关节轨迹。
7.一种机器人的轨迹规划方法，包括如下步骤：
步骤100、信息输入，利用传感器从环境中获取环境点云图以及通过人机交互接口获取机器人目的地信息；
步骤200、路径规划，机器人目的地信息和环境点云图信息被预处理并导入到路径规划器中训练好的深度强化学习智能体中采用基于深度强化学习算法的在线实时路径规划算法，生成从机器人当前位置到目的位置的避障路径与步态周期，构成落脚点序列；
步骤300、步态规划，接收落脚点序列和步态周期，根据零力矩点(ZMP)理论和预观测控制理论(MPC)生成机器人质心(COM)轨迹，利用多次样条曲线拟合出左脚轨迹和右脚轨迹，输出机器人质心轨迹、左脚轨迹、右脚轨迹以及经过重采样后的落脚点序列；
步骤400、关节轨迹规划，根据机器人质心轨迹、左脚轨迹、右脚轨迹和落脚点序列，利用模型预观测控制(MPC)理论和人机耦合11杆动力学模型，及融合动力学过滤器及神经网络(CWNN)的机器人关节轨迹规划算法，计算得到驱动机器人的关节轨迹。
8.根据权利要求7所述的一种机器人的轨迹规划方法，其特征在于所述步骤300具体为：
步骤310、输入为来自路径规划器的落脚点序列和步态周期，落脚点序列被看作未来若干个步态周期的零力矩点参考值；
步骤320、控制系统伺服周期决定采样周期；
步骤330、输出机器人质心轨迹、左脚轨迹、右脚轨迹以及经过重采样后的落脚点序列。
9.根据权利要求8所述的一种机器人的轨迹规划方法，其特征在于所述步骤330中的机器人质心轨迹求解具体为：
将机器人的动力学模型简化为空间三维小车-桌子模型建立机器人质心位置和规划落脚点位置的关系，求得机器人质心位置。
10.根据权利要求8所述的一种机器人的轨迹规划方法，其特征在于所述步骤330中的左脚轨迹和右脚轨迹的求解具体为：通过轨迹拟合求得左脚轨迹、右脚轨迹。
11.根据权利要求7所述的一种机器人的轨迹规划方法，其特征在于所述步骤400具体为：
步骤401、机器人质心轨迹、左脚轨迹、右脚轨迹分别存入第一缓存器和预先训练好的质心位置—腰部中心位置映射神经网络(CWNN)中，求得腰部中心位置轨迹、左脚轨迹、右脚轨迹；
步骤402、利用机器人运动学逆解模型，求得规划的关节位置序列；
步骤403、利用数值解法求得关节速度序列和加速度序列；
步骤404、通过机器人运动学正解模型求得机器人各个连杆的位置序列、速度序列以及加速度序列；
步骤405、利用建立的人机耦合11杆动力学模型，求得各个连杆的动量序列和角动量序列，求得连杆的动量导数序列和角动量导数序列；
步骤406、求得真实的零力矩点理论值，求得真实的零力矩点理论值与规划的零力矩点理论值之间的差值；
步骤407、差值存入第二缓存器中；
步骤408、利用第二缓存器中的零力矩点理论差值，根据模型预观测控制器理论求得用于补偿动力学建模误差的机器人质心轨迹修正值，修正后的机器人质心轨迹；
步骤409、将修正后的机器人质心轨迹值与存入第一缓存器的左脚轨迹与右脚轨迹输入至神经网络中，求得修正后的腰部位置值；
步骤410、将修正后的腰部位置值、左脚轨迹与右脚轨迹输入机器人运动学逆解模型中，求得用于驱动机器人的关节轨迹指令。
12.根据权利要求11所述的一种机器人的轨迹规划方法，其特征在于所述步骤401中，神经网络的网络结构含有三个隐藏层，每个隐藏层的节点均为128个，两层之间使用Relu激活函数。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011426513.0A CN112720462B (zh) | 2020-12-09 | 2020-12-09 | 一种机器人的轨迹规划系统和方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011426513.0A CN112720462B (zh) | 2020-12-09 | 2020-12-09 | 一种机器人的轨迹规划系统和方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112720462A CN112720462A (zh) | 2021-04-30 |
| CN112720462B true CN112720462B (zh) | 2021-08-27 |
Family
ID=75598559
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011426513.0A Active CN112720462B (zh) | 2020-12-09 | 2020-12-09 | 一种机器人的轨迹规划系统和方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112720462B (zh) |
Families Citing this family (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113253724B (zh) * | 2021-04-30 | 2024-05-21 | 深圳市优必选科技股份有限公司 | 一种步态规划方法、装置、计算机可读存储介质及机器人 |
| CN113253727B (zh) * | 2021-05-13 | 2024-06-14 | 乐聚(深圳)机器人技术有限公司 | 一种机器人的轨迹确定方法、装置、设备及存储介质 |
| CN113962073A (zh) * | 2021-05-18 | 2022-01-21 | 追觅创新科技(苏州)有限公司 | 智能机器人的步态控制方法及装置、存储介质、电子装置 |
| CN113204892B (zh) * | 2021-05-26 | 2023-01-31 | 深圳市优必选科技股份有限公司 | 质心轨迹生成方法、装置、计算机可读存储介质及机器人 |
| CN113377116B (zh) * | 2021-07-14 | 2024-07-19 | 乐聚(深圳)机器人技术有限公司 | 机器人的步态规划方法、装置、处理设备及介质 |
| CN113520275A (zh) * | 2021-08-13 | 2021-10-22 | 元化智能科技(深圳)有限公司 | 无线胶囊内窥镜的轨迹追踪系统、设备及介质 |
| CN114179085B (zh) * | 2021-12-16 | 2024-02-06 | 上海景吾智能科技有限公司 | 机器人控制、轨迹衔接与平滑处理的方法及系统 |
| CN114700948B (zh) * | 2022-04-20 | 2023-07-18 | 中国科学技术大学 | 基于发散运动分量的下肢外骨骼机器人控制系统 |
| CN115042174B (zh) * | 2022-06-07 | 2024-08-30 | 中国北方车辆研究所 | 一种分层驱动的自主无人系统类人控制架构 |
| CN117011679A (zh) * | 2023-06-01 | 2023-11-07 | 深圳云天励飞技术股份有限公司 | 人行道病害检测方法、装置、电子设备及存储介质 |
| CN116604571B (zh) * | 2023-07-14 | 2023-10-27 | 湖南大学 | 一种基于深度强化学习的机器人三维测量路径规划方法 |
| CN117901124B (zh) * | 2024-03-20 | 2024-05-31 | 沈阳奇辉机器人应用技术有限公司 | 基于摘钩机器人的翻车机智能识别车钩方法及系统 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5594644A (en) * | 1992-05-22 | 1997-01-14 | Honda Giken Kogyo Kabushiki Kaisha | Method and system for generating trajectory of robot and the like |
| CN104570732A (zh) * | 2014-12-15 | 2015-04-29 | 浙江大学 | 一种仿人机器人步行模式的在线生成方法 |
| KR20170109438A (ko) * | 2016-03-21 | 2017-09-29 | 국방과학연구소 | 착용형 로봇의 제어를 위한 zmp 기반 보행궤적 생성 방법 및 보행 궤적 생성 시스템의 제어 방법 |
| CN108469732A (zh) * | 2018-03-13 | 2018-08-31 | 同济大学 | 基于dmp的机器人工作空间适应性行走控制系统及方法 |
| CN110315543A (zh) * | 2019-07-29 | 2019-10-11 | 北京理工大学 | 一种双足机器人步态生成与优化方法 |
| CN110361026A (zh) * | 2019-06-05 | 2019-10-22 | 华南理工大学 | 一种基于3d点云的仿人机器人路径规划方法 |
| CN110764415A (zh) * | 2019-10-31 | 2020-02-07 | 清华大学深圳国际研究生院 | 一种四足机器人腿部运动的步态规划方法 |
| CN110883776A (zh) * | 2019-11-29 | 2020-03-17 | 河南大学 | 一种快速搜索机制下改进dqn的机器人路径规划算法 |
-
2020
- 2020-12-09 CN CN202011426513.0A patent/CN112720462B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5594644A (en) * | 1992-05-22 | 1997-01-14 | Honda Giken Kogyo Kabushiki Kaisha | Method and system for generating trajectory of robot and the like |
| CN104570732A (zh) * | 2014-12-15 | 2015-04-29 | 浙江大学 | 一种仿人机器人步行模式的在线生成方法 |
| KR20170109438A (ko) * | 2016-03-21 | 2017-09-29 | 국방과학연구소 | 착용형 로봇의 제어를 위한 zmp 기반 보행궤적 생성 방법 및 보행 궤적 생성 시스템의 제어 방법 |
| CN108469732A (zh) * | 2018-03-13 | 2018-08-31 | 同济大学 | 基于dmp的机器人工作空间适应性行走控制系统及方法 |
| CN110361026A (zh) * | 2019-06-05 | 2019-10-22 | 华南理工大学 | 一种基于3d点云的仿人机器人路径规划方法 |
| CN110315543A (zh) * | 2019-07-29 | 2019-10-11 | 北京理工大学 | 一种双足机器人步态生成与优化方法 |
| CN110764415A (zh) * | 2019-10-31 | 2020-02-07 | 清华大学深圳国际研究生院 | 一种四足机器人腿部运动的步态规划方法 |
| CN110883776A (zh) * | 2019-11-29 | 2020-03-17 | 河南大学 | 一种快速搜索机制下改进dqn的机器人路径规划算法 |
Non-Patent Citations (1)
| Title |
|---|
| 基于稳定阈度分析的外骨骼动态步长规划方法;陈春杰等;《仪器仪表学报》;20170331;第38卷(第3期);523-529 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112720462A (zh) | 2021-04-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112720462B (zh) | 一种机器人的轨迹规划系统和方法 | |
| Kashyap et al. | Particle Swarm Optimization aided PID gait controller design for a humanoid robot | |
| CN112051741B (zh) | 一种双足机器人动态运动生成与控制方法 | |
| Huang et al. | A high stability, smooth walking pattern for a biped robot | |
| JP4815611B2 (ja) | 受動歩行脚式ロボット | |
| Kim et al. | Realization of dynamic walking for the humanoid robot platform KHR-1 | |
| CN103149933A (zh) | 基于闭环控制的仿人机器人全向行走方法 | |
| CN114248855B (zh) | 双足机器人空间域步态规划与控制的方法 | |
| KR20040075335A (ko) | 다리식 이동로봇의 보용 생성 장치 | |
| CN114022824A (zh) | 一种面向狭窄环境的四足机器人运动规划方法 | |
| Yue et al. | Simultaneous balancing and trajectory tracking control for two-wheeled inverted pendulum vehicles: a composite control approach | |
| Heydari et al. | Robust model predictive control of biped robots with adaptive on-line gait generation | |
| WO2004033160A1 (ja) | ロボット装置の動作制御装置及び動作制御方法 | |
| CN116859975A (zh) | 基于bp神经网络的轮腿机器人轮-足切换控制方法 | |
| Falsafi et al. | Fuzzy motion control for wheeled mobile robots in real-time | |
| Tan et al. | A hierarchical framework for quadruped locomotion based on reinforcement learning | |
| CN116619382A (zh) | 机器人运动控制方法、系统及电子设备 | |
| Kim et al. | ZMP based neural network inspired humanoid robot control | |
| CN114578836A (zh) | 一种双轮足机器人跳跃控制方法 | |
| Sombolestan et al. | Adaptive force-based control of dynamic legged locomotion over uneven terrain | |
| Tian et al. | Behavioral dynamics-based impedance control for collision avoidance of human-following robots | |
| Xie et al. | Gait optimization and energy-based stability for biped locomotion using large-scale programming | |
| Bachega et al. | Flexibility in Hexapod Robots: Exploring Mobility of the Body | |
| Shahrokhshahi et al. | Optimal stair climbing pattern generation for humanoids using virtual slope and distributed mass model | |
| WO2021095471A1 (ja) | 二足歩行ロボット、および二足歩行ロボットの制御方法 |
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 | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20231016 Address after: 518000 A-301, office building, Shenzhen Institute of advanced technology, No. 1068, Xue Yuan Avenue, Shenzhen University Town, Shenzhen, Guangdong, Nanshan District, China Patentee after: Shenzhen shen-tech advanced Cci Capital Ltd. Address before: 1068 No. 518055 Guangdong city in Shenzhen Province, Nanshan District City Xili University School Avenue Patentee before: SHENZHEN INSTITUTES OF ADVANCED TECHNOLOGY |
|
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20240112 Address after: 200120 Building 1, No. 1235 and 1237, Miaoxiang Road, Lingang New Area, China (Shanghai) Pilot Free Trade Zone, Pudong New Area, Shanghai Patentee after: SHANGHAI NOZOLI MACHINE TOOLS TECHNOLOGY Co.,Ltd. Address before: 518000 A-301, office building, Shenzhen Institute of advanced technology, No. 1068, Xue Yuan Avenue, Shenzhen University Town, Shenzhen, Guangdong, Nanshan District, China Patentee before: Shenzhen shen-tech advanced Cci Capital Ltd. |