专利名称：仿人机器人足部姿态实时检测系统和方法
技术领域：
本发明涉及自动化领域，特别涉及一种仿人机器人足部姿态实时检测系统和方法。
技术背景仿人机器人是与人类最接近的一种机器人，它与传统的轮式机器人或履带机器人相比，更能适应人类F1常生活环境，同时更便于使用为人类发明设计的各种工具，因此，仿人机器 人具有广阔的发展前景。目前，对于仿人机器人的研究仍主要集中在探索更加有效、可靠的稳定性控制方法，在 这些研究中，机器人的足部作为行走过程中与外界唯一接触和受力的部位，就成为解决仿人 机器人稳定行走的关键所在。在一些比较复杂的环境下，比如在不平整路面上步行、转弯、 上下楼梯时，其足部姿态信息就显得更为重要。而足部姿态信息中的脚面倾角信息是仿人机 器人稳定控制的重要依据。它可以让仿人机器人感知脚面倾斜情况，在稳定着地时，脚面倾 角还可以反映地面倾斜状态，是仿人机器人稳定控制的一个关键参数。目前，现有技术中对仿人机器人足部姿态的检测，有一种人形机器人脚的设计方法，通 过这种方法可得到足部姿态脚面倾角信息。参见图l，该人形机器人脚包括橡胶脚底层、脚板、 六维力传感器、上法兰、下法兰、倾角传感器、触觉阵列垫、信号处理系统和脚面。信号处 理系统安装在脚板的前部，倾角传感器安装在中部，六维力传感器安装在脚跟处。当机器人 行走时，人形机器人脚上的六维力传感器检测机器人行走过程中脚与路面接触时的地面反作 用力，倾角传感器检测脚板的倾斜角度即足部姿态脚面倾角信息。在对上述现有技术进行分析后，发明人发现现有技术中采用的这些倾角传感器只能检测静态条件下的倾角，即机器人脚必须与地面 稳定接触的条件下的脚面倾角。但是，在仿人机器人实际工作工程中，其要完成步行、转弯、 上下楼梯、舞蹈表演等多种动作，绝大多数时间脚与地面之间是存在相对运动的，而不是处 于静态，所以这种检测仿人机器人脚面倾角的方法具有很大的局限性。发明内容为了解决现有技术中只能测量静态条件下的仿人机器人脚面倾角的局限，本发明提出了一种仿人机器人足部姿态实时检测系统和方法。 所述技术方案如下一种仿人机器人足部姿态实时检测系统，所述系统包括 DSP控制器、三轴加速度传感器、角速率陀螺仪；所述DSP控制器用于向所述三轴加速度传感器写入控制信号，并对所述三轴加速度传感器 的输出进行计算得到静态脚面倾角信息；所述DSP控制器用于向所述角速率陀螺仪写入控制信号，并对所述角速率陀螺仪的输出进 行计算得到角度变化值；所述DSP控制器根据所述静态脚面倾角信息和角度变化值进行计算，得到实时脚面倾角信息。所述DSP控制器具体包括第一读取模块，用于从所述三轴加速度传感器读出三轴加速度信息；第一计算模块用于对所述三轴加速度信息进行计算得到静态脚面倾角信息，具体用于 设Ax为x轴加速度传感器输出的加速度值，Ay为y轴加速度传感器输出的加速度值，Az为z轴加速度传感器输出的加速度值；6x为x轴与水平面夹角，ey为y轴与水平面夹角，9z为z轴与水平面夹角；对所述加速度值Ax进行反正旋运算得到所述0X;对所述加速度值Ay进行反正旋运算得到所述6y;对所述加速度值Az进行反正旋运算得到所述ez;即根据所述三轴加速度信息(Ax， Ay， Az)得到了所述静态脚面倾角(ex， 6y, 0z)。 所述角速率陀螺仪包括x轴角速率陀螺仪、y轴角速率陀螺仪和z轴角速率陀螺仪；所 述DSP控制器具体包括-第二读取模块，用于从所述角速率陀螺仪读出角速率信息； 第二计算模块，用于对所述角速率信息进行计算得到角度变化值，具体用于 设^0')为t=iT时刻x轴角速率陀螺仪输出的角速率信息，为t与T时刻y轴角速率陀 螺仪输出的角速率信息，为t=iT时刻z轴角速率陀螺仪输出的角速率信息；eM为从t=0 到t=iT时刻x轴角度变化值，eye为从t=0到t=iT时刻y轴角度变化值，6 为从t=0到t=iT 时刻z轴角度变化值；对所述x轴角速率信息^(/)进行积分得到所述角度变化值ero; 对所述y轴角速率信息 (/)进行积分得到所述角度变化值eye;对所述z轴角速率信息^(/)进行积分得到所述角度变化值ez。；即根据所述角速率信息(~(0， (/)， rz(o)得到了所述角度变化值(eM， eye， ezc)。所述DSP控制器还包括求和模块，用于根据所述静态脚面倾角信息和角度变化值进行计 算，得到实时脚面倾角信息，具体用于设e 为x轴与水平面之间的实时倾角，e,,为y轴与水平面之间的实时脚面倾角，e2,为z轴与水T面之间的实时脚面倾角；e,。为在t=o时刻x轴的静态脚面倾角，e^。为在t=o时刻y轴的静态脚面倾角，e力为在t=0时刻Z轴的静态脚面倾角；对所述e,。与所述eM求和得到x轴实时脚面倾角e,,; 对所述ey。与所述6ye求和得到y轴实时脚面倾角e》,； 对所述e z。与所述e z￡求和得到z轴实时脚面倾角e; 所述实时脚面倾角信息为(e,,, ey,， ea)。 所述角速率信息为x轴角速率信息、y轴角速率信息和z轴角速率信息。 种仿人机器人足部姿态实时检测方法，所述方法包括对采集的三轴加速度信息进行计算得到静态脚面倾角信息； 对采集的角速率信息进行计算得到角度变化值；根据所述静态脚面倾角信息和所述角度变化值进行计算，得到实时脚面倾角信息。所述对采集的三轴加速度信息进行计算得到静态脚面倾角信息，具体包括设Ax为x轴加速度传感器输出的加速度值，Ay为y轴加速度传感器输出的加速度值， 单位为g， Az为z轴加速度传感器输出的加速度值9x为x轴与水平面夹角，9y为y轴与水 平面夹角，9z为z轴与水平面夹角；对所述加速度值AX进行反正旋运算得到所述9X;对所述加速度值Ay进行反正旋运算得到所述ey; 对所述加速度值Az进行反正旋运算得到所述9z;即根据所述三轴加速度信息(Ax， Ay， Az)得到了所述静态脚面倾角(6x， 0y, 0z)。 所述对釆集的角速率信息进行计算得到角度变化值，具体包括设^0')为t=iT时刻x轴角速率陀螺仪输出的角速率信息，为t=iT时刻y角速率陀螺 仪输出的角速率信息，/;(! )为t=iT时刻z轴角速率陀螺仪输出的角速率信息；为从t=0到 t=iT时刻x轴角度变化值，e"为从t=0到t=iT时刻y轴角度变化值，6^为从t=0到t=iT时 刻z轴角度变化值；对所述x轴角速率信息/;(/)进行积分得到所述角度变化值e,。； 对所述y轴角速率信息 (/)进行积分得到所述角度变化值ey。；对所述z轴角速率信息r力')进行积分得到所述角度变化值ez。；即根据所述角速率信息(~0')， ~(i)， 得到了所述角度变化值(eTC， ey。， ezc)。所述根据所述静态脚面倾角信息和所述角度变化值进行计算，得到实时脚面倾角信息，具体包括设e^为x轴与水平面之间的实时倾角，e^为y轴与水平面之间的实时脚面倾角，02,为2轴与水平面之间的实时脚面倾角；e,。为在t^K)时刻x轴的静态脚面倾角，^。为在t二0时刻y轴的静态脚面倾角，92。为在 t=0时刻Z轴的静态脚面倾角；对所述e,。与所述e 求和得到x轴实时脚面倾角ew; 对所述与所述6y。求和得到y轴实时脚面倾角ey,; 对所述e"与所述ez。求和得到z轴实时脚面倾角ez,; 所述实时脚面倾角信息为(e力，ey,， e^)。与现有技术相比，本发明的有益效果在于本发明提供的仿人机器人足部姿态实时检测 系统和方法，能够实时、连续的测量脚面倾角信息。


图1是现有技术提供的一种人形机器人脚示意图；图2是本发明实施例1提供的仿人机器人足部姿态实时检测系统结构示意图； 图3是本发明实施例2提供的仿人机器人足部姿态实时检测系统结构示意图； 图4是本发明实施例3提供的仿人机器人足部姿态实时检测方法流程图； 图5是本发明实施例提供的由三轴加速度信息计算静态脚面倾角的方法示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进 一步地详细描述。 实施例1参见图2，本发明实施例提供了一种仿人机器人足部姿态实时检测系统，所述系统包括-DSP (Digital Signal Processing,数字信号处理)控制器ll、三轴加速度传感器12、角速率陀螺仪13。该DSP控制器11用于向三轴加速度传感器12写入控制信号，并对三轴加速度传感器12的输 出进行计算得到静态脚面倾角信息；该DSP控制器11还用于向角速率陀螺仪13写入控制信号，并对角速率陀螺仪的输出进行计 算得到角度变化值；该DSP控制器根据得到的静态脚面倾角信息和角度变化值进行计算，得到实时脚面倾角信息。其中，DSP控制器具体包括第一读取模块14，用于从所述三轴加速度传感器读出三轴加速度信息； 第一计算模块15:用于对所述三轴加速度信息进行计算得到静态脚面倾角信息，具体用于本实施例以x轴为例，介绍如何根据加速度信息计算静态脚面倾角。参见图5，设Ax为X轴加速度传感器输出的加速度值，单位为g; 6 X为X轴与水平面的夹角；重力加速度为lg。在静止条件下，加速度传感器只受到重力加速度的影响，因此加速度传感器输出的加速度值Ax只与重力加速度lg以及X轴与水平面的夹角0X有关系，如下Ax=sin 6 xX lg而重力加速度lg可以认为是一个与地理位置有关的常量，可以通过査表获得。因此Ax 与e X之间存在唯一对应关系，如下6 x=arcsin Ax通过上式即可由加速度Ax计算得到x轴与水平面的夹角9x。在本系统中，夹角0x即 对应仿人机器人脚面沿x轴方向与水平面之间的静态脚面倾角。同理，设Ay为y轴加速度传感器输出的加速度值，Az为z轴加速度传感器输出的加速 度值；0y为y轴与水平面的夹角，对应为仿人机器人脚面沿y轴与水平面之间的静态脚面倾 角，9z为z轴与水平面的夹角，对应为仿人机器人脚面沿z轴与水平之间的静态脚面倾角；对y轴进行与x轴相同的计算得到6尸arcsin Ay，对z轴进行与x轴相同的计算得到 9 z^arcsin Az通过上述计算，将获得的加速度信息(Ax， Ay， Az)对应成为静态脚面倾角(6x, 9y， e z)。其中，角速率陀螺仪包括x轴角速率陀螺仪7， y轴角速率陀螺仪8， z轴角速率陀螺仪9， DSP控制器还包括第二读取模块16，用于从所述角速率陀螺仪读出角速率信息；第二计算模块17，用于对所述角速率信息进行计算得到角度变化值，具体用于设^0')为t-iT时刻角速率陀螺仪输出的角速率信息;T为系统采样周期，在本系统中为lms; 9 K为从，0到t—T时刻的角度变化值，对^(/)积分即可得到角度变化值9 TC，艮P同理，设^(!')为t=iT时刻y轴角速率陀螺仪输出的角速率信息，为t=iT时刻z轴角 速率陀螺仪输出的角速率信息e"为从t=0到t=iT时刻y轴角度变化值，ez。为从t=0到t—T 时刻z轴角度变化值；对y轴角速率信息 (/)用与x轴相同的方法进行积分得到所述角度变化值9 "，即对；°轴角速率信息(0用与x轴相同的方法进行积分得到所述角度变化值e ze ，艮卩 9 zc,(/"r;即M据角速率信息(>x(/)，"/))得到了角度变化值(^， ey。， ezc)。其中，DSP控制器还包括求和模块18，用于根据静态脚面倾角信息和角度变化值进行计算，得到实时脚面倾角信息，具体用于设o w为仿人机器人脚面沿x轴与水平面之间的实时脚面倾角，e y,为仿人机器人脚面沿 y轴与水平面之间的实时脚面倾角，e z,为仿人机器人脚面沿z轴与水平面之间的实时脚面倾角；e Ifl为在t=o时刻x轴的静态脚面倾角，e '。为在t=o时刻y轴的静态脚面倾角，e z。为在t=0时刻Z轴的静态脚面倾角；对所述e x。与所述e xc求和得到x轴实时脚面倾角e w ，即e,, = e ,。 + e ,c; 对所述e '。与所述e y。求和得到y轴实时脚面倾角o yt,即o yt= e ,。+ e y。； 对所述e 2。与所述0 ze求和得到z轴实时脚面倾角9 z,，即e 2,= 9 z。+ e z。； 即得到实时脚面倾角信息为(e w ， e y, ， e a )。 实施例2参见图3，本发明实施例提供了一种仿人机器人足部姿态实时检测系统，所述系统具体包括包括DSP控制器1、三轴加速度传感器2、角速率陀螺仪、SPI (Serial Peripheral Interface, 串行同步通讯协议)总线3、 CAN(Con加ller Area Network,控制器局域网络)总线4。其中，DSP控制器包括CAN接口5和SPI接口6;三轴加速度传感器2通过SPI总线3与DSP控制器1的SPI接口6相连，用于测量互相正交的x、 y、 z轴的加速度；x轴角速率陀螺仪7、 y轴角速率陀螺仪8和z轴角速率陀螺仪9分别通过SPI 总线3与DSP控制器1的SPI接口6相连，用于测量x、 y、 z轴的角速率信息。本实施例中采用的三轴加速度传感器的型号是MMA7455L， MMA7455L是一种三轴 MEMS (Micro-Electro-Mechanical Systems,微机电系统)加速度传感器。x轴角速率陀螺仪7、y轴角速率陀螺仪8和z轴角速率陀螺仪9的型号都是ADIS16255， ADIS16255是一种单轴MEMS角速率传感器。DSP控制器1可以通过SPI接口 6向三轴加速度传感器2MMA7455L写入控制信号，包 括量程选择、采集频率、标定参数等，同时，DSP控制器1可以通过SPI接口 6以数字形式 读出三轴加速度信息，并对三轴加速度信息进行计算得到静态脚面倾角信息。具体计算方法 如下本实施例以x轴为例，介绍如何根据加速度信息计算静态脚面倾角。参见图5，设Ax为 x轴加速度传感器输出的加速度值，单位为g; 8 x为x轴与水平面的夹角；重力加速度为lg。在静止条件下，加速度传感器只受到重力加速度的影响，因此加速度传感器输出的加速 度值Ax只与重力加速度lg以及x轴与水平面的夹角e x有关系，如下Ax=sin 9 x X lg而重力加速度lg可以认为是一个与地理位置有关的常量，可以通过査表获得。因此Ax 与6x之间存在唯一对应关系，如下9 x=arcsin Ax通过上式即可由加速度Ax计算得到x轴与水平面的夹角9x。在本系统中，夹角0x即 对应仿人机器人脚面沿x轴方向与水平面之间的静态脚面倾角。同理，设Ay为y轴加速度传感器输出的加速度值，Az为z轴加速度传感器输出的加速 度值；ey为y轴与水平面的夹角，对应为仿人机器人脚面沿y轴与水平面之间的静态脚面倾 角，Gz为z轴与水平面的夹角，对应为仿人机器人脚面沿z轴与水平面之间的静态脚面倾角；对y轴进行与x轴相同的计算得到9 y=arcsin Ay，对z轴进行与x轴相同的计算得到 9 z=arcsin Az通过上述计算，将获得的加速度信息(Ax， Ay， Az)对应成为静态脚面倾角(0x， 6 y,0 7,)。DSP控制器l可以通过SPI接口6向x轴角速率陀螺仪7、y轴角速率陀螺仪8和z轴角速率陀螺仪9分别写入控制信号，包括量程选择、采集频率、标定参数等，同时，DSP控制器1可以通 过SPI接口6以数字形式分别读出x轴角速率信息、y轴角速率信息、z轴角速率信息；并对得到 的x轴角速率信息、y轴角速率信息、z轴角速率信息分别计算得到角度变化值。具体计算方法 如下-本实施例仍以x轴为例，介绍如何对采集的三轴角速率信息进行计算得到角度变化值。 设^(/)为t-iT时刻角速率陀螺仪输出的角速率信息；T为系统采样周期，在本系统中为 i ms; e tc为从t=0到tHT时刻的角度变化值，对rx (0积分即可得到角度变化值e w ，艮P、=! =0同理，设 (/)为t=iT时刻y轴角速率陀螺仪输出的角速率信息，~(/)为t=iT时刻z轴角 速率陀螺仪输出的角速率信息；e"为从t=0到t=iT时刻y轴角度变化值，0ze为从t=0到t=iT 时刻z轴角度变化值；对y轴角速率信息 (/)用与x轴相同的方法进行积分得到所述角度变化值9 y。，即对z轴角速率信息^(z')用与x轴相同的方法进行积分得到所述角度变化值e ze ，艮P即霜据角速率信息(/;(/)， (z')，得到了角度变化值(ete， eye， ezc)。 然后，DSP控制器根据静态脚面倾角信息和角度变化值进行计算，得到机器人实时、连续 的脚面倾角信息。具体计算方法如下本实施例仍以x轴为例，介绍如何根据静态脚面倾角信息和角度变化值计算得到实时脚面倾角信息；设e x,为仿人机器人脚面沿x轴与水平面之间的实时脚面倾角；e ,。为在t=o时刻计算得 到x轴的静态脚面倾角；对e x。与e k求和即可得到x轴实时脚面倾角9,,，即e = e ,。 + e同理，设为仿人机器人脚面沿y轴与水平面之间的实时脚面倾角，ez,为仿人机器人脚 面沿z轴与水平面之间的实时脚面倾角；e^为在t=o时刻y轴的静态脚面倾角，e^为在t=o时刻Z轴的静态脚面倾角；对e ,,。与所述e y。求和得到y轴实时脚面倾角e yt，即e yt= e y。+ e y。； 对e —。与所述e ^求和得到z轴实时脚面倾角9 z,，即e z,= e z。+ e ^;即得到实时脚面倾角信息(e,, ， 9 y( ， 9 a )。DSP控制器1的CAN接口 5通过CAN总线4与仿人机器人运动控制计算机11相连。 仿人机器人运动控制计算机11是仿人机器人运动控制的核心， -般采用功能强大的工控 机和专用的实时操作系统，足部姿态实时检测系统10通过CAN总线4与仿人机器人运动控 制计算机11相连，向仿人机器人运动控制计算机11发送仿人机器人实时足部姿态信息，包 括实时脚面倾角信息、三轴加速度信息和角度变化值，三轴加速度信息也可以是实时足部加 速度信息，角速率信息也可以是实时足部旋转角速率信息。该仿人机器人足部姿态实时检测系统的检测频率以及信号输出频率为lkHz。 本发明实施例的有益效果在于通过对实时采集得到的三轴加速度信息进行计算，得到 静态脚面倾角信息，通过对实时采集得到的角速率信息进行计算得到角度变化值，再通过得 到的静态脚面倾角信息和角度变化值得到实时机器人实时、连续的脚面倾角信息。实施例3参见图4，本发明实施例提供了一种仿人机器人足部姿态信息的实时检测方法，具体包括步骤101:初始化各传感器。 步骤102:采集三轴加速度信息。步骤103:对采集的三轴加速度信息进行计算得到静态脚面倾脚信息。本实施例以X轴为例，介绍如何根据加速度信息计算静态脚面倾角的方法。参见图5，设 Ax为X轴加速度传感器输出的加速度值，单位为g; 0X为X轴与水平面的夹角；重力加速 度为lg。在静止条件下，加速度传感器只受到重力加速度的影响，因此加速度传感器输出的加速 度值Ax只与重力加速度lg以及X轴与水平面的夹角Sx有关系，如下Ax二sin 6 xX lg而重力加速度lg可以认为是一个与地理位置有关的常量，可以通过査表获得。因此Ax 与ex之间存在唯一对应关系，如下-S x=arcsin Ax通过上式即可由加速度Ax计算得到x轴与水平面的夹角0x。在本方法中，夹角9x即 对应仿人机器人脚面沿x轴方向与水平面之间的静态脚面倾角。同理，设Ay为y轴加速度传感器输出的加速度值，Az为z轴加速度传感器输出的加速 度值；6y为y轴与水平面的夹角，对应为仿人机器人脚面沿y轴与水平面之间的静态脚面倾 角，0z为z轴与水平面的夹角，对应为仿人机器人脚面沿z轴与水平面之间的静态脚面倾角；对y轴进行与x轴相同的计算得到e y=arcsin Ay，对z轴进行与x轴相同的计算得到9z=arcsin Az，通过上述计算，将获得的加速度信息(Ax, Ay， Az)对应成为静态脚面倾角(0x, ey, 9 z)。步骤104:采集三轴角速率信息。歩骤105:对采集的三轴角速率信息进行计算得到角度变化值。本实施例仍以x轴为例，介绍如何对采集的三轴角速率信息进行计算得到角度变化值。 设^G')为t=iT时刻角速率陀螺仪输出的角速率信息；T为系统采样周期，在本方法中为lms; e ,e为从t=0到t=iT时刻的角度变化值，对~(z〕积分即可得到角度变化值e m ，艮p、=ix")"z=o同理，设 (/)为t=iT时刻y轴角速率陀螺仪输出的角速率信息，~(/)为t=iT时刻z轴角 速率陀螺仪输出的角速率信息；6y。为从t=0到t=iT时刻y轴角度变化值，02(;为从t=0到t=iT 时刻z轴角度变化值；对y轴角速率信息 G)用与x轴相同的方法进行积分得到所述角度变化值9 y。，即eyc=^ry(/)*r;对'^轴角速率信息^(/)用与x轴相同的方法进行积分得到所述角度变化值9 Z￡，艮PezC=l>z(o*r;即霜据角速率信息"x(/)，rz(/))得到了角度变化值(ew， eyc， ezc)。步骤106:根据步骤103得到的静态脚面倾角信息和步骤105得到的角度变化值进行计 算，得到实时脚面倾角信息，同时返回步骤104。本实施例仍以x轴为例，介绍如何根据静态脚面倾角和角度变化值计算得到实时脚面倾 角信息；设e,(为仿人机器人脚面沿x轴与水平面之间的实时脚面倾角；e ,。为在t=o时刻计 算得到的x轴的静态脚面倾角；对e ,。与e M求和即可得到x轴实时脚面倾角<formula>formula see original document page 14</formula>同理，设ev,为仿人机器人脚面沿y轴与水平面之间的实时脚面倾角，ez,为仿人机器人脚面沿z轴与水平面之间的实时脚面倾角；、。为在t-0时刻y轴的静态脚面倾角，e"为在to 时刻z轴的静态脚面倾角；对9 y。与所述e ye求和得到y轴实时脚面倾角e yt，即0 yt=0 ,,。+e y。； 对e :。与所述e ze求和得到z轴实时脚面倾角9 ，即e z,= 0 :。+ e zc; 即得到实时脚面倾角信息(e ,, ， e y, ， 9 a )。本仿人机器人足部姿态实时检测方法中的检测频率以及信号输出频率为lkHz。 本发明的有益效果在于通过对实时采集得到的三轴加速度信息进行计算，得到机器人 静态脚面倾脚信息，通过对实时采集得到的角速率信息进行计算得到机器人角度变化值，再 通过得到的静态脚面倾脚信息和角度变化值得到实时机器人实时、连续的脚面倾角信息。
权利要求
1.一种仿人机器人足部姿态实时检测系统，其特征在于，所述系统包括DSP控制器、三轴加速度传感器、角速率陀螺仪；所述DSP控制器用于向所述三轴加速度传感器写入控制信号，并对所述三轴加速度传感器的输出进行计算得到静态脚面倾角信息；所述DSP控制器用于向所述角速率陀螺仪写入控制信号，并对所述角速率陀螺仪的输出进行计算得到角度变化值；所述DSP控制器根据所述静态脚面倾角信息和角度变化值进行计算，得到实时脚面倾角信息。
2. 根据权利要求l所述的仿人机器人足部姿态实时检测系统，其特征在于，所述DSP控制 器具体包括第一读取模块，用于从所述三轴加速度传感器读出三轴加速度信息；第一计算模块，用于对所述三轴加速度信息进行计算得到静态脚面倾角信息，具体用于 设Ax为x轴加速度传感器输出的加速度值，Ay为y轴加速度传感器输出的加速度值，Az为z轴加速度传感器输出的加速度值；6x为x轴与水平面夹角，ey为y轴与水平面夹角，6z为z轴与水平面夹角；对所述加速度值Ax进行反正旋运算得到所述eX;对所述加速度值Ay进行反正旋运算得到所述0y; 对所述加速度值Az进行反正旋运算得到所述0z;即根据所述三轴加速度信息(Ax， Ay， Az)得到了所述静态脚面倾角(ex， 6y， 9z)。
3. 根据权利要求1或2所述的仿人机器人足部姿态实时检测系统，其特征在于，所述角 速率陀螺仪包括x轴角速率陀螺仪、y轴角速率陀螺仪和z轴角速率陀螺仪；所述DSP控制 器具体包括第二读取模块，用于从所述角速率陀螺仪读出角速率信息；第二计算模块，用于对所述角速率信息进行计算得到角度变化值，具体用于-设~(/)为t=iT时刻x轴角速率陀螺仪输出的角速率信息，为t=iT时刻y轴角速率陀 螺仪输出的角速率信息，^(/)为t^T时刻z轴角速率陀螺仪输出的角速率信息；01(:为从1=0 到t^T时刻x轴角度变化值，eye为从tO到tHT时刻y轴角度变化值，9^为从1=0到1=汀 时刻z轴角度变化值；对所述x轴角速率信息 (Z')进行积分得到所述角度变化值; 对所述y轴角速率信息进行积分得到所述角度变化值e》。；对所述z轴角速率信息&o')进行积分得到所述角度变化值eze;即根据所述角速率信息(~(/)， ~(/), ~(/))得到了所述角度变化值(9M， eye， ezc)。
4. 根据权利要求3所述的仿人机器人足部姿态实时检测系统，其特征在于，所述DSP控制器还包括求和模块，用于根据所述静态脚面倾角信息和角度变化值进行计算，得到实时脚面倾角信息，具体用于设e力为x轴与水平面之间的实时倾角，、,为y轴与水平面之间的实时脚面倾角，ez,为z轴与水平面之间的实时脚面倾角；e《。为在t=o时刻x轴的静态脚面倾角，为在t=o时刻y轴的静态脚面倾角，ez。为在t=o时刻z轴的静态脚面倾角；对所述9,。与所述e,。求和得到x轴实时脚面倾角ert; 对所述^。与所述9ye求和得到y轴实时脚面倾角; 对所述e力与所述^e求和得到z轴实时脚面倾角ez,; 所述实时脚面倾角信息为(9"， ey,， ea)。
5. 根据权利要求3所述的仿人机器人足部姿态实时检测系统，其特征在于，所述角速率信息为x轴角速率信息、y轴角速率信息和z轴角速率信息。
6. —种仿人机器人足部姿态实时检测方法，其特征在于，所述方法包括 对采集的三轴加速度信息进行计算得到静态脚面倾角信息； 对采集的角速率信息进行计算得到角度变化值；根据所述静态脚面倾角信息和所述角度变化值进行计算，得到实时脚面倾角信息。
7. 根据权利要求6所述的仿人机器人足部姿态实时检测方法，其特征在于，所述对采集 的三轴加速度信息进行计算得到静态脚面倾角信息，具体包括-设Ax为x轴加速度传感器输出的加速度值，Ay为y轴加速度传感器输出的加速度值， 单位为g， Az为z轴加速度传感器输出的加速度值；9x为x轴与水平面夹角，6y为y轴与水 平面夹角，9z为z轴与水平面夹角；对所述加速度值Ax进行反正旋运算得到所述9x;对所述加速度值Ay进行反正旋运算得到所述9y;对所述加速度值Az进行反正旋运算得到所述9z;即根据所述三轴加速度信息(Ax, Ay， Az)得到了所述静态脚面倾角(9x, 6y， 6z)。
8. 根据权利要求6或7所述的仿人机器人足部姿态实时检测方法，其特征在于，所述对采 集的角速率信息进行计算得到角度变化值，具体包括设^(Z')为t=iT时刻x轴角速率陀螺仪输出的角速率信息，为t=iT时刻y角速率陀螺仪输出的角速率信息，为t=iT时刻z轴角速率陀螺仪输出的角速率信息；为从t=0到 t=iT时刻x轴角度变化值，eye为从t=0到t=iT时刻y轴角度变化值，eze为从t=0到t=iT时 刻z轴角度变化值；对所述x轴角速率信息^0')进行积分得到所述角度变化值^。；对所述y轴角速率信息A"y(/)进行积分得到所述角度变化值9y。；对所述z轴角速率信息^(/)进行积分得到所述角度变化值ez。；即根据所述角速率信息(g(O， ， 得到了所述角度变化值。 ， eye， eze)。
9. 根据权利要求8所述的仿人机器人足部姿态实时检测方法，其特征在于，所述根据所述静态脚面倾角信息和所述角度变化值进行计算，得到实时脚面倾角信息，具体包括设e. 为x轴与水平面之间的实时倾角，ey为y轴与水平面之间的实时脚面倾角，ez,为z 轴与水平面之间的实时脚面倾角；为在t=o时刻x轴的静态脚面倾角，e v。为在t=o时刻y轴的静态脚面倾角，ez。为在 t=o时刻z轴的静态脚面倾角；对所述^。与所述求和得到x轴实时脚面倾角;对所述e》。与所述e"求和得到y轴实时脚面倾角ey,;对所述ez。与所述eze求和得到z轴实时脚面倾角ez,;所述实时脚面倾角信息为(e,,, ev,， erf)。
全文摘要
本发明公开了一种仿人机器人足部姿态实时检测系统和方法，属于自动化领域。所述系统包括DSP控制器、三轴加速度传感器、角速率陀螺仪；所述DSP控制器通过三轴加速度传感器得到静态脚面倾角信息，再通过角速率陀螺仪得到角度变化值，然后根据所述静态脚面倾角信息和角度变化值得到实时脚面倾角信息。本发明还公开了一种仿人机器人足部姿态实时检测方法，所述方法包括对采集的三轴加速度信息进行计算得到静态脚面倾角信息；对采集的角速率信息进行计算得到角度变化值；根据所述静态脚面倾角信息和所述角度变化值进行计算，得到实时脚面倾角信息。本发明提供的仿人机器人足部姿态实时检测系统和方法，能够实时、连续的测量脚面倾角。
文档编号G01C21/10GK101403619SQ200810224579
公开日2009年4月8日 申请日期2008年10月21日 优先权日2008年10月21日
发明者张伟民, 李建玺, 辛洪兵, 强 黄 申请人:北京理工大学