CN104986241A - 四足机器人及其步态规划方法 - Google Patents

Abstract

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

Description

四足机器人及其步态规划方法

技术领域

本发明涉及一种四足机器人及其步态规划方法。

背景技术

现有四足机器人还有很多的缺陷，比如均存在行走不稳定，功能较为单一的特点，大部分都只能在平坦地面行走，没有应对外界干扰的和躲避障碍物的能力，具有视觉的四足机器人更少。并且具有负载能力的四足机器人体型一般都较大、能耗高，在设计和调试中会有很多麻烦，不利于算法验证和参数调整，所以设计一个小型四足机器人，方便在实验室里进行算法验证，这对四足机器人的研究提供了一种高效的方式。

发明内容

本发明的目的就是为了解决上述问题，提供一种四足机器人及其步态规划方法，它具有通过建立D-H坐标系、设计运动学方程和求解逆运动学方程，利用ARM单片机作为主控制器，实现机器人步态规划，并通过多传感器实现了机器人的稳定性、自动避障和视觉传输能力，在Android平台上设计的上位机可以有地控制机器人的优点。

为了实现上述目的，本发明采用如下技术方案：

一种四足机器人，包括：机器人的躯干，所述机器人的躯干前端的上方通过机器人颈部连接机器人头部，所述机器人躯干的下方连接四条腿：第一、第二、第三和第四条腿；其中第一、第二条腿安装在机器人躯干的前端，第三、第四条腿安装在机器人躯干的后端；每条腿均包括从下而上依次连接的小腿、大腿、垂直杆和水平杆四个部分；所述小腿与大腿、大腿与垂直杆、垂直杆与水平杆之间均设有舵机；所述躯干上设有控制装置，控制装置通过控制舵机来控制机器人的平稳行走。

第一条腿中，小腿与大腿之间通过第一舵机连接，大腿和垂直杆之间通过第二舵机连接，垂直杆与水平杆之间通过第三舵机连接；

第二条腿中，小腿与大腿之间通过第四舵机连接，大腿和垂直杆之间通过第五舵机连接，垂直杆与水平杆之间通过第六舵机连接；

第三条腿中，小腿与大腿之间通过第七舵机连接，大腿和垂直杆之间通过第八舵机连接，垂直杆与水平杆之间通过第九舵机连接；

第四条腿中，小腿与大腿之间通过第十舵机连接，大腿和垂直杆之间通过第十一舵机连接，垂直杆与水平杆之间通过第十二舵机连接；

所述机器人颈部设有第十三舵机；所述机器人头部设有第十四舵机。

所述第三舵机，也称水平髋关节，仿照哺乳动物髋部关节的外摆内收功能，负责机器人平行于身体方向的垂直方向运动，是机器人的转向运动时的驱动源，其旋转量用θ₀表示。

所述第二舵机，也称垂直髋关节，仿照哺乳动物的髋关节的前后收腿功能，负责机器人的平行于身体方向的运动，是整个机器人的承重关节，其旋转量用θ₁表示。

所述第一舵机，也称膝关节，所述膝关节仿照哺乳动物的膝关节的前后运动关节，负责驱动机器人腿部与地面接触，是机器人的步态轨迹的最终呈现关节，其旋转量用θ₂表示。

所述控制装置包括：单片机STM32，所述单片机STM32分别与上述十四个舵机、加速度计传感器和超声波传感器连接，所述单片机STM32还通过蓝牙模块与上位机连接，所述单片机STM32还通过单片机MT7620与网络摄像头连接，所述单片机STM32还通过稳压电路与电源连接，所述单片机STM32还通过单片机MEGA644与OLED液晶显示屏连接。

所述单片机STM32作为主控芯片是机器人控制的核心，通过定时器模块产生20ms定时中断作为机器人的运动周期，通过输出PWM波控制舵机转角完成机器人的步态，同时stm32通过采集加速度计反馈的值得到机器人的姿态，控制机器人的平衡，控制超声波模块的定时工作来采集机器人周围的障碍物情况，同时通过串口和蓝牙、mt7620连接来进行通信；

所述电源，用于给单片机供电；

所述稳压电路，用于提供5v、3.3v、6v不同的电压分别给传感器、arm芯片和舵机供电；

所述舵机，用于驱动机器人进行步态行走和头部摇动；

所述网络摄像头，用于远程监控，传送机器人周围的环境信息；

所述单片机MT7620，用于挂载OpenWRT系统以此将摄像头的图像通过wifi发送出；

所述加速度计传感器，安装在机器人的机体中心位置用于采集机器人实时的姿态；

所述超声波传感器，安装在机器人头部正上方用于检测机器人周围是否有障碍物；

所述单片机MEGA644，用于和stm32通信得到控制信息并解码后通过液晶显示；

所述OLED液晶显示屏，用于显示机器人当前信息；

所述蓝牙模块，用于和andorid上位机间通信，传输指令和数据；

所述上位机，用于发送指令和接收机器人反馈的数据；

一种四足机器人的步态规划方法，包括如下步骤：

步骤(1)：机器人初始化；

步骤(2)：机器人接收上位机指令；将指令解码，通过OLED液晶显示屏显示；

步骤(3)：机器人对上位机指令进行判断并执行指令，判断是舵机调试指令、步态变换指令、方向改变指令还是其他功能指令，所述其他功能指令包括自适应稳定性、拍照指令和自动避障指令。

所述步骤(3)中，

如果是舵机调试指令，则开始单个舵机调试，判断是否收到调试值，如果收到就开始步态实现，步态实现后返回步骤(2)；如果没收到就结束；

如果是步态变换指令，则开始步态实现，步态实现后返回步骤(2)；

如果是方向改变指令，则调整运动学方程，开始步态实现，步态实现后返回步骤(2)；

如果是其他功能指令，则根据指令完成稳定性调整、拍照和自动避障功能，返回步骤(2)。

所述步骤(3)中，

如果收到其他功能指令，首先判断是自适应稳定性指令、拍照指令还是自动避障指令；

如果是自适应稳定性指令，则加速度计传感器采集当前机器人躯体的倾角并和稳态时比较；计算偏差；步态实现；

如果是拍照指令，则网络摄像头拍照，将照片通过单片机MT7620内嵌的OpenWRT，上传给单片机STM32，最终通过蓝牙模块上传给上位机；

如果是自动避障指令，则进入如下步骤：

步骤(3-1)：超声波传感器启动，判断是否在设定安全距离内，如果是就开始步态实现，返回步骤(3-1)；如果否则机器人头部的第十四舵机通过转动再次检测；判断是否找到无障碍方向，如果是就改变方向，开始步态实现，返回步骤(3-1)，如果否就结束。

所述步态实现的具体步骤如下：

步骤(4-1)：建立DH坐标系，设定初始坐标点；

步骤(4-2)：建立运动学方程；求解逆运动学方程；

步骤(4-3)：设计运动轨迹；

步骤(4-4)：舵机关节映射；

步骤(4-5)：PWM输出控制舵机实现步态。

所述步骤(4-1)的步骤为：

首先，利用D-H(Dnavit-Harbenberg)方法对机器人建立坐标系，以此来表示各关节变量；第一关节的坐标系为(X1,Y1,Z1)，第二关节的坐标系为(X2,Y2,Z2)，第三关节的坐标系为(X3,Y3,Z3)，机器人躯干的中心位置的坐标系为(X0,Y0,Z0)

根据建立的D-H坐标系建立机器人D-H坐标参数表。

表1机器人D-H坐标参数表

а：表示每条公垂线的长度(连杆长度)；α：两个相邻两坐标系z轴间的角度；

θ：绕当前关节坐标系z轴旋转的角度；d:在当前关节坐标系z轴上两条相邻的公垂线之间的距离(称关节偏移)；关节1表示横向髋关节；关节2表示纵向髋关节；关节3表示膝关节；а0表示横向髋关节和纵向髋关节之间的连杆长度；a1表示纵向髋关节和膝关节之间的连杆长度；a2表示膝关节关节和腿末端节之间的连杆长度；k的取值范围是0～2；dik的参数含义是k和k-1坐标系中z轴上两条相邻的公垂线之间的距离；θik的参数含义是z轴从k-1坐标系旋转到k坐标系的角度；θi0的参数含义是需要求解的横向髋关节转动角度；θi1的参数含义是需要求解的纵向髋关节转动角度；θi2的参数含义是需要求解的膝关节转动角度

所述步骤(4-2)的步骤为：

建立运动学方程

x_i＝-a₁s₁-a₂s₁₂+δl     (1)

y_i＝a₀s₀+a₁s₀c₁+a₂s₀c₁₂+λw     (2)

z_i＝-a₀c₀-a₁c₀c₁-a₂c₀c₁₂-h     (3)

其中：

$\mathrm{\δ}=\left\{\begin{array}{cc}1& i=0,1\\ -1& i=2,3\end{array}\right.$

$\mathrm{\λ}=\left\{\begin{array}{cc}1& i=0,3\\ -1& i=1,2\end{array}\right.$

其中s₀表示sinθ₀，c₀表示cosθ₀，s₁表示sinθ₁，c₁表示cosθ₁,s₁₂表示sin(θ₁₊θ₂)，c₁₂表示cos(θ₁₊θ₂)。x_i表示第i坐标系中的x坐标a₁表示纵向髋关节和膝关节之间的连杆长度a₂表示膝关节关节和腿末端节之间的连杆长度，δ表示对应不同腿添加的标志位，为-1或1，l表示机器人机体的长度的一半；y_i表示第i坐标系中的y坐标；a₀表示横向髋关节和纵向髋关节之间的连杆长度。a₁表示纵向髋关节和膝关节之间的连杆长度；a₂表示膝关节关节和腿末端节之间的连杆长度；λ表示对应不同腿添加的标志位，为-1或1；w表示机器人的宽度的一半；z_i表示第i坐标系中的z坐标，h表示机器人的基坐标和坐标系(X1,Y1,Z1)之间的高度。

求解得到逆运动学方程：

${\mathrm{\θ}}_{i0}=arctan\left(\frac{y_i-\mathrm{\λ}w}{-z_i-h}\right)---\left(4\right)$

${\mathrm{\θ}}_{i2}=\left(arccos\right(\frac{{a_1}^2+{a_2}^2-{\mathrm{\ξ}}^2}{2a_1{a}_2})-\mathrm{\π}).\mathrm{\δ}---\left(6\right)$

其中：

$\mathrm{\ξ}=\sqrt{{(a_0+\frac{z_i+h}{{\mathrm{cos\θ}}_{i0}})}^2+{(x_i-\mathrm{\δ}l)}^2}$

其中，

θ_i0表示第i个坐标系中横向髋关节转动的角度，y_i表示第i坐标系中的y坐标，λ表示对应不同腿添加的标志位，为-1或1，z_i表示第i坐标系中的z坐标，h表示机器人的基坐标和坐标系(X1,Y1,Z1)之间的高度；θ_i1表示第i个坐标系中纵向髋关节转动的角度，表示一个综合的公式。δ表示对应不同腿添加的标志位，为-1或1，x_i表示第i坐标系中的x坐标，l表示机器人机体的长度的一半；a₀表示横向髋关节和纵向髋关节之间的连杆长度，z_i表示第i坐标系中的z坐标，h表示机器人的基坐标和坐标系1之间的高度。cosθ_i0表示第i个坐标系中横向髋关节转动角度的余弦值。θ_i2表示第i个坐标系中纵向膝关节转动的角度，ξ表示一个综合的公式，x_i表示第i坐标系中的x坐标；

(2)变形为：y_i-λw＝s₀(a₀+a₁c₁+a₂c₁₂)  (7)

(3)变形为：z_i+h＝-c₀(a₀+a₁c₁+a₂c₁₂)  (8)

联立以上两式得： ${\mathrm{\θ}}_{i0}=arctan\left(\frac{y_i-\mathrm{\λ}w}{-z_i-h}\right)$

由(8)得： $a_0+\frac{z_i+h}{c_0}=-(a_1{c}_1+a_2{c}_{12})---\left(9\right)$

(1)整理为：x_i-δl＝-a₁s₁-a₂s₁₂  (10)

(9)(10)两式两边分别平方，并和得：

${(x_i-\mathrm{\δ}l)}^2+{(\frac{z_i+h}{c_0}+a_0)}^2={(-a_1{c}_1-a_2{c}_{12})}^2+{(-a_1{s}_1-a_2{s}_{12})}^2---\left(11\right)$

化简后得：

${(x_i-\mathrm{\δ}l)}^2+{(\frac{z_i+h}{c_0}+a_0)}^2={a_1}^2+{a_2}^2+2a_1{a}_2(s_1{s}_{12}+c_1{c}_{12})---\left(12\right)$

由三角函数公式化简得：

${(x_i-\mathrm{\δ}l)}^2+{(\frac{z_i+h}{c_0}+a_0)}^2={a_1}^2+{a_2}^2+2a_1{a}_2{c}_2---\left(13\right)$

则：

${\mathrm{\θ}}_{i2}=\left(arccos\right(\frac{{a_1}^2+{a_2}^2-{(a_0+\frac{z_i+h}{{\mathrm{cos\θ}}_{i0}})}^2-{(x_i-\mathrm{\δ}l)}^2}{2a_1{a}_2})-\mathrm{\π}).\mathrm{\δ}---\left(14\right)$

令： $\mathrm{\ξ}=\sqrt{{(a_0+\frac{z_i+h}{{\mathrm{cos\θ}}_{i0}})}^2+{(x_i-\mathrm{\δ}l)}^2}---\left(15\right)$

则：

${\mathrm{\θ}}_{i2}=\left(arccos\right(\frac{{a_1}^2+{a_2}^2-{\mathrm{\ξ}}^2}{2a_1{a}_2})-\mathrm{\π}).\mathrm{\δ}---\left(16\right)$

求出θ_i0和θ_i2后求解θ_i1有多中方法，此处通过代入(1)求解整理后得：

${\mathrm{\θ}}_{i1}=\left(arccos\right(\frac{{a_1}^2-{a_2}^2+{\mathrm{\ξ}}^2}{2a_1\mathrm{\ξ}})-arctan\left(\frac{x_i-\mathrm{\δ}l}{-a_0-(z_i+h)/{\mathrm{cos\θ}}_{i0}}\right)).\mathrm{\δ}---\left(17\right)$

令：

则：

如果是自适应稳定性指令，通过加速度计传感器测得机器人重力加速度可以得到角度信息，与之稳定态的加速度差值通过PID算法得到机器人的偏移量，单片机驱动舵机通过调节髋关节实现重心向原来方向的反作用力，使腿部的反作用力抵消外力的干扰，机器人身体的基准是不变的，根据当前偏移基准值应用增量或者位置PID算法都可以实现自动调整，考虑到位置式PID的算法需要不断累加每次的偏移值，运算量较大，所以使用增量式PID算法。

通过超声波传感器得到当前机器人与前方物体的距离，当距离大于安全距离时机器人前行。当小于安全距离时，机器人头部舵机向左或者右摇动，检测该方向的障碍情况，当距离符合安全距离时记录该方向标识位，并以此向该方向移动。

本发明的有益效果：

1、搭建了四足机器人硬件平台。通过运动方程建模和逆运动学方程求解得到足端轨迹并通过编程在本发明中实现了步态和转向方法。

2、通过加速度姿态传感器和超声波距离传感器感知当前状态，实现机器人行为协调与自主导航。

3、通过OpenWRT挂载网络摄像头方式实现远程图像传输处理，在上位机成功得到图像信息。上位机通过蓝牙传输指令完成了对机器人控制。

4、虽然没有大型四足机器人的负载能力，但其结构和功能都与之相似，能够在该小型四足机器人平台上验证步态算法，调试参数，设计动作，并且通过上位机可以回传机器人状态信息。

附图说明

图1为本发明的实体结构和坐标系示意图；

图2为本发明的机器人腿部示意图；

图3为本发明的控制装置结构示意图；

图4为本发明的整体方法流程图；

图5为本发明的其他功能实现流程图；

图6为本发明的步态实现流程图；

图7.1为x-z足端轨迹空间图；

图7.2为x-y足端轨迹空间；

图7.3为足端轨迹图；

图7.4(a)和图7.4(b)为足端分解图；

图7.5为x轴轨迹设置图；

图7.6为z轴轨迹设置图；

图7.7为机器人足端轨迹图。

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

本发明中使用蓝牙通信的两端设定为：手机端为服务端，机器人蓝牙作为客户端。

服务端，手机端保持BluetoothServiceSocket打开，作为server。ServerSocket将监听，一旦连接成功就会创建产生一个BluetoothSocket。调用listenUsingRfcommWithServiceRecord(string,UUID)获得BluetoothServiceSocket，然后调用accept()开始监听连接请求，如果接受成功将返回一个已经连接的BluetoothSocket。

客户端，首先获得一个代表远程设备的BlutoohDevice对象，然后使用它获得BluetoothSocket发起连接。使用BluetoothDevice方法，调用creaatRfcommSocketTo-Service-Record(UUID)将得到BluetoothSocket，然后调用connect()建立连接。

建立以上连接后就可以在两端相互发送和接收协议指令，完成控制机器人和反馈信息的功能。

上位机控制机器人通过一定的指令协议发送，下位机端识别指令，这就需要设计一套指令协议。Andorid发送数据只有字节流和字符流，为简化令本设计采用发用一个字节数据按照其不同位的0、1方式来编码，部分控制指令编码如表2所示：

表2部分控制协议指令

指令	7	6	5	4	3	2	1	0	Hex
										Trot	0	0	0	0	0	0	0	1	0x01
Bound	0	0	0	0	0	0	1	0	0x02
										Walk	0	0	0	0	0	1	0	0	0x04
Pace	0	0	0	0	1	0	0	0	0x08
										Left	0	0	0	0	0	0	1	1	0x03

Right	0	0	0	0	0	1	0	1	0x05
										Back	0	0	0	0	0	1	1	0	0x06
Left_slide	0	0	0	0	0	1	1	1	0x07
										Left_parall	0	0	0	0	1	0	0	1	0x09
Right_slide	0	0	0	0	1	1	0	1	0x0d
										Right_parall	0	0	0	0	1	1	1	0	0x0e
Shakehead_left	0	0	0	1	0	0	0	0	0x10
										Shakehead_right	0	0	0	1	0	0	1	1	0x12

如图1所示，一种四足机器人，包括：机器人的躯干，所述机器人的躯干前端的上方通过机器人颈部连接机器人头部，所述机器人躯干的下方连接四条腿：第一、第二、第三和第四条腿；其中第一、第二条腿安装在机器人躯干的前端，第三、第四条腿安装在机器人躯干的后端；每条腿均包括从下而上依次连接的小腿、大腿、垂直杆和水平杆四个部分；所述躯干上设有控制装置，控制装置用于控制机器人的行走。

第一条腿中，小腿与大腿之间通过第一舵机连接，大腿和垂直杆之间通过第二舵机连接，垂直杆与水平杆之间通过第三舵机连接；

第二条腿中，小腿与大腿之间通过第四舵机连接，大腿和垂直杆之间通过第五舵机连接，垂直杆与水平杆之间通过第六舵机连接；

第三条腿中，小腿与大腿之间通过第七舵机连接，大腿和垂直杆之间通过第八舵机连接，垂直杆与水平杆之间通过第九舵机连接；

第四条腿中，小腿与大腿之间通过第十舵机连接，大腿和垂直杆之间通过第十一舵机连接，垂直杆与水平杆之间通过第十二舵机连接；

所述机器人颈部设有第十三舵机；所述机器人头部设有第十四舵机。

所述第三舵机，也称水平髋关节，仿照哺乳动物髋部关节的外摆内收功能，负责机器人平行于身体方向的垂直方向运动，是机器人的转向运动时的驱动源，其旋转量用θ₀表示。

所述第二舵机，也称垂直髋关节，仿照哺乳动物的髋关节的前后收腿功能，负责机器人的平行于身体方向的运动，是整个机器人的承重关节，其旋转量用θ₁表示。

所述第一舵机，也称膝关节，所述膝关节仿照哺乳动物的膝关节的前后运动关节，负责驱动机器人腿部与地面接触，是机器人的步态轨迹的最终呈现关节，其旋转量用θ₂表示。

如图3所示，所述控制装置包括：单片机STM32，所述单片机STM32分别与上述十四个舵机、加速度计传感器和超声波传感器连接，所述单片机STM32还通过蓝牙模块与上位机连接，所述单片机STM32还通过单片机MT7620与网络摄像头连接，所述单片机STM32还通过稳压电路与电源连接，所述单片机STM32还通过单片机MEGA644与OLED液晶显示屏连接。

所述单片机STM32作为主控芯片是机器人控制的核心，通过定时器模块产生20ms定时中断作为机器人的运动周期，通过输出PWM波控制舵机转角完成机器人的步态，同时stm32通过采集加速度计反馈的值得到机器人的姿态，通过算法来控制机器人的平衡，控制超声波模块的定时工作来采集机器人周围的障碍物情况，同时通过串口和蓝牙、mt7620连接来进行通信；所述电源，用于给单片机供电；

所述稳压电路，用于提供5v、3.3v、6v不同的电压分别给传感器、arm芯片和舵机供电；

所述舵机，用于驱动机器人进行步态行走和头部摇动；

所述网络摄像头，用于远程监控，传送机器人周围的环境信息；

所述单片机MT7620，用于挂载OpenWRT系统以此将摄像头的图像通过wifi发送出；

所述加速度计传感器，安装在机器人的机体中心位置用于采集机器人实时的姿态；

所述超声波传感器，安装在机器人头部正上方用于检测机器人周围是否有障碍物；

所述单片机MEGA644，用于和stm32通信得到控制信息并解码后通过液晶显示；

所述OLED液晶显示屏，用于显示机器人当前信息；

所述蓝牙模块，用于和andorid上位机间通信，传输指令和数据；

所述上位机，用于发送指令和接收机器人反馈的数据；

如图4所示，一种四足机器人的步态规划方法，包括如下步骤：

步骤(1)：机器人初始化；

步骤(2)：机器人接收上位机指令；将指令解码，通过OLED液晶显示屏显示；

步骤(3)：机器人对上位机指令进行判断，判断是舵机调试指令、步态变换指令、方向改变指令还是其他功能指令，所述其他功能指令包括自适应稳定性、拍照指令和自动避障指令。

所述步骤(3)中，

如果是舵机调试指令，则开始单个舵机调试，判断是否收到调试值，如果收到就开始步态实现，步态实现后返回步骤(2)；如果没收到就结束；

如果是步态变换指令，则开始步态实现，步态实现后返回步骤(2)；

如果是方向改变指令，则调整运动学方程，开始步态实现，步态实现后返回步骤(2)；

如果是其他功能指令，则根据指令完成稳定性调整、拍照和自动避障功能，返回步骤(2)。

如图5所示，所述步骤(3)中，

如果收到其他功能指令，首先判断是自适应稳定性指令、拍照指令还是自动避障指令；

如果是自适应稳定性指令，则加速度计传感器采集当前机器人躯体的倾角并和稳态时比较；计算偏差；步态实现；

如果是拍照指令，则网络摄像头拍照，将照片通过单片机MT7620内嵌的OpenWRT，上传给单片机STM32，最终通过蓝牙模块上传给上位机；

如果是自动避障指令，则进入如下步骤：

步骤(3-1)：超声波传感器启动，判断是否在设定安全距离内，如果是就开始步态实现，返回步骤(3-1)；如果否则机器人头部的第十四舵机通过转动再次检测；判断是否找到无障碍方向，如果是就改变方向，开始步态实现，返回步骤(3-1)，如果否就结束。

如图6所示，所述步态实现的具体步骤如下：

步骤(4-1)：建立DH坐标系，设定初始坐标点；

步骤(4-2)：建立运动学方程；求解逆运动学方程；

步骤(4-3)：设计运动轨迹；

步骤(4-4)：舵机关节映射；

步骤(4-5)：PWM输出控制舵机实现步态。

所述步骤(4-1)的步骤为：

如图1所示，首先，利用D-H(Dnavit-Harbenberg)方法对机器人建立坐标系，以此来表示各关节变量；第一关节的坐标系为(X1,Y1,Z1)，第二关节的坐标系为(X2,Y2,Z2)，第三关节的坐标系为(X3,Y3,Z3)，机器人躯干的中心位置的坐标系为(X0,Y0,Z0)

根据建立的D-H坐标系建立机器人D-H坐标参数表。

表1机器人D-H坐标参数表

а：表示每条公垂线的长度(连杆长度)；α：两个相邻两坐标系z轴间的角度；

θ：绕当前关节坐标系z轴旋转的角度；d:在当前关节坐标系z轴上两条相邻的公垂线之间的距离(称关节偏移)；关节1表示横向髋关节；关节2表示纵向髋关节；关节3表示膝关节；а0表示横向髋关节和纵向髋关节之间的连杆长度；a1表示纵向髋关节和膝关节之间的连杆长度；a2表示膝关节关节和腿末端节之间的连杆长度；k的取值范围是0～2；dik的参数含义是k和k-1坐标系中z轴上两条相邻的公垂线之间的距离；θik的参数含义是z轴从k-1坐标系旋转到k坐标系的角度；如图2所示，θi0的参数含义是需要求解的横向髋关节转动角度；θi1的参数含义是需要求解的纵向髋关节转动角度；θi2的参数含义是需要求解的膝关节转动角度；

所述步骤(4-2)的步骤为：

建立运动学方程

x_i＝-a₁s₁-a₂s₁₂+δl     (1)

y_i＝a₀s₀+a₁s₀c₁+a₂s₀c₁₂+λw     (2)

z_i＝-a₀c₀-a₁c₀c₁-a₂c₀c₁₂-h     (3)

其中：

$\mathrm{\δ}=\left\{\begin{array}{cc}1& i=0,1\\ -1& i=2,3\end{array}\right.$

$\mathrm{\λ}=\left\{\begin{array}{cc}1& i=0,3\\ -1& i=1,2\end{array}\right.$

其中s₀表示sinθ₀，c₀表示cosθ₀，s₁表示sinθ₁，c₁表示cosθ₁,s₁₂表示sin(θ₁₊θ₂)，c₁₂表示cos(θ₁₊θ₂)。x_i表示第i坐标系中的x坐标a₁表示纵向髋关节和膝关节之间的连杆长度a₂表示膝关节关节和腿末端节之间的连杆长度，δ表示对应不同腿添加的标志位，为-1或1，l表示机器人机体的长度的一半；y_i表示第i坐标系中的y坐标；a₀表示横向髋关节和纵向髋关节之间的连杆长度。a₁表示纵向髋关节和膝关节之间的连杆长度；a₂表示膝关节关节和腿末端节之间的连杆长度；λ表示对应不同腿添加的标志位，为-1或1；w表示机器人的宽度的一半；z_i表示第i坐标系中的z坐标，h表示机器人的基坐标和坐标系(X1,Y1,Z1)之间的高度。

求解得到逆运动学方程：

${\mathrm{\θ}}_{i0}=arctan\left(\frac{y_i-\mathrm{\λ}w}{-z_i-h}\right)---\left(4\right)$

${\mathrm{\θ}}_{i2}=\left(arccos\right(\frac{{a_1}^2+{a_2}^2-{\mathrm{\ξ}}^2}{2a_1{a}_2})-\mathrm{\π}).\mathrm{\δ}---\left(6\right)$

其中：

$\mathrm{\ξ}=\sqrt{{(a_0+\frac{z_i+h}{{\mathrm{cos\θ}}_{i0}})}^2+{(x_i-\mathrm{\δ}l)}^2}$

其中，

θ_i0表示第i个坐标系中横向髋关节转动的角度，y_i表示第i坐标系中的y坐标，λ表示对应不同腿添加的标志位，为-1或1，z_i表示第i坐标系中的z坐标，h表示机器人的基坐标和坐标系(X1,Y1,Z1)之间的高度。θ_i1表示第i个坐标系中纵向髋关节转动的角度，表示一个综合的公式。δ表示对应不同腿添加的标位，为-1或1，x_i表示第i坐标系中的x坐标，l表示机器人机体的长度的一半；a₀表示横向髋关节和纵向髋关节之间的连杆长度，z_i表示第i坐标系中的z坐标，h表示机器人的基坐标和坐标系1之间的高度。cosθ_i0表示第i个坐标系中横向髋关节转动角度的余弦值。θ_i2表示第i个坐标系中纵向膝关节转动的角度，ξ表示一个综合的公式，x_i表示第i坐标系中的x坐标；

(2)变形为：y_i-λw＝s₀(a₀+a₁c₁+a₂c₁₂)  (7)

(3)变形为：z_i+h＝-c₀(a₀+a₁c₁+a₂c₁₂)  (8)

联立以上两式得： ${\mathrm{\θ}}_{i0}=arctan\left(\frac{y_i-\mathrm{\λ}w}{-z_i-h}\right)$

由(8)得： $a_0+\frac{z_i+h}{c_0}=-(a_1{c}_1+a_2{c}_{12})---\left(9\right)$

(1)整理为：x_i-δl＝-a₁s₁-a₂s₁₂  (10)

(9)(10)两式两边分别平方，并和得：

${(x_i-\mathrm{\δ}l)}^2+{(\frac{z_i+h}{c_0}+a_0)}^2={(-a_1{c}_1-a_2{c}_{12})}^2+{(-a_1{s}_1-a_2{s}_{12})}^2---\left(11\right)$

化简后得：

${(x_i-\mathrm{\δ}l)}^2+{(\frac{z_i+h}{c_0}+a_0)}^2={a_1}^2+{a_2}^2+2a_1{a}_2(s_1{s}_{12}+c_1{c}_{12})---\left(12\right)$ 由三角函数公式化简得：

${(x_i-\mathrm{\δ}l)}^2+{(\frac{z_i+h}{c_0}+a_0)}^2={a_1}^2+{a_2}^2+2a_1{a}_2{c}_2---\left(13\right)$

则：

${\mathrm{\θ}}_{i2}=\left(arccos\right(\frac{{a_1}^2+{a_2}^2-{(a_0+\frac{z_i+h}{{\mathrm{cos\θ}}_{i0}})}^2-{(x_i-\mathrm{\δ}l)}^2}{2a_1{a}_2})-\mathrm{\π}).\mathrm{\δ}---\left(14\right)$

令： $\mathrm{\ξ}=\sqrt{{(a_0+\frac{z_i+h}{{\mathrm{cos\θ}}_{i0}})}^2+{(x_i-\mathrm{\δ}l)}^2}---\left(15\right)$

则：

${\mathrm{\θ}}_{i2}=\left(arccos\right(\frac{{a_1}^2+{a_2}^2-{\mathrm{\ξ}}^2}{2a_1{a}_2})-\mathrm{\π}).\mathrm{\δ}---\left(16\right)$

求出θ_i0和θ_i2后求解θ_i1有多中方法，此处通过代入(1)求解整理后得：

${\mathrm{\θ}}_{i1}=\left(arccos\right(\frac{{a_1}^2-{a_2}^2+{\mathrm{\ξ}}^2}{2a_1\mathrm{\ξ}})-arctan\left(\frac{x_i-\mathrm{\δ}l}{-a_0-(z_i+h)/{\mathrm{cos\θ}}_{i0}}\right)).\mathrm{\δ}---\left(17\right)$

令：

则：

如果是自适应稳定性指令，通过加速度计传感器测得机器人重力加速度可以得到角度信息，与之稳定态的加速度差值通过PID算法得到机器人的偏移量，单片机驱动舵机通过调节髋关节实现重心向原来方向的反作用力，使腿部的反作用力抵消外力的干扰，机器人身体的基准是不变的，根据当前偏移基准值应用增量或者位置PID算法都可以实现自动调整，考虑到位置式PID的算法需要不断累加每次的偏移值，运算量较大，所以使用增量式PID算法。

通过超声波传感器得到当前机器人与前方物体的距离，当距离大于安全距离时机器人前行。当小于安全距离时，机器人头部舵机向左或者右摇动，检测该方向的障碍情况，当距离符合安全距离时记录该方向标识位，并以此向该方向移动。

足端运动空间

以右前腿为例，在第三关节的坐标系(X3,Y3,Z3)中足端坐标为(L3,0,0)在第二关节的坐标系(X2,Y2,Z2)中第三关节的坐标系(X3,Y3,Z3)的坐标原点是沿着x轴方向平移L2然后绕z轴旋转θ₃得到，即变换矩阵为：

${}_3^{2}T=Trans(L2,0,0)Rot(z,{\mathrm{\θ}}_3)---\left(20\right)$

同理得到

${}_2^{1}T=Rot(x,-\frac{\mathrm{\π}}{2})Trans(L1,0,0)Rot(z,{\mathrm{\θ}}_2)---\left(21\right)$

${}_1^{0}T=Trans(\frac{L}{2},\frac{W}{2},-H)Rot(y,\frac{\mathrm{\π}}{2})Rot(z,-{\mathrm{\θ}}_1)---\left(22\right)$

其中，表示由第3坐标系(X3,Y3,Z3)移动到第2坐标系(X2,Y2,Z2)的平移坐标变换，Trans表示平移变换，Rot(z,θ₃)表示绕z轴旋转θ₃角度，z表示z轴方向，θ₃表示旋转的角度值，L2表示关节膝关节到足端的连杆长度

其中，表示由第2坐标系(X2,Y2,Z2)移动到第1坐标系(X1,Y1,Z1)的平移坐标变换，表示绕x轴顺时针旋转90°；Trans(L1,0,0)表示沿x轴方向平移L1长度，Rot(z,θ₂)表示绕z轴方向旋转θ₂角度。

其中，表示由第1坐标系(X1,Y1,Z1)移动到基坐标系(X0,Y0,Z0)的平移坐标变换，表示沿x轴方向移动L/2距离，沿y轴方向移动W/2距离，沿z轴方向移动-H距离，表示绕y轴方向旋转90°，Rot(z,-θ₁)表示绕z轴方向旋转-θ₁角度。

L：机器人身体长度；W:机器人身体宽度；H：机器人身体厚度

由此得到足端轨迹坐标点在基坐标位置为：

${(x_0,y_0,z_0,1)}^T{=}_1^0{T}_2^1{T}_3^{2}T(L3,0,0,1)---\left(23\right)$

限定θ₂范围是-45°～45°，θ₃范围是0°～90°得到足端轨迹空间图，如图7.1和图7.2。

图7.2中显示了足端的轨迹空间，选取框中的(68.5，77.5，-135.5)作为右前腿的初始坐标点，以该点作为初始点留出了做够的裕量，机器人足端有较大的灵活性。

7.4足端轨迹设计

通过研究发现哺乳动物足端的轨迹基本都是圆滑的弧，如图7.3，将其分解为x轴方向和z轴方向的运动轨迹如图7.4(a)和图7.4(b)。

设定每个运动周期分成1000份，以基坐标(X0,Y0,Z0)的X,Z轴构成的面做基准确定足端轨迹，使用matlab中cftool工具采用描点拟合的方法初步拟合我们需求的轨迹曲线(仅分析前半个周期)。

X轴轨迹及参数如图7.5。Z轴轨迹及参数如图7.6。

修正后得到的设计足端轨迹方程为：

$x\left(t\right)=\left(\begin{array}{c}{x}_{init}+L*(\frac{-64}{T^3}{t}^3+\frac{48}{T^2}{t}^2+\frac{-1}{T^3}{t}^1-1)\\ t\∈(0,\frac{T}{2})\\ x_{init}+L*(-1/T*t+1)\\ t\∈(\frac{T}{2},T)\end{array}\right)---\left(24\right)$

其中，x(t)表示x轴方向随时间变化的坐标点x_init表示初始时刻的x坐标点L表示步态的长度，T表示时间周期t表示时间。

$z\left(t\right)=\left(\begin{array}{c}{z}_{init}+H*(\frac{-22.5}{T^3}{t}^3+\frac{172}{T^2}{t}^2+\frac{80}{T}t)\\ t\∈(0,\frac{T}{2})\\ z_{init}\\ t\∈(\frac{T}{2},T)\end{array}\right)---\left(25\right)$

y(t)＝y_init     (26)

其中，z(t)表示x轴方向随时间变化的坐标点z_init表示初始时刻z轴的坐标点，H表示步态的高度。L为步态的长度，最终足端轨迹如图7.7。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种四足机器人，其特征是，包括：机器人的躯干，所述机器人的躯干前端的上方通过机器人颈部连接机器人头部，所述机器人躯干的下方连接四条腿：第一、第二、第三和第四条腿；其中第一、第二条腿安装在机器人躯干的前端，第三、第四条腿安装在机器人躯干的后端；每条腿均包括从下而上依次连接的小腿、大腿、垂直杆和水平杆四个部分；所述小腿与大腿、大腿与垂直杆、垂直杆与水平杆之间均设有舵机；所述躯干上设有控制装置，控制装置通过控制舵机来控制机器人的平稳行走。

2.如权利要求1所述的一种四足机器人，其特征是，

第一条腿中，小腿与大腿之间通过第一舵机连接，大腿和垂直杆之间通过第二舵机连接，垂直杆与水平杆之间通过第三舵机连接；第二条腿中，小腿与大腿之间通过第四舵机连接，大腿和垂直杆之间通过第五舵机连接，垂直杆与水平杆之间通过第六舵机连接；

第三条腿中，小腿与大腿之间通过第七舵机连接，大腿和垂直杆之间通过第八舵机连接，垂直杆与水平杆之间通过第九舵机连接；

第四条腿中，小腿与大腿之间通过第十舵机连接，大腿和垂直杆之间通过第十一舵机连接，垂直杆与水平杆之间通过第十二舵机连接；

所述机器人颈部设有第十三舵机；所述机器人头部设有第十四舵机。

3.如权利要求2所述的一种四足机器人，其特征是，

所述第三舵机，也称水平髋关节，仿照哺乳动物髋部关节的外摆内收功能，负责机器人平行于身体方向的垂直方向运动，是机器人的转向运动时的驱动源，其旋转量用θ₀表示；

所述第二舵机，也称垂直髋关节，仿照哺乳动物的髋关节的前后收腿功能，负责机器人的平行于身体方向的运动，是整个机器人的承重关节，其旋转量用θ₁表示；

所述第一舵机，也称膝关节，所述膝关节仿照哺乳动物的膝关节的前后运动关节，负责驱动机器人腿部与地面接触，是机器人的步态轨迹的最终呈现关节，其旋转量用θ₂表示。

4.如权利要求1所述的一种四足机器人，其特征是，

所述控制装置包括：单片机STM32，所述单片机STM32分别与上述十四个舵机、加速度计传感器和超声波传感器连接，所述单片机STM32还通过蓝牙模块与上位机连接，所述单片机STM32还通过单片机MT7620与网络摄像头连接，所述单片机STM32还通过稳压电路与电源连接，所述单片机STM32还通过单片机MEGA644与OLED液晶显示屏连接。

5.如权利要求4所述的一种四足机器人，其特征是，

所述单片机STM32作为主控芯片是机器人控制的核心，通过定时器模块产生20ms定时中断作为机器人的运动周期，通过输出PWM波控制舵机转角完成机器人的步态，同时stm32通过采集加速度计反馈的值得到机器人的姿态，控制机器人的平衡，控制超声波模块的定时工作来采集机器人周围的障碍物情况，同时通过串口和蓝牙、mt7620连接来进行通信；所述电源，用于给单片机供电；所述稳压电路用于提供5v、3.3v、6v不同的电压分别给加速度计传感器、超声波传感器、arm芯片和舵机供电；所述舵机用于驱动机器人进行步态行走和头部摇动；所述网络摄像头，用于远程监控，传送机器人周围的环境信息；所述单片机MT7620，用于挂载OpenWRT系统，将摄像头的图像通过wifi发送出；所述加速度计传感器安装在机器人的机体中心位置用于采集机器人实时的姿态；所述超声波传感器，安装在机器人头部正上方用于检测机器人周围是否有障碍物；所述单片机MEGA644，用于和stm32通信得到控制信息并解码后通过液晶显示；所述OLED液晶显示屏，用于显示机器人当前信息；所述蓝牙模块用于和andorid上位机间通信，传输指令和数据；所述上位机用于发送指令和接收机器人反馈的数据。

6.一种四足机器人的步态规划方法，其特征是，包括如下步骤：

步骤(1)：机器人初始化；

步骤(2)：机器人接收上位机指令；将指令解码，通过OLED液晶显示屏显示；

步骤(3)：机器人对上位机指令进行判断并执行指令，判断是舵机调试指令、步态变换指令、方向改变指令还是其他功能指令，所述其他功能指令包括自适应稳定性、拍照指令和自动避障指令。

7.如权利要求6所述的方法，其特征是，所述步骤(3)中，

如果是舵机调试指令，则开始单个舵机调试，判断是否收到调试值，如果收到就开始步态实现，步态实现后返回步骤(2)；如果没收到就结束；

如果是步态变换指令，则开始步态实现，步态实现后返回步骤(2)；

如果是方向改变指令，则调整运动学方程，开始步态实现，步态实现后返回步骤(2)；

如果是其他功能指令，则根据指令完成稳定性调整、拍照和自动避障功能，返回步骤(2)。

8.如权利要求6所述的方法，其特征是，所述步骤(3)中，

如果收到其他功能指令，首先判断是自适应稳定性指令、拍照指令还是自动避障指令；

如果是自适应稳定性指令，则加速度计传感器采集当前机器人躯体的倾角并和稳态时比较；计算偏差；步态实现；

如果是拍照指令，则网络摄像头拍照，将照片通过单片机MT7620内嵌的OpenWRT，上传给单片机STM32，最终通过蓝牙模块上传给上位机。

9.如权利要求8所述的方法，其特征是，

如果是自动避障指令，则进入如下步骤：

步骤(3-1)：超声波传感器启动，判断是否在设定安全距离内，如果是就开始步态实现，返回步骤(3-1)；如果否则机器人头部的第十四舵机通过转动再次检测；判断是否找到无障碍方向，如果是就改变方向，开始步态实现，返回步骤(3-1)，如果否就结束。

10.如权利要求7所述的方法，其特征是，所述步态实现的具体步骤如下：

步骤(4-1)：建立DH坐标系，设定初始坐标点；

步骤(4-2)：建立运动学方程；求解逆运动学方程；

步骤(4-3)：设计运动轨迹；

步骤(4-4)：舵机关节映射；

步骤(4-5)：PWM输出控制舵机实现步态。