一种地形自适应移动机器人升降架的控制方法
阅读说明：本技术 一种地形自适应移动机器人升降架的控制方法 (Control method of terrain adaptive mobile robot lifting frame ) 是由 董辉 田叮 吴宇航 吴祥 袁登鹏 董浩 周俊阳 于 2020-05-13 设计创作，主要内容包括：本发明公开了一种地形自适应移动机器人升降架的控制方法,包括获取移动机器人的角度和角速度；通过所述距离传感器获得距离传感器与地面之间的间距,根据该间距计算前后位置的电动推杆各自的目标伸长距离；根据目标伸长距离驱动前后位置的电控支架的电动推杆伸长,并读取编码器的数值,根据编码器的数值计算电动推杆当前的实际伸长距离；根据电动推杆的伸长距离和移动机器人的角度控制升降架。本发明有效解决了移动机器人在自启动时失衡摔倒的问题。(The invention discloses a control method of a terrain self-adaptive mobile robot lifting frame, which comprises the steps of obtaining the angle and the angular speed of a mobile robot; obtaining the distance between the distance sensor and the ground through the distance sensor, and calculating the respective target extension distance of the electric push rods at the front and rear positions according to the distance; driving an electric push rod of the electric control bracket at the front and rear positions to extend according to the target extension distance, reading the numerical value of the encoder, and calculating the current actual extension distance of the electric push rod according to the numerical value of the encoder; and the lifting frame is controlled according to the extension distance of the electric push rod and the angle of the mobile robot. The invention effectively solves the problem that the mobile robot falls down in unbalance when self-starting.)
技术领域
本申请属于移动机器人技术领域，具体涉及一种地形自适应移动机器人升降架的控制方法。
背景技术
伴随着社会的快速发展，机器人被越来越多的应用于当前的生产和生活中。轮式移动机器人具有驱动和控制较方便、自重轻、行走速度快，机构简单、工作效率高、机动灵活等多种优点，仍被农业、工业、家庭、空间探测、反恐防爆等领域广泛应用。两轮移动机器人的灵活性较强，行走上和两足机器人比较相似，在机器人领域有着广泛的应用。现有的两轮移动机器人在开机的启动的过程中，需要人为干预才能够进入稳定状态，并且是否能进入稳定状态以及进入稳定状态的速度与两轮移动机器人所处的地形有较大关系，还存在移动机器人不能够完全自主启动和遇到故障问题出现失去平衡摔倒等问题。
发明内容
本申请的目的在于提供一种地形自适应移动机器人升降架的控制方法，有效解决了移动机器人在自启动时失衡摔倒的问题。
为实现上述目的，本申请所采取的技术方案为：
一种地形自适应移动机器人升降架的控制方法，所述升降架包括两个分别安装在移动机器人前后位置的电控支架，各所述电控支架包括固定在移动机器人上的框架主体、固定在框架主体上且纵向伸缩的电动推杆、与所述电动推杆的驱动电机连接的编码器、以及安装在框架主体上用于测量与地面之间间距的距离传感器，所述地形自适应移动机器人升降架的控制方法，包括自启动控制方法如下：
步骤S1、获取移动机器人的角度和角速度；
步骤S2、通过所述距离传感器获得距离传感器与地面之间的间距，根据该间距计算前后位置的电动推杆各自的目标伸长距离；
步骤S3、根据目标伸长距离驱动前后位置的电控支架的电动推杆伸长，并读取编码器的数值，根据编码器的数值计算电动推杆当前的实际伸长距离；
步骤S4、根据电动推杆的伸长距离和移动机器人的角度控制升降架，包括：
若满足第一条件，则前后位置的电控支架的电动推杆均停止伸长，并根据所述角度判断移动机器人的倾斜状态，若移动机器人为前倾，则安装在前端的电动推杆继续伸长，安装在后端的电动推杆收回；若移动机器人为后倾，则安装在后端的电动推杆继续伸长，安装在前端的电动推杆收回；
若满足第二条件，则前后位置的电控支架的电动推杆均停止伸长，延时a秒后判断仍满足第二条件，则前后位置的电控支架的电动推杆均收回；
若即不满足第一条件，也不满足第二条件，则重新执行步骤S3；
其中，所述第一条件包括：前后位置的电动推杆的实际伸长距离均大于或等于目标伸长距离，同时移动机器人的角度在(-b)～(-c)或c～b度之间，并且c<b；所述第二条件包括：前后位置的电动推杆的实际伸长距离均大于或等于目标伸长距离，同时移动机器人的角度在(-c)～c度之间。
作为优选，通过所述距离传感器获得距离传感器与地面之间的间距，根据该间距计算电动推杆的目标伸长距离，包括：
通过安装在前端的电控支架的距离传感器，获得前端距离传感器与地面之间的间距D1，通过安装在后端的电控支架的距离传感器，获得后端距离传感器与地面之间的间距D2；
计算前后端距离传感器与地面之间间距的差值为Dt：
Dt＝|D1-D2|
若D1＞D2，则设置前端的电动推杆的目标伸长距离为Dt，后端的电动推杆的目标伸长距离为0；
若D1＜D2，则设置前端的电动推杆的目标伸长距离为0，后端的电动推杆的目标伸长距离为Dt；
若D1＝D2，则设置前端的电动推杆的目标伸长距离为0，后端的电动推杆的目标伸长距离为0。
作为优选，所述若移动机器人为前倾，则安装在前端的电动推杆继续伸长，安装在后端的电动推杆收回；若移动机器人为后倾，则安装在后端的电动推杆继续伸长，安装在前端的电动推杆收回，包括：
若移动机器人为前倾，则计算驱动电机的输出控制量U(k)，并根据输出控制量U(k)驱动安装在前端的电动推杆继续伸长，同时根据输出控制量U(k)控制安装在后端的电动推杆收回；若移动机器人为后倾，则计算驱动电机的输出控制量U(k)，并根据输出控制量U(k)驱动安装在后端的电控支架的电动推杆继续伸长，同时根据输出控制量U(k)控制安装在前端的电动推杆收回；
其中计算驱动电机的输出控制量U(k)，包括：
计算电动推杆当前的伸长速度V(k)如下：
V(k)＝l×(θ(k)+ω(k))
其中，l为比例系数，θ(k)、ω(k)为当前获取移动机器人的角度和角速度；
U(k)＝KPe(k)+Kd(e(k)-e(k-1))
其中，KP、Kd为PID控制中的比例系数和微分系数，e(k)＝V(k)-V1，e(k-1)＝V(k-1)-V2，V(k)为当前计算得到的伸长速度，V(k-1)为上一次计算得到的伸长速度，V1为移动机器人根据自身编码器数值计算出的当前的运动速度，V2为移动机器人根据自身编码器数值计算出的上一次的运动速度。
作为优选，所述地形自适应移动机器人升降架的控制方法，还包括故障处理控制方法如下：
步骤B1、判断移动机器人是否存在异常：监测异常信息，若移动机器人的异常信息触发，则控制前后电动推杆以最大速度伸长至指定长度；若移动机器人的异常信息未触发，则执行步骤B2；
步骤B2、判断移动机器人的姿态是否异常：判断移动机器人当前的角度是否在(-d)～d度范围内，若当前的角度不在(-d)～d度范围内，则控制前后电动推杆以最大速度伸长至指定长度；若当前的角度在(-d)～d度范围内，则执行步骤B3；
步骤B3、判断移动机器人是否需要关机：判断是否接收到关机指令，若接收到关机指令，则控制前后电动推杆以最大速度伸长至指定长度；若没有接收到关机指令，则重新执行步骤B1，继续循环监测。
本申请提供的地形自适应移动机器人升降架的控制方法，通过检测移动机器人的角度和角速度信息，结合PID控制算法实现对移动机器人的电控支架的智能控制，从而有效解决了移动机器人自启动和防止机器人失去平衡摔倒的问题。
附图说明
图1为本申请的升降架的安装示意图；
图2为本申请的电控支架的结构示意图；
图3为本申请地形自适应移动机器人升降架的控制方法中自启动控制方法的流程图；
图4为本申请地形自适应移动机器人升降架的控制方法中故障处理控制方法的流程图。
图中附图标记说明如下：
1、移动机器人；2、电控支架；21、框架主体；22、距离传感器；23、编码器；24、电动推杆。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本申请。
其中一个实施例中，提供一种地形自适应移动机器人升降架的控制方法，提高移动机器人对地形的适应能力，有效克服了移动机器人失衡摔倒的问题。这里的地形自适应移动机器人应理解基于本申请的控制方法能够自适应地形从而保证正常工作的两轮移动机器人，简称为移动机器人。
如图1所示，本实施例的移动机器人的升降架包括两个分别安装在移动机器人1前后位置的电控支架2。
需要说明的是，本申请中提及的前后位置，应理解为以移动机器人直线前进时的方位为前方，以移动机器人直线后退时的方位为后方。
电控支架2设置在移动机器人的前后位置，实现既不影响移动机器人的正常工作，又能在移动机器人失衡时防止摔倒。
移动机器人1为常规的两轮移动机器人，通常具有姿态传感器、两个运动滚轮，且各运动滚轮连接有各自的编码器，属于移动机器人领域的常规设备，这里不对移动机器人的内部结构展开赘述。
如图2所示，各电控支架2包括固定在移动机器人上的框架主体21、固定在框架主体21上且纵向伸缩的电动推杆24，该电动推杆24包括伸缩杆和控制伸缩杆动作的驱动电机、与电动推杆24的驱动电机连接的编码器23、以及安装在框架主体21上用于测量与地面之间间距的距离传感器22。图2中A所示部位即为距离传感器22与地面之间的间距。
需要说明的是，图2仅为一种优选示意图，并不限制为电控支架2的唯一结构，在满足上述电控支架2的部件要求的基础上具有多种变形。并且每个电控支架中电动推杆的数量大于或等于1个，优选设置2个，并且如图所示的在2个电动推杆的推杆端部通过连接件进行连接，以保证电控支架的稳定支撑。
如图3所示，本实施例的地形自适应移动机器人升降架的控制方法，包括自启动控制方法如下：
步骤S1、获取移动机器人的角度和角速度。
由于移动机器人具备姿态传感器，因此可以利用姿态传感器直接获得角度和角速度。当然也可以通过单独的角度传感器、角速度传感器等采集。
步骤S2、通过所述距离传感器获得距离传感器与地面之间的间距，根据该间距计算前后位置的电动推杆各自的目标伸长距离。
目标伸长距离作为电动推杆的初始伸长高度，计算目标伸长距离时可以前后距离传感器采集的间距的差值，也可以是前后距离传感器采集的间距的倍数。
在一实施例中，计算目标伸长距离的优选方案如下：
通过安装在前端的电控支架的距离传感器，获得前端距离传感器与地面之间的间距D1，通过安装在后端的电控支架的距离传感器，获得后端距离传感器与地面之间的间距D2；
计算前后端距离传感器与地面之间间距的差值为Dt：
Dt＝|D1-D2|
若D1＞D2，则设置前端的电动推杆的目标伸长距离为Dt，后端的电动推杆的目标伸长距离为0；
若D1＜D2，则设置前端的电动推杆的目标伸长距离为0，后端的电动推杆的目标伸长距离为Dt；
若D1＝D2，则设置前端的电动推杆的目标伸长距离为0，后端的电动推杆的目标伸长距离为0。
步骤S3、根据目标伸长距离驱动前后位置的电控支架的电动推杆伸长，并读取编码器的数值，根据编码器的数值计算电动推杆当前的实际伸长距离。
需要说明的是，若计算得到的目标伸长距离为0，即对应的电动推杆无需进行伸长操作。若电控支架上中设置有多个电动推杆，则位于同一个电控支架上的多个电动推杆同步控制。
并且根据编码器的数值计算对应的长度为移动机器人运动中常规的技术手段，这里不再进行赘述。应该理解的是，计算某一对象的参数时，采用的是与该对象连接的编码器的数值。
步骤S4、根据电动推杆的伸长距离和移动机器人的角度控制升降架，包括：
若满足第一条件，则前后位置的电控支架的电动推杆均停止伸长，并根据所述角度判断移动机器人的倾斜状态，若移动机器人为前倾，则安装在前端的电动推杆继续伸长，安装在后端的电动推杆收回；若移动机器人为后倾，则安装在后端的电动推杆继续伸长，安装在前端的电动推杆收回。
若满足第二条件，则前后位置的电控支架的电动推杆均停止伸长，延时a秒后判断仍满足第二条件，则前后位置的电控支架的电动推杆均收回。
若即不满足第一条件，也不满足第二条件，则重新执行步骤S3。
其中，所述第一条件包括：前后位置的电动推杆的实际伸长距离均大于或等于目标伸长距离，同时移动机器人的角度在(-b)～(-c)或c～b度之间，并且c<b；所述第二条件包括：前后位置的电动推杆的实际伸长距离均大于或等于目标伸长距离，同时移动机器人的角度在(-c)～c度之间。
在首次判断满足第二条件，但延时后判断不满足第二条件时，根据延时后的判断结果执行相应的操作，即若延时后判断满足第一条件，则执行第一条件对应的操作；若延时后判断既不满足第一条件，也不满足第二条件，则重新执行步骤S3。
为了保证控制效果，在一实施例中，设定a＝1，b＝1.5，c＝3。在其他实施例中，a、b、c的数值可根据实际需求进行调整。
在步骤S4中满足第一条件后，表示移动机器人还没有完全平衡，因此需要对升降架继续控制，控制对应的电动推杆伸长或收回，促进移动机器人尽快达到平衡状态。
在一实施例中，提供的继续控制的优选方案如下：
若移动机器人为前倾，则计算驱动电机的输出控制量U(k)，并根据输出控制量U(k)驱动安装在前端的电动推杆继续伸长，同时根据输出控制量U(k)控制安装在后端的电动推杆收回；若移动机器人为后倾，则计算驱动电机的输出控制量U(k)，并根据输出控制量U(k)驱动安装在后端的电控支架的电动推杆继续伸长，同时根据输出控制量U(k)控制安装在前端的电动推杆收回。
其中计算驱动电机的输出控制量U(k)，包括：
计算电动推杆当前的伸长速度V(k)如下：
V(k)＝l×(θ(k)+ω(k))
其中，l为比例系数，θ(k)、ω(k)为当前获取移动机器人的角度和角速度；
U(k)＝KPe(k)+Kd(e(k)-e(k-1))
其中，KP、Kd为PID控制中的比例系数和微分系数，e(k)＝V(k)-V1，e(k-1)＝V(k-1)-V2，V(k)为当前计算得到的伸长速度，V(k-1)为上一次计算得到的伸长速度，V1为移动机器人根据自身编码器数值计算出的当前的运动速度，V2为移动机器人根据自身编码器数值计算出的上一次的运动速度。
在判断移动机器人的倾斜状态时，根据角度的正负即可判断，角度的正负表示移动机器人的倾斜状态不同，为移动机器人的常规设定，这里不详细进行解释。通常角度为负，移动机器人为前倾；角度为正，移动机器人为后倾。
其中比例系数l的大小可根据驱动电机的速度进行调节，比例系数l的动态调节满足比例系数l与其他控制量的乘积最大值不超过电控支架的驱动电机规定的最大值。并且驱动电机根据输出控制量控制伸缩杆的伸缩为电动推杆使用的常规过程，因此具体的驱动原理和过程不再展开描述。
在步骤S4中满足第二条件后，表示移动机器人已基本未出平衡，因此不需要对升降架继续控制，此时控制前后位置的电控支架的电动推杆均收回，应理解为前后电动推杆均收回至最短状态，最短状态可以是电动推杆机械结构上的最短状态，也可以是预设的最短伸出状态。上述控制方案即控制电动推杆根据当前的状态计算得到最佳的输出控制量，当然在其他实施例中，输出控制量也可以是预先设定的定值，或者预先设定的变化值。
本实施例提供的自启动控制方法，通过检测移动机器人姿态的角度和角速度数据，结合PID控制算法实现对移动机器人的升降架的智能控制，实现在移动机器人处于不同状态下，控制前后电动推杆的伸长或收回保持自启动时移动机器人不摔倒，软硬件的相配合，在低成本的基础下实现既不影响移动机器人的正常工作，又可防止移动机器人在自启动时失衡摔倒。
在移动机器人的日常运行中，在自启动和遇到故障时容易失衡，而自启动时的控制方法上述实施例已经描述，以下通过另一实施例提供本申请的地形自适应移动机器人升降架的控制方法，在故障时的控制。
具体的，如图4所示，本申请的地形自适应移动机器人升降架的控制方法，还包括故障处理控制方法如下：
步骤B1、判断移动机器人是否存在异常：监测异常信息，若移动机器人的异常信息触发，则控制前后电动推杆以最大速度伸长至指定长度；若移动机器人的异常信息未触发，则执行步骤B2。
步骤B2、判断移动机器人的姿态是否异常：判断移动机器人当前的角度是否在(-d)～d度范围内，若当前的角度不在(-d)～d度范围内，则控制前后电动推杆以最大速度伸长至指定长度；若当前的角度在(-d)～d度范围内，则执行步骤B3。优选设置d＝6，当然d的值可根据实际需求进行调节。
步骤B3、判断移动机器人是否需要关机：判断是否接收到关机指令，若接收到关机指令，则控制前后电动推杆以最大速度伸长至指定长度；若没有接收到关机指令，则重新执行步骤B1，继续循环监测。
本实施例中设置步骤B1～B3中的指定长度相同，在其他实施例中也可以根据实际应用场景设定各步骤中的指定长度相同或不同。
本实施例提供的故障处理控制方法，保证移动机器人在异常、失衡或关机时不摔倒，提高了移动机器人的应变能力，降低故障损坏的概率。
需要说明的是，本申请的地形自适应移动机器人升降架的控制方法可以由微控制器进行执行，并且微控制器可以是一个，用于同时控制移动机器人本身、以及升降架；微控制器也可以是两个，分别控制移动机器人本身、以及升降架。
本申请的地形自适应移动机器人升降架的控制方法具有快速性，整体移动机器人的调节稳定。根据不同的场景的突发情况，电控支架具有稳定、快速的控制。并且电控支架的作用不止为了防止机器人摔倒，在机器人自主启动、特殊的环境(例如在具有一定坡度的路面，通过分别控制前后电动推杆的伸缩距离使移动机器人稳定停在路面上)中能够稳定的工作等作用。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。