[发明专利]一种用于床面自主移动机器人的弓字形路径规划控制方法

权利要求书

1.一种用于床面自主移动机器人的弓字形路径规划控制方法，其特征在于： 所述控制方法包括如下步骤：

步骤1：机器人放到床面上后，先进行机器热身运动；

步骤2：热身运动完成后进行状态校准和陀螺仪校准；

步骤3：校准完成后，控制直线运动，判断是否到悬崖或者障碍物，根据悬崖或者障碍物的位置设定距离进行后退避障处理；

步骤4：如果步骤3中的直线运动的时间超过最大直线运动时间或避障完成后，机器人控制转向运动，如果转向过程中遇到障碍时，进行避障处理后再转向运动，转向运动完后进入步骤2；

所述步骤1中的具体过程为：陀螺仪校准期间需要机器静止1-3s，通过原地左转30°再右转60°再左转30°结束热身运动进入静止，时间为1-3s；

所述步骤2中的状态校准和陀螺仪校准的具体过程为：

状态校准，在进入直线运动前对陀螺仪进行校准，每次校准后20s内不再进行校准，即校准动作的冷却，则跳过校准进入直线运行，由于机器人所处环境不一定是稳定静止的，如果进入校准状态5s内没有完成校准计算即校准超时，则放弃校准，进入直线运行模式；

陀螺仪校准，由于陀螺仪各轴在静止时，存在零点漂移，在运行过程中存在温度漂移，其中俯仰角和翻滚角可以通过垂直方向上的加速度进行互补滤波消除漂移，但偏航角不能，需要在开始运动之前，设备静止时进行陀螺仪校准，并在运行过程中适时的停下机器进行校准，进入校准时，关闭轮子，对6轴数据分别进行采样，并分别记录前20次的采样记录，如果单个轴上20次采样结果的方差小于5，则认为该轴处于静止状态，如果6个轴都处于静止状态，则分别对各个轴上20次的采样结果取平均值作为其轴的漂移校准值，更新旧的校准值之后，开启轮子正常运行，进入校准时间为，弓字形运动过程中，完成转向再走直线之前； 所述步骤3控制直线运动的具体过程为：

直线行走的角度仅有0°和180°两种，在进入直线行走时，根据上一次的角度设定本次直线行走的目标角度，如果上一次时0°，则本次为180°，反之，上一次为180°本次就为0°，行走角度通过PD控制，根据当前偏航角与目标角度的夹角以及当前角速度计算左右轮轮数差，计算公式如下：

其中：p为比例权重，根据实验设定为0.5，d为微分权重，根据实验设定为0.01，e为当前角度与目标角度的夹角，当前角度在目标角度左侧则e为正值，否则为负值，MaxPWM为可设置的最大占空比数值，MaxAngle为最大控制角度，如果|e|大于MaxAngle，则直接进行原地转向，设定为15°-30°，AngleSpd为陀螺仪读取的当前偏航角速度，计算出速度差后，对速度差进行限幅后设置期望占空比，再通过速度闭环控制函数计算最终设置的占空比进行设置；

所述步骤3中，避障运动的具体过程为：

转向方式为左转和右转，左转的控制为左轮占空比-100和右轮占空比500，右转的控制为左轮占空比500和右轮占空比-100，如果上一次直线运行时间小于上上次直线运行时间且上次直线运行时间小于5s则不切换转向方式，使用与上一次相同的转向方式，否则，使用与上一次不同的转向方式，如果到床边后运行线路与床边平行，无法触发上述逻辑进行掉头，如果在转向过程中遇到跌落，则下次转向不切换转向方向，使机器掉头；

当机器角度与下一条线路目标角度的夹角小于30°时，停止转向，切换为直线运动控制； 所述步骤4中控制转向运动的具体过程为：

悬崖位置表达为以轮轴中间点为坐标原点，机器头部朝向方向为0°角度构建机器局部极坐标，以局部极坐标的角度θ表示悬崖的方位，

悬崖位置判断：如果倾斜，则通过陀螺仪翻滚角及俯仰角确定悬崖方向，悬崖相对于机器头部方向的夹角θ的正切等于俯仰角除以翻滚角，故可得公式： ，

如果没有倾斜，只有红外地检触发，则根据所有触发地检在机器局部极坐标中的角度的平均角度作为悬崖方位，

由于倾斜时，所计算出来的悬崖相对于机器的位置，是近似垂直于床边的，故返回床面时，锁定机器的运行角度到悬崖所在方向进行后退运动，障碍的判断仅在直线运动及转向控制中没有检测到跌落的情况下处理，判断依据为：

直线运动2s内位移小于5mm；转向运动中2s内角度小于5°；检测到轮子电流超过设定保护值2s；机器前进方向加速度向前进方向反向突变，加速度为x轴加速度， 判断到障碍物后保持当前角度进行后退， 避障的结束：在后退过程中不断采样前进方向上的加速度，积分后得到各个时刻的速度，对速度进行积分计算后退的位移，当后退位移大于5cm且红外地检正常则完成悬崖规避。

2.根据权利要求1所述的一种用于床面自主移动机器人的弓字形路径规划控制方法，其特征在于：控制直线运动的速度为0.1m/s。