[发明专利]一种输电线巡检机器人的全自动耦合控制系统

权利要求书

1.一种输电线巡检机器人的全自动耦合控制系统，其特征在于，包括重心调整控制模块、轨迹跟踪控制模块、全自动控制模块、遥控功能模块、故障检测模块、电机控制模块、外设控制模块和滑行控制模块；重心调整控制模块、轨迹跟踪控制模块、全自动控制模块、遥控功能模块、故障检测模块、电机控制模块、外设控制模块和滑行控制模块均与中央处理器CPU连接；由中央处理器CPU对各个模块传递的数据进行运算和逻辑处理，并将控制指令发送给各个模块；

重心调整控制模块包括视觉和红外检测设备、工控机、时钟树、陀螺仪、IIC、CAN总线模块和电机；陀螺仪通过IIC连接中央处理器CPU，若干电机通过CAN总线模块连接中央处理器CPU；视觉和红外检测设备通过工控机连接中央处理器CPU；时钟树为外设配置时钟，陀螺仪检测机器人姿态，将姿态数据通过IIC传递给CPU，CPU通过CAN总线模块发送的控制指令，根据指令进行运动；电机本身的数据通过CAN总线模块反馈给CPU；重心调整控制模块的控制逻辑步骤如下：

步骤1：配置时钟树，时钟器每隔4ms产生中断，定时器flag置1；

步骤2：flag置1时，中央处理器CPU读取陀螺仪通过IIC发回巡检机器人整体姿态的数据，并对视觉和红外检测设备的数据进行处理，作为辅助判断数据；

步骤3：与步骤2同时进行，flag置1时，电机的编码器将电机的位置和速度信息通过CAN总线模块传递给中央处理器CPU；

步骤4：在步骤1和步骤2之后执行，flag置1时，中央处理器CPU对陀螺仪和电机编码器返回的数据进行处理，根据爬滑两运动模态高压输电线巡检机器人机构模型，计算机器人的重心；

步骤5：中央处理器CPU根据步骤4重心的偏移情况，计算电机的控制参数，及控制量通过CAN总线模块发送给电机3和电机4，实现姿态的调整，定时器flag置0；

步骤6：程序开始重复步骤1～步骤5；

轨迹跟踪控制模块包括时钟树、串口模块、直接内存访问模块DMA、脉冲宽度调制模块PWM、I/O口、CAN总线模块、视觉和红外检测设备、工控机、电机、舵机和光电门；视觉和红外检测设备用于检测高压输电线的状况及检测机器人的运行环境信息并将信息传递给工控机，工控机接收检测模块的数据，并对数据进行处理，将处理结果通过串口模块发送数据，数据并通过DMA存储在存储器中，中央处理器CPU读取存储器中的数据，并对数据进行处理运算，通过CAN总线模块控制电机运动，电机通过CAN总线模块将自身状态反馈给中央处理器CPU；中央处理器CPU通过脉冲宽度调制模块PWM控制舵机，通过I/O口读取光电门状态；轨迹跟踪控制模块主要应用于爬行越障时的实际运动轨迹对优化后的理想运动轨迹的跟踪控制，控制逻辑步骤如下：

步骤1：视觉和红外检测设备将视觉和红外检测模块收集到的信息，发送给工控机；

步骤2：工控机根据检测模块收集到的信息进行处理分类，非突发情况则匹配对应数据库，输出对应的爬行越障的运动轨迹，若为突发情况则暂时挂起其他动力源的控制任务，等待工控机根据情况做出的决策，输出突发情况的运动轨迹，遇见无法决策发送爬滑两运动模态高压输电线巡检机器人异常信号并挂机等待救助；

步骤3：工控机将爬行越障的运动轨迹，通过串口模块和直接内存访问模块DMA将优化的理想运动轨迹信息存储到固定存储器中，并将越障运动启动的cross_flag置1；

步骤4：cross_flag置1时，中央处理器CPU读取内存中的理想运动轨迹信息，并通过电机耦合控制方法生成对应的控制参数和控制量；

步骤5：中央处理器CPU通过脉冲宽度调制模块控制舵机旋转，松开机器人的机械自锁机构；

步骤:6：读取电机的编码器将电机的位置和速度信息通过CAN总线模块传递给中央处理器CPU；

步骤7：中央处理器CPU根据编码器信息和理想运动轨迹，通过电机耦合控制方法生成对应的控制参数和控制量；通过CAN总线模块发送给电机；

步骤8：重复步骤5和步骤6，中央处理器CPU通过I/O口读取光电门的状态，当光电门的I/O口置高时，通过脉冲宽度调制模块控制舵机旋转，完成机器人抓握的机械自锁；

步骤9：完成一个运动轨迹的跟踪，挂起相应的任务，等待下一个轨迹跟踪的任务。