[发明专利]水下清淤机器人的三维重力传感平衡姿态智能监控与调整系统

说明书

技术领域：

本发明涉及嵌入式控制电路设计、路径控制设计、自动姿态调整设计、电机控制设计和运动控制子系统的设计。 

背景技术：

我国污水处理进程中普遍存在“重水轻泥”的状况。据介绍，污泥不仅含有较丰富的氮、磷及多种微量元素和大量有机质，同时还含有病原菌、寄生虫（卵）、重金属、盐分及某些难分解的有机毒物。尤其是一些带有新病菌的污泥不经无害化处理而直接用于蔬菜瓜果的施肥，将会导致细菌性、病毒性疾病和寄生虫病的发病率骤增。因此，必须进行处理，才能二次利用，否则会引起二次污染。但是现在对污泥的后续资源化处理的研究较多，而对于前期淤泥提取方面的研究几乎没有。 

目前我国清除堰井淤泥一般依靠人工挖取，国外发达国家采用淤泥泵提取，由于淤泥含水率高，必须采用淤泥脱水机械配套分离，才能获得比较适宜运输的淤泥。按照目前国内外传统常规提取淤泥的技术有以下几种： 

◆带水缓慢人工挖、提取 

◆污泥泵提取加脱水装置 

◆整体脱水分离 

◆分离机械过滤分离 

◆沉淀浓缩分离 

◆刮渣提取 

◆中低速淤泥提升脱水分离。 

传统的提取工艺一方面能耗大、成本高；另一方面工厂一般采用埋地式污水槽，废水中的污泥提取必须将废水抽干，采用污泥泵或者人工结合提取污泥。一般污泥泵只能在某一个点提取，而大部分污水槽采用封闭式，或者半封闭式，造成污泥泵不能自由移动提取。污水槽内又存在毒气，采用人工挖取，容易中毒。其中三维重力传感平衡 姿态智能监控与调整是本发明中非常重要的部分。 

发明内容：

1、三维重力传感平衡姿态智能监控与调整 

传感器模块是由MMA8451Q 来对机器人行走时的加速度信号进行检测，中央控制器通过I2C总线接口，发送特定格式命令查询，三维重力加速度传感器收到有限命令查询后返回一串数据，其中控制器经过检验分析出机器的三维姿态。当控制器得到有侧翻等危险信号时，控制器将控制轮子驱动电机做一系列动作调整其此时危险状态。 

图1为MMA8451Q的方框图。 

附录1为MMA8451Q的驱动程序。 

附录1：MMA8451Q驱动程序： 

/******************************************* 

I2C START信号 

时钟脉冲高电平期间，数据线负跳变 

******************************************** 

void IIC_start(void) 

{ 

  IIC_SCL=0;//时钟线低 

  IIC_SDA=1;//数据线高 

  _asm(nop); 

  IIC_SCL=1;//时钟线高 

  _asm(nop); 

  IIC_SDA=0;//数据线从高跳变到低 

  _asm(nop); 

  IIC_SCL=0;//时钟线低 

} 

/******************************************* 

I2C STOP信号 

时钟脉冲高电平期间，数据线正跳变 

********************************************/ 

void IIC_stop(void) 

{ 

IIC_SCL=0;//时钟线低 

  _asm(nop); 

IIC_SDA=0;//数据线低 

  _asm(nop); 

IIC_SCL=1; //时钟线高 

  _asm(nop); 

IIC_SDA=1;//数据线正跳变 

_asm(nop); 

IIC_SCL=0;//时钟线低 

} 

//************************************************* 

//字节发送程序 

//发送c(可以是数据也可是地址)，送完后接收从应答 

//不考虑从应答位 

void send_byte(unsigned char c) 

{ 

unsigned char i;