CN109333534B - 预规划的实时步态控制算法 - Google Patents
预规划的实时步态控制算法 Download PDFInfo
- Publication number
- CN109333534B CN109333534B CN201811239754.7A CN201811239754A CN109333534B CN 109333534 B CN109333534 B CN 109333534B CN 201811239754 A CN201811239754 A CN 201811239754A CN 109333534 B CN109333534 B CN 109333534B
- Authority
- CN
- China
- Prior art keywords
- joint
- robot
- obstacle
- coordinate system
- 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.)
- Expired - Fee Related
Links
- 230000005021 gait Effects 0.000 title claims abstract description 36
- 238000000034 method Methods 0.000 claims abstract description 28
- 230000009471 action Effects 0.000 claims abstract description 7
- 238000012545 processing Methods 0.000 claims abstract description 5
- 210000002414 leg Anatomy 0.000 claims description 76
- 210000004394 hip joint Anatomy 0.000 claims description 48
- 210000001503 joint Anatomy 0.000 claims description 35
- 230000036544 posture Effects 0.000 claims description 20
- 230000005484 gravity Effects 0.000 claims description 19
- 238000005070 sampling Methods 0.000 claims description 15
- 210000000629 knee joint Anatomy 0.000 claims description 14
- 230000033001 locomotion Effects 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 14
- 230000009466 transformation Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 10
- 230000003287 optical effect Effects 0.000 claims description 9
- 230000008859 change Effects 0.000 claims description 8
- 238000001514 detection method Methods 0.000 claims description 8
- 238000013178 mathematical model Methods 0.000 claims description 6
- 238000013461 design Methods 0.000 claims description 5
- 230000002159 abnormal effect Effects 0.000 claims description 4
- 230000004888 barrier function Effects 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 238000004904 shortening Methods 0.000 claims description 3
- 230000000007 visual effect Effects 0.000 claims description 2
- 230000000875 corresponding effect Effects 0.000 abstract description 3
- 210000002683 foot Anatomy 0.000 description 15
- 238000010586 diagram Methods 0.000 description 9
- 210000000544 articulatio talocruralis Anatomy 0.000 description 4
- 238000011160 research Methods 0.000 description 3
- 238000005452 bending Methods 0.000 description 2
- 210000003127 knee Anatomy 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 210000002310 elbow joint Anatomy 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000009123 feedback regulation Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 210000000323 shoulder joint Anatomy 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
-
- 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/1628—Programme controls characterised by the control loop
- B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
-
- 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
- B25J9/1697—Vision controlled systems
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种预规划的实时步态控制算法,通过在双足机器人控制器的控制下,先建立双足机器人运动学坐标系模型,然后进行机器人的姿态规划和平稳性判断,最终完成步态行走,实时规划下一步的姿态,以保证机器人行走的稳定性。在算法的复杂度方面,需要的信息量少,方法简单;在算法的时效性方面,因为本发明需要的信息量少,实施的复杂度低,从而降低了算法的处理时间;在实际应用方面,本算法能够其能够结合机器人与地形的实际特点,预先设计出机器人每一步的动作使机器人按照计划完成相应的动作,并且通过实时采集前方图像信息进行避障,保证了行走的成功率。
Description
技术领域
本发明涉及步态算法技术领域,具体涉及一种预规划的实时步态控制算法。
背景技术
机器人集中了多种学科的最新研究成果,代表机电一体化的最高技术水平,为人工智能领域提供实践的机会。并且双足机器人作为灵活性、适应性最高的机器人之一,具有能够代替人类在恶劣环境下工作的潜力,成为国内外研究的热点。步态规划有利于提升机器人的稳定步行能力,因此对此方面的研究具有十分重要的意义。目前常见的步态算法有:圆规步态算法、三维线性倒立摆模型、捕获点理论等,总结目前已有技术可分为两类。第一类模型简单,易于计算,但其稳定性较差;第二类稳定性强,能量消耗少,但其计算繁杂,计算量巨大。
因此,如何提供一种计算简便而又稳定性较强的步态算法成为本领域技术人员目前需要解决的问题。
发明内容
本发明的目的是提出一种预规划的实时步态控制算法,在保证稳定性前提下,尽可能地简化算法。
为了实现上述任务,本发明采用以下技术方案:
一种预规划的实时步态控制算法,包括以下步骤:
步骤1,建立双足机器人运动学坐标系模型
所述的双足机器人身上的关节处安装有驱动关节旋转的舵机,其中在机器人腿部的裸关节具有前向关节和侧向关节,膝关节具有前向关节,髋关节具有前向关节、侧向关节及水平旋转关节;
步骤1.1,对机器人的各个关节进行编号,对于关节i,确定该关节的原点,原点确定的原则为:
当关节i的轴线与关节i+1的轴线相交时,取交点作为关节i的原点,当关节i的轴线与关节i+1的轴线异面时,取两条轴线公垂线与关节i+1轴线的交点作为关节i的原点;当关节i的轴线与关节i+1的轴线平行时,取关节i+1轴线与关节i+2轴线的公垂线与关节i+1轴线交点作为关节i的原点,记为Oi;
步骤1.2,确立关节i处坐标系的各个坐标轴Xi、Yi、Zi,以在各个关节处建立坐标系,确定原则为:
Zi坐标轴与关节i+1的轴线重合,Xi坐标轴沿i关节轴线、i+1关节轴线的公垂线并指向i+1关节,Yi坐标轴按右手法则确定;
步骤1.3,根据在各个关节处建立的坐标系,相邻的两个坐标系Oi与Oi-1相比,获得相邻两个坐标系的差距,即变换矩阵;
通过上面的变换矩阵得到相邻的两个坐标系之间的运动学关系,记双足机器人的左脚膝关节的坐标系为参考坐标系,可得到其他关节坐标系与参考坐标系之间的运动学关系;将每个关节处的坐标系与参考坐标系之间的变换矩阵作为该关节与参考坐标系所在关节之间的数学模型,所有数学模型构成双足机器人运动学坐标系模型;
步骤2,对双足机器人的姿态进行规划
步骤2.1,设计双足机器人行走步态流程
机器人通过摆动腿前摆改变重心,在摆动腿前摆时,身体重心从双腿中心移到支撑腿上,因此身体先要向支撑腿一侧扭动来移动重心,即通过髋关节的侧向关节来调整重心;为了防止在侧扭时抬腿而造成身体向摆动腿一侧倾倒、步态设计中将侧扭和抬腿分步设计;则机器人一次行走时的关节步态规划如下:
先通过右腿的髋关节、裸关节的侧向关节进行右侧扭来调整重心,侧扭到位后保持,然后左腿的裸关节、膝关节和髋关节的前向关节弯曲,以实现左腿抬起;左腿放下为相反动作,即先通过右腿的髋关节、裸关节左侧扭,然后左腿的裸关节、膝关节和髋关节的前向关节弯曲;
步骤2.2,规划机器人行走时各个关节的姿态
先假设髋关节的侧向不发生变化,则求解行走轨迹步骤如下:
(1)绘制出髋关节与裸关节随时间改变而变化的曲线图,由此获得髋关节、裸关节的运动学轨迹;
(2)与人类行走轨迹进行对比,按照人类行走过程的受力情况明确机器人髋关节、裸关节运动受力的关键点,然后通过三次样条插值的方式,对所有关键点进行插值处理;
(3)分别将髋关节、裸关节处理后的关键点与步骤(1)中髋关节、裸关节运动学轨迹中的多项式进行拟合,获得髋关节与裸关节的平滑轨迹;则该轨迹即为髋关节与裸关节每一步行走后形成的轨迹;
按照相同的方法,可获得机器人双腿上其他关节在行走时形成的轨迹;将双足机器人一次行走过程中所涉及的关节的轨迹作为规划姿态,则获得规划的姿态后,通过关节处的舵机驱动机器人按照规划的姿态进行行走。
进一步地,所述的预规划的实时步态控制算法,还包括:
步骤3,进行姿态规划平稳性判断
根据步骤2中的规划姿态,在机器人每次准备落脚时进行平稳性计算,判断当前机器人是否满足平稳性要求,判断依据为机器人的平稳点是否在设定的范围内,如在设定的范围内,则不干预,使机器人按照规划姿态进行行走,否则暂停行走,通过机器人的控制器调整机器人腿部的关节的姿态后,重新计算平稳性,直至平稳性满足要求,其中所述的平稳点计算方法为:
计算机器人的质心位置:
计算平稳点W:
W=-a+COM
其中,
上式中,ts为采样时间,COMi,COMi-1,COMi-2分别为第i次、第i-1次和第i-2次采样时机器人的质心位置,COM为机器人站立不动时的质心位置;
判断平稳点W的位置,若W在设定的范围内,则认为机器人此步行走稳定,否则不稳定。
进一步地,所述的机器人在行走时,按照以下方法进行障碍物的判断和避障:
步骤4,在机器人的头部安装有双目摄像头,其包括左右两个单目摄像头,用于机器人前方图像信息的采集;
对双目摄像头的左右两个摄像头分别进行标定,分别计算地面在左、右摄像头坐标系下的平面方程;
步骤5,通过双目摄像头采集机器人前方图像;
步骤6,对左右摄像头采集的两幅图像进行畸变和极线校正,消去失真,将匹配点约束在一条直线上,减少误匹配并大大缩短匹配时间,最终匹配得到一张视差图;
步骤7,计算视差图中匹配点在左摄像头坐标系下的三维坐标(x,y,z),获取其高度与水平距离,并与设定阈值比较,判断障碍物是否存在;
此处使用OpenCV视觉库中的cvFindCon—tours轮廓检测函数检测物体的轮廓,并用其外接矩形框标记;
对障碍物的判别算法:首先随机提取视差图中外接矩形框内物体上若干个白色像素点,计算其位于左摄像机坐标系下的三维坐标(x,y,z),其中x和z分别表示物体相对于左摄像头的高度距离与水平距离,y则表示物体相对于左摄像头光心的左右偏移距离,然后根据设定的阈值判断矩形框内的物体是否为障碍物;
步骤8,如判断前方无障碍物,则使机器人按照规划的路径行进;如判断前方有障碍物,则令机器人停止动作,并获取障碍物的参数信息;
步骤9,进行避障路径规划。
进一步地,所述的对障碍物的判别算法具体包括:
步骤7.1,随机提取视差图中外接矩形框内若干白色像素点i=1,2,3,...,计算其三维坐标di(xi,yi,zi),并按照水平距离zi的大小排序;
步骤7.2,取所有白色像素点水平距离zi的中位数z0,并设定阈值φ,对于所述的白色像素点,如白色像素点i的水平距离zi满足|zi-z0|>φ,则将白色像素点i作为异常点剔除,剩余白色像素点构成集合I;
步骤7.3,计算集合I中白色像素点水平距离zi的平均值z作为外接矩形框内障碍物距离机器人的水平距离;取集合I中白色像素点三维坐标中xi的最大值 xmax作为所述障碍物的高度;
进一步地,所述的如判断前方无障碍物,则使机器人按照规划的路径行进;如判断前方有障碍物,则令机器人停止动作,并获取障碍物的参数信息,具体包括:
步骤8.1,将障碍物的外接矩形框的宽度作为障碍物的宽度,记为Q,外接矩形框的中心作为障碍物中心点,记为O;
步骤8.2,通过深度检测获取O的三维坐标di(x,y,z),其中y表示中心点相对于左摄像头光心的偏移距离Li;获取外界矩形框中最左侧的一个白色像素点在左摄像头坐标系下的坐标(xL,yL,zL);
步骤8.3,根据障碍物的宽度计算出障碍物中心点O的纵坐标,即O点相对于左摄像头光心的偏移距离Lo:
步骤8.4,由于双目摄像头安装在机器人的中轴线上,因此障碍物中心点O 相对于机器人中轴线的偏距d为:
其中d>0表示偏右,d<0表示偏左,d=0表示居中。
进一步地,所述的进行避障路径规划,具体包括:
步骤9.1,设定障碍物的长度为W;
步骤9.2,当障碍物相对于机器人偏左时,机器人右转角度θ并沿平行安全距离直行一段距离X,机器人中心与障碍物中心的连线与规划路径垂直时,左转角度2θ并直行距离X,使其回归规划路径;
对于机器人的避障过程,有:
上式中,L为机器人的身宽;θ和2θ为机器人的旋转角度;D为障碍物相对机器人的距离;yL为障碍物左侧边界相对于机器人中心的偏移距离;d为考虑存在的误差和安全问题所增加的补偿距离。
本发明与现有技术相比,具有以下技术特点:
1.在算法的复杂度方面,需要的信息量少,方法简单;在算法的时效性方面,因为本发明需要的信息量少,实施的复杂度低,从而降低了算法的处理时间。
2.在实际应用方面,本算法能够其能够结合机器人与地形的实际特点,预先设计出机器人每一步的动作使机器人按照计划完成相应的动作,并且通过实时采集前方图像信息进行避障,保证了行走的成功率。
3.本算法步态稳定性强,且计算简单,符合工程思想,在实际操作领域实用性很强,对双足机器人步态算法领域有一定的指导作用。
附图说明
图1为本发明算法的流程示意图;
图2的(a)和(b)分别为本发明的机器人实物外形结构示意图,以及机器人上舵机安装位置示意图;
图3为本算法中在关节处建立坐标系的示意图;
图4为关节部分的结构示意图;
图5为机器人向前行走时不同步态的示意图;
图6为机器人腿部在行走时偏转角度示意图;
图7为机器人脚部受力示意图。
图8为避障算法中障碍物轮廓检测示意图;
图9为避障算法的避障路径规划示意图。
具体实施方式
如图1所示,本发明的基本思想是,通过在双足机器人控制器的控制下,先建立双足机器人运动学坐标系模型,然后进行机器人的姿态规划和平稳性判断,最终完成步态行走,实时规划下一步的姿态,以保证机器人行走的稳定性。一种预规划的实时步态控制算法,包括以下步骤:
步骤1,建立双足机器人运动学坐标系模型
所述的双足机器人身上的关节处安装有驱动关节旋转的舵机,如图2的(a) 和(b)所示,本实施例中的双足机器人全身共有20个舵机,其中在机器人腿部分布有多处关节,通过舵机的驱动使每条腿部具有6个自由度,即:裸关节具有向前和侧向两个自由度,分别称为踝关节的前向关节和侧向关节(图2中的15、17和16、18);膝关节一个前向自由度(图2中的13、14),称为膝关节的前向关节;髋关节具有三个自由度,包括前向、侧向及水平旋转自由度,分别称为髋关节的前向关节、侧向关节和水平旋转关节(图2中的7、9、11和8、10、12)。机器人上半身中,肘关节具有一个自由度(图2中5、6);肩关节有两个自由度 (图2中的1、3和2、4),颈关节一个自由度(图2中的19),头部一个自由度 (图2中的20)。
双足机器人的关键是由活动式连接的杆件构成的,相邻的两个杆件连接处称为一个关节,如图4所示。所述的杆件是指机器人躯干上的部分,例如手肘处作为一个关节,其小臂、大臂分别视为一个杆件;在双足机器人身上,相邻的杆件之间活动式连接,使相邻的杆件之间可以发生角度变化,例如相邻的杆件连接处构成转轴式结构。所述的舵机安装在每个关节处,用于驱动关节旋转,舵机可采用例如伺服电机。
该步骤的思路是,在机器人的每个关节处构件坐标系,通过坐标系的变换来实现关节处的两个杆件上坐标的变换;对于本方案的多关节机器人,每个关节由两个杆件组成,则机器人可看作是多杆件***,则多次利用坐标变换,就可以建立坐标系之间的关系,从而构成运动学坐标系模型,具体步骤如下:
步骤1.1,对机器人的各个关节进行编号,对于关节i,确定该关节的原点,原点确定的原则为:
当关节i的轴线(关节的旋转轴线)与关节i+1的轴线相交时,取交点作为关节i的原点,当关节i的轴线与关节i+1的轴线异面时,取两条轴线公垂线与关节i+1轴线的交点作为关节i的原点;当关节i的轴线与关节i+1的轴线平行时,取关节i+1轴线与关节i+2轴线的公垂线与关节i+1轴线交点作为关节i的原点,记为Oi。
步骤1.2,确立关节i处坐标系的各个坐标轴Xi、Yi、Zi,以在各个关节处建立坐标系,确定原则为:
Zi坐标轴与关节i+1的轴线重合,Xi坐标轴沿i关节轴线、i+1关节轴线的公垂线并指向i+1关节,Yi坐标轴按右手法则确定。
如图3所示,本方案中将双足机器人的左脚膝关节(前向关节)的坐标系 O0-X0Y0Z0为参考坐标系,左脚踝关节坐标系为O6-X6Y6Z6,右脚膝关节的坐标系为O12-X12Y12Z12。后两者为虚拟坐标系,仅为计算结果提供辅助和参考。
按照上述方法,在机器人各个关节处建立了坐标系。
上式中,s表示坐标系Oi-1到坐标系Oi的距离,c表示坐标系Oi-1到坐标系 Oi的角度,ai-1表示Xi到Xi+1的距离,di表示Xi到坐标系Oi的测量距离,θi表示从Xi到Xi-1转动的角度。由于一个关节i对应一个杆件,因此通过上述矩阵可求得相邻杆件之间的运动学关系。
通过上面的变换矩阵得到相邻的两个坐标系之间的运动学关系,由此得到左脚踝关节坐标系O6-X6Y6Z6与参考坐标系O0-X0Y0Z0之间的运动学关系,同理可得到其他关节坐标系与参考坐标系之间的运动学关系。
本方案中,将每个关节处的坐标系与参考坐标系之间的变换矩阵作为该关节与参考坐标系所在关节之间的数学模型,所有数学模型构成双足机器人运动学坐标系模型。
步骤2,对双足机器人的姿态进行规划
在本方案中,人形机器人的姿态规划则是建立在所建立的机器人运动学坐标系模型上的,即是以各关节在步骤1所建立的运动学坐标系模型中的坐标来表示,具体实施步骤如下:
步骤2.1,设计双足机器人行走步态流程
为使规划易于进行,将前向行走步态设计分为重心右移(先是右腿支撑)、左腿抬起、重心移到双腿中间、重心左移、右腿抬起、右腿放下、重心移到双腿间八个阶段,示意图如图5所示。
本方案的步态规划是通过摆动腿(抬起的腿)前摆改变重心的,在摆动腿前摆时,身体重心从双腿中心移到支撑腿上,因此身体先要向支撑腿(与地面接触的腿)一侧扭动来移动重心,即通过髋关节的侧向关节来调整重心;为了防止在侧扭时抬腿而造成身体向摆动腿一侧倾倒、步态设计中将侧扭和抬腿分步设计。以先抬起左脚为例,机器人一次行走时的关节步态规划如下:
先通过右腿的髋关节、裸关节的侧向关节进行右侧扭来调整重心,侧扭到位后保持,然后抬起左腿(摆动腿),具体为:左腿的裸关节、膝关节和髋关节的前向关节弯曲,以实现左腿抬起;左腿放下为相反动作,即先通过右腿的髋关节、裸关节左侧扭,然后左腿的裸关节、膝关节和髋关节的前向关节弯曲(弯曲方向与抬起时相反),此时左脚落地;右腿抬起、落地同理。
步骤2.2,规划机器人行走时各个关节的姿态
前向运动即是机器人腿部向前或先后摆动,侧向运动即是机器人腿部向左或向右摆动,对前向与侧向运动进行分别建模,完成机器人的前向行走规划。
前向运动由四个侧向关节和六个前向关节(左右裸关节、髋关节的侧向关节,左右裸关节、膝关节、髋关节的前向关节)的协调运动来实现,通过侧向关节的运动来移动机构的重心,双腿的前向关节的协调运动使机器人向前行走,具体实现如下:
对机器人双腿的关节旋转角度进行定义,θ1代表的是左腿从下到上的偏转角度;θ2代表的是左腿从上到下的偏转角度;θ3代表的是右腿从下到上的偏转角度;θ4代表的左腿从上到下的偏转角度,示意图见图6;则在前向运动中,由θ1、θ2、θ3、θ4运动实现。
先假设髋关节的侧向不发生变化,也就是四个关节(即左右腿的髋关节的两个转弯关节和侧向关节)的侧向偏转角度均为0,则求解向前或向后运动的方程的步骤如下,以髋关节与踝关节为例:
(1)绘制出髋关节与裸关节随时间改变而变化的曲线图(即建立在步骤1坐标系上的二维曲线图),由此获得髋关节、裸关节的运动学轨迹;
(2)与人类行走轨迹进行对比,按照人类行走过程的受力情况明确机器人髋关节、裸关节运动受力的关键点,然后通过三次样条插值的方式,对所有关键点进行插值处理;
(3)分别将髋关节、裸关节处理后的关键点与步骤(1)中髋关节、裸关节运动学轨迹中的多项式进行拟合,获得髋关节与裸关节的平滑轨迹;则该轨迹即为髋关节与裸关节每一步行走后形成的轨迹。
按照相同的方法,可获得机器人双腿上其他关节在行走时形成的轨迹;将双足机器人一次行走(左脚从抬起到放下为一次行走)过程中所涉及的关节的轨迹作为规划姿态,则获得规划的姿态后,通过关节处的舵机驱动机器人按照规划的姿态进行行走。
在机器人每次落脚时会处于一定的不稳定状态,因此还需要进行稳定性判断,根据稳定性判断的结果,对机器人的落脚点进行反馈调节。
步骤3,进行姿态规划平稳性判断
地面反作用力对脚部的影响是复杂的,本方案中将其简化成有任意一点P,而地面的反作用力对脚部的贡献可以等价为一个力R与力矩M。而零力矩点的定义就是在地面上存在一点P,使得与地面平行轴方向的、由惯性力(F=ma) 与重力(G=mg)所产生的净力矩为零的点。图7给出了机器人脚部所受作用力分布的例子,沿足底分布的负载具有相同的方向,他们等效于一个合力N,合力 N所通过的在足底上的这个作用点被称为平稳点。该点可以理解为重力和惯性力的合力在地面上的投影,而且在这一点上合力的力矩在水平方向为零。
根据步骤2中的规划姿态,在机器人每次准备落脚时进行平稳性计算,判断当前机器人是否满足平稳性要求,判断依据为机器人的平稳点是否在设定的范围内,如在设定的范围内,则不干预,使机器人按照规划姿态进行行走,否则暂停行走,通过机器人的控制器调整机器人腿部的关节(髋关节、膝关节、裸关节) 的姿态后,重新计算平稳性,直至平稳性满足要求,其中所述的平稳点计算方法为:
计算机器人的质心位置:
计算平稳点W:
W=-a+COM
其中,
上式中,ts为采样时间,COMi,COMi-1,COMi-2分别为第i次、第i-1次和第i-2次采样时机器人的质心位置,COM为机器人站立不动时的质心位置。
判断平稳点W的位置,若W在设定的范围内,则认为机器人此步行走稳定,否则不稳定。本实施例中,所述的设定范围为机器人抬脚后脚中心在地面投影点前后-5~5cm范围。
综上所述,本方案通过对人体步行轨迹的模仿来预先设计双足机器人的步态规划,按照先姿态规划后稳定性判断,预先规划出机器人完整的姿态,实现实时步态控制。
本方案提出的预规划的实时步态控制算法解决了机器人正常行走的问题,避开了传统算法的复杂与繁琐,预先设计出机器人每一步的动作使机器人按照计划完成相应的动作,保证了行走的成功率。
在上述技术方案的基础上,机器人按照规划姿态行走时,采用下面的方法进行障碍物的判断和避障:
步骤4,在机器人的头部安装有双目摄像头,其包括左右两个单目摄像头,用于机器人前方图像信息的采集;
对双目摄像头的左右两个摄像头分别进行标定,分别计算地面在左、右摄像头坐标系下的平面方程;
步骤5,通过双目摄像头采集机器人前方图像;
步骤6,对左右摄像头采集的两幅图像(左右摄像头同时分别采集一张图像) 进行畸变和极线校正,消去失真,将匹配点约束在一条直线上,减少误匹配并大大缩短匹配时间,最终匹配得到一张视差图;
步骤7,计算视差图中匹配点在左摄像头坐标系下的三维坐标(x,y,z),获取其高度与水平距离,并与设定阈值比较,判断障碍物是否存在;
此处使用OpenCV视觉库中的cvFindCon—tours轮廓检测函数检测物体的轮廓,并用其外接矩形框标记,如图8所示。根据其中的白色像素点,利用双目视觉测距原理计算出物体相对左摄像头坐标系下的三维坐标。
对障碍物的判别算法:首先随机提取视差图中外接矩形框内物体上若干个白色像素点,计算其位于左摄像机坐标系下的三维坐标(x,y,z),其中x和z分别表示物体相对于左摄像头的高度距离与水平距离,y则表示物体相对于左摄像头光心的左右偏移距离,然后根据设定的阈值判断矩形框内的物体是否为障碍物。
具体步骤如下:
步骤7.1,随机提取视差图中外接矩形框内若干白色像素点i=1,2,3,...,计算其三维坐标di(xi,yi,zi),并按照水平距离zi(即三维坐标中的zi值)的大小排序;
步骤7.2,因为提取和计算过程中存在误差,所以可能存在异常数据,因此取所有白色像素点水平距离zi的中位数z0,并设定阈值φ,对于所述的白色像素点,如白色像素点i的水平距离zi满足|zi-z0|>φ,则将白色像素点i作为异常点剔除,剩余白色像素点构成集合I;
步骤8,如判断前方无障碍物,则使机器人按照规划的路径行进;如判断前方有障碍物,则令机器人停止动作,并获取障碍物的参数信息,具体如下:
步骤8.1,将障碍物的外接矩形框的宽度作为障碍物的宽度,记为Q,外接矩形框的中心作为障碍物中心点,记为O;
步骤8.2,通过深度检测获取(即通过双目摄像头检测)O的三维坐标 di(x,y,z),其中y表示中心点相对于左摄像头光心的偏移距离Li;获取外界矩形框中最左侧的一个白色像素点在左摄像头坐标系下的坐标(xL,yL,zL);
步骤8.3,根据障碍物的宽度计算出障碍物中心点O的纵坐标,即O点相对于左摄像头光心的偏移距离Lo:
步骤8.4,由于双目摄像头安装在机器人的中轴线上,因此障碍物中心点O 相对于机器人中轴线的偏距d为:
其中d>0表示偏右,d<0表示偏左,d=0表示居中。
步骤9,进行避障路径规划
步骤9.1,设定障碍物的长度为W;
在障碍物检测过程中,已经能够比较精确地获得障碍物的宽度尺寸及其相对于机器人的方位信息,但是较难得到障碍的长度尺寸,一般假设其长度为W;
步骤9.2,当障碍物相对于机器人偏左时,为了能更快绕过障碍物,根据障碍物的位置及实验室环境的不同,设置一个恒定的安全距离,机器人右转角度θ并沿平行安全距离直行一段距离X,机器人中心与障碍物中心的连线与规划路径垂直时,左转角度2θ并直行距离X,使其回归规划路径;障碍物偏右和居中时同理,障碍物偏右时机器人左转,障碍物居中时随机选择左转或右转,不再赘述;示意图如图9所示:在图中,L为机器人的身宽;θ和2θ为机器人的旋转角度; D为障碍物相对机器人的距离;yL为障碍物左侧边界相对于机器人中心的偏移距离;d为考虑存在的误差和安全问题所增加的补偿距离(一般取d=yL)。
对于机器人的避障过程,有:
至此则可完成障碍检测和避障路径规划。
Claims (4)
1.一种预规划的实时步态控制算法,其特征在于,包括以下步骤:
步骤1,建立双足机器人运动学坐标系模型
所述的双足机器人身上的关节处安装有驱动关节旋转的舵机,其中在机器人腿部的裸关节具有前向关节和侧向关节,膝关节具有前向关节,髋关节具有前向关节、侧向关节及水平旋转关节;
步骤1.1,对机器人的各个关节进行编号,对于关节i,确定该关节的原点,原点确定的原则为:
当关节i的轴线与关节i+1的轴线相交时,取交点作为关节i的原点,当关节i的轴线与关节i+1的轴线异面时,取两条轴线公垂线与关节i+1轴线的交点作为关节i的原点;当关节i的轴线与关节i+1的轴线平行时,取关节i+1轴线与关节i+2轴线的公垂线与关节i+1轴线交点作为关节i的原点,记为Oi;
步骤1.2,确立关节i处坐标系的各个坐标轴Xi、Yi、Zi,以在各个关节处建立坐标系,确定原则为:
Zi坐标轴与关节i+1的轴线重合,Xi坐标轴沿i关节轴线、i+1关节轴线的公垂线并指向i+1关节,Yi坐标轴按右手法则确定;
步骤1.3,根据在各个关节处建立的坐标系,相邻的两个坐标系Oi与Oi-1相比,获得相邻两个坐标系的差距,即变换矩阵;
通过上面的变换矩阵得到相邻的两个坐标系之间的运动学关系,记双足机器人的左脚膝关节的坐标系为参考坐标系,可得到其他关节坐标系与参考坐标系之间的运动学关系;将每个关节处的坐标系与参考坐标系之间的变换矩阵作为该关节与参考坐标系所在关节之间的数学模型,所有数学模型构成双足机器人运动学坐标系模型;
步骤2,对双足机器人的姿态进行规划
步骤2.1,设计双足机器人行走步态流程
机器人通过摆动腿前摆改变重心,在摆动腿前摆时,身体重心从双腿中心移到支撑腿上,因此身体先要向支撑腿一侧扭动来移动重心,即通过髋关节的侧向关节来调整重心;为了防止在侧扭时抬腿而造成身体向摆动腿一侧倾倒、步态设计中将侧扭和抬腿分步设计;则机器人一次行走时的关节步态规划如下:
先通过右腿的髋关节、裸关节的侧向关节进行右侧扭来调整重心,侧扭到位后保持,然后左腿的裸关节、膝关节和髋关节的前向关节弯曲,以实现左腿抬起;左腿放下为相反动作,即先通过右腿的髋关节、裸关节左侧扭,然后左腿的裸关节、膝关节和髋关节的前向关节弯曲;
步骤2.2,规划机器人行走时各个关节的姿态
先假设髋关节的侧向不发生变化,则求解行走轨迹步骤如下:
(1)绘制出髋关节与裸关节随时间改变而变化的曲线图,由此获得髋关节、裸关节的运动学轨迹;
(2)与人类行走轨迹进行对比,按照人类行走过程的受力情况明确机器人髋关节、裸关节运动受力的关键点,然后通过三次样条插值的方式,对所有关键点进行插值处理;
(3)分别将髋关节、裸关节处理后的关键点与步骤(1)中髋关节、裸关节运动学轨迹中的多项式进行拟合,获得髋关节与裸关节的平滑轨迹;则该轨迹即为髋关节与裸关节每一步行走后形成的轨迹;
按照相同的方法,可获得机器人双腿上其他关节在行走时形成的轨迹;将双足机器人一次行走过程中所涉及的关节的轨迹作为规划姿态,则获得规划的姿态后,通过关节处的舵机驱动机器人按照规划的姿态进行行走;
步骤3,进行姿态规划平稳性判断
根据步骤2中的规划姿态,在机器人每次准备落脚时进行平稳性计算,判断当前机器人是否满足平稳性要求,判断依据为机器人的平稳点是否在设定的范围内,如在设定的范围内,则不干预,使机器人按照规划姿态进行行走,否则暂停行走,通过机器人的控制器调整机器人腿部的关节的姿态后,重新计算平稳性,直至平稳性满足要求;平稳点计算方法为:
计算机器人的质心位置:
计算平稳点W:
W=-a+COM
其中,
上式中,ts为采样时间,COMi,COMi-1,COMi-2分别为第i次、第i-1次和第i-2次采样时机器人的质心位置,COM为机器人站立不动时的质心位置;
判断平稳点W的位置,若W在设定的范围内,则认为机器人此步行走稳定,否则不稳定;
所述的机器人在行走时,按照以下方法进行障碍物的判断和避障:
步骤4,在机器人的头部安装有双目摄像头,其包括左右两个单目摄像头,用于机器人前方图像信息的采集;
对双目摄像头的左右两个摄像头分别进行标定,分别计算地面在左、右摄像头坐标系下的平面方程;
步骤5,通过双目摄像头采集机器人前方图像;
步骤6,对左右摄像头采集的两幅图像进行畸变和极线校正,消去失真,将匹配点约束在一条直线上,减少误匹配并大大缩短匹配时间,最终匹配得到一张视差图;
步骤7,计算视差图中匹配点在左摄像头坐标系下的三维坐标(x,y,z),获取其高度与水平距离,并与设定阈值比较,判断障碍物是否存在;
此处使用OpenCV视觉库中的cvFindCon—tours轮廓检测函数检测物体的轮廓,并用其外接矩形框标记;
对障碍物的判别算法:首先随机提取视差图中外接矩形框内物体上若干个白色像素点,计算其位于左摄像机坐标系下的三维坐标(x,y,z),其中x和z分别表示物体相对于左摄像头的高度距离与水平距离,y则表示物体相对于左摄像头光心的左右偏移距离,然后根据设定的阈值判断矩形框内的物体是否为障碍物;
步骤8,如判断前方无障碍物,则使机器人按照规划的路径行进;如判断前方有障碍物,则令机器人停止动作,并获取障碍物的参数信息;
步骤9,进行避障路径规划。
2.如权利要求1所述的预规划的实时步态控制算法,其特征在于,所述的对障碍物的判别算法具体包括:
步骤7.1,随机提取视差图中外接矩形框内若干白色像素点i=1,2,3,...,计算其三维坐标di(xi,yi,zi),并按照水平距离zi的大小排序;
步骤7.2,取所有白色像素点水平距离zi的中位数z0,并设定阈值φ,对于所述的白色像素点,如白色像素点i的水平距离zi满足|zi-z0|>φ,则将白色像素点i作为异常点剔除,剩余白色像素点构成集合I;
3.如权利要求1所述的预规划的实时步态控制算法,其特征在于,所述的如判断前方无障碍物,则使机器人按照规划的路径行进;如判断前方有障碍物,则令机器人停止动作,并获取障碍物的参数信息,具体包括:
步骤8.1,将障碍物的外接矩形框的宽度作为障碍物的宽度,记为Q,外接矩形框的中心作为障碍物中心点,记为O;
步骤8.2,通过深度检测获取O的三维坐标di(x,y,z),其中y表示中心点相对于左摄像头光心的偏移距离Li;获取外界矩形框中最左侧的一个白色像素点在左摄像头坐标系下的坐标(xL,yL,zL);
步骤8.3,根据障碍物的宽度计算出障碍物中心点O的纵坐标,即O点相对于左摄像头光心的偏移距离Lo:
步骤8.4,由于双目摄像头安装在机器人的中轴线上,因此障碍物中心点O相对于机器人中轴线的偏距d为:
其中d>0表示偏右,d<0表示偏左,d=0表示居中。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811239754.7A CN109333534B (zh) | 2018-10-23 | 2018-10-23 | 预规划的实时步态控制算法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811239754.7A CN109333534B (zh) | 2018-10-23 | 2018-10-23 | 预规划的实时步态控制算法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109333534A CN109333534A (zh) | 2019-02-15 |
CN109333534B true CN109333534B (zh) | 2021-12-17 |
Family
ID=65311403
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811239754.7A Expired - Fee Related CN109333534B (zh) | 2018-10-23 | 2018-10-23 | 预规划的实时步态控制算法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109333534B (zh) |
Families Citing this family (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110371213A (zh) * | 2019-07-12 | 2019-10-25 | 沈阳城市学院 | 一种双足机器人步行规划和控制方法 |
CN110834685B (zh) * | 2019-09-29 | 2021-05-28 | 中国北方车辆研究所 | 一种四足机器人动态跨越凹障碍方法 |
CN113050616A (zh) * | 2019-12-27 | 2021-06-29 | 深圳市优必选科技股份有限公司 | 一种双足机器人行走的控制方法及双足机器人 |
CN111515954B (zh) * | 2020-05-06 | 2021-08-20 | 大连理工大学 | 一种机械臂高质量运动路径生成方法 |
CN112180958B (zh) * | 2020-09-23 | 2022-08-19 | 北航歌尔(潍坊)智能机器人有限公司 | 机器人及其运动协调方法、控制装置和可读存储介质 |
CN112220650B (zh) * | 2020-12-09 | 2021-04-16 | 南京伟思医疗科技股份有限公司 | 一种外骨骼机器人对侧训练用在线步态生成控制*** |
CN112731953B (zh) * | 2020-12-24 | 2024-07-19 | 深圳市优必选科技股份有限公司 | 机器人控制方法、装置、计算机可读存储介质及机器人 |
CN112731952B (zh) * | 2020-12-24 | 2022-03-01 | 深圳市优必选科技股份有限公司 | 一种机器人质心规划方法、装置、可读存储介质及机器人 |
CN114700948B (zh) * | 2022-04-20 | 2023-07-18 | 中国科学技术大学 | 基于发散运动分量的下肢外骨骼机器人控制*** |
Family Cites Families (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4184679B2 (ja) * | 2001-08-01 | 2008-11-19 | 本田技研工業株式会社 | 二足歩行移動体の床反力推定方法及び二足歩行移動体の関節モーメント推定方法 |
JP3646169B2 (ja) * | 2002-05-07 | 2005-05-11 | 独立行政法人産業技術総合研究所 | 脚式ロボットの歩行制御装置 |
JP4810880B2 (ja) * | 2005-05-10 | 2011-11-09 | トヨタ自動車株式会社 | ロボットとその制御方法 |
KR101687631B1 (ko) * | 2010-01-18 | 2016-12-20 | 삼성전자주식회사 | 로봇의 보행 제어 장치 및 그 제어 방법 |
JP5859036B2 (ja) * | 2014-02-04 | 2016-02-10 | 本田技研工業株式会社 | ロボット |
CN105608309B (zh) * | 2015-12-11 | 2019-08-02 | 杭州南江机器人股份有限公司 | 一种双足机器人步行规划和控制方法 |
CN105938499A (zh) * | 2016-01-08 | 2016-09-14 | 浙江大学 | 一种3d双足机器人的坐标系建立方法 |
CN105965506A (zh) * | 2016-05-16 | 2016-09-28 | 北京格分维科技有限公司 | 一种基于遗传算法的人形双足机器人步行姿态控制方法 |
CN106176149A (zh) * | 2016-09-08 | 2016-12-07 | 电子科技大学 | 一种基于多传感融合的外骨骼步态分析***及方法 |
CN108009680A (zh) * | 2017-11-30 | 2018-05-08 | 航天科工智能机器人有限责任公司 | 基于多目标粒子群算法的仿人机器人步态规划方法 |
-
2018
- 2018-10-23 CN CN201811239754.7A patent/CN109333534B/zh not_active Expired - Fee Related
Non-Patent Citations (2)
Title |
---|
仿人机器人的步态规划和步行控制研究;付根平;《中国博士学位论文全文数据库信息科技辑》;20130915;第19-22、32-60页 * |
基于双目视觉的AGV障碍物检测与避障;王铮等;《计算机集成制造***》;20180730;第400-409页 * |
Also Published As
Publication number | Publication date |
---|---|
CN109333534A (zh) | 2019-02-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109333534B (zh) | 预规划的实时步态控制算法 | |
CN109333506B (zh) | 一种人形智能机器人*** | |
CN113696186B (zh) | 复杂光照条件下基于视触融合的机械臂自主移动抓取方法 | |
JP4479372B2 (ja) | 環境地図作成方法、環境地図作成装置、及び移動型ロボット装置 | |
CN107901041B (zh) | 一种基于图像混合矩的机器人视觉伺服控制方法 | |
Nishiwaki et al. | Autonomous navigation of a humanoid robot over unknown rough terrain using a laser range sensor | |
US8873831B2 (en) | Walking robot and simultaneous localization and mapping method thereof | |
US9043146B2 (en) | Systems and methods for tracking location of movable target object | |
EP1975046B1 (en) | Legged locomotion robot | |
JP6826069B2 (ja) | ロボットの動作教示装置、ロボットシステムおよびロボット制御装置 | |
Oßwald et al. | Autonomous climbing of spiral staircases with humanoids | |
US20220390954A1 (en) | Topology Processing for Waypoint-based Navigation Maps | |
KR102094004B1 (ko) | 탁구 로봇 제어 방법 및 이를 위한 시스템 | |
CN113362396A (zh) | 一种移动机器人3d手眼标定方法及装置 | |
WO2022000713A1 (zh) | 一种基于航空装配的增强现实自定位方法 | |
CN113751981B (zh) | 基于双目视觉伺服的空间高精度装配方法和*** | |
CN113172659B (zh) | 基于等效中心点识别的柔性机器人臂形测量方法及*** | |
CN114378827B (zh) | 一种基于移动机械臂整体控制的动态目标跟踪抓取方法 | |
Camurri et al. | Real-time depth and inertial fusion for local SLAM on dynamic legged robots | |
CN117794704A (zh) | 机器人控制设备、机器人控制***以及机器人控制方法 | |
Gouda et al. | Complex motion planning for nao humanoid robot | |
US20220388174A1 (en) | Autonomous and teleoperated sensor pointing on a mobile robot | |
US20240192695A1 (en) | Anchoring based transformation for aligning sensor data of a robot with a site model | |
CN117921682A (zh) | 基于双目视觉的焊接机器人快速示教装置及方法 | |
Wongwilai et al. | SLAM-based grasping framework for robotic arm navigation and object model construction |
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 | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20211217 |
|
CF01 | Termination of patent right due to non-payment of annual fee |