一种地下线缆机器人小车维修编队控制装置及方法

1.本发明涉及机器人控制技术领域，具体地是一种地下线缆机器人小车维修编队控制装置及方法，该装置与方法是基于压力传感器的低成本机器人小车编队控制装备的硬件和软件设计。


背景技术：

2.随着科学进步，智能机器人小车技术与无人控制机器人小车编队技术的发展为高度危险、人员无法长时间执行等领域的自动化维修技术的进步奠定了基础。地下线缆是目前世界通用的城市电力系统布局走线方式，这种方法将电力线缆集中在一个空间狭小的线缆通道中，有利于高度节省空间。但是地下线缆槽或者线缆通道空间过于狭小，维修或者维护的时候不利于人员通行，因此日常维护及故障抢修极其困难，通常需要将故障段挖掘暴露后方能维修，维修后还需要重新回填土方。在现有技术中，目前多数的主流编队控制算法有：
3.(1)领航者-跟随者算法
4.领航者-跟随者算法只设定领航者的期望路径，通过信息交互控制跟随者的行动。
5.(2)虚拟结构算法
6.虚拟结构算法将整个队列视作一个刚性整体，车与车之间有极强的队形反馈机制，虚拟结构算法编队整齐，队形不容易被打乱，但是对障碍物的规避能力不佳。
7.(3)基于行为的编队算法
8.基于行为的编队算法将任务分解来达成编队目标，将总体编队要求分解到每一台机器人小车个体，此外基于行为的编队算法要求其中每台都具有一定的自主判断能力，即具有一定的外界感知能力、判断能力和自主运动能力。
9.分析以上这些编队控制算法：
10.(1)编队中机器人小车单体之间必须可以通信，并且对通信的要求很高，当通信受到干扰时，将直接影响编队的控制效果；
11.(2)编队控制成本较高，为实现上述编队控制算法，编队中每台机器人小车一般具有激光雷达、高精度声呐、高精度mems传感器，在这种开阔场合的编队控制的精度与成本直接相关。
12.地下线缆维修场合，维修编队机器人小车工作在光线阴暗、封闭狭窄、路径复杂的地下空间，并且这个地下空间会受地震、坍塌、积尘、积水、堵塞等干扰。因此上述极端依赖通信的编队算法会由于复杂的地形变化从而导致编队解列，每台单机均具有一定智能性的算法可以独立控制每台个体，但是整体成本极高，不易普及。在地下线缆槽/管道这种复杂的地下迷宫环境，需要一种低通信依赖/无通信、低成本的全自动维修装置。该装置能够不过分依赖外界导引，具有一定程度自主智能、无人值守、全天候、全自动更换故障线缆的维护装置。


技术实现要素：

13.本发明针对地下线缆日常巡检与维修领域，设计了一种低成本的自动化机器人小车编队维护装置。本发明所述的装置可以不破坏地下线缆槽或者线缆通道的结构，自动行驶至故障位置自动进行故障线缆的跟换工作，为维护电力系统的运行稳定性，提高故障维修效率，降低人员工作量具有巨大的作用；本发明涉及编队控制算法，编队算法用于多台机器人小车共同拖拽电力线缆组成特定的队形行驶在线缆槽中。
14.本发明是基于压力传感器的低成本地下线缆维修机器人小车编队控制装置及方法，所述编队中所有跟随机器人小车均没有通信模块，仅通过压力传感器判断编队中每台机器人小车的位置状态，从而控制每台机器人小车的行驶方向和速度，达到队形维持与按预设路径行驶的目的。
15.本发明采用的具体技术方案如下：
16.一种地下线缆机器人小车维修编队控制装置，包括用来连接各个机器人小车的柔性线缆和设置在各个机器人上的压力传感器，用柔性线缆连接的机器人小车编队的第一台机器人小车和最后一台机器人小车设定为领航机器人小车，其余机器人小车设定为跟随机器人小车，领航机器人小车上设置有导航系统和压力传感器，跟随机器人小车上设置有速度模型和压力传感器，该速度模型包括陀螺仪和编码器。
17.本发明的进一步改进，上述压力传感器包括线性套管，该线性套管上设置有两个间隔一段距离的固定钮，固定钮用来固定柔性线缆，在线性套管的底部设置有复位弹簧和拉力传感器，拉力传感器通过复位弹簧获知拉力的变化，上述压力传感器还包括一个可以自由旋转的结构设定为旋转机构，该旋转机构的上半部分固定有线性套管、压力传感器与复位弹簧，该旋转机构的下半部分固定安装在机器人小车的顶部，旋转机构的上半部分和旋转机构的下半部分之间安装有绝对值式光码盘或者多组霍尔传感器。
18.本发明还披露了一种地下线缆机器人小车维修编队控制方法，具体包括以下步骤：
19.步骤1定义机器人小车坐标系及其运动变量；
20.步骤2计算左右驱动轮转速实现机器人小车运动控制；
21.步骤3地下线缆维修用的机器人小车蛇形编队建立；
22.步骤4设计用于无信息通信的机器人小车压力传感器；
23.步骤5线性蛇形编队中任意机器人小车的队形调整控制；
24.步骤6线性蛇形编队在线缆槽中的定点转弯控制。
25.本发明的进一步改进，在上述步骤1中，将地下线缆槽的电子地图定义为世界坐标系e-xy。设单台机器人小车的线速度为v，机器人小车车身的宽度为d，在世界坐标系下的倾斜角为θ，绕世界坐标系z轴逆时针方向倾斜角θ变化的角速度为ω。设离散控制采样时间段为δt，控制器计算过程中设机器人小车的左右驱动轮移动速度分别为v
l
和vr，行驶的路程分别为δs
l
和δsr，θ
*
是机器人小车采样时间δt时段内的角度变化。机器人小车驱动轮电机安装有同轴转速传感器，通常是光码盘或者霍尔传感器，通过同轴转速传感器记录当前电机的转速，转速与定时器累积时间相乘可以得到当前行驶的流程。
26.本发明的进一步改进，在上述步骤2中，离散控制采样时间段δt实际上微处理器计算一个周期所需要的时长，随着技术的进步，所需的计算时长通常较短。因此可以忽略机
器人小车转弯时的弧度，将左右驱动轮经过的路程δs
l
和δsr等效为直线段，得到公式1：
[0027][0028]
公式1中，机器人小车的线速度v和角速度ω为输入量，左右驱动轮移动速度v
l
和vr为输出值。即如果知晓机器人小车运行时的线速度v和角速度ω就可以对其进行驱动控制。
[0029]
本发明的进一步改进，在上述步骤3中，由左至右将所有机器人小车进行编号，编号由1开始，逐个递增。设编队中机器人小车的数量为n，编队的首尾端均为领航机器人小车，因此左首第一辆机器人小车为领航机器人小车，编号为1，右首最后一辆机器人小车也为领航机器人小车，编号为n。设k为蛇形编队中任意的一台机器人小车的编号，由此可见：
[0030][0031]
由公式2可知，当1《k《n时，k为跟随机器人小车。
[0032]
蛇形编队中第k辆机器人小车跟随第k-1辆机器人小车前进，由此可见，正序蛇形编队中第k-1辆机器人下车为前车，第k辆机器人小车为后车。定义为第k辆机器人小车相对于世界坐标系的角度，vk为其线速度，ωk为其角速度。表示第k辆机器人小车的受力方向，fk表示第k辆机器人小车的受力大小。
[0033]
本发明的进一步改进，在上述步骤3中，具体的数据计算步骤如下：
[0034]
步骤3.1数据接收：第k辆后车小车由压力传感器数据接收得到fk，由陀螺仪，编码器得到后车的线速度vk和角度
[0035]
步骤3.2数据计算：将接收得到的数据，按以下方式代入计算。
[0036]
步骤3.2.1当fk》f
occ
时，后车检测到其与前车拉力大于合适距离所对应的拉力，即第k辆后车与第k-1辆前车距离过远。并结合受力角度计算第k辆后车的线速度vk和角速度ωk。其中为判断大角度转弯的阈值，可以根据实际进行设定。
[0037]
①
当大于阈值时，采用大角度转弯的计算方式计算第k辆后车的线速度vk和角速度ωk。
[0038][0039]
公式3中α为线速度灵敏度系数，β为角速度灵敏度系数，为大角度转弯时角速度的补偿系数，通过min函数(两值比较取最小)限制后车的线速度为v
max
。
[0040]
②
当小于阈值时，按编队调整的计算方式计算第k辆后车的线速度vk和角速度ωk。即：
[0041][0042]
步骤3.2.2当f
kt
《f
occ
时，后车检测到其与前车拉力小于合适距离所对应的拉力，即第k辆后车与第k-1辆前车距离过近，需要调整后车使其拉大距离。
[0043][0044]
公式5中rc为预设的安全距离，l为编队时两车的理想距离。通过max函数(两值比较取最大)限制后车的线速度为v
max
。
[0045]
步骤3.3带入速度模型求取左右轮速度：根据公式1可以求得后车的线速度vk和角速度ωk。即：
[0046][0047]
公式6中v
kl
，v
kr
分别为第k辆机器人小车的左右轮速度。
[0048]
重复所述的3.1、3.2、3.3三个步骤，得到蛇形编队中任意的第k辆小车的控制方式。
[0049]
本发明的进一步改进，在上述步骤4包括：
[0050]
步骤4.1设计压力传感器硬件；
[0051]
步骤4.2压力传感器的数学建模设计；
[0052]
其中，步骤4.2压力传感器的数学建模设计设前后车距离合适时受力数值f
occ
对应的前后车之间的距离为l。建立弹簧伸长量与受力之间关系的公式如下：
[0053][0054]
公式7中τ为弹簧系数，f
l
为受力大小，δl为弹簧伸长量。
[0055]
在世界坐标系e-xy下描述前后车的位置，因此前车行进角度后车行进角度后车所受的拉力角度均以世界坐标系中x轴为基准进行描述。前车行进速度v
前车
，后车行进速度v
后车
，后车受到的前车拉力f
l
。
[0056]
分解前车和后车的速度，得到公式8：
[0057]
δl＝v
前车
δtcos(θ
前车-θf)-v
后车
δtcos(θ
后车-θf)
ꢀꢀ
(8)
[0058]
公式8反映了弹簧伸缩量与前后车角度、受力角度之间的关系。
[0059]
当本发明所述的压力传感器测得受力大小f
l
和受力方向后，联立公式7和公式8，可以得到拉力f
l
，和v
前车
，v
后车
的关系式：
[0060]fl
＝τ(v
前车
δtcos(θ
前车-θf)-v
后车
δtcos(θ
后车-θf))+f
occ
ꢀꢀ
(9)
[0061]
如此，通过本发明所设计的压力传感器可以建立前后机器人小车当前位置与拉力的关系。
[0062]
本发明的进一步改进，在上述步骤5中，后车根据所装设的压力传感器所检测的受力大小f
l
和受力方向对自身的线速度v
后车
和角度进行调整，具体地说：
[0063]
步骤5.1当拉力过大时候的处理，计算后车需要后续调整的速度：
[0064]
后车需根据受力大小加速以缩短两车的距离。结合公式9可以求得弹簧伸长量δl与后车线速度v
后车
的关系公式10：
[0065]v后车
＝v
后车
+αδl
ꢀꢀ
(10)
[0066]
公式10反应了δl》0时，线速度与弹簧伸长量之间的关系，α为后车线速度灵敏度系数。
[0067]
结合公式7得到拉力过大时编队的后车速度控制方程如公式11，通过min函数(两值选其中最小的值)限定后车线速度最大值。
[0068][0069]
公式11中v
max
为后车线速度最大值。
[0070]
如此，通过公式11建立在拉力过大时，后车线速度与受力大小的关系。
[0071]
步骤5.2当拉力适合时候的处理：不需要任何动作。
[0072]
步骤5.3当拉力过小时候的处理，计算后车需要后续调整的速度：
[0073]
设蛇形编队时前后机器人小车间的预设距离为l，rc为安全距离，即两车之间允许的最短距离。为保证前后机器人小车之间的距离大于安全距离rc，需要设计后车速度控制：
[0074][0075]
将公式12代入公式7得到拉力过大时的后车速度控制方程：
[0076][0077]
公式13中v
min
为后车线速度最小值，后车的速度通过max函数求取，max函数在函数括号中的两值之间选取其中最大的值。通过公式13可以建立拉力过小时，后车线速度与受力之间的关系。
[0078]
步骤5.4根据受力方向计算后车角速度实现蛇形编队微调：
[0079]
蛇形编队行驶在线缆槽中，由于受到地面颠簸，陀螺仪误差累积，编队中前后车会出现方向不一致的问题，即压力传感器检测到的受力方向发生变化，此时需要根据受力方向重新计算后车的角速度实现蛇形编队的队形微调。
[0080]
步骤5.4.1当本发明所设计的压力传感器检测到拉力过大时，后车需要根据传感器得到的受力方向θf调整角度：
[0081]
ω
后车
＝(θ
后车-θf)β
ꢀꢀ
(14)
[0082]
由公式14可以计算出后车调整的角速度，其中β为后车角速度灵敏度系数。
[0083]
步骤5.4.2当压力传感器检测到前后车之间拉力过小时，为了保证队形不变且两个机器人小车不会相撞，在减速的同时后车角度不做调整，即：
[0084]
ω
后车
＝0
ꢀꢀ
(15)。
[0085]
本发明的进一步改进，在上述步骤6中，后车通过检测压力传感器的拉力方向与陀螺仪所感知的自身角度之间的差值，来推断前车转弯的角度和位置，从而寻找定点转弯的转弯定点。
[0086]
设为后车自身角度与拉力方向之间差值的绝对值。
[0087]
当时，为预设的转弯阈值，后车自身角度与拉力方向差值的绝对值小于该阈值，则此时后车与前车在一条直线，不存在定点转弯。
[0088]
当后车行驶至d点位置时，后车自身角度与拉力方向存在极大的差异，此时由此可知前车发生大幅度转弯。
[0089]
需要计算此时后车定点转弯时的线速度v
后车
和角速度ω
后车
。
[0090][0091]
公式16中为角速度的补偿系数。
[0092]
由此，通过实时检测后车自身角度与拉力方向的差值的变化，从而判断前车大角度转弯的时刻，对蛇形编队中的后车进行控制，实现定点转弯。
[0093]
本发明的有益效果：
[0094]
①
与目前编队控制方案相比，本发明仅用压力传感器就可以实现地下线缆维修机器人小车的有序编队，完全摒弃了编队之间的通信模块，不存在复杂环境下通信受限问题。
[0095]
②
地下线缆位置装置采用蛇形编队，这种形式极其适用于空间狭小，长度极长的地下线缆槽。此外，这种形式中一个或者多个机器人小车故障时不影响整体编队的稳定性，即编队具有冗余性。
[0096]
③
编队中的机器人小车仅通过特殊设计的压力传感器模块检测相互之间的距离和方向，并由此推算蛇形编队中每台机器人小车的状态，从而输出编队控制信息。本发明所述方法尽管牺牲了部分定位精度，但是装置成本极低，更重要的是所述方法适用于环境恶劣，运行精度要求相对较低的地下线缆槽环境。
[0097]
④
本发明所设计的自适应蛇形编队算法，可以在保证线缆完好度的前提下提供运送整体线缆所需的牵引力，实现编队定点转弯，能够自动行驶到故障位置进行更换维修作业。
附图说明
[0098]
图1为本发明中编队整体示意图。
[0099]
图2为本发明中机器人小车蛇形编队的硬件结构拓扑图。
[0100]
图3为本发明中单个机器人小车运动坐标系示意图。
[0101]
图4为本发明中机器人小车运动情况示意图。
[0102]
图5为本发明中机器人小车左右轮转速计算示意图。
[0103]
图6为本发明中控制算法拓扑图。
[0104]
图7为本发明中压力传感器整体结构图。
[0105]
图8为本发明中机器人小车运动位置示意图。
[0106]
图9为本发明中拉力较大时前后机器人小车位置示意图。
[0107]
图10为本发明中拉力过小时前后机器人小车位置示意图。
[0108]
图11为本发明中机器人小车的定点转弯阶段1示意图。
[0109]
图12为本发明中机器人小车的定点转弯阶段2示意图。
具体实施方式
[0110]
为了加深对本发明的理解，下面将结合附图和实施例对本发明做进一步详细描述，该实施例仅用于解释本发明，并不对本发明的保护范围构成限定。
[0111]
实施例：如图1所示，编队中机器人小车之间仅通过本专利设计的压力传感器和柔性线缆连接，机器人小车编队行驶在复杂地下线缆槽时，本专利通过每个机器人小车所装设的压力传感器检测所受到的拉扯力大小和方向，通过计算后完成自身的路线调整，从而实现对前车的跟随。多台机器人小车首尾相连形成串联蛇形编队。
[0112]
如图2所示为机器人小车蛇形编队的硬件结构拓扑图，整个蛇形编队的头车(第一台机器人小车)和尾车(最后一台机器人小车)都是具有导航系统和压力传感器的领航机器人小车，蛇形编队头车和尾车之间均为跟随机器人小车。为降低系统成本，跟随机器人小车不具备导航功能，仅搭载陀螺仪，编码器和压力传感器。即本专利所述的低成本地下线缆维修蛇形编队机器人小车控制装置中，所有的机器人小车之间没有通信模块，仅具有本专利特殊设计的压力传感器，机器人小车之间通过压力传感器、陀螺仪和光线编码器(测量驱动电机转速)来判断相互之间的距离与角度，从而给出每台机器人小车的控制指令。
[0113]
步骤1定义机器人小车坐标系及其运动变量：
[0114]
如图3所示，将地下线缆槽的电子地图定义为世界坐标系e-xy。设单台机器人小车的线速度为v，机器人小车车身的宽度为d，在世界坐标系下的倾斜角为绕世界坐标系z轴逆时针方向倾斜角变化的角速度为ω。
[0115]
如图4所示，设离散控制采样时间段为δt，控制器计算过程中设机器人小车的左右驱动轮移动速度分别为v
l
和vr，行驶的路程分别为δs
l
和δsr，是机器人小车采样时间δt时段内的角度变化。机器人小车驱动轮电机安装有同轴转速传感器，通常是光码盘或者霍尔传感器，通过同轴转速传感器记录当前电机的转速，转速与定时器累积时间相乘可以得到当前行驶的流程。
[0116]
步骤2计算左右驱动轮转速实现机器人小车运动控制：
[0117]
如图5所示，离散控制采样时间段δt实际上微处理器计算一个周期所需要的时长，随着技术的进步，所需的计算时长通常较短。因此可以忽略机器人小车转弯时的弧度，将左右驱动轮经过的路程δs
l
和δsr等效为直线段，得到公式1：
[0118]
[0119]
公式1中，机器人小车的线速度v和角速度ω为输入量，左右驱动轮移动速度v
l
和vr为输出值。即如果知晓机器人小车运行时的线速度v和角速度ω就可以对其进行驱动控制。
[0120]
步骤3地下线缆维修用的机器人小车蛇形编队建立：
[0121]
如图2所示，由左至右将所有机器人小车进行编号，编号由1开始，逐个递增。设编队中机器人小车的数量为n，编队的首尾端均为领航机器人小车，因此左首第一辆机器人小车为领航机器人小车，编号为1，右首最后一辆机器人小车也为领航机器人小车，编号为n。设k为蛇形编队中任意的一台机器人小车的编号，由此可见：
[0122][0123]
由公式2可知，当1《k《n时，k为跟随机器人小车。
[0124]
蛇形编队中第k辆机器人小车跟随第k-1辆机器人小车前进，由此可见，正序蛇形编队中第k-1辆机器人下车为前车，第k辆机器人小车为后车。定义为第k辆机器人小车相对于世界坐标系的角度，vk为其线速度，ωk为其角速度。表示第k辆机器人小车的受力方向，fk表示第k辆机器人小车的受力大小。
[0125]
如图6所示为控制算法拓扑图，对应具体的数据计算步骤如下：
[0126]
步骤3.1数据接收：第k辆后车小车由压力传感器数据接收得到fk，由陀螺仪，编码器得到后车的线速度vk和角度
[0127]
步骤3.2数据计算：将接收得到的数据，按以下方式代入计算。
[0128]
步骤3.2.1当fk》f
occ
时，后车检测到其与前车拉力大于合适距离所对应的拉力，即第k辆后车与第k-1辆前车距离过远。并结合受力角度计算第k辆后车的线速度vk和角速度ωk。其中为判断大角度转弯的阈值，可以根据实际进行设定。
[0129]
①
当大于阈值时，采用大角度转弯的计算方式计算第k辆后车的线速度vk和角速度ωk。
[0130][0131]
公式3中α为线速度灵敏度系数，β为角速度灵敏度系数，为大角度转弯时角速度的补偿系数，通过min函数(两值比较取最小)限制后车的线速度为v
max
。
[0132]
②
当小于阈值时，按编队调整的计算方式计算第k辆后车的线速度vk和角速度ωk。即：
[0133][0134]
步骤3.2.2当f
kt
《f
occ
时，后车检测到其与前车拉力小于合适距离所对应的拉力，即第k辆后车与第k-1辆前车距离过近，需要调整后车使其拉大距离。
[0135][0136]
公式5中rc为预设的安全距离，l为编队时两车的理想距离。通过max函数(两值比较取最大)限制后车的线速度为v
max
。
[0137]
步骤3.3带入速度模型求取左右轮速度：根据公式1可以求得后车的线速度vk和角速度ωk。即：
[0138][0139]
公式6中v
kl
，v
kr
分别为第k辆机器人小车的左右轮速度。
[0140]
重复所述的3.1、3.2、3.3三个步骤，得到蛇形编队中任意的第k辆小车的控制方式。
[0141]
步骤4设计用于无信息通信的机器人小车压力传感器：
[0142]
步骤4.1设计压力传感器硬件：
[0143]
本专利这里所述的压力传感器需要检测编队中机器人小车任意时刻所受力的大小fk，性质(拉力/斥力，拉力表示前后车距离较远，斥力表示前后车距离较近)和受力方向
[0144]
如图7所示，图7是为测量本专利所述蛇形编队前后车所受拉力、拉力性质、方向所设计的压力传感器。线缆通过固定钮固定在压力传感器上，压力传感器设计成线性套管，两个固定钮间隔一段空间，这段空间容纳部分空闲线缆。即线缆安装后，并不是拉紧状态，其在两个固定钮之间有部分松散状态的线缆，这种安装保证了两车距离变远后线缆具有一定拉伸的裕量。在线性套管的底部设置有复位弹簧，拉力传感器通过复位弹簧获知拉力的变化。注意，这里复位弹簧的初试位置并不是零点，而是一个负值-f
ε
，f
ε
值可以根据实际需要进行设定。复位弹簧的初试负值-f
ε
是因为本专利所述蛇形编队中前后车初试状态为距离过近，根据步骤3所述的编队政策调整前后车的距离，使其拉力至为f
occ
，此时才是编队中前后车距离合适。当两车之间的距离过远，线缆拉力会变大，拉力传感器输出值将大于f
occ
，即需要根据步骤3缩短前后车的距离。当后车和前车的距离过近，线缆会在复位弹簧的作用下又变为负值，拉力传感器输出值将小于f
occ
，即需要根据步骤3拉长前后车的距离。
[0145]
除此之外，本专利所设计的压力传感器还设计了一个可以自由旋转的结构。上面所述的套管、压力传感器与复位弹簧均安装在可以旋转的上半部分，固定的底座安装在机器人小车的顶部。两部分之间安装有绝对值式光码盘或者多组霍尔传感器。绝对值式光码盘可用于高精度的旋转角度测量，霍尔传感器组可用于低精度的旋转角度测量。
[0146]
通过上述方式，本专利所设计的压力传感器可以检测出编队中机器人小车任意时刻所受力的大小fk，拉力性质和受力方向
[0147]
步骤4.2压力传感器的数学建模设计：
[0148]
设前后车距离合适时受力数值f
occ
对应的前后车之间的距离为l。建立弹簧伸长量与受力之间关系的公式如下：
[0149][0150]
公式7中τ为弹簧系数，f
l
为受力大小，δl为弹簧伸长量。
[0151]
图8为两个机器人小车运动时位置示意图。图8显示了任意时刻前车与后车之间的位置关系。如图8所示，在世界坐标系e-xy下描述前后车的位置，因此前车行进角度后车行进角度后车所受的拉力角度均以世界坐标系中x轴为基准进行描述。前车行进速度v
前车
，后车行进速度v
后车
，后车受到的前车拉力f
l
。
[0152]
分解前车和后车的速度，得到公式8：
[0153]
δl＝v
前车
δtcos(θ
前车-θf)-v
后车
δtcos(θ
后车-θf)
ꢀꢀ
(8)
[0154]
公式8反映了弹簧伸缩量与前后车角度、受力角度之间的关系。
[0155]
当本专利所述的压力传感器测得受力大小f
l
和受力方向后，联立公式7和公式8，可以得到拉力f
l
，和v
前车
，v
后车
的关系式：
[0156]fl
＝τ(v
前车
δtcos(θ
前车-θf)-v
后车
δtcos(θ
后车-θf))+f
occ
ꢀꢀ
(9)
[0157]
如此，通过本专利所设计的压力传感器可以建立前后机器人小车当前位置与拉力的关系。
[0158]
步骤5线性蛇形编队中任意机器人小车的队形调整控制：
[0159]
在一段直线线缆槽中，蛇形编队中每一台机器人小车都可能存在与前车距离合适、距离近、距离远、角度偏离情况。此时需要后车根据所装设的压力传感器所检测的受力大小f
l
和受力方向对自身的线速度v
后车
和角度进行调整。
[0160]
步骤5.1当拉力过大时候的处理，计算后车需要后续调整的速度：
[0161]
如图9为拉力较大时，前后两个机器人小车的位置示意图，此时压力传感器中复位弹簧的伸长量δl》0。此时需要缩短前后两车的距离，使压力传感器所受到的拉力值下降，使复位弹簧形变距离变短。
[0162]
后车需根据受力大小加速以缩短两车的距离。结合公式9可以求得弹簧伸长量δl与后车线速度v
后车
的关系公式10：
[0163]v后车
＝v
后车
+αδl
ꢀꢀ
(10)
[0164]
公式10反应了δl》0时，线速度与弹簧伸长量之间的关系，α为后车线速度灵敏度系数。
[0165]
结合公式7得到拉力过大时编队的后车速度控制方程如公式11，通过min函数(两值选其中最小的值)限定后车线速度最大值。
[0166][0167]
公式11中v
max
为后车线速度最大值。
[0168]
如此，通过公式11建立在拉力过大时，后车线速度与受力大小的关系。
[0169]
步骤5.2当拉力适合时候的处理：不需要任何动作。
[0170]
步骤5.3当拉力过小时候的处理，计算后车需要后续调整的速度：
[0171]
如图10为拉力较小时，前后两个机器人小车的位置示意图，此时压力传感器中复位弹簧的伸长量δl《0。此时需要增加前后两车的距离，使压力传感器所受到的拉力值上升，使复位弹簧形变距离变长。
[0172]
设蛇形编队时前后机器人小车间的预设距离为l，rc为安全距离，即两车之间允许的最短距离。为保证前后机器人小车之间的距离大于安全距离rc，需要设计后车速度控制：
[0173][0174]
将公式12代入公式7得到拉力过大时的后车速度控制方程：
[0175][0176]
公式13中v
min
为后车线速度最小值，后车的速度通过max函数求取，max函数在函数括号中的两值之间选取其中最大的值。通过公式13可以建立拉力过小时，后车线速度与受力之间的关系。
[0177]
步骤5.4根据受力方向计算后车角速度实现蛇形编队微调：
[0178]
蛇形编队行驶在线缆槽中，由于受到地面颠簸，陀螺仪误差累积，编队中前后车会出现方向不一致的问题，即压力传感器检测到的受力方向发生变化，此时需要根据受力方向重新计算后车的角速度实现蛇形编队的队形微调。
[0179]
步骤5.4.1当本专利所设计的压力传感器检测到拉力过大时，后车需要根据传感器得到的受力方向调整角度：
[0180]
ω
后车
＝(θ
后车-θf)β
ꢀꢀ
(14)
[0181]
由公式14可以计算出后车调整的角速度，其中β为后车角速度灵敏度系数。
[0182]
步骤5.4.2当压力传感器检测到前后车之间拉力过小时，为了保证队形不变且两个机器人小车不会相撞，在减速的同时后车角度不做调整，即：
[0183]
ω
后车
＝0
ꢀꢀ
(15)
[0184]
步骤6线性蛇形编队在线缆槽中的定点转弯：
[0185]
当蛇形编队在地下线缆槽中行进时，线缆槽空间有限，为了避免碰到角度较大的拐角时，容易造成两车卡死在拐角上，造成行进故障。因此针对地下线缆槽直角弯、锐角弯的特点设计定点转弯控制方法。
[0186]
如图11所示，后车通过检测压力传感器的拉力方向与陀螺仪所感知的自身角度之间的差值，来推断前车转弯的角度和位置，从而寻找定点转弯的转弯定点。
[0187]
设为后车自身角度与拉力方向之间差值的绝对值。
[0188]
当时，为预设的转弯阈值，后车自身角度与拉力方向差值的绝对值小于该阈值，则此时后车与前车在一条直线，不存在定点转弯。
[0189]
如图12所示，当后车行驶至d点位置时，后车自身角度与拉力方向存在极大的差异，此时由此可知前车发生大幅度转弯。
[0190]
需要计算此时后车定点转弯时的线速度v
后车
和角速度ω
后车
。。
[0191][0192]
公式16中为角速度的补偿系数。
[0193]
由此，通过实时检测后车自身角度与拉力方向的差值的变化，从而判断前车大角度转弯的时刻，对蛇形编队中的后车进行控制，实现定点转弯。
[0194]
以上显示和描述了本发明的基本原理、主要特征及优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。