CN106826813A - 基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法 - Google Patents
基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法 Download PDFInfo
- Publication number
- CN106826813A CN106826813A CN201611138340.6A CN201611138340A CN106826813A CN 106826813 A CN106826813 A CN 106826813A CN 201611138340 A CN201611138340 A CN 201611138340A CN 106826813 A CN106826813 A CN 106826813A
- Authority
- CN
- China
- Prior art keywords
- formula
- point
- robot
- leg
- foot
- 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
Links
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/1671—Programme controls characterised by programming, planning systems for manipulators characterised by simulation, either to verify existing program or to create and verify new program, CAD/CAM oriented, graphic oriented programming systems
-
- 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/1669—Programme controls characterised by programming, planning systems for manipulators characterised by special application, e.g. multi-arm co-operation, assembly, grasping
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2111/00—Details relating to CAD techniques
- G06F2111/04—Constraint-based CAD
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- General Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Manipulator (AREA)
Abstract
一种基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法,包括以下步骤:步骤一:基于曲线拟合算法对摆动腿足端所处局部环境进行建模;步骤二:根据所拟合出的局部环境,结合摆动腿足端工作空间,拟合出摆动腿足端可达区域;步骤三:根据步骤二中所拟合出的摆动腿足端可达区域,结合多重约束落足点评估算法,对摆动腿足端可达区域内的可规划落足点进行评估分析;步骤四:根据步骤一至步骤三,结合多重姿态转换算法,解析机器人关节输出位置,并对关节转角进行多项式插补运算,完成机器人在非结构环境下的稳定运动控制。本发明提出一种地形适应能力强、高效的稳定运动控制方法。
Description
技术领域
本发明涉及多足机器人非结构环境下局部环境建模、理想落足点评估及稳定运动控制领域,具体涉及基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法。
背景技术
多足机器人具有丰富的驱动冗余和离散的步态点,能够实现复杂的运动方式,具有较强的地形适应能力。据统计,地球上有90%以上的地表属于崎岖表面,要使多足机器人在户外环境下进行作业,就必须对其在非结构环境下的运动进行规划,实现运动的稳定性和高效性。
在非结构环境下,多足机器人的稳定运动包括三个过程:(1)机器人机身重心移到工作空间内指定的点;(2)拟合摆动腿足端所处环境,摆动腿抬起落到理想落足点;(3)摆动相和支撑相互换,新的摆动相落到理想落足点。其中,拟合摆动腿足端所处环境和足端理想落足点分析是多足机器人能否在非结构环境下稳定运动的关键。
现有的局部环境建模算法,主要的思想是取局部环境中的一些特征点,然后利用这些特征点来模拟局部环境。这种算法的优点是环境拟合的准确度较高,缺点是要准确地拟合环境需要提取大量的特征点,而且,提取特征点的算法又具有许多种类,其中,不少算法的算法复杂度较高,因此,整个局部环境模拟算法不管是算法本身的复杂度还是时间复杂度都较高,不利于多足机器人在非结构环境下的运动规划。因此,目前迫切需要提出一种高效、精确的局部环境拟合算法,并将其应用于非结构环境下多足机器人稳定运动控制中。
现有的摆动腿足端理想落足点评估算法,主要的思想是建立落足点评估约束函数,对局部环境中的落足点候选点进行评估,如哈工大的陈杰等通过分析局部地形信息获得地面几何参数,并由此建立约束函数,然后代入一组落足候选点的位置坐标进行代数分析,最终使得效用函数取得最大值的那组点为最佳落足点。这种方法的优点是运算量较小,缺点是建立效用函数的过程中并没有引入任何与机器人状态相关的参数,因此得到的一组落足点可能并不一定是最佳落足点。西班牙学者J.Estremera在分析地形参数的基础上,加入了机器人的步态稳定裕量,并由此建立了效用函数。这种方法的缺点是没有考虑到相邻腿坐标对摆动腿的影响,可能当摆动腿达到理想落足点时,出现机身“死锁”的情况。因此,目前迫切需要提出一种基于多种评估条件的落足点评估算法,包括对落足点所处局部环境几何结构的分析以及机器人本身位姿状态的分析。
发明内容
鉴于现有局部环境拟合算法和理想落足点评估方法的效率低及评估过程中容易出现“坏点”的情况,本发明提出一种曲线拟合局部环境建模算法和多重约束落足点评估算法,并将两个算法发应用于多足机器人非结构环境下的稳定运动控制,最终提出一种地形适应能力强、高效的稳定运动控制方法。
本发明解决其技术问题所采用的技术方案是:
一种基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法,包括以下步骤:
步骤一:基于曲线拟合算法对摆动腿足端所处局部环境进行建模,包括传感器位置分布,局部环境网格化,网格化局部环境点插补及二次曲线拟合;
步骤二:根据步骤一中所拟合出的局部环境,结合摆动腿足端工作空间,拟合出摆动腿足端可达区域;
步骤三:根据步骤二中所拟合出的摆动腿足端可达区域,结合多重约束落足点评估算法,对摆动腿足端可达区域内的可规划落足点进行评估分析;
步骤四:根据步骤一至步骤三,结合多重姿态转换算法,解析机器人关节输出位置,并对关节转角进行多项式插补运算,完成机器人在非结构环境下的稳定运动控制。
进一步,所述步骤一中,首先,完成测距传感器的位置分布,测距传感器主要分布在六足机器人的五个根关节处;其次,对局部环境进行网格化,用Pi S[i=1,2,3,4,6]来表示测距传感器在世界坐标系上的五个垂直投影点,再用表示多足机器前后四个足端点在世界坐标系上的五个垂直投影点,其位置坐标可以通过机器人正运动学求解获得;至此,一共获得九个已知坐标的点,其表达式如下:
用Gij=[xij yij zij]T来表示网格的节点坐标,其中,i代表的是网格的行,j代表网格的列,即Gij代表的是网格第i行和第j列交点处的坐标,至此,一共获得20个网格点,实现对局部环境的网格化;
然后,求解网格中未知点在世界坐标系下的位置坐标,首先求解G32的坐标位置:
其次,根据网格点G22、G32、G42可以拟合出一条二次曲线C1:
x=xsensor1
z=A1y2+B1y+C1
而根据网格点G23、G33、G43也可以拟合出一条二次曲线C2:
x=xsensor3
z=A2y2+B2y+C2
然后根据二次曲线C1和C2,可以求出网络点G12和G13的坐标位置:
G13=C1×Π1(G11,G14)
G12=C2×Π1(G11,G14)
式中,∏1(G11,G14)为网格点G11和G14所在的平面,运算符“×”表示求解曲线和平面的交点;
根据二次曲线C1和C2,还可以求出网络点G52和G53的坐标位置:
G53=C1×Π1(G51,G54)
G52=C2×Π1(G51,G54)
最后,求解其余未知的六个网格节点坐标位置:
式中k=2,3,4。
式中i=2,3,4,j=1,4;
最后,根据网络节点G11、G12、G14得到一条二次曲线C3:
x=xsensor2
z=A3y2+B3y+C3
C3经过G11、G12、G13、G14这四个节点,根据上述方法同样可以得到另外一条二次曲线C4:
x=xsensor4
z=A4y2+B4y+C4
根据上述曲面,可对摆动腿足端所处的局部环境曲面进行拟合,所得拟合曲面为:
S=∑Ci(i=1,2,3,4)。
再进一步,所述步骤二中,首先,求解摆动腿足端在根关节坐标系的位置,令摆动腿连杆长为Li,关节转角为θsi,则摆动腿足端在根关节坐标系下的位置为:
px=[L1+L2cθs2+L3c(θs2+θs3)]cθs1
py=[L1+L2cθs2+L3c(θs2+θs3)]sθs1
pz=L2sθs2+L3s(θs2+θs3)
式中,sθ=sinθ,cθ=cosθ;
根据上式得出摆动腿足端的工作空间,结合步骤一通过曲线拟合算法拟合出的局部环境曲面,拟合出摆动腿足端可达区域:
Ri=Ti∩S
式中,Ti为摆动腿足端的工作空间,S为摆动腿足端所处的局部环境拟合曲面,表示如下:
f(x,y)=h
式中,S(x,y)代表局部环境单元点的坐标,而h代表单元点所处的地形高度。
更进一步,所述步骤三中,首先,解析理想落足可接受点约束函数:首先,建立摆动腿足端所处地形的结构几何模型:
f=(f1,f2,f3,f4)T
式中,f1,f2分别为曲面对于机器人足端在x、y方向上的一阶偏导数,f3,f4分别为曲面对于机器人足端在x、y方向上的二阶偏导数。
考虑到摆动腿足端和地形结构之间的相互约束关系,对上式作出相应变换,得出下式:
F=(F1,F2,F3,F4)T
式中,F1=|f1|,F2=|f2|,F3=f3(0.55-f3),F4=f4(0.35-f4)。
则落足可接受点评估函数为
U(F)=αTF
式中,α=(α1,α2,α3,α4)T为权重比向量,F=(F1,F2,F3,F4)T为地形几何结构特征参数向量;
其次,解析理想落足点约束函数:首先,提出四种基于机器人运动状态的约束:
约束1:禁止落足点区域,其约束函数如下:
式中,Fp'=(x'p,y'p)T为理想落足点在世界坐标系下的平面坐标,Fp=(xp,yp)T为当前落足点在世界坐标系下的平面坐标。为落足点搜索方向角,RT(FF,FC,FR)为摆动相各条腿的步幅;
约束2:机器人静态稳定性,其约束函数如下:
C2:SM(FF,FC,FR)≥0
式中,SM(FF,FC,FR)为机器人机身重心在新支撑相形成的支撑多边形内的稳定裕量;
约束3:摆动腿落足点可达约束,其约束函数如下:
式中,RW(FF,FC,FR)和RL(FF,FC,FR)分别代表理想落足点到当前摆动相所在点横向和纵向的距离,W(FF,FC,FR)和L(FF,FC,FR)分别代表摆动相的横向和纵向摆动距离;
约束4:机器人步态效率,其约束函数如下:
式中L'(FF,FC,FR)为摆动相的实际摆动距离;
然后,根据这四个约束耦合得出理想落足点评估函数如下:
U(F)=βTC
式中,βT=(β1,β2,β3,β4)为评估函数的权重比向量。
所述步骤四中,首先,求解站立腿足端和各关节输出位置之间的映射关系:
θd2=2arctan t1
θd3=2arctant2-2arctan t1
式中,t1=tan(θd2/2),t2=tan[(θd2-θd3)/2],为站立腿足端在根关节坐标系下的位置;
其次,求解站立腿足端在根关节坐标系下的位置:
式中,为机身处于理想目标位姿时机身坐标系与世界坐标系之间的位置转换矩阵,其表达式如下:
式中,为机身处于理想位姿下时,根关节坐标系和机身坐标系之间的位置转换矩阵,为机身处于理想位姿下时,机身坐标系与世界坐标系之间的位置坐标转换,为机身处于初始位姿下时,机身坐标系与世界坐标系之间的位置坐标转换;
最后,结合上面两式,解析出站立腿各关节输出位置和机身重心位置之间的映射关系:
θd1=π-Atan2(byC,bxC)
θd2=2arctant1
θd3=2arctan t2-2arctan t1
式中,t1=tan(θd2/2),t2=tan[(θd2-θd3)/2],BAC(bxc,byc,bzc)为机身坐标系在根关节坐标系下的位置;
最后,对关节输出角进行多项式插补运算,以六足机器人为例,完成机器人在非结构环境下的稳定运动控制。
本发明的有益效果主要表现在:
1、基于曲线拟合的局部环境建模算法提高了局部环境建模的准确性和高效性;
2、基于多约束落足点评估算法能够防止得到的摆动腿理想落足点出现“坏点”;
3、基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法,提高了多足机器人在非结构环境下运动的稳定性。
4、基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法,提高了多足机器人对非结构环境的地形适应能力。
附图说明
图1是本发明的基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法应用的六足机器人结构示意图;
图2是六足机器人测距传感器的分布位置图;
图3是在根关节坐标系下,多足机器人单摆动腿足端位置和关节输出位置之间的映射关系;
图4是局部环境网格化节点图,图中,Gij代表网格节点位于第i行,第j列,Pi S(i=1,2,3,4,6)代表传感器在全局坐标系下的垂直投影点,Pi F(i=1,3,4,6)代表足端在全局坐标系下的垂直投影点;
图5是通过网格点拟合出的4条二次曲线;
图6是通过4条二次曲线拟合出的曲面环境;
图7是在足端可达区域下32个落足候选点的地形特征值F1和F2;
图8是在足端可达区域下32个落足候选点的地形特征值F3和F4;
图9是基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法流程框图。
具体实施方式
下面结合附图对本发明作进一步描述。
参照图1~图9,一种基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法,以六足机器人为例,所述六足机器人包括六足机器人机身和六条相互独立的步行腿,所述机器人稳定运行控制方法包括以下步骤:
步骤一:基于曲线拟合算法拟合出摆动腿足端所处局部环境,主要包括传感器位置分布,环境网格化,局部环境点插补及二次曲线拟合。
首先,完成对测距传感器的位置分布,测距传感器主要分布在六足机器人的五个根关节处,如图2所示。其次,对局部环境进行网格化,用Pi S[i=0,1,2,3,4]来表示测距传感器在地面上的五个垂直投影点,再取六足机器人前后四个足端点,用表示。至此,一共获得九个已知坐标的点,其表达式如下:
用Gij=[xij yij zij]T来表示网格的节点坐标,其中,i代表的是网格的行,j代表网格的列,即Gij代表的是网格第i行和第j列交点处的坐标,至此,一共获得20个网格点,如图3所示,图中,Gij代表网格节点位于第i行,第j列,Pi S(i=1,2,3,4,6)代表传感器在全局坐标系下的垂直投影点,Pi F(i=1,3,4,6)代表足端在全局坐标系下的垂直投影点。
然后,对网格中未知点的位置进行插补,首先得出G32的坐标位置:
其次,根据网格点G22、G32、G42可以拟合出一条二次曲线C1:
x=xsensor1
z=A1y2+B1y+C1
而根据网格点G23、G33、G43也可以拟合出一条二次曲线C2:
x=xsensor3
z=A2y2+B2y+C2
然后根据二次曲线C1和C2,可以求出网络点G12和G13的坐标位置:
G13=C1×Π1(G11,G14)
G12=C2×Π1(G11,G14)
式中,∏1(G11,G14)为网格点G11和G14所在的平面。
根据二次曲线C1和C2,还可以求出网络点G52和G53的坐标位置:
G53=C1×Π1(G51,G54)
G52=C2×Π1(G51,G54)
式中,∏1(G51,G54)为网格点G51和G54所在的平面。
最后,求解其余未知的六个网格节点坐标位置:
式中k=2,3,4。
式中i=2,3,4,j=1,4。
最后,根据网络节点G11和G14可以得到一条二次曲线C3:
x=xsensor2
z=A3y2+B3y+C3
C3经过G11、G12、G13、G14这四个节点,根据上述方法同样可以得到另外一条二次曲线C4:
x=xsensor4
z=A4y2+B4y+C4
根据上述曲面,可对摆动腿足端所处的局部环境曲面进行拟合,所得拟合曲面为:
S=∑Ci(i=1,2,3,4)
假设六足机器人前后四条腿足端在其中一个根关节坐标系下的坐标分别是G11(130,90,26)、G14(190,90,28)、G51(130,30,35)、G54(190,30,30),而五个传感器投影点在根关节坐标系下的坐标分别为G22(150,70,30)、G23(170,70,28)、G33(170,60,35)、G42(150,40,40)、G43(170,40,38),根据上一小节未知点插补算法可得剩余几个点的坐标为G32(150,60,29)、G12(150,90,24.237)、G13(170,90,29.257)、G52(150,30,37.867)、G53(170,30,36.907)。
首先,根据点G22、G32、G42坐标,在Matlab上拟合出二次曲线C1
x=150
z=-0.004308y2+0.3058y+31.61
同理,拟合出C2-C4这三条二次曲线的代数表达式如下:
x=170
z=0.002825y2-0.4825y+49.8
y=90
z=0.008475x2-2.462x+202.8
y=30
z=-0.004784x2+1.483x-76.92
然后,四条三维二次曲线如图4所示,通过则四条曲线模拟出的局部环境如图5所示,图中,不难看出这四条曲线精确地拟合出了足端所处的局部环境。
步骤二:根据步骤一中所拟合出的局部环境,结合摆动腿足端工作空间,拟合出摆动腿足端可达区域。
令摆动腿的连杆长为Li,关节转角为θi,建立摆动腿足端和关节输出位置之间的映射关系图如图6所示,由此求得摆动腿足端在根关节坐标系下的位置:
px=[L1+L2cθs2+L3c(θs2+θs3)]cθs1
py=[L1+L2cθs2+L3c(θs2+θs3)]sθs1
pz=L2sθs2+L3s(θs2+θs3)
式中,sθ=sinθ,cθ=cosθ,下同。
利用上式得出摆动腿足端的工作空间,结合步骤一曲线拟合得到的局部环境曲面,解析出摆动腿足端可达区域:
Ri=Li∩S
式中,Li为摆动腿足端的工作空间,S为摆动腿足端所处的局部环境拟合曲线,可表示如下:
f(x,y)=h
式中,S(x,y)代表局部环境单元点的坐标,而h代表单元点所处的地形高度。可达区域的实际求解主要依据的是在空间坐标系中,摆动腿足端工作空间边界曲面的坐标和局部环境模型坐标的交集,如第二小节中摆动腿足端工作空间边界曲面三个坐标的取值范围分别是:-37.5≤X≤448.63、-215.45≤Y≤215.41、-18.549≤Z≤237.56;而局部环境模型三个坐标的取值范围分别是:150≤X≤170、20≤Y≤100、17.863≤Z≤41.283。不难看出,局部环境模型三个坐标的取值都包含在摆动腿足端工作空间三个坐标范围内。因此,摆动腿足端的可达区域即局部环境模型。当然,上面一小节对局部环境建模时所选取的点是为了仿真算法随意选取,在实际建模过程中,足端可达区域并不一定是局部环境模型。
步骤三:根据步骤二中所拟合出的摆动腿足端可达区域,基于多维度落足点评估算法,对摆动腿足端可达区域内的可规划落足点进行评估,具体包括:
首先,解析理想落足可接受点约束函数,先建立摆动腿足端所处地形的几何结构模型:
f=(f1,f2,f3,f4)T
式中,f1,f2分别为曲面对于机器人足端在x、y方向上的一阶偏导数,f3,f4分别为曲面对于机器人足端在x、y方向上的二阶偏导数。
考虑到摆动腿足端和地形结构之间的相互约束关系,对上式作出相应变换,得出下式:
F=(F1,F2,F3,F4)T
式中,F1=|f1|,F2=|f2|,F3=f3(0.55-f3),F4=f4(0.35-f4)。
则落足可接受点的效用评估函数为
U(F)=αTF
式中,α=(α1,α2,α3,α4)T为权重比向量,F=(F1,F2,F3,F4)T为地形结构特征参数向量。通过Matlab中的SVM工具包,集合样本学习理论得出落足可接受点效用评估函数为:
U(F)=αTF=-0.2993F1-0.4115F2+0.8319F3+0.7489F4
将80mmx40mm的曲面划分为32个10mmx10mm的区域,每一个区域近似看成一个落足候选点,得出32个落足候选点的地形几何特征向量如图7、图8所示。其次,解析理想落足点约束函数,首先,提出四个约束:
约束1:禁止落足点区域,其效用函数如下:
式中,Fp'=(x'p,y'p)T为理想落足点的平面坐标,Fp=(xp,yp)T为当前落足点的平面坐标。为搜索方向角,RT(FF,FC,FR)为摆动相各条腿的步幅。
约束2:静态稳定性,其效用函数如下:
C2:SM(FF,FC,FR)≥0
式中,SM(FF,FC,FR)为机器人机身重心在新支撑相形成的支撑多边形内的稳定裕量。
约束3:落足点可达约束,其效用函数如下:
式中,RW(FF,FC,FR)和RL(FF,FC,FR)分别代表理想落足点到当前摆动相所在点横向和纵向的距离,W(FF,FC,FR)和L(FF,FC,FR)分别代表摆动相的横向和纵向摆动距离。
约束4:步态效率,其效用函数如下:
式中L'(FF,FC,FR)为摆动相的实际摆动距离。
然后,根据这四个约束得出理想落足点的评估函数如下:
U(F)=βTC
式中,βT=(β1,β2,β3,β4)为效用函数的权重比向量。
步骤四:根据步骤一至步骤三,基于多重姿态转换算法,解析机器人关节输出位置,完成非结构环境下的步态优化,具体包括:
首先,求解摆动腿足端和摆动腿各关节输出位置之间的映射关系:
θs2=2arctan t1
θs3=2arctan t2-2arctan t1
式中,t1=tan(θs2/2),t2=tan[(θs2-θs3)/2]。
其次,求解站立腿足端在根关节坐标系下的位置:
最后,结合上面两式,解析出站立腿各关节输出位置和机身重心位置之间的映射关系:
θd1=π-Atan2(byC,bxC)
θd2=2arctan t1
θd3=2arctan t2-2arctan t1
式中,t1=tan(θd2/2),t2=tan[(θd2-θd3)/2]。
本发明基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法流程框图如图9所示,图中,要实现多足机器人在非结构环境下的稳定、高效运动,最终需要求得多足机器人在非结构环境下摆动腿和站立腿关节的输出位置,即关节转角θs和θd。站立腿的关节转角主要由机身工作空间和机身逆运动学来确定,而摆动腿的关节转角主要由理想落足点的位置和摆动腿运动学决定。
Claims (5)
1.一种基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法,其特征在于:包括以下步骤:
步骤一:基于曲线拟合算法对摆动腿足端所处局部环境进行建模,包括传感器位置分布,局部环境网格化,网格化局部环境点插补及二次曲线拟合;
步骤二:根据步骤一中所拟合出的局部环境,结合摆动腿足端工作空间,拟合出摆动腿足端可达区域;
步骤三:根据步骤二中所拟合出的摆动腿足端可达区域,结合多重约束落足点评估算法,对摆动腿足端可达区域内的可规划落足点进行评估分析;
步骤四:根据步骤一至步骤三,结合多重姿态转换算法,解析机器人关节输出位置,并对关节转角进行多项式插补运算,完成机器人在非结构环境下的稳定运动控制。
2.如权利要求1所述的基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法,其特征在于:所述步骤一中,首先,完成测距传感器的位置分布,测距传感器主要分布在六足机器人的五个根关节处;其次,对局部环境进行网格化,用Pi S[i=1,2,3,4,6]来表示测距传感器在世界坐标系上的五个垂直投影点,再用表示多足机器前后四个足端点在世界坐标系上的五个垂直投影点,其位置坐标通过机器人正运动学求解获得;至此,一共获得九个已知坐标的点,其表达式如下:
用Gij=[xijyijzij]T来表示网格的节点坐标,其中,i代表的是网格的行,j代表网格的列,即Gij代表的是网格第i行和第j列交点处的坐标,至此,一共获得20个网格点,实现对局部环境的网格化;
然后,求解网格中未知点在世界坐标系下的位置坐标,首先求解G32的坐标位置:
其次,根据网格点G22、G32、G42拟合出一条二次曲线C1:
x=xsensor1
z=A1y2+B1y+C1
而根据网格点G23、G33、G43也拟合出一条二次曲线C2:
x=xsensor3
z=A2y2+B2y+C2
然后根据二次曲线C1和C2,可以求出网络点G12和G13的坐标位置:
G13=C1×Π1(G11,G14)
G12=C2×Π1(G11,G14)
式中,∏1(G11,G14)为网格点G11和G14所在的平面,运算符“×”表示求解曲线和平面的交点;
根据二次曲线C1和C2,还可以求出网络点G52和G53的坐标位置:
G53=C1×Π1(G51,G54)
G52=C2×Π1(G51,G54)
最后,求解其余未知的六个网格节点坐标位置:
式中k=2,3,4;
式中i=2,3,4,j=1,4;
最后,根据网络节点G11、G12、G14得到一条二次曲线C3:
x=xsensor2
z=A3y2+B3y+C3
C3经过G11、G12、G13、G14这四个节点,根据上述方法同样得到另外一条二次曲线C4:
x=xsensor4
z=A4y2+B4y+C4
根据上述曲面,对摆动腿足端所处的局部环境曲面进行拟合,所得拟合曲面为:
S=∑Ci(i=1,2,3,4)。
3.如权利要求1或2所述的基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法,其特征在于:所述步骤二中,首先,求解摆动腿足端在根关节坐标系的位置,令摆动腿连杆长为Li,关节转角为θsi,则摆动腿足端在根关节坐标系下的位置为:
px=[L1+L2cθs2+L3c(θs2+θs3)]cθs1
py=[L1+L2cθs2+L3c(θs2+θs3)]sθs1
pz=L2sθs2+L3s(θs2+θs3)
式中,sθ=sinθ,cθ=cosθ;
根据上式得出摆动腿足端的工作空间,结合步骤一通过曲线拟合算法拟合出的局部环境曲面,拟合出摆动腿足端可达区域:
Ri=Ti∩S
式中,Ti为摆动腿足端的工作空间,S为摆动腿足端所处的局部环境拟合曲面,表示如下:
f(x,y)=h
式中,S(x,y)代表局部环境单元点的坐标,而h代表单元点所处的地形高度。
4.如权利要求1或2所述的基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法,其特征在于:所述步骤三中,首先,解析理想落足可接受点约束函数:首先,建立摆动腿足端所处地形的结构几何模型:
f=(f1,f2,f3,f4)T
式中,f1,f2分别为曲面对于机器人足端在x、y方向上的一阶偏导数,f3,f4分别为曲面对于机器人足端在x、y方向上的二阶偏导数;
考虑到摆动腿足端和地形结构之间的相互约束关系,对上式作出相应变换,得出下式:
F=(F1,F2,F3,F4)T
式中,F1=|f1|,F2=|f2|,F3=f3(0.55-f3),F4=f4(0.35-f4);
则落足可接受点评估函数为
U(F)=αTF
式中,α=(α1,α2,α3,α4)T为权重比向量,F=(F1,F2,F3,F4)T为地形几何结构特征参数向量;
其次,解析理想落足点约束函数:首先,提出四种基于机器人运动状态的约束:
约束1:禁止落足点区域,其约束函数如下:
C1:
式中,F′p=(x'p,y'p)T为理想落足点在世界坐标系下的平面坐标,Fp=(xp,yp)T为当前落足点在世界坐标系下的平面坐标,为落足点搜索方向角,RT(FF,FC,FR)为摆动相各条腿的步幅;
约束2:机器人静态稳定性,其约束函数如下:
C2:SM(FF,FC,FR)≥0
式中,SM(FF,FC,FR)为机器人机身重心在新支撑相形成的支撑多边形内的稳定裕量;
约束3:摆动腿落足点可达约束,其约束函数如下:
C3:
式中,RW(FF,FC,FR)和RL(FF,FC,FR)分别代表理想落足点到当前摆动相所在点横向和纵向的距离,W(FF,FC,FR)和L(FF,FC,FR)分别代表摆动相的横向和纵向摆动距离;
约束4:机器人步态效率,其约束函数如下:
C4:
式中L'(FF,FC,FR)为摆动相的实际摆动距离;
然后,根据这四个约束耦合得出理想落足点评估函数如下:
U(F)=βTC
式中,βT=(β1,β2,β3,β4)为评估函数的权重比向量。
5.如权利要求1或2所述的基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法,其特征在于:所述步骤四中,首先,求解站立腿足端和各关节输出位置之间的映射关系:
θd2=2arctan t1
θd3=2arctan t2-2arctan t1
式中,t1=tan(θd2/2),t2=tan[(θd2-θd3)/2],为站立腿足端在根关节坐标系下的位置;
其次,求解站立腿足端在根关节坐标系下的位置:
式中,为机身处于理想目标位姿时机身坐标系与世界坐标系之间的位置转换矩阵,其表达式如下:
式中,为机身处于理想位姿下时,根关节坐标系和机身坐标系之间的位置转换矩阵,为机身处于理想位姿下时,机身坐标系与世界坐标系之间的位置坐标转换,为机身处于初始位姿下时,机身坐标系与世界坐标系之间的位置坐标转换;
最后,结合上面两式,解析出站立腿各关节输出位置和机身重心位置之间的映射关系:
θd1=π-A tan 2(byC,bxC)
θd2=2arctan t1
θd3=2arctan t2-2arctan t1
式中,t1=tan(θd2/2),t2=tan[(θd2-θd3)/2],BAC(bxc,byc,bzc)为机身坐标系在根关节坐标系下的位置;
最后,对关节输出角进行多项式插补运算,以六足机器人为例,完成机器人在非结构环境下的稳定运动控制。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201611138340.6A CN106826813B (zh) | 2016-12-12 | 2016-12-12 | 一种六足机器人稳定运动控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201611138340.6A CN106826813B (zh) | 2016-12-12 | 2016-12-12 | 一种六足机器人稳定运动控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106826813A true CN106826813A (zh) | 2017-06-13 |
CN106826813B CN106826813B (zh) | 2019-08-23 |
Family
ID=59139760
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201611138340.6A Active CN106826813B (zh) | 2016-12-12 | 2016-12-12 | 一种六足机器人稳定运动控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106826813B (zh) |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107745756A (zh) * | 2017-10-25 | 2018-03-02 | 重庆工商大学 | 六足草莓采摘机器人的行走方法 |
CN110597267A (zh) * | 2019-09-27 | 2019-12-20 | 长安大学 | 一种足式机器人的局部最优落足点选取方法 |
CN110877337A (zh) * | 2019-11-29 | 2020-03-13 | 武汉科技大学 | 一种三连杆机械手智能控制方法、芯片和*** |
CN111290272A (zh) * | 2020-02-15 | 2020-06-16 | 中国海洋大学 | 基于多足机器人的姿态平稳性调整方法 |
CN112428274A (zh) * | 2020-11-17 | 2021-03-02 | 张耀伦 | 一种多自由度机器人的空间运动规划方法 |
CN112987769A (zh) * | 2021-02-22 | 2021-06-18 | 武汉科技大学 | 四足机器人在变刚度地形稳定过渡的腿部主动调节方法 |
CN113377113A (zh) * | 2021-07-02 | 2021-09-10 | 北方工业大学 | 足式机器人足端轨迹规划方法、***及控制方法、*** |
WO2022048266A1 (zh) * | 2020-09-07 | 2022-03-10 | 腾讯科技(深圳)有限公司 | 足式机器人运动控制方法、装置、设备及介质 |
WO2022048472A1 (zh) * | 2020-09-07 | 2022-03-10 | 腾讯科技(深圳)有限公司 | 足式机器人运动控制方法、装置、设备及介质 |
EP4144487A4 (en) * | 2020-10-26 | 2024-01-03 | Tencent Technology (Shenzhen) Company Limited | LEG ROBOT MOTION CONTROL METHOD AND APPARATUS, DEVICE, MEDIUM AND PROGRAM |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH01146570A (ja) * | 1987-12-03 | 1989-06-08 | Masao Tanaka | 多足で前進する玩具体 |
KR20010001691A (ko) * | 1999-06-08 | 2001-01-05 | 정해성 | 폐쇄 기구형 다리를 갖는 조향가능 다족보행장치 |
CN102314533A (zh) * | 2010-06-02 | 2012-01-11 | 利弗莫尔软件技术公司 | 将计算出的曲线拟合到目标曲线的方法和*** |
CN102897245A (zh) * | 2012-10-30 | 2013-01-30 | 同济大学 | 单一驱动的多足机器人机身模块化联动装置 |
CN104192221A (zh) * | 2014-09-26 | 2014-12-10 | 哈尔滨工业大学 | 一种电驱动六足机器人运动控制***及方法 |
-
2016
- 2016-12-12 CN CN201611138340.6A patent/CN106826813B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH01146570A (ja) * | 1987-12-03 | 1989-06-08 | Masao Tanaka | 多足で前進する玩具体 |
KR20010001691A (ko) * | 1999-06-08 | 2001-01-05 | 정해성 | 폐쇄 기구형 다리를 갖는 조향가능 다족보행장치 |
CN102314533A (zh) * | 2010-06-02 | 2012-01-11 | 利弗莫尔软件技术公司 | 将计算出的曲线拟合到目标曲线的方法和*** |
CN102897245A (zh) * | 2012-10-30 | 2013-01-30 | 同济大学 | 单一驱动的多足机器人机身模块化联动装置 |
CN104192221A (zh) * | 2014-09-26 | 2014-12-10 | 哈尔滨工业大学 | 一种电驱动六足机器人运动控制***及方法 |
Cited By (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107745756A (zh) * | 2017-10-25 | 2018-03-02 | 重庆工商大学 | 六足草莓采摘机器人的行走方法 |
CN107745756B (zh) * | 2017-10-25 | 2019-09-24 | 重庆工商大学 | 六足草莓采摘机器人的行走方法 |
CN110597267A (zh) * | 2019-09-27 | 2019-12-20 | 长安大学 | 一种足式机器人的局部最优落足点选取方法 |
CN110597267B (zh) * | 2019-09-27 | 2023-01-10 | 长安大学 | 一种足式机器人的局部最优落足点选取方法 |
CN110877337A (zh) * | 2019-11-29 | 2020-03-13 | 武汉科技大学 | 一种三连杆机械手智能控制方法、芯片和*** |
CN110877337B (zh) * | 2019-11-29 | 2020-11-24 | 武汉科技大学 | 一种三连杆机械手智能控制方法、芯片和*** |
CN111290272B (zh) * | 2020-02-15 | 2021-02-09 | 中国海洋大学 | 基于多足机器人的姿态平稳性调整方法 |
CN111290272A (zh) * | 2020-02-15 | 2020-06-16 | 中国海洋大学 | 基于多足机器人的姿态平稳性调整方法 |
WO2022048266A1 (zh) * | 2020-09-07 | 2022-03-10 | 腾讯科技(深圳)有限公司 | 足式机器人运动控制方法、装置、设备及介质 |
WO2022048472A1 (zh) * | 2020-09-07 | 2022-03-10 | 腾讯科技(深圳)有限公司 | 足式机器人运动控制方法、装置、设备及介质 |
EP4144487A4 (en) * | 2020-10-26 | 2024-01-03 | Tencent Technology (Shenzhen) Company Limited | LEG ROBOT MOTION CONTROL METHOD AND APPARATUS, DEVICE, MEDIUM AND PROGRAM |
CN112428274A (zh) * | 2020-11-17 | 2021-03-02 | 张耀伦 | 一种多自由度机器人的空间运动规划方法 |
CN112987769A (zh) * | 2021-02-22 | 2021-06-18 | 武汉科技大学 | 四足机器人在变刚度地形稳定过渡的腿部主动调节方法 |
CN113377113A (zh) * | 2021-07-02 | 2021-09-10 | 北方工业大学 | 足式机器人足端轨迹规划方法、***及控制方法、*** |
CN113377113B (zh) * | 2021-07-02 | 2022-01-04 | 北方工业大学 | 足式机器人足端轨迹规划方法、***及控制方法、*** |
Also Published As
Publication number | Publication date |
---|---|
CN106826813B (zh) | 2019-08-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106826813A (zh) | 基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法 | |
CN105825510B (zh) | 一种兴趣点与道路网的自动配准方法 | |
CN107145161A (zh) | 无人机访问多目标点的航迹规划方法及装置 | |
CN106078752A (zh) | 一种基于Kinect的仿人机器人人体行为模仿方法 | |
CN105512621A (zh) | 一种基于Kinect的羽毛球动作指导*** | |
CN106055522A (zh) | 冗余空间机械臂最小基座姿态扰动的轨迹规划方法 | |
CN107330146A (zh) | 一种同时考虑平动和转动效应的节理岩质边坡极限承载力分析上限法 | |
CN104777839B (zh) | 基于bp神经网络和距离信息的机器人自主避障方法 | |
CN104070525B (zh) | 用于空间机械臂连续轨迹跟踪的方法 | |
CN107103164A (zh) | 无人机执行多任务的分配方法及装置 | |
CN105389569A (zh) | 一种人体姿态估计方法 | |
CN107972034A (zh) | 一种基于ros平台的复杂工件轨迹规划仿真*** | |
CN103085069A (zh) | 一种新型机器人运动学建模方法 | |
CN107564065A (zh) | 一种协作环境下人机最小距离的测算方法 | |
CN104570736A (zh) | 一种星-臂耦合***的动力学参数在轨辨识方法和装置 | |
US20180099411A1 (en) | Robot behavior generation method | |
CN107818318A (zh) | 一种人形机器人模仿相似度评价方法 | |
CN108318010A (zh) | 一种基于bim的基坑监测测点快速选取方法 | |
CN106683169A (zh) | 一种关节运动感知的稀疏局部分解及重构算法 | |
CN102693350B (zh) | 面向虚拟现实牙科操作训练的多点接触力觉反馈*** | |
CN113779663A (zh) | 基于bim的地铁车站围护结构三维建模方法、***和介质 | |
CN111985774A (zh) | 一种控规调整的自动化三维交互审查***与方法 | |
CN109931940A (zh) | 一种基于单目视觉的机器人定位置信度评估方法 | |
CN100594520C (zh) | 基于关键帧时刻优化的空中运动仿真方法 | |
Tong et al. | A least squares-based method for adjusting the boundaries of area objects |
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 |