专利名称：柔性两轮自平衡机器人的姿态检测方法
技术领域：
本发明属于机器人控制领域，特别涉及机器人姿态检测。
背景技术：
通常所说的柔性机器人一般是指柔性机械臂，柔性机械臂姿态检测大多 是忽略结构的弹性变形对结构刚体运动的影响，将其姿态检测转化为刚性测 量。它并不是真正的柔性机器人。
虽然在柔性机械臂姿态检测中也使用信息融合器来测算姿态信息，但这 种信息融合器并不能有效针对真正的柔性机器人。
发明内容
本发明的目的是提供一种用于柔性两轮自平衡机器人的姿态检测方法， 该方法能够有效探测到机器人的质心变化，测量偏差在可接受范围内。
本发明的方法，首先分别在机器人头部、躯干和底座设置三组倾角仪1
和陀螺仪2，然后将三个倾角仪1输出的头部角度信息《、躯干角度信息《、 底座角度信息《和三个陀螺仪2输出的头部角速度信息《、躯干角速度信息 《、底座角速度信息《通过并行采集模块7 ,入的信息融合器8，由信息融合 器8输出质心估计角度^和质心估计角速度^的信息；所述信息融合器8由乘 法器将头部角度信息《、躯干角度信息&、底座角度信息《分别与权值A、 尺2、 i^相乘后经加法器相加得到质心估计角度^然后由乘法器将头部角速度 信息A、躯干角速度信息《、底座角寧度信息《分别与权值iQ、 &、 i^相乘 后经加法器相加得到质心估计角速度S ;其中&大于等于0，小于等于2.0, &大于等于-490，小于等于-110.0， ^大于等于179，小于等于718， &大于 等于0.2，小于等于1.9， &大于等于-12，小于等于0.8， i^5大于等于31，小 于等于112。
根据粒子滤波(PF)算法分析得到&、 &、 & 、 &、《5、 &的优选值 为1.2、 -170.1、 276.411、 1.3541、 -3,456、 81.1241或1、 -231.11、 326.3442、 0.9843、 -5.564、 52.3631。
相信本领域技术人员可以理解的是，信息融合器8可以由专门的硬件电
路实现，也可以是一台通用计算机，如PC机。
采用本发明的装置测得的柔性两轮自平衡机器人质心变化，其偏差很小
达到10'2数量级。
附图l，本发明中倾角仪和陀螺仪的位置示意图； 附图2，本发明装置的逻辑模块示意附图3，为起始角度=0.12弧度时本发明的装置所测得的质心参数变化曲
线；
附图4，为起始角度=0.12弧度时实际测量质心参数变化曲线；
附图5，为起始角度=0.26弧度时本发明的装置所测得的质心参数变化曲
线；
附图6，为起始角度=0.26弧度时实际测量质心参数变化曲线；
具体实施例方式
请见图l、图2，本发明所针对的机器人是柔性两轮自平衡机器人，在头 部、躯干和底座处分别放置三对倾角仪1和陀螺仪2，分别测量头部、躯干和 底座的角度和角速度信息，测量值分别以(《,《A,《,《,《)设定表示。
信息融合器8是一台通用计算机，并行采集模块7是安装在信息融合器8 上的计算机接口卡。
测量时，首先启动信息融合器8上的信息融合软件，初始化计算机内存， 并将权值&、&、《3、&、&、^5设置为初值，初值可以选1.2、-170.1、 276.411、 1.3541、 -3.456、 81.1241或1、 -231.11、 326.3442、 0.9843、 -5.564、 52.3631。
当机器人移动时，测量值(《，《,《,《,《，《)通过连接在计算机上的数据采 集接口卡输入到前述的计算机内存中；然后由计算机上运行的信息融合软件 将头部角度信息《乘权值&、躯干角度信息&乘权值i^、底座角度信息《乘 权值&，把三个乘积相加得到质心估计角度^再由信息融合软件将头部角 速度信息《乘权值&、躯干角速度信息^乘权值&、底座角速度信息《乘权 值《6，把三个乘积得至y质心估计角速度^ ;最后将前面所得到的质心估计角 度S和质心估计角速度S输出。
请见图3-6，从图中可以看出，信息融合器输出的质心参数变化曲线与实 测曲线相当接近，达到了发明目的的要求。
权利要求
1.一种柔性两轮自平衡机器人的姿态检测方法，其特征在于首先分别在机器人头部、躯干和底座设置三组倾角仪(1)和陀螺仪(2)，然后将三个倾角仪(1)输出的头部角度信息(θ1)、躯干角度信息(θ2)、底座角度信息(θ3)和三个陀螺仪(2)输出的头部角速度信息躯干角速度信息底座角速度信息通过并行采集模块(7)输入的信息融合器(8)，由信息融合器(8)输出质心估计角度和质心估计角速度的信息；所述信息融合器(8)由乘法器将头部角度信息(θ1)、躯干角度信息(θ2)、底座角度信息(θ3)分别与权值K1、K2、K3相乘后经加法器相加得到质心估计角度然后由乘法器将头部角速度信息躯干角速度信息底座角速度信息分别与权值K4、K5、K6相乘后经加法器相加得到质心估计角速度其中K1大于等于0，小于等于2.0，K2大于等于-490，小于等于-110.0，K3大于等于179，小于等于718，K4大于等于0.2，小于等于1.9，K5大于等于-12，小于等于0.8，K6大于等于31，小于等于112。
2. 根据权利要求1所述的方法，其特征在于i^为1.2， &为-170.1， & 为276.411， ^为1.3541，《5为-3.456， ^为81.1241。
3. 根据权利要求1所述的装置，其特征在于&为1， &为-231.11， & 为326.3442， ^为0.9843，《5为-5.564， ^为52.3631。
全文摘要
一种柔性两轮自平衡机器人的姿态检测方法，属于机器人控制领域。本发明的方法将设置于柔性两轮自平衡机器人头部、躯干部和底座的三组倾角仪1和陀螺仪2输出的头部角度信息θ<sub>1</sub>、躯干角度信息θ<sub>2</sub>、底座角度信息θ<sub>3</sub>、头部角速度信息θ<sub>1</sub>、躯干角速度信息θ<sub>2</sub>、底座角速度信息θ<sub>3</sub>通过并行采集模块7输入信息融合器8；由信息融合器8将θ<sub>1</sub>、θ<sub>2</sub>、θ<sub>3</sub>分别乘以权值K<sub>1</sub>、K<sub>2</sub>、K<sub>3</sub>，并将乘积相加得到质心估计角度θ；再由信息融合器8将θ<sub>2</sub>、θ<sub>2</sub>、θ<sub>3</sub>分别乘以权值K<sub>4</sub>、K<sub>5</sub>、K<sub>6</sub>，并将乘积相加得到质心估计角速度θ。所得的输出值，其精度很高，与实测值的偏差可达10<sup>-2</sup>数量级。
文档编号B25J13/00GK101100059SQ20071011962
公开日2008年1月9日 申请日期2007年7月27日 优先权日2007年7月27日
发明者于乃功, 江 刘, 亮 孙, 李欣源, 狄海江, 赵岗金, 赵建伟, 阮晓钢 申请人:北京工业大学