CN104192221B - 一种电驱动六足机器人运动控制系统及方法 - Google Patents

Abstract

一种电驱动六足机器人运动控制系统及方法，本发明涉及六足机器人运动控制领域，本发明要解决器人普遍存在自主灵活度不高，整体适应性差，运动控制响应速度低，对工作环境的依赖性强等缺点以及自由度繁多从而增加了控制系统的复杂性的问题，控制系统由足式模块和轮式模块组成，该系统具体是按照以下步骤进行的：1、建立六足机器人进行建模模块；2、建立坐标系运算模块；3、运动控制器控制伺服电机进行精确的位置运动；位姿运动模块运用机器人坐标变换矩阵确定机器人平台质心变化；4、实现轮式系统的前进、后退、左转和右转等步骤实现的。本发明应用于六足机器人运动控制领域。

Description

一种电驱动六足机器人运动控制系统及方法

技术领域

本发明涉及一种电驱动六足机器人运动控制系统及方法。

背景技术

电驱动六足机器人运动控制系统是足式系统的核心单元，承担着将来自操纵者的指令转化为具体的驱动单元运动的重要任务。以保证平台运动稳定性、灵活性以及操作便易性为追求目标，运动控制系统的设计重点关注了系统行走功能与操作性能的对应与互补。

目前，我国足式机器人起步较晚，机器人普遍存在自主灵活度不高，整体适应性差，运动控制响应速度低，对工作环境的依赖性强等缺点。六足机器人在结构上属于串并联混合结构，在控制方面属于多输入多输出的冗余耦合系统，六足机器人由于自由度繁多从而增加了控制系统的复杂性。

发明内容

本发明的目的是为了解决机器人普遍存在自主灵活度不高，整体适应性差，运动控制响应速度低，对工作环境的依赖性强等缺点以及自由度繁多从而增加了控制系统的复杂性的问题而提出了一种电驱动六足机器人运动控制系统及方法。

上述的发明目的是通过以下技术方案实现的：

一种电驱动六足机器人运动控制系统包括：足式模块和轮式模块；

所述的足式模块包括：六足机器人建模模块、坐标系运算模块、步态运动模块、旋转运动模块、位姿运动模块和单腿运动模块；

一种电驱动六足机器人运动控制方法具体是按照以下步骤完成的：

步骤一、建立六足机器人进行建模模块，将机器人机体简化成一个平板和将各腿及驱动电机简化为杆件的默认状态下选择机器人参数；

步骤二、根据设置的机器人参数，利用机器人腿部的正逆运动学建立坐标系运算模块；

步骤三、步态运动模块和旋转运动模块运用低冲击轨迹规划法确定步态运动的足末端轨迹和旋转运动的足末端轨迹；单腿运动模块根据腿部正逆运动学确定足末端轨迹，根据足末端轨迹利用坐标系运算模块计算得出足末端位置的笛卡尔坐标F[X_F,Y_F,Z_F]，将足末端位置的笛卡尔坐标提供给PMAC运动控制器，运动控制器控制伺服电机进行精确的位置运动；位姿运动模块运用机器人坐标变换矩阵确定机器人平台质心变化，通过各腿的同时运动使机器人平台发生平移或倾斜；

其中，步态运动模块和旋转运动模块运用低冲击轨迹规划法确定步态运动的足末端轨迹和旋转运动的足末端轨迹具体过程为：

(1)设足端x轴坐标p_x，

p_x＝S(1)

式中，S为支撑宽度，即足端与机体的横向距离；

(2)设足端y轴坐标p_y，

时单腿处于摆动相，令足端在y₀方向的位置y、速度和加速度需要满足以下约束条件：

$\left\{\begin{array}{c}\stackrel{\·\·}{y}\left(0\right)=\stackrel{\·\·}{y}\left(\frac{T}{2}\right)=0\\ \stackrel{\·}{y}\left(0\right)=\stackrel{\·}{y}\left(\frac{T}{2}\right)=0\\ y\left(0\right)=-\frac{\mathrm{\λ}}{2}\\ y\left(\frac{T}{2}\right)=\frac{\mathrm{\λ}}{2}\end{array}\right.---\left(2\right)$

式中，T为步态周期，λ为半步长；令：y₀具体为足端在y方向的初始位置；

$\stackrel{\·\·}{y}\left(t\right)=c_1\·\sin \left({\mathrm{\ω}}_{1}t\right)---\left(3\right)$

式中，c₁为常数，ω₁为角频率；式(3)两次积分有：

$\stackrel{\·}{y}\left(t\right)=-\frac{c_1}{{\mathrm{\ω}}_1}\cos \left({\mathrm{\ω}}_{1}t\right)+c_2$

$y\left(t\right)=-\frac{c_1}{{{\mathrm{\ω}}_1}^2}\sin \left({\mathrm{\ω}}_{1}t\right)+c_{2}t+c_3---\left(4\right)$

式中，c₂和c₃为常数；结合式(2)的约束条件求出c₁＝2λω₁/T，c₂＝2λ/T，c₃＝-λ/2，ω₁＝2nπ/T，n＝2,4,…，取n＝2，故单腿处于摆动相时足端坐标p_y的轨迹方程为：

$y\left(t\right)=-\frac{\mathrm{\λ}}{2\mathrm{\π}}\sin \left(\frac{4\mathrm{\π}}{T}t\right)+\frac{2\mathrm{\λ}}{T}t-\frac{\mathrm{\λ}}{2},t\∈(0,\frac{T}{2})---\left(5\right)$

时单腿处于支撑相，足端在y₀方向的运动为摆动相时的逆运动，故将式(5)中的t替换为T-t，则腿部处于支撑相时有：

$y\left(t\right)=-\frac{\mathrm{\λ}}{2\mathrm{\π}}\sin (4\mathrm{\π}-\frac{4\mathrm{\π}}{T}t)-\frac{2\mathrm{\λ}}{T}t+\frac{3\mathrm{\λ}}{2},t\∈(\frac{T}{2},T)---\left(6\right)$

(3)足端坐标p_z

时，足端从地面抬至最高点，令z、和分别为足端在z₀方向的位置、速度和加速度，需满足：

$\left\{\begin{array}{c}\stackrel{\·\·}{z}\left(0\right)=0\\ \stackrel{\·}{z}\left(0\right)=\stackrel{\·}{z}\left(\frac{T}{4}\right)=0\\ z\left(0\right)=-H\\ z\left(\frac{T}{4}\right)=h_{\max }-H\end{array}\right.---\left(7\right)$

式中，h_max为足端最大抬起高度；令：

$\stackrel{\·\·}{z}\left(t\right)=c_1^{\′}\·\sin \left({\mathrm{\ω}}_1^{\′}t\right)---\left(8\right)$

式中，c′₁为常数，ω′₁为频率；式(8)两次积分，同理求得c′₁＝4h_maxω′₁/T，c′₂＝4h_max/T，c′₃＝-H，ω′₁＝2nπ/T，n＝4,8,…；取n＝4，则足端坐标p_z的轨迹方程为：

$z\left(t\right)=-\frac{h_{\max }}{2\mathrm{\π}}\sin \left(\frac{8\mathrm{\π}}{T}t\right)+\frac{4h_{\max }}{T}t-H,t\∈(0,\frac{T}{4})---\left(9\right)$

时，足端从最高点落回地面，其在z₀方向的运动为时的逆运动，将式(9)中的t替换为T/2-t，则该阶段p_z的轨迹方程为：z₀具体为足端在z方向的初始位置；

$z\left(t\right)=-\frac{h_{\max }}{2\mathrm{\π}}\sin (4\mathrm{\π}-\frac{8\mathrm{\π}}{T}t)-\frac{4h_{\max }}{T}t+2h_{\max }-H,t\∈(\frac{T}{4},\frac{T}{2})---\left(10\right)$

时单腿处于支撑相，p_z＝-H；得到的足端轨迹曲线和单腿足端速度曲线；

步骤四、轮式运动通过设定轮式系统的运动时间、速度及转弯角速度等进行轮式模块的设定，实现轮式系统的前进、后退、左转和右转功能；从而建立六足机器人运动控制系统；即完成了一种电驱动六足机器人运动控制方法。

发明效果

通过对六足机器人运动控制系统的研究，设计了一个灵活性好，适应性强，足地接触冲击低、且能实现多自由度协调运动的六足机器人运动控制系统。该系统首先解析来自操控系统的一系列指令,结合环境感知反馈与综合分析,生成机体位姿、主运动参数和步态参数。运动规划将根据操纵系统的指令信息，规划出合理的机身位姿变化规律，通过步态生成算法细化得到机体质心运动轨迹、其足端运动轨迹如图5所示，摆动相轨迹及支撑相轨迹。由运动规划、反馈修正所形成的最终控制量全部统一为足端位移，进而依照腿部逆运动学解算为所有腿部关节角位移，其单腿三关节关节角度如图6所示，其单腿足端速度如图14所示。这种规划使得足端与关节运动平滑无冲击，足端与关节在AEP和PEP位置的加速度和速度为零，足端与关节的速度和加速度连续无突变，并最终由底层关节位置伺服控制实现运动输出。为了提高机器人在复杂环境下的运行可靠性，控制系统也为人工直接干预预留接口，便于当机器人在极端复杂环境中行走时直接操控单腿运动。

本运动控制平台是以UMAC控制器为基础，结合我们对电驱动六足机器人的一些理论研究，进一步开发的成果。该运动控制平台包含了六足机器人建模模块、坐标系运算模块、步态运动模块、旋转运动模块、位姿运动模块、单腿运动模块及轮式运动模块等7个模块组成。

结合运动控制器的特点，我们将运动控制程序分成三级，分别为主控级、职能控制级和运动执行级。由主控级来完成系统参数的初始化，启动实时位置显示，接收操控的命令并按照命令启动和停止职能控制级相应运动程序；职能控制级由完成特定功能的PLC程序组成，由主控级启动，完成运动数据的转换和组织，运动的节律控制，运动执行级程序的启动等；运动执行级为坐标系所能执行的Program运动程序，包含了各轴的运动形式，运动距离和运动时间等具体运动控制，如图1示。整个运动控制过程不依赖工作环境，在运动控制器中，有两种可用于程序运算的变量，P变量和Q变量。P变量是全局变量，可用于运动程序中和PLC程序中；Q变量是坐标系变量，用于坐标系运算程序中。

附图说明

图1是具体实施方式一提出的运动控制系统设计结构示意图；

图2是具体实施方式二提出的机器人机构示意图；

图3是具体方式五提出的机器人二步态行走示意图；其中，oi(i＝1..6)分别代表为机器人的六条腿；机器人一共六条腿，六条腿分成两组，分别为0组和1组，0组摆动相，1组对应支撑相；

图4是具体方式五提出的机器人运动相序图；其中，摆动相为机器人行走时腿端抬离地面并向前摆动的过程；支撑相为机器人行走时腿端支撑地面并推动机体向前移动的过程；

图5是具体方式四提出的足端运动轨迹示意图；

图6是具体方式四提出的单腿三关节关节角度示意图；

图7(a)是具体方式四提出的六足机器人单腿侧视图坐标系示意图；图7(b)是具体方式四提出的六足机器人单腿俯视图坐标系示意图；图7(c)是具体方式四提出的六足机器人单简化图坐标系示意图；六足机器人腿部由三部分组成：根关节、髋关节及膝关节；腿部笛卡尔坐标系如图7(a)和图7(b)所示，X轴代表机体的前进方向，Y轴代表机体的横向方向，Z轴代表机体的高度方向，H_o代表躯干初始高度，腿部的横向距离为S，α、β、γ分别为根、髋和膝关节的转角；机器人单腿机构的简化如图7(c)所示，F点为小腿末端，L₁、L₂、L₃分别为根部、大腿和小腿的尺寸；R为机器人六边形平台的半径；

图8是具体实施方式四提出的六足机器人平台全局坐标系与单腿坐标简化示意图；其中，六足机器人平台分为全局坐标系和局部坐标系：平台全局坐标系是以平台O圆心，X轴、Y轴和Z轴满足右手螺旋法则，Z为沿着重力反方向的坐标轴，X为沿着机体前进方向的坐标轴，Y为沿着机体横向的坐标轴，平台全局坐标系用于描述各个腿与平台间的位置关系；平台局部坐标系分别在六条腿上，基节位置是坐标系的原点，六条腿分别为oi(i＝1..6)，xi(i＝1..6)轴和yi(i＝1..6)轴分别是经过坐标平移和旋转得来的单腿方向坐标轴，相对于六个坐标系分别为CSi(i＝1..6)；

图9是具体实施方式六提出的六足机器人二步态旋转示意图；其中，此图的前、后极限位置：在机器人足端所能够达到的区域范围内，相对于机体向前所达到的极限位置称为前极限位置(AEP)，相对于机体向后所达到的极限位置称为后极限位置(PEP)；机器人绕X轴旋转θ角度；

图10是具体实施方式九提出的六足机器人位姿运动示意图；其中，F为足端坐标，假设机器人绕X轴旋转θ角度，六足末端位置在平台全局坐标系中的坐标为[X_i,Y_i,Z_i]，通过旋转变换矩阵，将平台全局坐标系旋转θ度，将足末端位置在新的平台坐标系中表示出来[X′_i,Y′_i,Z′_i]，在原平台全局坐标系中，将各足末端位置[X_i,Y_i,Z_i]运动到新位置[X′_i,Y′_i,Z′_i]，即机器人完成指定位姿运动；

图11是具体实施方式三提出的六足机器人足式模式主程序流程图；

图12是具体实施方式七提出的六足机器人步态程序流程图；

图13是具体实施方式七提出的单腿控制程序流程图；

图14是具体实施方式四提出的单腿足端速度曲线示意图；其中，y方向为机体前进方向，x方向为机体侧向方向，z方向为机体垂直方向；摆动相时为连续平滑的弧形，支撑相时为直线。

具体实施方式

具体实施方式一：本实施方式的一种电驱动六足机器人运动控制系统包括：足式模块和轮式模块；

所述的足式模块包括：六足机器人建模模块、坐标系运算模块、步态运动模块、旋转运动模块、位姿运动模块和单腿运动模块。

本实施方式效果：

通过对六足机器人运动控制系统的研究，设计了一个灵活性好，适应性强，足地接触冲击低、且能实现多自由度协调运动的六足机器人运动控制系统。该系统首先解析来自操控系统的一系列指令,结合环境感知反馈与综合分析,生成机体位姿、主运动参数和步态参数。运动规划将根据操纵系统的指令信息，规划出合理的机身位姿变化规律，通过步态生成算法细化得到机体质心运动轨迹、其足端运动轨迹如图5所示，摆动相轨迹及支撑相轨迹。由运动规划、反馈修正所形成的最终控制量全部统一为足端位移，进而依照腿部逆运动学解算为所有腿部关节角位移，其单腿三关节关节角度如图6所示，其单腿足端速度如图14所示。这种规划使得足端与关节运动平滑无冲击，足端与关节在AEP和PEP位置的加速度和速度为零，足端与关节的速度和加速度连续无突变，并最终由底层关节位置伺服控制实现运动输出。为了提高机器人在复杂环境下的运行可靠性，控制系统也为人工直接干预预留接口，便于当机器人在极端复杂环境中行走时直接操控单腿运动。

本运动控制平台是以UMAC控制器为基础，结合我们对电驱动六足机器人的一些理论研究，进一步开发的成果。该运动控制平台包含了六足机器人建模模块、坐标系运算模块、步态运动模块、旋转运动模块、位姿运动模块、单腿运动模块及轮式运动模块等7个模块组成。

结合运动控制器的特点，我们将运动控制程序分成三级，分别为主控级、职能控制级和运动执行级。由主控级来完成系统参数的初始化，启动实时位置显示，接收操控的命令并按照命令启动和停止职能控制级相应运动程序；职能控制级由完成特定功能的PLC程序组成，由主控级启动，完成运动数据的转换和组织，运动的节律控制，运动执行级程序的启动等；运动执行级为坐标系所能执行的Program运动程序，包含了各轴的运动形式，运动距离和运动时间等具体运动控制，如图1示。整个运动控制过程不依赖工作环境，在运动控制器中，有两种可用于程序运算的变量，P变量和Q变量。P变量是全局变量，可用于运动程序中和PLC程序中；Q变量是坐标系变量，用于坐标系运算程序中。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述的六足机器人建模模块用于直观的看到机器人构型，计算机器人各条腿的关节角度以及机器人腿部运动学解算；机器人机构示意图如图2所示；其中，六足机器人腿部由跟关节、髋关节及膝关节三部分组成；

所述的足式模块用于包括二步态行走、三步态行走、六步态行走、越沟态行走、爬坡态行走、越障态行走、原地旋转、姿态调整、单腿控制、六足站立以及六足收起功能；

所述的坐标系运算模块用于表示运动关系的一组运动系，被控制的各关节定义到坐标系的轴上，在运动程序中执行运动，其中，坐标系运算是运控系统的重要组成部分；

所述的步态运动模块用于解决支撑相和摆动相的时序问题，结合低冲击足端轨迹规划方法，用于降低机器人摆动相的足地接触冲击，以及支撑运动和摆动运动的实现问题；

所述的旋转运动模块用于通过平台坐标系旋转的方式完成了机器人绕坐标轴的旋转运动，这里我们通过姿态和步态融合的方式形成旋转步态规划；

所述的位姿运动模块用于机器人完成指定位姿运动；

所述的单腿运动模块用于单独对机器人足端和关节进行控制，使其实现机器人的全局坐标系和局部坐标系下的运动模式；

所述的轮式模块用于实现轮式系统的前进、后退、左转、右转等功能即上位机输入电机的参数来控制驱动轮的转速，从而达到机器人的轮式前行。其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述的足式模块主程序的流程为如图11所示：

(1)系统对PLC显示程序和初始化程序进行参数初始化；

(2)选择启动原地转向程序、六足站立收起程序、爬坡、越障、越沟程序及其自动和手动程序，运动控制程序根据操控系统的用户需求进行下位机程序的执行；其中，运动控制程序包括原地转向程序、六足站立收起程序、爬坡、越障、越沟程序及其自动和手动程序。其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：一种电驱动六足机器人运动控制方法具体是按照以下步骤完成的：

步骤一、建立六足机器人进行建模模块，在机器人建模之前，我们对机器人的各部分运动进行了分析，为了减少不必要的运算以及碰撞检测，对机器人作了简化，将机器人机体简化成一个平板和将各腿及驱动电机简化为杆件的默认状态下选择机器人参数如图7(a)～图7(c)和表2所示；

表2机器人机构参数

步骤二、根据机器人腿部结构分析即设置的机器人参数，如图7(a)～图7(c)所示，通过机器人腿部的正逆运动学建立坐标系运算模块；

步骤三、通过低冲击轨迹规划法并融合腿部正逆运动学计算出落足点间的轨迹，步态运动模块和旋转运动模块运用低冲击轨迹规划法确定步态运动的足末端轨迹和旋转运动的足末端轨迹；单腿运动模块根据腿部正逆运动学确定足末端轨迹，根据足末端轨迹利用坐标系运算模块计算得出足末端位置的笛卡尔坐标F[X_F,Y_F,Z_F]如图8，将足末端位置的笛卡尔坐标提供给PMAC运动控制器，运动控制器控制伺服电机进行精确的位置运动；位姿运动模块运用机器人坐标变换矩阵确定机器人平台质心变化，通过各腿的同时运动使机器人平台发生平移或倾斜；

其中，低冲击轨迹规划法建立步态运动模块在宏观上规划了六条腿的运动顺序，具体运动方式靠单腿连贯动作来实现；单腿摆动相时单腿前摆，单腿足端由后极限位置PEP运动到前极限位置AEP；支撑相时单腿后摆，足端由AEP位置回到PEP位置；足端由AEP位置回到PEP位置的两落足点间的轨迹曲线无穷多，无论采用何种轨迹，须满足：①足端与关节运动平滑无冲击；②足端与关节在AEP和PEP位置的加速度和速度为零；③足端与关节的速度和加速度连续无突变；为减小足地接触冲击，计算两落足点间的轨迹方法为低冲击足端轨迹规划方法：

所述的步态运动模块和旋转运动模块运用低冲击轨迹规划法确定步态运动的足末端轨迹和旋转运动的足末端轨迹具体过程为：

(1)设足端x轴坐标p_x，

p_x＝S(1)

式中，S为支撑宽度，即足端与机体的横向距离；

(2)设足端y轴坐标p_y，

时单腿处于摆动相，令足端在y₀方向的位置y、速度和加速度需要满足以下约束条件：

$\left\{\begin{array}{c}\stackrel{\·\·}{y}\left(0\right)=\stackrel{\·\·}{y}\left(\frac{T}{2}\right)=0\\ \stackrel{\·}{y}\left(0\right)=\stackrel{\·}{y}\left(\frac{T}{2}\right)=0\\ y\left(0\right)=-\frac{\mathrm{\λ}}{2}\\ y\left(\frac{T}{2}\right)=\frac{\mathrm{\λ}}{2}\end{array}\right.---\left(2\right)$

式中，T为步态周期，λ为半步长；令：y₀具体为足端在y方向的初始位置

$\stackrel{\·\·}{y}\left(t\right)=c_1\·\sin \left({\mathrm{\ω}}_{1}t\right)---\left(3\right)$

式中，c₁为常数，ω₁为角频率；式(3)两次积分有：

$\stackrel{\·}{y}\left(t\right)=-\frac{c_1}{{\mathrm{\ω}}_1}\cos \left({\mathrm{\ω}}_{1}t\right)+c_2$

$y\left(t\right)=-\frac{c_1}{{{\mathrm{\ω}}_1}^2}\sin \left({\mathrm{\ω}}_{1}t\right)+c_{2}t+c_3---\left(4\right)$

式中，c₂和c₃为常数；结合式(2)的约束条件求出c₁＝2λω₁/T，c₂＝2λ/T，c₃＝-λ/2，ω₁＝2nπ/T，n＝2,4,…；为避免足端频繁加减速，取n＝2，故单腿处于摆动相时足端坐标p_y的轨迹方程为：

$y\left(t\right)=-\frac{\mathrm{\λ}}{2\mathrm{\π}}\sin \left(\frac{4\mathrm{\π}}{T}t\right)+\frac{2\mathrm{\λ}}{T}t-\frac{\mathrm{\λ}}{2},t\∈(0,\frac{T}{2})---\left(5\right)$

时单腿处于支撑相，足端在y₀方向的运动为摆动相时的逆运动，故将式(5)中的t替换为T-t，则腿部处于支撑相时有：

$y\left(t\right)=-\frac{\mathrm{\λ}}{2\mathrm{\π}}\sin (4\mathrm{\π}-\frac{4\mathrm{\π}}{T}t)-\frac{2\mathrm{\λ}}{T}t+\frac{3\mathrm{\λ}}{2},t\∈(\frac{T}{2},T)---\left(6\right)$

(3)足端坐标p_z

时，足端从地面抬至最高点，令z、和分别为足端在z₀方向的位置、速度和加速度，需满足：

$\left\{\begin{array}{c}\stackrel{\·\·}{z}\left(0\right)=0\\ \stackrel{\·}{z}\left(0\right)=\stackrel{\·}{z}\left(\frac{T}{4}\right)=0\\ z\left(0\right)=-H\\ z\left(\frac{T}{4}\right)=h_{\max }-H\end{array}\right.---\left(7\right)$

式中，h_max为足端最大抬起高度；令：

$\stackrel{\·\·}{z}\left(t\right)=c_1^{\′}\·\sin \left({\mathrm{\ω}}_1^{\′}t\right)---\left(8\right)$

式中，c′₁为常数，ω′₁为频率；式(8)两次积分，同理可求得c′₁＝4h_maxω′₁/T，c′₂＝4h_max/T，c′₃＝-H，ω′₁＝2nπ/T，n＝4,8,…；取n＝4，则足端坐标p_z的轨迹方程为：

$z\left(t\right)=-\frac{h_{\max }}{2\mathrm{\π}}\sin \left(\frac{8\mathrm{\π}}{T}t\right)+\frac{4h_{\max }}{T}t-H,t\∈(0,\frac{T}{4})---\left(9\right)$

时，足端从最高点落回地面，其在z₀方向的运动为时的逆运动，将式(9)中的t替换为T/2-t，则该阶段p_z的轨迹方程为：z₀具体为为足端在z方向的初始位置；

$z\left(t\right)=-\frac{h_{\max }}{2\mathrm{\π}}\sin (4\mathrm{\π}-\frac{8\mathrm{\π}}{T}t)-\frac{4h_{\max }}{T}t+2h_{\max }-H,t\∈(\frac{T}{4},\frac{T}{2})---\left(10\right)$

其中，时单腿处于支撑相，p_z＝-H；

得到的足端轨迹曲线如图5所示和单腿足端速度曲线如图14；

机器人足端运动轨迹和单腿三关节角度曲线和可选参数表如图6和表4所示；

表4步态运动参数表

步骤四、轮式运动通过设定轮式系统的运动时间、速度及转弯角速度等进行轮式模块的设定，实现轮式系统的前进、后退、左转、右转等功能；轮式运动参数如表5所示：

表5轮式运动参数

；从而建立六足机器人运动控制系统；即完成了一种电驱动六足机器人运动控制方法。其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：步骤三中步态运动模块运用低冲击轨迹规划法确定步态运动的足末端轨迹具体为：

(1)机器人在足式移动阶段采用六足移动方式，六足六条腿对应18个自由度；其中，一条腿上有三个关节，三个关节对应三个自由度，六足六条腿对应18个自由度；

(2)系统进行初始化参数，选择启动原地转向程序，六足站立收起程序，爬坡，越障，越沟程序和运动控制程序，并根据操控系统的用户需求进行下位机程序的执行；足式模式主程序流程图如图11所示；机体行进时18个自由度协调运动，达到指定的运动轨迹；其中，下位机程序的执行功能程序包括：二步态子程序、三步态子程序、六步态子程序、爬坡态子程序、越沟态子程序、越障态子程序、原地旋转子程序、姿态调整子程序、单腿控制子程序、六足站立子程序和六足收起子程序，六足机器人的步态运动是通过腿部持续的摆动或支撑交替运动实现的，为了满足机器人直线稳定运动，要求各支撑相在支撑运动中，运动方向和运动距离保持一致；步态规划主要解决支撑相和摆动相的时序问题，以及支撑运动和摆动运动的实现问题；例如机器人二步态行走示意图如图3与运动相序和图4所示。其它步骤及参数与具体实施方式一至四之一相同。

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：步骤三中旋转运动模块运用低冲击轨迹规划法确定旋转运动的足末端轨迹具体为：

(1)旋转运动是六足机器人以机器平台为中心，通过步态运动使平台在水平方向旋转；旋转步态和行进步态节律性相同，采用行进步态的分组和节律控制方法；

(2)机器人在旋转运动中，要求所有的支撑足在整个支撑过程中相对位置保持不变，否则机器人平台将受到腿部之间产生的巨大应力，有可能损坏机器人机械结构；在姿态运动中，我们通过平台坐标系旋转的方式完成了机器人绕坐标轴的旋转运动，这里我们通过姿态和步态融合的方式形成旋转步态规划；六足机器人二步态旋转运动如图9所示；

(3)摆动相的运动和步态的相同，即从当前点摆动到AEP位置；其中，AEP位置的求解方法是，将足末端基准位点通过坐标系反向旋转变换后的新坐标点作为AEP点；

旋转运动是六足机器人以平台为中心，通过步态运动使平台在水平方向旋转；旋转步态和行进步态节律性相同，因此采用行进步态的分组和节律控制方法；机器人在旋转运动中，要求所有的支撑足在整个支撑过程中相对位置保持不变，否则机器人平台将受到腿部之间产生的巨大应力，有可能损坏机器人机械结构；在姿态运动中，我们通过平台坐标系旋转的方式完成了机器人绕坐标轴的旋转运动，这里我们通过姿态和步态融合的方式形成旋转步态规划；六足机器人二步态旋转运动如图9所示；

摆动相的运动和步态的相同，即从当前点摆动到AEP位置；AEP位置的求解方法是，将足末端基准位点通过坐标系反向旋转变换后的新坐标点作为AEP点；支撑相运动采用类似位姿规划的方法，将足末端点从当前位置沿到平台坐标系中心半径相等的圆弧运动，直到平台旋转了设定的角度；

设第i＝1，2，3，4，5，6腿当前足端点N坐标为(X_in,Y_in,Z_in)，足末端基准位点O为(X_io,Y_io,Z_io)，则腿i的AEP位置坐标(X_iAEP,Y_iAEP,Z_iAEP)求解如公式(3)所示：

$\left[\begin{array}{c}{X}_{\mathrm{iAEP}}\\ Y_{\mathrm{iAEP}}\\ Z_{\mathrm{iAEP}}\end{array}\right]=\mathrm{rot}{(z,-\mathrm{\θ})}^{-1}\left[\begin{array}{c}{X}_{\mathrm{io}}\\ Y_{\mathrm{io}}\\ Z_{\mathrm{io}}\end{array}\right]---\left(3\right)$

采用这种坐标变换的方法，结合步态运动规划模块即解算出机器人摆动相和支撑相的轨迹规划。其它步骤及参数与具体实施方式一至五之一相同。

具体实施方式七：本实施方式与具体实施方式一至六之一不同的是：步骤三中机器人单腿运动模块为腿坐标系运动，对机器人足端和关节进行控制：使其实现机器人的全局坐标系和局部坐标系下的运动模式；单腿运控参数如表5所示：

表5单腿运动参数

(1)当操纵系统执行步态运动模块程序时，步态运动模块程序将根据操控系统软件设置的参数值生成控制指令，并根据控制器的控制指令来完成机器人步态行走过程；

其中所述的机器人步态程序流程为：a系统进行初始化步态参数，b执行步态参数设定，c如果设定失败，返回到步态参数阶段，d如果设定成功则生成步态参数，e进行起步规划判断，进而执行起步规划程序，其步态程序流程图如图12所示，设定成功具体为点击步态运动六足机器人可以运动即为设定成功；

(2)当操纵系统执行单腿运动模块程序时，步态程序通过PMAC运动控制器生成控制指令来完成机器人单腿控制；

其中单腿控制流程为：a系统进行初始化单腿控制参数；b执行单腿控制参数设定，如果设定成功则生成位姿调整参数；c选择在全局坐标系下运动或局部坐标系下运动，当选择之后执行坐标系程序，其单腿控制流程图如图13所示；设定成功为点击单腿运动中的关节模式或足端模式六足机器人可以运动即为设定成功；

结合运动控制器的特点，我们将运动控制程序分成三级，分别为主控级、职能控制级和运动执行级；由主控级来完成系统参数的初始化，启动实时位置显示，接收操控的命令并按照命令启动和停止职能控制级相应运动程序；职能控制级由完成特定功能的PLC程序组成，由主控级启动，完成运动数据的转换和组织，运动的节律控制，运动执行级程序的启动等；运动执行级为坐标系所能执行的Program运动程序，包含了各轴的运动形式，运动距离和运动时间等具体运动控制，如图1示；在运动控制器中，有两种可用于程序运算的变量，P变量和Q变量；P变量是全局变量，可用于运动程序中和PLC程序中；Q变量是坐标系变量，用于坐标系运算程序中；运动控制系统的程序架构参数如下表所示：

。其它步骤及参数与具体实施方式一至六之一相同。

具体实施方式八：本实施方式与具体实施方式一至七之一不同的是：步骤三中利用坐标系运算模块计算得出足末端位置的笛卡尔坐标F[X_F,Y_F,Z_F]具体过程为：

(1)PMAC(ProgrammableMulti-AxisController可编程多轴运动控制器)运动控制器最多支持有16个坐标系；

(2)六足机器人每条腿有三个关节，每个关节的运动都关系到足末端位置，将这三个关节定义到同一坐标系的三条轴上，则坐标系中足末端位置的笛卡尔坐标F[X_F,Y_F,Z_F]，这样只需着重考虑坐标规划(足末端轨迹规划)，不需要过多关注关节角度；其中，坐标系是有运动关系的一组运动系，被控制的电机定义到坐标系的轴上，在运动程序中执行运动；坐标系运算是运控系统的重要组成部分；坐标系运算参数如表3所示；

表3坐标系运算参数

(3)假设已知单腿关节转角α、β和γ的角度，根据单腿关节转角求解出机器人腿部的运动学正解一种空间碎片减缓措施评估及费效评估系统为公式所示：

$\left\{\begin{array}{c}{X}_F=(L_1+L_2\cos \mathrm{\β}+L_3\cos (\mathrm{\γ}-\mathrm{\β}))\sin \mathrm{\α}\\ Y_F=(L_1+L_2\cos \mathrm{\β}+L_3\cos (\mathrm{\γ}-\mathrm{\β}))\cos \mathrm{\α}\\ Z_F=H+L_2\sin \mathrm{\β}-L_3\sin (\mathrm{\γ}-\mathrm{\β})\end{array}\right.,$

由正运动学方程及三角形关系，求出机器人腿部的运动学逆解为：

$\left\{\begin{array}{c}\mathrm{\α}={\tan }^{-1}\left(\frac{X_F}{Y_F}\right)\\ \mathrm{\β}={\tan }^{-1}\frac{K}{H-Z_F}-{\sin }^{-1}(\frac{\sqrt{{(H-Z_F)}^2+K^2}}{2L_2}-\frac{L_3^2-L_2^2}{2L_2\sqrt{{(H-Z_F)}^2+K^2}})\\ \mathrm{\γ}=\mathrm{\π}-{\sin }^{-1}(\frac{\sqrt{{(H-Z_F)}^2+K^2}}{2L_3}+\frac{L_3^2-L_2^2}{2L_3\sqrt{{(H-Z_F)}^2+K^2}})\\ -{\sin }^{-1}(\frac{\sqrt{{(H-Z_F)}^2+K^2}}{2L_2}-\frac{L_3^2-L_2^2}{2L_2\sqrt{{(H-Z_F)}^2+K^2}}),(K=\frac{Y_F}{\cos \mathrm{\α}}-L_1)\end{array}\right.,$ 从而求解出足末端坐标为F[X_F,Y_F,Z_F]，其中，L₁、L₂、L₃分别为根部、大腿和小腿的尺寸。其它步骤及参数与具体实施方式一至七之一相同。

具体实施方式九：本实施方式与具体实施方式一至八之一不同的是：步骤三中位姿运动模块运用机器人坐标变换矩阵确定机器人平台质心变化具体为：

(1)假设机器人绕X轴旋转θ度，六足末端位置在平台全局坐标系中的坐标为(X_i,Y_i,Z_i)，通过旋转变换矩阵，将平台全局坐标系旋转θ度，将足末端位置在新的平台坐标系中表示出来(X′_i,Y′_i,Z′_i)，在原平台全局坐标系中，将各足末端位置(X_i,Y_i,Z_i)运动到新位置(X′_i,Y′_i,Z′_i)，即机器人完成指定位姿运动；机器人位姿运动如图10所示；

(2)假定机器人绕X轴旋转θ角度，在X轴坐标为x，则新平台坐标系相对于原平台坐标系旋转θ角度，变换矩阵T和T^-1如公式(4)及(5)所示：

$T=\mathrm{rot}(x,\mathrm{\θ})=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\\ 0& \sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]---\left(4\right)$

$T^{-1}=\mathrm{rot}{(x,\mathrm{\θ})}^{-1}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ 0& -\sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]---\left(5\right)$

通过变换矩阵，足末端新位置(X′_i,Y′_i,Z′_i)的表示如公式(6)所示：

$\left[\begin{array}{c}{X}_i^{\′}\\ Y_i^{\′}\\ Z_i^{\′}\end{array}\right]=T^{-1}\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right]=\left[\begin{array}{c}{X}_i\\ Y_i\cos \mathrm{\θ}+Z_i\sin \mathrm{\θ}\\ -Y_i\sin \mathrm{\θ}+Z_i\cos \mathrm{\θ}\end{array}\right]---\left(6\right).$ 其它步骤及参数与具体实施方式一至八之一相同。

Claims (8)

1.一种电驱动六足机器人运动控制系统，其特征在于：一种电驱动六足机器人运动控制系统包括：足式模块和轮式模块；

所述的足式模块包括：六足机器人建模模块、坐标系运算模块、步态运动模块、旋转运动模块、位姿运动模块和单腿运动模块；

所述的六足机器人建模模块用于直观的看到机器人构型，计算机器人各条腿的关节角度以及机器人腿部运动学解算；其中，六足机器人腿部由跟关节、髋关节及膝关节三部分组成；

所述的足式模块程序用于包括二步态行走、三步态行走、六步态行走、越沟态行走、爬坡态行走、越障态行走、原地旋转、姿态调整、单腿控制、六足站立以及六足收起功能；

所述的坐标系运算模块用于表示运动关系的一组运动系，被控制的各关节定义到坐标系的轴上，在运动程序中执行运动；

所述的步态运动模块用于解决支撑相和摆动相的时序问题，结合低冲击足端轨迹规划方法，用于降低机器人摆动相的足地接触冲击以及支撑运动和摆动运动的实现问题；

所述的旋转运动模块用于通过姿态和步态融合的方式形成旋转步态规划；

所述的位姿运动模块用于机器人完成指定位姿运动；

所述的单腿运动模块用于单独对机器人足端和关节进行控制，使其实现机器人的全局坐标系和局部坐标系下的运动模式；

所述的轮式模块用于实现轮式系统的前进、后退、左转、右转功能即上位机输入电机的参数来控制驱动轮的转速，从而达到机器人的轮式前行。

2.根据权利要求1所述一种电驱动六足机器人运动控制系统，其特征在于：所述的足式模块主程序的流程为：

(1)系统对PLC显示程序和初始化程序进行参数初始化；

(2)选择启动原地转向程序、六足站立收起程序、爬坡、越障、越沟程序及其自动和手动程序，运动控制程序根据操控系统的用户需求进行下位机程序的执行；其中，运动控制程序包括原地转向程序、六足站立收起程序、爬坡、越障、越沟程序及其自动和手动程序。

3.一种电驱动六足机器人运动控制方法，其特征在于：一种电驱动六足机器人运动控制方法具体是按照以下步骤完成的：

步骤一、建立六足机器人建模模块，将机器人机体简化成一个平板和将各腿及驱动电机简化为杆件的默认状态下选择机器人参数；

步骤二、根据设置的机器人参数，利用机器人腿部的正逆运动学建立坐标系运算模块；

步骤三、步态运动模块和旋转运动模块运用低冲击轨迹规划法确定步态运动的足末端轨迹和旋转运动的足末端轨迹；单腿运动模块根据腿部正逆运动学确定足末端轨迹，根据足末端轨迹利用坐标系运算模块计算得出足末端位置的笛卡尔坐标F[X_F,Y_F,Z_F]，将足末端位置的笛卡尔坐标提供给PMAC运动控制器，运动控制器控制伺服电机进行精确的位置运动；位姿运动模块运用机器人坐标变换矩阵确定机器人平台质心变化，通过各腿的同时运动使机器人平台发生平移或倾斜；

其中，步态运动模块和旋转运动模块运用低冲击轨迹规划法确定步态运动的足末端轨迹和旋转运动的足末端轨迹，具体过程为：

(1)设足端x轴坐标p_x，

p_x＝S(1)

式中，S为支撑宽度，即足端与机体的横向距离；

(2)设足端y轴坐标p_y，

时单腿处于摆动相，令足端在y₀方向的位置y、速度和加速度需要满足以下约束条件：

$\left\{\begin{array}{c}\stackrel{\·\·}{y}\left(0\right)=\stackrel{\·\·}{y}\left(\frac{T}{2}\right)=0\\ \stackrel{\·}{y}\left(0\right)=\stackrel{\·}{y}\left(\frac{T}{2}\right)=0\\ y\left(0\right)=-\frac{\mathrm{\λ}}{2}\\ y\left(\frac{T}{2}\right)=\frac{\mathrm{\λ}}{2}\end{array}\right.---\left(2\right)$

式中，T为步态周期，λ为半步长；令：y₀具体为足端在y方向的初始位置；

$\stackrel{\·\·}{y}\left(t\right)=c_1\·sin\left({\mathrm{\ω}}_{1}t\right)---\left(3\right)$

式中，c₁为常数，ω₁为角频率；式(3)两次积分有：

$\stackrel{\·}{y}\left(t\right)=-\frac{c_1}{{\mathrm{\ω}}_1}cos\left({\mathrm{\ω}}_{1}t\right)+c_2$

$y\left(t\right)=-\frac{c_1}{{{\mathrm{\ω}}_1}^2}sin\left({\mathrm{\ω}}_{1}t\right)+c_{2}t+c_3---\left(4\right)$

式中，c₂和c₃为常数；结合式(2)的约束条件求出c₁＝2λω₁/T，c₂＝2λ/T，c₃＝-λ/2，ω₁＝2nπ/T，n＝2,4,…，取n＝2，故单腿处于摆动相时足端坐标p_y的轨迹方程为：

$\begin{array}{cc}y\left(t\right)=-\frac{\mathrm{\λ}}{2\mathrm{\π}}\sin \left(\frac{4\mathrm{\π}}{T}t\right)+\frac{2\mathrm{\λ}}{T}t-\frac{\mathrm{\λ}}{2}& t\∈(0,\frac{T}{2})\end{array}---\left(5\right)$

时单腿处于支撑相，足端在y₀方向的运动为摆动相时的逆运动，故将式(5)中的t替换为T-t，则腿部处于支撑相时有：

$\begin{array}{cc}y\left(t\right)=-\frac{\mathrm{\λ}}{2\mathrm{\π}}sin(4\mathrm{\π}-\frac{4\mathrm{\π}}{T}t)-\frac{2\mathrm{\λ}}{T}t+\frac{3\mathrm{\λ}}{2}& t\∈(\frac{T}{2},T)\end{array}---\left(6\right)$

(3)足端坐标p_z

时，足端从地面抬至最高点，z₀具体为足端在z方向的初始位置；令z、和分别为足端在z₀方向的位置、速度和加速度，需满足：

$\left\{\begin{array}{c}\stackrel{\·\·}{z}\left(0\right)=0\\ \stackrel{\·}{z}\left(0\right)=\stackrel{\·}{z}\left(\frac{T}{4}\right)=0\\ z\left(0\right)=-H\\ z\left(\frac{T}{4}\right)=h_{\max }-H\end{array}\right.---\left(7\right)$

式中，h_max为足端最大抬起高度，令：

$\stackrel{\·\·}{z}\left(t\right)=c_1^{\′}\·sin\left({\mathrm{\ω}}_1^{\′}t\right)---\left(8\right)$

式中，c′₁为常数，ω′₁为频率；式(8)两次积分，同理求得c′₁＝4h_maxω′₁/T，c′₂＝4h_max/T，c′₃＝-H，ω′₁＝2nπ/T，n＝4,8,…；取n＝4，则足端坐标p_z的轨迹方程为：

$\begin{array}{cc}z\left(t\right)=-\frac{h_{max}}{2\mathrm{\π}}sin\left(\frac{8\mathrm{\π}}{T}t\right)+\frac{4h_{max}}{T}t-H& t\∈(0,\frac{T}{4})\end{array}---\left(9\right)$

时，足端从最高点落回地面，其在z₀方向的运动为时的逆运动，将式(9)中的t替换为T/2-t，则该阶段p_z的轨迹方程为：

$\begin{array}{cc}z\left(t\right)=-\frac{h_{\max }}{2\mathrm{\π}}sin(4\mathrm{\π}-\frac{8\mathrm{\π}}{T}t)-\frac{4h_{\max }}{T}t+2h_{max}-H& t\∈(\frac{T}{4},\frac{T}{2})\end{array}---\left(10\right)$

时单腿处于支撑相，p_z＝-H；

步骤四、轮式运动通过设定轮式模块的运动时间、速度及转弯角速度进行轮式模块的设定，实现轮式系统的前进、后退、左转和右转功能；从而建立六足机器人运动控制系统；即完成了一种电驱动六足机器人运动控制方法。

4.根据权利要求3所述一种电驱动六足机器人运动控制方法，其特征在于：步骤三中步态运动模块运用低冲击轨迹规划法确定步态运动的足末端轨迹，具体为：

(1)机器人在足式移动阶段采用六足移动方式；其中，一条腿上有三个关节，三个关节对应三个自由度，六足六条腿对应十八个自由度；

(2)系统进行初始化参数，选择启动原地转向程序，六足站立收起程序，爬坡、越障和越沟程序运动控制程序，并根据操控系统的用户需求进行下位机程序的执行；从而机体行进时十八个自由度协调运动，达到指定的运动轨迹；其中，下位机程序的执行功能程序包括：二步态子程序、三步态子程序、六步态子程序、爬坡态子程序、越沟态子程序、越障态子程序、原地旋转子程序、姿态调整子程序、单腿控制子程序、六足站立子程序和六足收起子程序。

5.根据权利要求4所述一种电驱动六足机器人运动控制方法，其特征在于：步骤三中旋转运动模块运用低冲击轨迹规划法确定旋转运动的足末端轨迹具体为：

(1)旋转运动是六足机器人以机器平台为中心，通过步态运动使平台在水平方向旋转；旋转步态和行进步态节律性相同；

(2)机器人在旋转运动中，要求所有的支撑足在整个支撑过程中相对位置保持不变，在姿态运动中，通过平台坐标系旋转的方式完成了机器人绕坐标轴的旋转运动，通过姿态和步态融合的方式形成旋转步态规划；

(3)摆动相的运动和步态的相同，即从当前足端点N坐标为(X_in,Y_in,Z_in)摆动到AEP位置；其中，AEP位置的求解方法是，将足末端基准位点通过坐标系反向旋转变换后的新坐标点作为AEP点；

在机器人足端所能够达到的区域范围内，相对于机体向前所达到的极限位置称为前极限位置即AEP位置；

设第i＝1，2，3，4，5，6腿当前足端点N坐标为(X_in,Y_in,Z_in)，足末端基准位点O为(X_io,Y_io,Z_io)，则腿i的AEP位置坐标(X_iAEP,Y_iAEP,Z_iAEP)求解如公式(11)所示：

$\left[\begin{array}{c}{X}_{iAEP}\\ Y_{iAEP}\\ Z_{iAEP}\end{array}\right]=rot{(z,-\mathrm{\θ})}^{-1}\left[\begin{array}{c}{X}_{io}\\ Y_{io}\\ Z_{io}\end{array}\right]---\left(11\right)$

采用这种坐标变换的方法，结合步态运动模块即解算出机器人摆动相和支撑相的轨迹规划。

6.根据权利要求5所述一种电驱动六足机器人运动控制方法，其特征在于：步骤三中机器人单腿运动模块为腿坐标系运动，对机器人足端和关节进行控制：

(1)当操控系统执行步态运动模块程序时，步态运动模块程序将根据操控系统软件设置的参数值生成控制指令，并根据控制器的控制指令来完成机器人步态行走过程；

其中所述的机器人步态程序流程为：a操控系统进行初始化步态参数，b执行步态参数设定，c如果设定失败，返回到步态参数阶段a，d如果设定成功则生成步态参数，e进行起步规划判断，进而执行起步规划程序；

(2)当操控系统执行单腿运动模块程序时，步态程序通过PMAC运动控制器生成控制指令来完成机器人单腿控制；

其中单腿控制流程为：a操控系统进行初始化单腿控制参数；b执行单腿控制参数设定，如果设定成功则生成位姿调整参数；c选择在全局坐标系下运动或局部坐标系下运动，当选择之后执行坐标系程序；单腿运动中的关节模式或足端模式即为设定成功。

7.根据权利要求6所述一种电驱动六足机器人运动控制方法，其特征在于：步骤三中利用坐标系运算模块计算得出足末端位置的笛卡尔坐标F[X_F,Y_F,Z_F]，具体过程为：

(1)PMAC运动控制器最多支持有16个坐标系；

(2)六足机器人每条腿有三个关节，每个关节的运动都关系到足末端位置，将这三个关节定义到同一坐标系的三条轴上，则坐标系中足末端位置的笛卡尔坐标F[X_F,Y_F,Z_F]，其中，坐标系是有运动关系的一组运动系，被控制的电机定义到坐标系的轴上，在运动程序中执行运动；

(3)假设已知单腿关节转角α、β和γ的角度，根据单腿关节转角求解出机器人腿部的运动学正解为公式所示：

$\left\{\begin{array}{c}{X}_F=(L_1+L_{2}cos\mathrm{\β}+L_{3}cos(\mathrm{\γ}-\mathrm{\β}\left)\right)sin\mathrm{\α}\\ Y_F=(L_1+L_2\cos \mathrm{\β}+L_3\cos (\mathrm{\γ}-\mathrm{\β}\left)\right)\cos \mathrm{\α}\\ Z_F=H+L_2\sin \mathrm{\β}-L\sin (\mathrm{\γ}-\mathrm{\β})\end{array}\right.,$

由正运动学方程及三角形关系，求出机器人腿部的运动学逆解为：

从而求解出足末端坐标为F[X_F,Y_F,Z_F]，其中，L₁、L₂、L₃分别为根部、大腿和小腿的尺寸。

8.根据权利要求7所述一种电驱动六足机器人运动控制方法，其特征在于：步骤三中位姿运动模块运用机器人坐标变换矩阵确定机器人平台质心变化，具体为：

(1)假设机器人绕X轴旋转θ角度，六足末端位置在平台全局坐标系中的坐标为(X_i，Y_i，Z_i)，通过旋转变换矩阵，将平台全局坐标系旋转θ角度，将足末端位置在新的平台坐标系中表示出来(X′_i，Y′_i，Z′_i)，在原平台全局坐标系中，将各足末端位置(X_i,Y_i,Z_i)运动到新位置(X′_i，Y′_i，Z′_i)；

(2)假定机器人在X轴坐标为x，则新平台坐标系相对于原平台坐标系旋转θ角度，变换矩阵T和T^-1如公式(12)及(13)所示：

$T=rot(x,\mathrm{\θ})=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\mathrm{\θ}& -\sin \mathrm{\θ}\\ 0& sin\mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]---\left(12\right)$

$T^{-1}=rot{(x,\mathrm{\θ})}^{-1}=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\mathrm{\θ}& sin\mathrm{\θ}\\ 0& -\sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]---\left(13\right)$

通过变换矩阵，足末端新位置(X′_i，Y′_i，Z′_i)的表示如公式(14)所示：

$\left[\begin{array}{c}{X}_i^{\′}\\ Y_i^{\′}\\ Z_i^{\′}\end{array}\right]=T^{-1}\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right]=\left[\begin{array}{c}{X}_i\\ Y_i\cos \mathrm{\θ}+Z_i\sin \mathrm{\θ}\\ -Y_i\sin \mathrm{\θ}+Z_i\cos \mathrm{\θ}\end{array}\right]---\left(14\right).$