CN104986241B - 四足机器人的步态规划方法 - Google Patents

四足机器人的步态规划方法 Download PDF

Info

Publication number
CN104986241B
CN104986241B CN201510368506.2A CN201510368506A CN104986241B CN 104986241 B CN104986241 B CN 104986241B CN 201510368506 A CN201510368506 A CN 201510368506A CN 104986241 B CN104986241 B CN 104986241B
Authority
CN
China
Prior art keywords
mrow
msub
robot
msup
coordinate
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
Application number
CN201510368506.2A
Other languages
English (en)
Other versions
CN104986241A (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.)
Shandong University Weihai
Original Assignee
Shandong University Weihai
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 Shandong University Weihai filed Critical Shandong University Weihai
Priority to CN201510368506.2A priority Critical patent/CN104986241B/zh
Publication of CN104986241A publication Critical patent/CN104986241A/zh
Application granted granted Critical
Publication of CN104986241B publication Critical patent/CN104986241B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Abstract

本发明公开了四足机器人及其步态规划方法;它包括:机器人的躯干,所述机器人的躯干前端的上方通过机器人颈部连接机器人头部,所述机器人躯干的下方连接四条腿:第一、第二、第三和第四条腿;其中第一、第二条腿安装在机器人躯干的前端,第三、第四条腿安装在机器人躯干的后端;每条腿均包括从下而上依次连接的小腿、大腿、垂直杆和水平杆四个部分;所述躯干上设有控制装置,控制装置用于控制机器人的行走。本发明的机器人具有平稳行走、自动避障等功能。

Description

四足机器人的步态规划方法
技术领域
[0001] 本发明涉及一种四足机器人及其步态规划方法。
背景技术
[0002] 现有四足机器人还有很多的缺陷,比如均存在行走不稳定,功能较为单一的特点, 大部分都只能在平坦地面行走,没有应对外界干扰的和躲避障碍物的能力,具有视觉的四 足机器人更少。并且具有负载能力的四足机器人体型一般都较大、能耗高,在设计和调试中 会有很多麻烦,不利于算法验证和参数调整,所以设计一个小型四足机器人,方便在实验室 里进行算法验证,这对四足机器人的研究提供了一种高效的方式。
发明内容
[0003] 本发明的目的就是为了解决上述问题,提供一种四足机器人及其步态规划方法, 它具有通过建立D-H坐标系、设计运动学方程和求解逆运动学方程,利用ARM单片机作为主 控制器,实现机器人步态规划,并通过多传感器实现了机器人的稳定性、自动避障和视觉传 输能力,在Andro i d平台上设计的上位机可以有地控制机器人的优点。
[0004] 为了实现上述目的,本发明采用如下技术方案:
[0005] —种四足机器人,包括:机器人的躯干,所述机器人的躯干前端的上方通过机器人 颈部连接机器人头部,所述机器人躯干的下方连接四条腿:第一、第二、第三和第四条腿;其 中第一、第二条腿安装在机器人躯干的前端,第三、第四条腿安装在机器人躯干的后端;每 条腿均包括从下而上依次连接的小腿、大腿、垂直杆和水平杆四个部分;所述小腿与大腿、 大腿与垂直杆、垂直杆与水平杆之间均设有舵机;所述躯干上设有控制装置,控制装置通过 控制舵机来控制机器人的平稳行走。
[0006] 第一条腿中,小腿与大腿之间通过第一舵机连接,大腿和垂直杆之间通过第二舵 机连接,垂直杆与水平杆之间通过第三舵机连接;
[0007] 第二条腿中,小腿与大腿之间通过第四舵机连接,大腿和垂直杆之间通过第五舵 机连接,垂直杆与水平杆之间通过第六舵机连接;
[0008] 第三条腿中,小腿与大腿之间通过第七舵机连接,大腿和垂直杆之间通过第八舵 机连接,垂直杆与水平杆之间通过第九舵机连接;
[0009] 第四条腿中,小腿与大腿之间通过第十舵机连接,大腿和垂直杆之间通过第十一 舵机连接,垂直杆与水平杆之间通过第十二舵机连接;
[0010] 所述机器人颈部设有第十三舵机;所述机器人头部设有第十四舵机。
[0011] 所述第三舵机,也称水平髋关节,仿照哺乳动物髋部关节的外摆内收功能,负责机 器人平行于身体方向的垂直方向运动,是机器人的转向运动时的驱动源,其旋转量用θ〇表 不。
[0012] 所述第二舵机,也称垂直髋关节,仿照哺乳动物的髋关节的前后收腿功能,负责机 器人的平行于身体方向的运动,是整个机器人的承重关节,其旋转量用S1表示。
[0013] 所述第一舵机,也称膝关节,所述膝关节仿照哺乳动物的膝关节的前后运动关节, 负责驱动机器人腿部与地面接触,是机器人的步态轨迹的最终呈现关节,其旋转量用92表 不。
[0014] 所述控制装置包括:单片机STM32,所述单片机STM32分别与十四个舵机、加速度计 传感器和超声波传感器连接,所述单片机STM32还通过蓝牙模块与上位机连接,所述单片机 STM32还通过单片机MT7620与网络摄像头连接,所述单片机STM32还通过稳压电路与电源连 接,所述单片机STM32还通过单片机MEGA644与OLED液晶显示屏连接。
[0015] 所述单片机STM32作为主控芯片是机器人控制的核心,通过定时器模块产生20ms 定时中断作为机器人的运动周期,通过输出PWM波控制舵机转角完成机器人的步态,同时 stm32通过采集加速度计反馈的值得到机器人的姿态,控制机器人的平衡,控制超声波模 块的定时工作来采集机器人周围的障碍物情况,同时通过串口和蓝牙、mt7620连接来进行 通信;
[0016] 所述电源,用于给单片机供电;
[0017] 所述稳压电路,用于提供5v、3.3v、6v不同的电压分别给传感器、arm芯片和舵机供 电;
[0018] 所述舵机,用于驱动机器人进行步态行走和头部摇动;
[0019] 所述网络摄像头,用于远程监控,传送机器人周围的环境信息;
[0020] 所述单片机MT7620,用于挂载OpenWRT系统以此将摄像头的图像通过Wifi发送出;
[0021] 所述加速度计传感器,安装在机器人的机体中心位置用于采集机器人实时的姿 态;
[0022] 所述超声波传感器,安装在机器人头部正上方用于检测机器人周围是否有障碍 物;
[0023] 所述单片机MEGA644,用于和stm32通信得到控制信息并解码后通过液晶显示;
[0024] 所述OLED液晶显示屏,用于显示机器人当前信息;
[0025] 所述蓝牙模块,用于和andorid上位机间通信,传输指令和数据;
[0026] 所述上位机,用于发送指令和接收机器人反馈的数据;
[0027] —种四足机器人的步态规划方法,包括如下步骤:
[0028] 步骤(1):机器人初始化;
[0029] 步骤⑵:机器人接收上位机指令;将指令解码,通过OLED液晶显示屏显示;
[0030] 步骤(3):机器人对上位机指令进行判断并执行指令,判断是舵机调试指令、步态 变换指令、方向改变指令还是其他功能指令,所述其他功能指令包括自适应稳定性、拍照指 令和自动避障指令。
[0031] 所述步骤⑶中,
[0032] 如果是舵机调试指令,则开始单个舵机调试,判断是否收到调试值,如果收到就开 始步态实现,步态实现后返回步骤(2);如果没收到就结束;
[0033] 如果是步态变换指令,则开始步态实现,步态实现后返回步骤⑵;
[0034] 如果是方向改变指令,则调整运动学方程,开始步态实现,步态实现后返回步骤 (2);
[0035] 如果是其他功能指令,则根据指令完成稳定性调整、拍照和自动避障功能,返回步 骤⑵。
[0036] 所述步骤⑶中,
[0037] 如果收到其他功能指令,首先判断是自适应稳定性指令、拍照指令还是自动避障 指令;
[0038] 如果是自适应稳定性指令,则加速度计传感器采集当前机器人躯体的倾角并和稳 态时比较;计算偏差;步态实现;
[0039] 如果是拍照指令,则网络摄像头拍照,将照片通过单片机MT7620内嵌的OpenWRT, 上传给单片机STM32,最终通过蓝牙模块上传给上位机;
[0040] 如果是自动避障指令,则进入如下步骤:
[0041] 步骤(3-1):超声波传感器启动,判断是否在设定安全距离内,如果是就开始步态 实现,返回步骤(3-1);如果否则机器人头部的第十四舵机通过转动再次检测;判断是否找 到无障碍方向,如果是就改变方向,开始步态实现,返回步骤(3-1),如果否就结束。
[0042] 所述步态实现的具体步骤如下:
[0043] 步骤(4-1):建立DH坐标系,设定初始坐标点;
[0044] 步骤(4-2):建立运动学方程;求解逆运动学方程;
[0045] 步骤(4-3):设计运动轨迹;
[0046] 步骤(4-4):舵机关节映射;
[0047] 步骤(4-5) : PffM输出控制舵机实现步态。
[0048] 所述步骤(4-1)的步骤为:
[0049] 首先,利用D-H (Dnavit-Harbenberg)方法对机器人建立坐标系,以此来表示各关 节变量;第一关节的坐标系为(X1,Y1,Z1),第二关节的坐标系为(X2,Y2,Z2),第三关节的坐 标系为(Χ3,Υ3,Ζ3),机器人躯干的中心位置的坐标系为(Χ0,Υ0,Ζ0);
[0050] 根据建立的D-H坐标系建立机器人D-H坐标参数表。
[0051] 表1机器人D-H坐标参数表
[0052]
Figure CN104986241BD00091
[0053] a:表示每条公垂线的长度;α:两个相邻两坐标系ζ轴间的角度;
[0054] Θ:绕当前关节坐标系ζ轴旋转的角度;d:在当前关节坐标系ζ轴上两条相邻的公垂 线之间的距离;关节1表示横向髋关节;关节2表示纵向髋关节;关节3表示膝关节;ao表示横 向髋关节和纵向髋关节之间的连杆长度;m表示纵向髋关节和膝关节之间的连杆长度;&2表 示膝关节关节和腿末端节之间的连杆长度;k的取值范围是1〜3;dlk的参数含义是k和k-Ι坐 标系中Z轴上两条相邻的公垂线之间的距离Jl0i-D的参数含义是Z轴从k-1坐标系旋转到k 坐标系的角度;9lQ的参数含义是需要求解的横向髋关节转动角度;θη的参数含义是需要求 解的纵向髋关节转动角度;θι2的参数含义是需要求解的膝关节转动角度;
[0055] 所述步骤(4-2)的步骤为:
[0056] 建立运动学方程
[0057] Xi = -aisi-a2si2+51 (1)
[0058] yi = aoso+aisoci+a2S〇ci2+^w (2)
[0059] Zi = -aoc〇-aicoci-a2C〇ci2~h (3)
[0060] 其中:
Figure CN104986241BD00101
[0063] 其中so表不sin9。,c。表不cos9。,si表不sin9i,ci表不cos9!,si2表不sin (Θ1+Θ2),ci2 表示COS (θ1 + θ2)表示第i坐标系中的X坐标,ai表示纵向髋关节和膝关节之间的连杆长 度,a2表示膝关节关节和腿末端节之间的连杆长度,δ表示对应不同腿添加的标志位,为-1 或1,1表示机器人机体的长度的一半;yi表示第i坐标系中的y坐标;ao表示横向髋关节和纵 向髋关节之间的连杆长度。ai表示纵向髋关节和膝关节之间的连杆长度;a2表示膝关节关节 和腿末端节之间的连杆长度;λ表示对应不同腿添加的标志位,为-1或l;w表示机器人的宽 度的一半;Zi表示第i坐标系中的z坐标,h表示机器人的基坐标和坐标系(XI,Y1,Z1)之间的 高度。
[0064] 求解得到逆运动学方程:
Figure CN104986241BD00102
[0068]其中:
Figure CN104986241BD00103
[0071] 其中,
[0072] Θ1()表示第i个坐标系中横向髋关节转动的角度,yi表示第i坐标系中的y坐标,λ表 示对应不同腿添加的标志位,为-1或l,Zl表示第i坐标系中的ζ坐标,h表示机器人的基坐标 和坐标系(XI,Y1,Z1)之间的高度;θη表示第i个坐标系中纵向髋关节转动的角度,表示一 个综合的公式。S表示对应不同腿添加的标志位,为-1或l,Xl表示第i坐标系中的X坐标,1表 示机器人机体的长度的一半;ao表示横向髋关节和纵向髋关节之间的连杆长度,Z1表示第i 坐标系中的z坐标,h表示机器人的基坐标和坐标系1之间的高度。c〇S0iQ表示第i个坐标系中 横向髋关节转动角度的余弦值。θι2表示第i个坐标系中纵向膝关节转动的角度,ξ表示一个 综合的公式,Xi表示第i坐标系中的X坐标;
[0073] ⑵变形为:yi_Aw = s。(ao+aici+awi2) (7)
[0074] (3)变形为:zi+h = -co (ao+aici+a2Ci2) (8)
[0075] 联立以上两式得:
Figure CN104986241BD00111
[0076] 由⑶得
Figure CN104986241BD00112
[0077] ⑴整理为:Xi-51 = -aisi_a2Si2 (10)
[0078] (9) (10)两式两边分别平方,并和得:
[0079]
Figure CN104986241BD00113
[0080] 化简后得:
[0081]
Figure CN104986241BD00114
[0082]由三角函数公式化简得:
Figure CN104986241BD00115
[0089] 求出Θ i()和Θ i2后求解θ u有多中方法,此处通过代入⑴求解整理后得:
Figure CN104986241BD00116
[0092] 贝丨J:
Figure CN104986241BD00121
[0093] 如果是自适应稳定性指令,通过加速度计传感器测得机器人重力加速度可以得到 角度信息,与之稳定态的加速度差值通过PID算法得到机器人的偏移量,单片机驱动舵机通 过调节髋关节实现重心向原来方向的反作用力,使腿部的反作用力抵消外力的干扰,机器 人身体的基准是不变的,根据当前偏移基准值应用增量或者位置PID算法都可以实现自动 调整,考虑到位置式PID的算法需要不断累加每次的偏移值,运算量较大,所以使用增量式 PID算法。
[0094] 通过超声波传感器得到当前机器人与前方物体的距离,当距离大于安全距离时机 器人前行。当小于安全距离时,机器人头部舵机向左或者右摇动,检测该方向的障碍情况, 当距离符合安全距离时记录该方向标识位,并以此向该方向移动。
[0095] 本发明的有益效果:
[0096] 1、搭建了四足机器人硬件平台。通过运动方程建模和逆运动学方程求解得到足端 轨迹并通过编程在本发明中实现了步态和转向方法。
[0097] 2、通过加速度姿态传感器和超声波距离传感器感知当前状态,实现机器人行为协 调与自主导航。
[0098] 3、通过OpenWRT挂载网络摄像头方式实现远程图像传输处理,在上位机成功得到 图像信息。上位机通过蓝牙传输指令完成了对机器人控制。
[0099] 4、虽然没有大型四足机器人的负载能力,但其结构和功能都与之相似,能够在该 小型四足机器人平台上验证步态算法,调试参数,设计动作,并且通过上位机可以回传机器 人状态信息。
附图说明
[0100] 图1为本发明的实体结构和坐标系示意图;
[0101] 图2为本发明的机器人腿部示意图;
[0102] 图3为本发明的控制装置结构示意图;
[0103] 图4为本发明的整体方法流程图;
[0104] 图5为本发明的其他功能实现流程图;
[0105] 图6为本发明的步态实现流程图;
[0106] 图7.1为x-z足端轨迹空间图;
[0107] 图7.2为x-y足端轨迹空间;
[0108] 图7.3为足端轨迹图;
[0109] 图7.4 (a)和图7.4 (b)为足端分解图;
[0110] 图7.5为X轴轨迹设置图;
[0111] 图7.6为z轴轨迹设置图;
[0112] 图7.7为机器人足端轨迹图。
具体实施方式
[0113] 下面结合附图与实施例对本发明作进一步说明。
[0114] 本发明中使用蓝牙通信的两端设定为:手机端为服务端,机器人蓝牙作为客户端。
[0115] 服务端,手机端保持 BluetoothServiceSocket 打开,作为server。ServerSocket 将 监听,一旦连接成功就会创建产生一个BluetoothSocket。调用 IistenUsingRfcommWithServiceRecord (string ,UUID)获得BluetoothServiceSocket,然 后调用accept ()开始监听连接请求,如果接受成功将返回一个已经连接的 BluetoothSocket〇
[0116] 客户端,首先获得一个代表远程设备的BlutoohDevice对象,然后使用它获得 BI uetoothSocket 发起连接。使用B IuetoothDevi ce方法,调用creaatRfcommSocketTo-Service- Record (UUID)将得到BluetoothSocket,然后调用connect ()建立连接。
[0117] 建立以上连接后就可以在两端相互发送和接收协议指令,完成控制机器人和反馈 信息的功能。
[0118] 上位机控制机器人通过一定的指令协议发送,下位机端识别指令,这就需要设计 一套指令协议。Andorid发送数据只有字节流和字符流,为简化令本设计采用发用一个字节 数据按照其不同位的〇、1方式来编码,部分控制指令编码如表2所示:
[0119] 表2部分控制协议指令
Figure CN104986241BD00131
[0122]如图1所示,一种四足机器人,包括:机器人的躯干,所述机器人的躯干前端的上方 通过机器人颈部连接机器人头部,所述机器人躯干的下方连接四条腿:第一、第二、第三和 第四条腿;其中第一、第二条腿安装在机器人躯干的前端,第三、第四条腿安装在机器人躯 干的后端;每条腿均包括从下而上依次连接的小腿、大腿、垂直杆和水平杆四个部分;所述 躯干上设有控制装置,控制装置用于控制机器人的行走。
[0123] 第一条腿中,小腿与大腿之间通过第一舵机连接,大腿和垂直杆之间通过第二舵 机连接,垂直杆与水平杆之间通过第三舵机连接;
[0124] 第二条腿中,小腿与大腿之间通过第四舵机连接,大腿和垂直杆之间通过第五舵 机连接,垂直杆与水平杆之间通过第六舵机连接;
[0125] 第三条腿中,小腿与大腿之间通过第七舵机连接,大腿和垂直杆之间通过第八舵 机连接,垂直杆与水平杆之间通过第九舵机连接;
[0126] 第四条腿中,小腿与大腿之间通过第十舵机连接,大腿和垂直杆之间通过第十一 舵机连接,垂直杆与水平杆之间通过第十二舵机连接;
[0127] 所述机器人颈部设有第十三舵机;所述机器人头部设有第十四舵机。
[0128] 所述第三舵机,也称水平髋关节,仿照哺乳动物髋部关节的外摆内收功能,负责机 器人平行于身体方向的垂直方向运动,是机器人的转向运动时的驱动源,其旋转量用θ〇表 不。
[0129] 所述第二舵机,也称垂直髋关节,仿照哺乳动物的髋关节的前后收腿功能,负责机 器人的平行于身体方向的运动,是整个机器人的承重关节,其旋转量用S1表示。
[0130] 所述第一舵机,也称膝关节,所述膝关节仿照哺乳动物的膝关节的前后运动关节, 负责驱动机器人腿部与地面接触,是机器人的步态轨迹的最终呈现关节,其旋转量用92表 不。
[0131] 如图3所示,所述控制装置包括:单片机STM32,所述单片机STM32分别与十四个舵 机、加速度计传感器和超声波传感器连接,所述单片机STM32还通过蓝牙模块与上位机连 接,所述单片机STM32还通过单片机ΜΤ7620与网络摄像头连接,所述单片机STM32还通过稳 压电路与电源连接,所述单片机STM32还通过单片机MEGA644与OLED液晶显示屏连接。
[0132] 所述单片机STM32作为主控芯片是机器人控制的核心,通过定时器模块产生20ms 定时中断作为机器人的运动周期,通过输出PWM波控制舵机转角完成机器人的步态,同时 stm32通过采集加速度计反馈的值得到机器人的姿态,通过算法来控制机器人的平衡,控 制超声波模块的定时工作来采集机器人周围的障碍物情况,同时通过串口和蓝牙、mt7620 连接来进行通信;所述电源,用于给单片机供电;
[0133] 所述稳压电路,用于提供5v、3.3v、6v不同的电压分别给传感器、arm芯片和舵机供 电;
[0134] 所述舵机,用于驱动机器人进行步态行走和头部摇动;
[0135] 所述网络摄像头,用于远程监控,传送机器人周围的环境信息;
[0136] 所述单片机MT7620,用于挂载OpenWRT系统以此将摄像头的图像通过wifi发送出;
[0137] 所述加速度计传感器,安装在机器人的机体中心位置用于采集机器人实时的姿 态;
[0138] 所述超声波传感器,安装在机器人头部正上方用于检测机器人周围是否有障碍 物;
[0139] 所述单片机MEGA644,用于和stm32通信得到控制信息并解码后通过液晶显示;
[0140] 所述OLED液晶显示屏,用于显示机器人当前信息;
[0141] 所述蓝牙模块,用于和andorid上位机间通信,传输指令和数据;
[0142] 所述上位机,用于发送指令和接收机器人反馈的数据;
[0143] 如图4所示,一种四足机器人的步态规划方法,包括如下步骤:
[0144] 步骤(1):机器人初始化;
[0145] 步骤⑵:机器人接收上位机指令;将指令解码,通过OLED液晶显示屏显示;
[0146] 步骤⑶:机器人对上位机指令进行判断,判断是舵机调试指令、步态变换指令、方 向改变指令还是其他功能指令,所述其他功能指令包括自适应稳定性、拍照指令和自动避 障指令。
[0147] 所述步骤⑶中,
[0148] 如果是舵机调试指令,则开始单个舵机调试,判断是否收到调试值,如果收到就开 始步态实现,步态实现后返回步骤(2);如果没收到就结束;
[0149] 如果是步态变换指令,则开始步态实现,步态实现后返回步骤⑵;
[0150] 如果是方向改变指令,则调整运动学方程,开始步态实现,步态实现后返回步骤 (2);
[0151] 如果是其他功能指令,则根据指令完成稳定性调整、拍照和自动避障功能,返回步 骤⑵。
[0152] 如图5所示,所述步骤⑶中,
[0153] 如果收到其他功能指令,首先判断是自适应稳定性指令、拍照指令还是自动避障 指令;
[0154] 如果是自适应稳定性指令,则加速度计传感器采集当前机器人躯体的倾角并和稳 态时比较;计算偏差;步态实现;
[0155] 如果是拍照指令,则网络摄像头拍照,将照片通过单片机MT7620内嵌的OpenWRT, 上传给单片机STM32,最终通过蓝牙模块上传给上位机;
[0156] 如果是自动避障指令,则进入如下步骤:
[0157] 步骤(3-1):超声波传感器启动,判断是否在设定安全距离内,如果是就开始步态 实现,返回步骤(3-1);如果否则机器人头部的第十四舵机通过转动再次检测;判断是否找 到无障碍方向,如果是就改变方向,开始步态实现,返回步骤(3-1),如果否就结束。
[0158] 如图6所示,所述步态实现的具体步骤如下:
[0159] 步骤(4-1):建立DH坐标系,设定初始坐标点;
[0160] 步骤(4-2):建立运动学方程;求解逆运动学方程;
[0161] 步骤(4-3):设计运动轨迹;
[0162]步骤(4-4):舵机关节映射;
[0163] 步骤(4-5) : PffM输出控制舵机实现步态。
[0164] 所述步骤(4-1)的步骤为:
[0165] 如图1所示,首先,利用D-H (Dnavit-Harbenberg)方法对机器人建立坐标系,以此 来表示各关节变量;第一关节的坐标系为(XI,Yl,Zl),第二关节的坐标系为(X2,Y2,Z2),第 三关节的坐标系为(Χ3,Υ3,Ζ3),机器人躯干的中心位置的坐标系为(Χ0,Υ0,Ζ0);
[0166] 根据建立的D-H坐标系建立机器人D-H坐标参数表。
[0167] 表1机器人D-H坐标参数表
[0168]
Figure CN104986241BD00161
[0170] a:表示每条公垂线的长度;α:两个相邻两坐标系z轴间的角度;
[0171] Θ:绕当前关节坐标系ζ轴旋转的角度;d:在当前关节坐标系ζ轴上两条相邻的公垂 线之间的距离;关节1表示横向髋关节;关节2表示纵向髋关节;关节3表示膝关节;表示横 向髋关节和纵向髋关节之间的连杆长度;&表示纵向髋关节和膝关节之间的连杆长度;&2表 示膝关节关节和腿末端节之间的连杆长度;k的取值范围是1〜3;dlk的参数含义是k和k-Ι坐 标系中ζ轴上两条相邻的公垂线之间的距离的参数含义是ζ轴从k-Ι坐标系旋转到k 坐标系的角度;如图2所示,9iQ的参数含义是需要求解的横向髋关节转动角度;Qil的参数含 义是需要求解的纵向髋关节转动角度;θι2的参数含义是需要求解的膝关节转动角度;
[0172] 所述步骤(4-2)的步骤为:
[0173] 建立运动学方程
[0174] Xi = -aisi-a2Si2+51 (1)
[0175] yi = a〇s〇+ais〇ci+a2S〇ci2+^w (2)
[0176] Zi = -a〇c〇-aic〇ci-a2C〇ci2~h (3)
[0177] 其中:
Figure CN104986241BD00162
[0180] 其中8〇表不8;[119(),。()表不(308 9(),81表不8;[1191,(31表不(308 91,812表不8;[11(91+02),(;12 表示COS (θ1 + θ2) 表示第i坐标系中的X坐标,ai表示纵向髋关节和膝关节之间的连杆长 度,a2表示膝关节关节和腿末端节之间的连杆长度,δ表示对应不同腿添加的标志位,为-1 或1,1表示机器人机体的长度的一半;yi表示第i坐标系中的y坐标;a0表示横向髋关节和纵 向髋关节之间的连杆长度。ai表示纵向髋关节和膝关节之间的连杆长度;a2表示膝关节关节 和腿末端节之间的连杆长度;λ表示对应不同腿添加的标志位,为-1或l;w表示机器人的宽 度的一半;Zi表示第i坐标系中的ζ坐标,h表示机器人的基坐标和坐标系(XI,Y1,Z1)之间的 高度。
[0181] 求解得到逆运动学方程:
Figure CN104986241BD00171
[0185]其中:
Figure CN104986241BD00172
[0188] 其中,
[0189] Θ1()表示第i个坐标系中横向髋关节转动的角度,yi表示第i坐标系中的y坐标,λ表 示对应不同腿添加的标志位,为-1或l,Zl表示第i坐标系中的ζ坐标,h表示机器人的基坐标 和坐标系(XI,Y1,Z1)之间的高度。Q11表示第i个坐标系中纵向髋关节转动的角度,表示一 个综合的公式。S表示对应不同腿添加的标位,为-1或l,Xl表示第i坐标系中的X坐标,1表示 机器人机体的长度的一半;ao表示横向髋关节和纵向髋关节之间的连杆长度,Z1表示第i坐 标系中的ζ坐标,h表示机器人的基坐标和坐标系1之间的高度。c〇S0iQ表示第i个坐标系中横 向髋关节转动角度的余弦值。θι2表示第i个坐标系中纵向膝关节转动的角度,ξ表示一个综 合的公式,Xi表示第i坐标系中的X坐标;
[0190] (2)变形为:yi_Aw = s。(ao+aici+a2Ci2) (7)
[0191] (3)变形为:zi+h = -co (ao+aici+a2Ci2) (8)
[0192] 联立以上两式得
Figure CN104986241BD00173
[0193] 由⑶得:
Figure CN104986241BD00174
[0194] ⑴整理为:Xi-51 = -aisi_a2Si2 (10)
[0195] (9) (10)两式两边分别平方,并和得:
[0196]
Figure CN104986241BD00175
[0197] 化简后得:
[0198]
Figure CN104986241BD00176
由三角函数公 式化简得:
[0199]
Figure CN104986241BD00177
[0200] 贝 lj:
Figure CN104986241BD00181
[0205] 求出0iQ和0i2后求解θη有多中方法,此处通过代入⑴求解整理后得:
Figure CN104986241BD00182
[0209] 如果是自适应稳定性指令,通过加速度计传感器测得机器人重力加速度可以得到 角度信息,与之稳定态的加速度差值通过PID算法得到机器人的偏移量,单片机驱动舵机通 过调节髋关节实现重心向原来方向的反作用力,使腿部的反作用力抵消外力的干扰,机器 人身体的基准是不变的,根据当前偏移基准值应用增量或者位置PID算法都可以实现自动 调整,考虑到位置式PID的算法需要不断累加每次的偏移值,运算量较大,所以使用增量式 PID算法。
[0210] 通过超声波传感器得到当前机器人与前方物体的距离,当距离大于安全距离时机 器人前行。当小于安全距离时,机器人头部舵机向左或者右摇动,检测该方向的障碍情况, 当距离符合安全距离时记录该方向标识位,并以此向该方向移动。
[0211] 足端运动空间
[0212] 以右前腿为例,在第三关节的坐标系(Χ3,Υ3,Ζ3)中足端坐标为(L3,0,0)在第二关 节的坐标系(X2,Y2,Z2)中第三关节的坐标系(X3,Y3,Z3)的坐标原点是沿着X轴方向平移L2 然后绕ζ轴旋转θ3得到,即变换矩阵为:
[0213]
Figure CN104986241BD00183
(20)
[0214] 同理得到
[0215]
Figure CN104986241BD00184
ρ;1)
[0216]
Figure CN104986241BD00191
(22}
[0217] 其中,次表示由第3坐标系(X3,Y3,Z3)移动到第2坐标系(X2,Y2,Z2)的平移坐标变 换,Trans表示平移变换,Rot (ζ,θ3)表示绕z轴旋转θ3角度,z表示z轴方向,θ3表示旋转的角 度值,L2表示关节膝关节到足端的连杆长度
[0218] 其中,丨F表示由第2坐标系(Χ2,Υ2,Ζ2)移动到第1坐标系(Χ1,Υ1,Ζ1)的平移坐标变 换,
Figure CN104986241BD00192
表示绕1轴顺时针旋转90°;1'瓜1^仏1,0,0)表示沿1轴方向平移1^1长度,1?的 (ζ,θ2)表示绕ζ轴方向旋转θ2角度。
[0219] 其中,,表示由第1坐标系(XI,Y1,Ζ1)移动到基坐标系(Χ0,Υ0,Ζ0)的平移坐标变 换,
Figure CN104986241BD00193
>表示沿X轴方向移动L/2距离,沿y轴方向移动W/2距离,沿ζ轴方向移 动-H距离,
Figure CN104986241BD00194
表示绕y轴方向旋转90° ,Rot(Z^e1)表示绕ζ轴方向旋转-Q1角度。
[0220] L:机器人身体长度;W:机器人身体宽度;H:机器人身体厚度
[0221] 由此得到足端轨迹坐标点在基坐标位置为:
[0222]
Figure CN104986241BD00195
(23)
[0223] 限定θ2范围是-45°〜45°,θ3范围是〇°〜90°得到足端轨迹空间图,如图7.1和图 7.2。
[0224] 图7.2中显示了足端的轨迹空间,选取框中的(68.5,77.5,-135.5)作为右前腿的 初始坐标点,以该点作为初始点留出了做够的裕量,机器人足端有较大的灵活性。
[0225] 7.4足端轨迹设计
[0226] 通过研究发现哺乳动物足端的轨迹基本都是圆滑的弧,如图7.3,将其分解为X轴 方向和ζ轴方向的运动轨迹如图7.4 (a)和图7.4 (b)。
[0227] 设定每个运动周期分成1000份,以基坐标(Χ0,Υ0,Ζ0)的X,Z轴构成的面做基准确 定足端轨迹,使用matlab中cftool工具采用描点拟合的方法初步拟合我们需求的轨迹曲线 (仅分析前半个周期)。
[0228] X轴轨迹及参数如图7.5。Z轴轨迹及参数如图7.6。
[0229] 修正后得到的设计足端轨迹方程为:
Figure CN104986241BD00196
[0230] (24)
[0231] 其中,X⑴表示X轴方向随时间变化的坐标点Xinit表示初始时刻的X坐标点L表示 步态的长度,T表示时间周期t表示时间。
[0232]
Figure CN104986241BD00201
(25)
[0233] y ⑴=yinit (26)
[0234] 其中,z⑴表示X轴方向随时间变化的坐标点Zinit表示初始时亥Ijz轴的坐标点,H表 示步态的高度。L为步态的长度,最终足端轨迹如图7.7。
[0235] 上述虽然结合附图对本发明的具体实施方式进行了描述,但并非对本发明保护范 围的限制,所属领域技术人员应该明白,在本发明的技术方案的基础上,本领域技术人员不 需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (1)

  1. I. 一种四足机器人的步态规划方法,其特征是,包括如下步骤: 步骤(1):机器人初始化; 步骤(2):机器人接收上位机指令;将指令解码,通过OLED液晶显示屏显示; 步骤(3):机器人对上位机指令进行判断并执行指令,判断是舵机调试指令、步态变换 指令、方向改变指令还是其他功能指令,所述其他功能指令包括自适应稳定性、拍照指令和 自动避障指令; 如果是舵机调试指令,则开始单个舵机调试,判断是否收到调试值,如果收到就开始步 态实现,步态实现后返回步骤(2);如果没收到就结束; 如果是步态变换指令,则开始步态实现,步态实现后返回步骤(2); 如果是方向改变指令,则调整运动学方程,开始步态实现,步态实现后返回步骤(2); 如果是其他功能指令,则根据指令完成稳定性调整、拍照和自动避障功能,返回步骤 (2); 如果收到其他功能指令,首先判断是自适应稳定性指令、拍照指令还是自动避障指令; 如果是自适应稳定性指令,则加速度计传感器采集当前机器人躯体的倾角并和稳态时 比较;计算偏差;步态实现; 如果是拍照指令,则网络摄像头拍照,将照片通过单片机MT7620内嵌的OpenWRT,上传 给单片机STM32,最终通过蓝牙模块上传给上位机; 所述步态实现的具体步骤如下: 步骤(4-1):建立DH坐标系,设定初始坐标点; 步骤(4-2):建立运动学方程;求解逆运动学方程; 步骤(4-3):设计运动轨迹; 步骤(4-4):舵机关节映射; 步骤(4-5) :PffM输出控制舵机实现步态; 所述步骤(4-1)的步骤为: 首先,利用D-H方法对机器人建立坐标系,以此来表示各关节变量;第一关节的坐标系 为(X1,Y1,Z1),第二关节的坐标系为(X2,Y2,Z2),第三关节的坐标系为(X3,Y3,Z3),机器人 躯干的中心位置的坐标系为(Χ0,YO,Ζ0); 根据建立的D-H坐标系建立机器人D-H坐标参数表; 表1机器人D-H坐标参数表
    Figure CN104986241BC00021
    a:表示每条公垂线的长度;α:两个相邻两坐标系ζ轴间的角度; Θ:绕当前关节坐标系Z轴旋转的角度;d:在当前关节坐标系Z轴上两条相邻的公垂线之 间的距1¾ ;关节1表不横向髓关节;关节2表;^纵向髓关节;关节3表;^膝关节;ao表;^横向髓 关节和纵向髋关节之间的连杆长度;m表示纵向髋关节和膝关节之间的连杆长度;a2表示膝 关节关节和腿末端节之间的连杆长度;k的取值范围是1〜3;dlk的参数含义是k和k-Ι坐标系 中z轴上两条相邻的公垂线之间的距离Ji0i-D的参数含义是z轴从k-Ι坐标系旋转到k坐标 系的角度;Ιο的参数含义是需要求解的横向髋关节转动角度;θη的参数含义是需要求解的 纵向髋关节转动角度;θι2的参数含义是需要求解的膝关节转动角度; 所述步骤(4-2)的步骤为: 建立运动学方程
    Figure CN104986241BC00031
    其中:
    Figure CN104986241BC00032
    其中so表不sin^o, Co表不cos9〇, si表不sin^i,Ci表不cos^i,si2表不sin (θι+02),ci2表不 COS (Θ0Θ2) ; Xi表示第i坐标系中的X坐标,&1表示纵向髋关节和膝关节之间的连杆长度,&2表 示膝关节关节和腿末端节之间的连杆长度,S表示对应不同腿添加的标志位,为-1或1,1表 示机器人机体的长度的一半;yi表示第i坐标系中的y坐标;ao表示横向髋关节和纵向髋关节 之间的连杆长度;m表示纵向髋关节和膝关节之间的连杆长度;a2表示膝关节关节和腿末端 节之间的连杆长度;λ表示对应不同腿添加的标志位,为-1或l;w表示机器人的宽度的一半; Zi表示第i坐标系中的z坐标,h表示机器人的基坐标和坐标系(XI,Y1,Z1)之间的高度; 求解得到逆运动学方程:
    Figure CN104986241BC00033
    其中, θ1〇表示第i个坐标系中横向髋关节转动的角度,yi表示第i坐标系中的y坐标,λ表示对 应不同腿添加的标志位,为-1或I,Zl表示第i坐标系中的Z坐标,h表示机器人的基坐标和坐 标系(XI,Y1,Z1)之间的高度;θη表示第i个坐标系中纵向髋关节转动的角度,辦表示一个综 合的公式;δ表示对应不同腿添加的标志位,为-1或l,Xl表示第i坐标系中的X坐标,1表示机 器人机体的长度的一半;a〇表示横向髋关节和纵向髋关节之间的连杆长度,Z1表示第i坐标 系中的z坐标,h表示机器人的基坐标和坐标系1之间的高度;c〇S0iQ表示第i个坐标系中横向 髋关节转动角度的余弦值;θι2表示第i个坐标系中纵向膝关节转动的角度,ξ表示一个综合 的公式,Xi表示第i坐标系中的X坐标;
    Figure CN104986241BC00041
    ⑼(10)两式两边分别平方,并和得:
    Figure CN104986241BC00042
    化简后得:
    Figure CN104986241BC00043
    由三角函数公式化简得:
    Figure CN104986241BC00044
    则:
    Figure CN104986241BC00045
    求出0lQ和θι2后求解θη有多种方法,此处通过代入⑴求解整理后得:
    Figure CN104986241BC00051
    如果是自适应稳定性指令,通过加速度计传感器测得机器人重力加速度可以得到角度 信息,与之稳定态的加速度差值通过PID算法得到机器人的偏移量,单片机驱动舵机通过调 节髋关节实现重心向原来方向的反作用力,使腿部的反作用力抵消外力的干扰,机器人身 体的基准是不变的,根据当前偏移基准值应用增量或者位置PID算法都可以实现自动调整, 考虑到位置式PID的算法需要不断累加每次的偏移值,运算量较大,所以使用增量式PID算 法; 通过超声波传感器得到当前机器人与前方物体的距离,当距离大于安全距离时机器人 前行;当小于安全距离时,机器人头部舵机向左或者右摇动,检测该方向的障碍情况,当距 离符合安全距离时记录该方向标识位,并以此向该方向移动; 四足机器人包括:机器人的躯干,所述机器人的躯干前端的上方通过机器人颈部连接 机器人头部,所述机器人躯干的下方连接四条腿:第一、第二、第三和第四条腿;其中第一、 第二条腿安装在机器人躯干的前端,第三、第四条腿安装在机器人躯干的后端;每条腿均包 括从下而上依次连接的小腿、大腿、垂直杆和水平杆四个部分;所述小腿与大腿、大腿与垂 直杆、垂直杆与水平杆之间均设有舵机;所述躯干上设有控制装置,控制装置通过控制舵机 来控制机器人的平稳行走; 所述控制装置包括:单片机STM32,所述单片机STM32分别与十四个舵机、加速度计传感 器和超声波传感器连接,所述单片机STM32还通过蓝牙模块与上位机连接,所述单片机 STM32还通过单片机MT7620与网络摄像头连接,所述单片机STM32还通过稳压电路与电源连 接,所述单片机STM32还通过单片机MEGA644与OLED液晶显示屏连接; 所述单片机STM32作为主控芯片是机器人控制的核心,通过定时器模块产生20ms定时 中断作为机器人的运动周期,通过输出PWM波控制舵机转角完成机器人的步态,同时stm32 通过采集加速度计反馈的值得到机器人的姿态,控制机器人的平衡,控制超声波模块的定 时工作来采集机器人周围的障碍物情况,同时通过串口和蓝牙、mt7620连接来进行通信;所 述电源,用于给单片机供电;所述稳压电路用于提供5v、3.3v、6v不同的电压分别给加速度 计传感器、超声波传感器、arm芯片和舵机供电;所述舵机用于驱动机器人进行步态行走和 头部摇动;所述网络摄像头,用于远程监控,传送机器人周围的环境信息;所述单片机 MT7620,用于挂载OpenWRT系统,将摄像头的图像通过wif i发送出;所述加速度计传感器安 装在机器人的机体中心位置用于采集机器人实时的姿态;所述超声波传感器,安装在机器 人头部正上方用于检测机器人周围是否有障碍物;所述单片机MEGA644,用于和stm32通信 得到控制信息并解码后通过液晶显示;所述OLED液晶显示屏,用于显示机器人当前信息;所 述蓝牙模块用于和andorid上位机间通信,传输指令和数据;所述上位机用于发送指令和接 收机器人反馈的数据; 第一条腿中,小腿与大腿之间通过第一舵机连接,大腿和垂直杆之间通过第二舵机连 接,垂直杆与水平杆之间通过第三舵机连接; 第二条腿中,小腿与大腿之间通过第四舵机连接,大腿和垂直杆之间通过第五舵机连 接,垂直杆与水平杆之间通过第六舵机连接; 第三条腿中,小腿与大腿之间通过第七舵机连接,大腿和垂直杆之间通过第八舵机连 接,垂直杆与水平杆之间通过第九舵机连接; 第四条腿中,小腿与大腿之间通过第十舵机连接,大腿和垂直杆之间通过第十一舵机 连接,垂直杆与水平杆之间通过第十二舵机连接; 所述机器人颈部设有第十三舵机;所述机器人头部设有第十四舵机; 所述第三舵机,也称水平髋关节,仿照哺乳动物髋部关节的外摆内收功能,负责机器人 平行于身体方向的垂直方向运动,是机器人的转向运动时的驱动源,其旋转量用θ〇表示; 所述第二舵机,也称垂直髋关节,仿照哺乳动物的髋关节的前后收腿功能,负责机器人 的平行于身体方向的运动,是整个机器人的承重关节,其旋转量用表示; 所述第一舵机,也称膝关节,所述膝关节仿照哺乳动物的膝关节的前后运动关节,负责 驱动机器人腿部与地面接触,是机器人的步态轨迹的最终呈现关节,其旋转量用θ2表示。
CN201510368506.2A 2015-06-29 2015-06-29 四足机器人的步态规划方法 Active CN104986241B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510368506.2A CN104986241B (zh) 2015-06-29 2015-06-29 四足机器人的步态规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510368506.2A CN104986241B (zh) 2015-06-29 2015-06-29 四足机器人的步态规划方法

Publications (2)

Publication Number Publication Date
CN104986241A CN104986241A (zh) 2015-10-21
CN104986241B true CN104986241B (zh) 2018-04-24

Family

ID=54298177

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510368506.2A Active CN104986241B (zh) 2015-06-29 2015-06-29 四足机器人的步态规划方法

Country Status (1)

Country Link
CN (1) CN104986241B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106383518A (zh) * 2016-09-29 2017-02-08 国网重庆市电力公司电力科学研究院 一种多传感器隧道机器人避障控制系统及方法
CN106394724A (zh) * 2016-11-09 2017-02-15 江苏信息职业技术学院 自适应运动多关节行走机器人
CN106695791B (zh) * 2017-01-11 2019-02-05 山东科技大学 四足仿生机器人连续自由步态生成方法
CN109343523A (zh) * 2018-09-30 2019-02-15 大连海事大学 可绘制3d地图并实时传输图像的洞穴探测爬行机器人
CN109709967B (zh) * 2019-01-22 2022-05-31 深圳市幻尔科技有限公司 机器人低运算要求的动态步态的实现方法
CN110361026B (zh) * 2019-06-05 2020-12-22 华南理工大学 一种基于3d点云的仿人机器人路径规划方法
CN112193345A (zh) * 2020-09-30 2021-01-08 江苏集萃智能制造技术研究所有限公司 一种分段式五次多项式曲线越障步态规划方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3278467B2 (ja) * 1992-08-18 2002-04-30 本田技研工業株式会社 移動ロボットの制御装置
CN202657138U (zh) * 2012-07-16 2013-01-09 山东大学 小型仿生四足机器人
CN202771260U (zh) * 2012-09-05 2013-03-06 浙江大学 一种基于DaNI移动机器人平台的智能安全巡逻系统
CN103085070B (zh) * 2013-01-15 2015-03-11 上海交通大学 面向复杂地形的四足机器人运动规划方法
CN104709374B (zh) * 2015-02-13 2017-03-08 中国北方车辆研究所 一种防止足式机器人左右倾斜的方法

Also Published As

Publication number Publication date
CN104986241A (zh) 2015-10-21

Similar Documents

Publication Publication Date Title
CN104986241B (zh) 四足机器人的步态规划方法
Almetwally et al. Real-time tele-operation and tele-walking of humanoid Robot Nao using Kinect Depth Camera
US8751041B2 (en) Method to generate humanlike motion of humanoid robot
Carignan et al. Development of an exoskeleton haptic interface for virtual task training
JP4836592B2 (ja) ロボット装置及びその制御方法
CN109093626B (zh) 四足机器人的机身姿态控制方法及装置
CN101943912B (zh) 一种消除双足仿人机器人上身姿态晃动的控制方法
CN105137969B (zh) 基于支撑线运动分解的四足机器人trot步态本体位姿控制方法
WO2019218805A1 (zh) 一种四足机器人的运动闭环控制方法
CN103901889A (zh) 一种基于蓝牙通信的多机器人编队控制路径跟踪方法
Xu et al. Development of a hybrid motion capture method using MYO armband with application to teleoperation
US9014854B2 (en) Robot and control method thereof
CN104199460A (zh) 基于图像识别跟踪的四旋翼飞行器智能侦察系统
Yokoi et al. Experimental study of humanoid robot HRP-1S
Park et al. Online walking pattern generation and its application to a biped humanoid robot—KHR-3 (HUBO)
US20120059518A1 (en) Walking robot and control method thereof
TW201743154A (zh) 機械手臂控制裝置,包含該控制裝置的機械手臂系統及機械手臂控制方法
Garrido et al. Modular design and control of an upper limb exoskeleton
Gong et al. Bionic quadruped robot dynamic gait control strategy based on twenty degrees of freedom
CN109871018A (zh) 一种具有腰部自由度四足机器人的控制方法
CN109159120A (zh) 基于康复机械臂关节电机电流反馈的主动控制方法及系统
Yasin et al. Biped robot push detection and recovery
CN107485540B (zh) 一种用于智能协步手杖的能量注入系统
Meriçli et al. Improving biped walk stability using real-time corrective human feedback
Hoffman et al. Upper limb compliant strategy exploiting external physical constraints for humanoid fall avoidance

Legal Events

Date Code Title Description
PB01 Publication
C06 Publication
SE01 Entry into force of request for substantive examination
C10 Entry into substantive examination
GR01 Patent grant
GR01 Patent grant