[发明专利]四足机器人的步态规划方法

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

θ：绕当前关节坐标系z轴旋转的角度；d:在当前关节坐标系z轴上两条相邻的公垂线之间的距离；关节1表示横向髋关节；关节2表示纵向髋关节；关节3表示膝关节；a₀表示横向髋关节和纵向髋关节之间的连杆长度；a₁表示纵向髋关节和膝关节之间的连杆长度；a₂表示膝关节关节和腿末端节之间的连杆长度；k的取值范围是1～3；d_ik的参数含义是k和k-1坐标系中z轴上两条相邻的公垂线之间的距离；θ_i(k-1)的参数含义是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)

其中：

δ=1i=0,1-1i=2,3]]>

λ=1i=0,3-1i=1,2]]>

其中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)之间的高度；

求解得到逆运动学方程：

θi0=arctan(yi-λw-zi-h)---(4)]]>

θi2=(arccos(a12+a22-ξ22a1a2)-π).δ---(6)]]>

其中：

ξ=(a0+zi+hcosθi0)2+(xi-δ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)

联立以上两式得：

由(8)得：

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

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

(xi-δl)2+(zi+hc0+a0)2=(-a1c1-a2c12)2+(-a1s1-a2s12)2---(11)]]>

化简后得：

(xi-δl)2+(zi+hc0+a0)2=a12+a22+2a1a2(s1s12+c1c12)---(12)]]>

由三角函数公式化简得：

(xi-δl)2+(zi+hc0+a0)2=a12+a22+2a1a2c2---(13)]]>

则：

θi2=(arccos(a12+a22-(a0+zi+hcosθi0)2-(xi-δl)22a1a2)-π).δ---(14)]]>

令：

则：

θi2=(arccos(a12+a22-ξ22a1a2)-π).δ---(16)]]>

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

θi1=(arccos(a12-a22+ξ22a1ξ)-arctan(xi-δl-a0-(zi+h)/cosθi0)).δ---(17)]]>

令：

则：

如果是自适应稳定性指令，通过加速度计传感器测得机器人重力加速度可以得到角度信息，与之稳定态的加速度差值通过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系统，将摄像头的图像通过wifi发送出；所述加速度计传感器安装在机器人的机体中心位置用于采集机器人实时的姿态；所述超声波传感器，安装在机器人头部正上方用于检测机器人周围是否有障碍物；所述单片机MEGA644，用于和stm32通信得到控制信息并解码后通过液晶显示；所述OLED液晶显示屏，用于显示机器人当前信息；所述蓝牙模块用于和andorid上位机间通信，传输指令和数据；所述上位机用于发送指令和接收机器人反馈的数据；

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

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

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

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

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

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

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

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