CN102183960A - 一种适用于独立式自动跟踪的局部可柔度虚杆转弯控制系统 - Google Patents

Abstract

本发明公开了一种适用于独立式自动跟踪的局部可柔度虚杆转弯控制系统，该系统包括有激光测距仪、GPS导航设备、三轴陀螺仪、码盘、摄像头、第一运动状态检测单元、第二运动状态检测单元和局部可柔虚杆自主跟踪系统。该系统首先通过激光雷达对目标机器人进行检测，得到目标机器人相对于自主跟随机器人的距离与位姿，利用自主跟随机器人的自我定位信息得到目标机器人在全局坐标系中的位置坐标，由目标机器人当前点以及历史点即可通过局部可柔度虚杆模型得到目标机器人运动的轨迹曲线，进而获知刚性段与柔性段的拐点。更能使自主跟随机器人精确地复现目标机器人的运动轨迹。

Description

一种适用于独立式自动跟踪的局部可柔度虚杆转弯控制系统

技术领域

本发明涉及一种适用于智能移动机器人的自主控制，更特别地说，是指一种适用于独立式自动跟踪的局部可柔度虚杆转弯控制系统。

背景技术

在公开号CN101947788A，公开日2011年1月19日中公开了名为“一种智能机器人”，该机器人包括人机接口模块、主控制器、环境监测模块、机器人定位模块、电机驱动模块、视觉模块、语音模块、数据存储模块、无线数据传输模块和电源管理模块；人机接口模块与用户进行人机信息交互，将接受数据传输至主控制器；环境监测模块将检测到的信息输入主控制器；机器人定位模块将外部信息传输给主控制器；电机驱动模块由主控制器控制，完成对电机驱动模块内驱动电机的伺服控制；视觉模块和语音模块将采集到的信号输入主控制器内，主控制器将采集到的各种信号均输入数据存储模块内，与数据存储模块内预先调用的服务数据包进行对比；主控制器控制无线数据传输模块按预先设定的定时时间与服务端进行信息交互；机器人内的各模块均由电源管理模块供电。

独立式自动跟踪系统与合作式系统的根本区别在于它不需要和外界进行信息的通信交互，在使用上不需要依赖目标机器人和道路设施，因此具有广泛的应用前景。由于独立式跟踪系统无法直接获得目标机器人的运动状态信息，所以无法简单的复现目标机器人的速度、加速度等信息，需要通过自动跟踪系统对目标机器人的位置、姿态信息进行检测来实现跟踪。常规方法是通过采集到的环境数据直接得到目标机器人的位置、姿态信息，然后将该信息存储在寄存器中，在跟踪过程中不断地读取已经存储的目标机器人的历史轨迹信息来进行逐点跟踪，这种方法只是采取了跟踪目标机器人轨迹上的一系列的点，本质上无法把目标机器人的轨迹完全的复现出来。

当目标机器人出现突然拐弯时，跟随机器人若不能很好处理拐弯情况，跟随机器人的运动会偏离目标机器人的轨迹，增大跟踪时的偏差，降低跟踪性能，还有可能失去跟踪目标，无法达到跟踪目的，甚至出现撞向路边或者障碍物的危险情况。

发明内容

为了解决独立式自动跟踪系统中无法把目标的轨迹完全进行复现以及转弯时的跟踪偏差和丢失目标等问题，本发明提供一种适用于独立式自动跟踪的局部可柔虚杆转弯控制系统。该系统首先通过激光雷达对目标机器人进行检测，得到目标机器人相对于自主跟随机器人的距离与位姿，利用自主跟随机器人的自我定位信息得到目标机器人在全局坐标系中的位置坐标，由目标机器人当前点以及历史点即可通过局部可柔度虚杆模型得到目标机器人运动的轨迹曲线，进而获知刚性段与柔性段的拐点。更能使自主跟随机器人精确地复现目标机器人的运动轨迹。

本发明的一种适用于独立式自动跟踪的局部可柔度虚杆转弯控制系统，该局部可柔度虚杆转弯控制系统加载在智能移动机器人的主控制器内，所述的独立式自动跟踪是指自主跟随智能移动机器人复现目标智能移动机器人的前行轨迹。该局部可柔度虚杆转弯控制系统包括有激光测距仪(1)、GPS导航设备(2)、三轴陀螺仪(3)、码盘(4)、摄像头(5)、第一预处理单元(10)、第二预处理单元(20)、第三预处理单元(30)、第四预处理单元(40)、第一运动状态检测单元(7)、第二运动状态检测单元(8)和局部可柔虚杆自主跟踪系统(6)；

激光测距仪(1)、GPS导航设备(2)、三轴陀螺仪(3)、码盘(4)和摄像头(5)安装在自主跟随智能移动机器人上；

激光测距仪(1)用于检测自主跟随机器人(后车)前方180°范围内物体距其的位置信息D₁＝{P₁，P₂，…，P_i，…}；第一预处理单元(10)对D₁＝{P₁，P₂，…，P_i，…}依据距离与角度关系

进行处理，得到前车相对后车的位置信息ρ_C，θ_C；

GPS导航设备(2)用于测量自主跟随机器人(后车)当前的位置信息D₂＝(D_经度，D_纬度)；第二预处理单元(20)依据全局坐标转换关系

对D₂＝(D_经度，D_纬度)进行坐标映射，得到自主跟随机器人(后车)在全局坐标系O-XY下的位置

三轴陀螺仪(3)用于测量自主跟随机器人(后车)当前运动的角速度信息D₃＝(D₃₁，D₃₂，D₃₃)；第三预处理单元(30)对D₃＝(D₃₁，D₃₂，D₃₃)依据航向角关系Δβ＝D₃₃×t_s进行处理，得到自主跟随机器人(后车)当前航行的角度相对前一时刻t′的角度增量Δβ，t_s表示陀螺仪的采样时间；

码盘(4)用于测量自主跟随机器人(后车)运动过程以脉冲序列形式输出的距离信息D₄；第四预处理单元(40)对D₄利用脉冲-距离关系ΔS＝∑D₄×M进行处理，得到自主跟随机器人(后车)在采样时间间隔Δt＝t-t′内前进的位移ΔS；

摄像头(5)用于实时采集目标机器人(前车)前行的图像信息PCK；

第二运动状态检测单元(8)第一方面依据航位推算方法对Δβ和ΔS进行处理，来更新自主机器人在全局坐标系O-XY下的当前位置坐标；第二方面利用位移与速度的关系

对ΔS进行处理，得到当前时刻自主跟随机器人(后车)的速度信息v_F；然后利用速度与加速度的关系

求出当前时刻自主跟随机器人(后车)的加速度信息a_F；第三方面进行位移ΔS的累积，即S_F＝∑ΔS；所述第二运动状态检测单元(8)一方面输出

给第一运动状态检测单元(7)，另一方面输出D₈₂＝{v_F，a_F，S_F}给局部可柔虚杆自主跟踪系统(6)；

第一运动状态检测单元(7)首先依据位姿转换关系式

对接收的D_F-L＝{ρ_C，θ_C}和

进行处理，得到目标机器人(前车)在全局坐标系O-XY下的位置坐标

然后依据位移与速度的关系获得目标机器人(前车)在当前时刻的速度信息v_L；然后利用速度与加速度的关系

求出当前时刻目标机器人(前车)的加速度信息a_L；所述第一运动状态检测单元(7)输出

给局部可柔虚杆自主跟踪系统(6)；

局部可柔虚杆自主跟踪系统(6)对接收的

和D₈₂＝{v_F，a_F，S_F}进行复现跟随处理，得到自主跟随机器人(后车)在当前虚杆的横向力的作用下的期望角速度w、自主跟随机器人(后车)在当前虚杆内的纵向力的作用下的加速度a_Fnew和期望线速度v_Fnew；最后主控制器根据所述的w、a_Fnew和v_Fnew对自主跟随机器人(后车)的执行结构进行控制，实现沿目标智能移动机器人前行轨迹进行前行。

本发明局部可柔度虚杆转弯控制系统的优点在于：

①本发明采用对前车直行和拐弯时采取两种不同的控制策略分别进行控制。当前车直线行驶时，局部可柔度虚杆只有沿切向的纵向力，拖动后车前行，而当前车转弯后当后车运动到拐点时，局部可柔度虚杆既有沿切向的纵向力后车又有沿杆的法向的横向力，后车在横向力的作用下开始转弯。当前车已经转弯，而后车还未到达拐点时，虚杆是局部可柔的，虚杆由直杆部分的刚性段和弯曲部分的柔性段这两个部分组成。保证后车在转弯时的跟踪性能，将误差降到最低。

②应用描述的局部可柔度虚杆形态，以及对虚杆的局部可柔度进行分析，求解出沿虚杆切线方向的纵向力和与该纵向力垂直的横向力。纵向力的大小取决于前车速度、前车加速度、后车速度、后车加速度以及虚杆的长度，该纵向力直接作用到后车上去，从而使后车的运动速度、加速度能够实时响应前车的运动变化；横向力的大小取决于虚杆形态和该点处前车线速度的大小，该横向力作用于后车使后车沿杆的的变形程度进行转弯，保证后车能够复现前车的轨迹。

③这样通过虚杆中纵向力与横向力的共同作用从而实现了后车对于前车的跟踪运动。通过横向力与纵向力的引入，不仅确保了后车能够实时的响应前车的运动变化从而使两车之间始终保持一个合理的安全距离，而且保证了前车在进行转弯运动的时候，后车不会立即跟随前车进行转弯，而是在纵向力的作用下运动到转弯点后才进行转弯运动，从而确保了后车的行车安全。

④引入力控制的概念，将虚杆中的纵向力F_l和横向力F_n作为驱动后车运动状态发生改变的条件，通过F_l和F_n的牵引作用，能够使后车更佳精确地复现前车的轨迹。

附图说明

图1是本发明局部可柔度虚杆转弯控制系统的信号流向结构框图。

图1A是本发明局部可柔虚杆自主跟踪系统的信号流向结构框图。

图1B是目标机器人(前车)与自主跟随机器人(后车)的跟随状态示意图。

图2是本发明对激光测距仪测得扫描点对前车后端所在直线进行拟合的示意图。

图2A是本发明中对前车宽度直线获知前车后端中心点相对后车位置示意图。

图3是本发明前后两车在全局坐标系下的位姿关系图。

图4是本发明自主跟踪系统处理得到的局部可柔度虚杆形态示意图。

图5是本发明自主跟随机器人(后车)坐标系下局部可柔度虚杆的微分示意图。

图6是自主跟随机器人(后车)在不同状态下的跟随示意图。

具体实施方式

下面将结合附图对本发明做进一步的详细说明。

本发明将局部可柔度虚杆转弯控制系统设置在现有智能移动机器人的主控制器上，所述主控制器的中心处理器采用三星公司生产的S3C2440芯片，并在该S3C2440芯片上应用Qt/embedded-4.4.3软件开发平台，设置了能够实现自主跟随机器人完全复现目标机器人的运动轨迹、以及在转弯时精确拐弯的局部可柔度虚杆转弯控制系统。该局部可柔度虚杆转弯控制系统加载在智能移动机器人的主控制器内，在本发明中涉及的独立式自动跟踪是指自主跟随智能移动机器人复现目标智能移动机器人的前行轨迹。

参见图3所示，目标智能移动机器人(前车)与自主跟随智能移动机器人(后车)之间相对跟随的距离记为D_F-L，在目标机器人进行直行、转弯或者进行曲线运动时(参见图1B所示)，自主跟随机器人的移动会复现目标机器人的前行轨迹。由于目标机器人和自主跟随机器人均为可移动的载体，为了方便叙述，下文将目标机器人也称作前车，自主跟随机器人也称作后车。

参见图1、图1A所示，本发明的一种适用于独立式自动跟踪的局部可柔度虚杆转弯控制系统，该系统包括有激光测距仪1、GPS导航设备2、三轴陀螺仪3、码盘4、摄像头5、第一预处理单元10、第二预处理单元20、第三预处理单元30、第四预处理单元40、第一运动状态检测单元7、第二运动状态检测单元8和局部可柔虚杆自主跟踪系统6；其中，局部可柔虚杆自主跟踪系统6包括有局部可柔度虚杆形态解析单元61、可柔度分析单元62、直杆控制单元63、曲杆控制单元64、线速度控制单元65、角速度控制单元66和显示输出单元67。

参见图2、图2A所示，在本发明中，自主跟随机器人(后车)的坐标系记为O_F-X_FY_FZ_F，X_F轴与自主跟随机器人(后车)后轮的中轴线重合，Y_F轴与自主跟随机器人(后车)前进方向保持一致，O_F是自主跟随机器人(后车)后轮中轴线的中心点，该中心点的正上方(即Z_F轴)为激光测距仪1的安装位置。

在本发明中，自主跟随机器人(后车)上设置的激光测距仪1用于检测自主跟随机器人(后车)前方180°范围内物体距其的位置信息D₁＝{P₁，P₂，…，P_i，…}，P₁表示激光测距仪1检测到的第1个扫描点，P₂表示激光测距仪1检测到的第2个扫描点，P_i表示激光测距仪1检测到的第i个扫描点；

连接P₁与O_F两点的线段长度记为ρ₁，该线段与X_F轴的夹角记为θ₁；

连接P₂与O_F两点的线段长度记为ρ₂，该线段与X_F轴的夹角记为θ₂；

连接P_i与O_F两点的线段长度记为ρ_i，该线段与X_F轴的夹角记为θ_i；

在利用激光测距仪1进行前车与后车之间的距离测量时，对激光测距仪1测得的激光扫描点P₁，P₂，…，P_i，…采用最小二乘法拟合，得到前车后端在O_F-X_FY_FZ_F坐标下的拟合直线ST。所述的激光扫描点P₁，P₂，…，P_i，…中，必然存在有目标机器人(前车)的后端两个边缘返回的激光扫描点分别记为A扫描点和B扫描点，连接A扫描点与B扫描点形成的线段记为AB，所述线段AB的长度即为前车的车宽。线段AB的中心点记为C点，则有：

连接A与O_F两点的线段长度记为ρ_A，该线段与X_F轴的夹角记为θ_A；

连接B与O_F两点的线段长度记为ρ_B，该线段与X_F轴的夹角记为θ_B；

连接C与O_F两点的线段长度记为ρ_C，该线段与X_F轴的夹角记为θ_C；则

参见图2A所示，在本发明中，自主跟随机器人(后车)的坐标系记为O_F-X_FY_FZ_F，X_F轴与自主跟随机器人(后车)后轮的中轴线重合，Y_F轴与自主跟随机器人(后车)前进方向保持一致，O_F是自主跟随机器人(后车)后轮中轴线的中心点，该中心点的正上方(即Z_F轴)为激光测距仪1的安装位置。

在利用激光测距仪1进行前车与后车之间的距离测量时，对激光测距仪1测得的激光扫描点采用最小二乘法拟合，得到前车后端在O_F-X_FY_FZ_F坐标下的拟合直线ST。

自主跟随机器人(后车)上设置的GPS导航设备2用于测量自主跟随机器人(后车)当前的位置信息D₂＝(D_经度，D_纬度)，其中D_经度表示自主跟随机器人(后车)利用GPS导航设备测得的经度，D_纬度表示自主跟随机器人(后车)利用GPS导航设备测得的纬度。

自主跟随机器人(后车)上设置的三轴陀螺仪3用于测量自主跟随机器人(后车)当前运动的角速度信息D₃＝(D₃₁，D₃₂，D₃₃)，其中D₃₁为自主跟随机器人(后车)绕X_F轴的角速度，D₃₂为自主跟随机器人(后车)绕Y_F轴的角速度，D₃₃为自主跟随机器人(后车)绕Z_F轴的角速度。

自主跟随机器人(后车)上设置的码盘4用于测量自主跟随机器人(后车)运动过程以脉冲序列形式输出的距离信息D₄；

自主跟随机器人(后车)上设置的摄像头5用于实时采集目标机器人(前车)前行的图像信息PCK。由于本发明中选用的中心处理器S3C2440芯片可以直接与摄像头的输出接口连接，故可以不用采集卡便实现了对采集到的图像进行在智能移动机器人上显示。

下面将对自主跟随机器人(后车)采集到的各路信息进行处理：

(一)第一预处理单元10

第一预处理单元10对接收到的D₁＝{P₁，P₂，…，P_i，…}依据距离与角度关系

进行处理，得到前车相对后车的位置信息ρ_C，θ_C，该位置信息ρ_C，θ_C表征了前后两车的相对跟随距离D_F-L＝(ρ_C，θ_C)；n表示线段AB上所有激光扫描点的个数，m表示求和公式中的变量；ρ_m表示线段AB上扫描点P_m与O_F两点的线段长度，该线段P_mO_F与X_F轴的夹角记为θ_m。

(二)第二预处理单元20

参见图3所示，自主跟随机器人(后车)的全局坐标系记为O-XY，X轴上的数值代表的是地球的纬度，Y轴上的数值代表的是地球的经度。所述的经度、纬度的数值为GPS导航设备2采集到的自主跟随机器人(后车)位置信息。

第二预处理单元20依据全局坐标转换关系对接收到的位置信息D₂＝(D_经度，D_纬度)进行坐标映射，得到自主跟随机器人(后车)在全局坐标系O-XY下的位置

其中：x_gF表示自主跟随机器人(后车)上的点O_F映射在全局坐标系O-XY下X轴上的值；y_gF表示自主跟随机器人(后车)上的点O_F映射在全局坐标系O-XY下Y轴上的值；

表示自主跟随机器人(后车)当前时刻的位置坐标；O_经度表示全局坐标系中原点O处的经度值；O_纬度表示全局坐标系中原点O处的纬度值。

在本发明中，利用全局坐标系O-XY对自主跟随机器人(后车)的位置进行量化，便于不同位置在同一坐标基准下进行解算。

(三)第三预处理单元30

第三预处理单元30对接收的D₃＝(D₃₁，D₃₂，D₃₃)依据航向角关系Δβ＝D₃₃×t_s进行处理，得到自主跟随机器人(后车)当前航行的角度相对前一时刻t′的角度增量Δβ，t_s表示陀螺仪的采样时间。

在本发明中，前进夹角关系Δβ＝D₃₃×t_s是指当前时刻t与前一时刻t′的自主跟随机器人(后车)前进方向之间的夹角。

(四)第四预处理单元40

第四预处理单元40对接收的距离信息D₄利用脉冲-距离关系ΔS＝∑D₄×M进行处理，得到自主跟随机器人(后车)在采样时间间隔Δt＝t-t′内前进的位移ΔS；

M为自主跟随机器人(后车)在码盘每输出一个脉冲间隔内前进的距离；

(五)第二运动状态检测单元8

第二运动状态检测单元8第一方面依据航位推算方法

Δβ和ΔS进行处理，来更新自主机器人在全局坐标系O-XY下的当前位置坐标；表示自主跟随机器人(后车)前一时刻的位置坐标；β_t′表示自主跟随机器人(后车)前一时刻的航向角度大小；

第二运动状态检测单元8第二方面利用位移与速度的关系

对ΔS进行处理，得到当前时刻自主跟随机器人(后车)的速度信息v_F；然后利用速度与加速度的关系

求出当前时刻自主跟随机器人(后车)的加速度信息a_F；v′_F表示前一时刻自主跟随机器人(后车)的速度信息；

第二运动状态检测单元8第三方面进行位移ΔS的累积，即S_F＝∑ΔS。

在本发明中，第二运动状态检测单元8输出信息包括有当前时刻自主跟随机器人(后车)的速度信息v_F、当前时刻自主跟随机器人(后车)的加速度信息a_F和累积位移S_F，采用数学集合形式表达为D₈₂＝{v_F，a_F，S_F}；同时第二运动状态检测单元8输出给第一运动状态检测单元7的信息D₈₁为自主跟随机器人(后车)当前时刻在全局坐标系O-XY下的位置坐标，表示为

(六)第一运动状态检测单元7

第一运动状态检测单元7首先依据位姿转换关系式

对接收的D_F-L＝{ρ_C，θ_C}和

进行处理，得到目标机器人(前车)在全局坐标系O-XY下的位置坐标

然后依据位移与速度的关系

获得目标机器人(前车)在当前时刻的速度信息v_L；然后利用速度与加速度的关系

求出当前时刻目标机器人(前车)的加速度信息a_L；

表示目标机器人(前车)前一时刻的位置坐标；v′_L表示前一时刻目标机器人(前车)的速度信息。

在本发明中，第一运动状态检测单元7输出信息包括有目标机器人(前车)在全局坐标系O-XY下的位置坐标

当前时刻目标机器人(前车)的速度信息v_L、当前时刻目标机器人(前车)的加速度信息a_L，采用数学集合形式表达为

(七)局部可柔度虚杆形态解析单元61

参见图4所示，两个机器人之间的虚杆在全局坐标系O-XY下形态如图示曲线表示。

局部可柔度虚杆形态解析单元61对接收的

进行方程描述，获得虚杆在自主跟随机器人(后车)坐标系O_F-X_FY_F下的x′_FL≤x≤x_FL段的描述方程然后对f(x)_k进行一阶求导得到f(x)_k在x′_FL≤x≤x_FL段的一阶导数方程f′(x)_k，对f(x)_k进行二阶求导得到f(x)_k在x′_FL≤x≤x_FL段的二阶导数方程f″(x)_k，然后利用曲线长度与一阶导数f′(x)_k的关系求出虚杆在x′_FL≤x≤x_FL段的长度L_k；其中：

h＝x_gL-x′_gL；

$M_0=-\frac{2}{h}(\tan \left({\mathrm{\θ}}_C\right)+2\tan \left({\mathrm{\θ}}_C^{\′}\right)-3\frac{y_{\mathrm{gL}}-y_{\mathrm{gL}}^{\′}}{h});$

$M_1=-\frac{2}{h}(2\tan \left({\mathrm{\θ}}_C\right)+\tan \left({\mathrm{\θ}}_C^{\′}\right)-3\frac{y_{\mathrm{gL}}-y_{\mathrm{gL}}^{\′}}{h});$

x_FL＝(x_gL-x_gF)cosβ+(y_gL-y_gF)sinβ；

x′_FL＝(x′_gL-x′_gF)cosβ+(y′_gL-y′_gF)sinβ；

y_FL＝(y_gL-y_gF)cosβ-(x_gL-x_gF)sinβ；

y′_FL＝(y′_gL-y′_gF)cosβ-(x′_gL-x′_gF)sinβ；

θ′_C表示前一时刻目标机器人(前车)的航向角；

x_FL表示目标机器人(前车)上的点C映射在自主跟随机器人(后车)坐标系O_F-X_FY_F下X轴上的值；

y_FL表示目标机器人(前车)上的点C映射在自主跟随机器人(后车)坐标系O_F-X_FY_F下Y轴上的值；

f(x)_k表示在x′_FL≤x≤x_FL取值范围内的虚拟柔性曲杆的曲线方程，f(x)_k中的x为自变量，k表示跟随过程中的某一时间；

f′(x′_FL)_k表示f′(x)_k在x′_FL处的函数值；

f′(x_FL)_k表示f′(x)_k在x_FL处的函数值；

表示f′(x)_k在

处的函数值。

在本发明中，局部可柔度虚杆形态解析单元61输出信息包括有局部可柔度虚杆在x′_FL≤x≤x_FL段的描述方程f(x)_k、f(x)_k的一阶导数f′(x)_k、虚拟柔性曲杆的长度L_k，采用数学集合形式表达为D₆₁＝{f(x)_k，f′(x)_k，L_k}。

(八)可柔度分析单元62

可柔度分析单元62根据虚杆的二次导数关系式f″(x)_k＝0进行计算，分析当前状态下虚杆的形状。

若在虚杆各处的二次导数f″(x)_k全部等于0，则有D₆₂＝0的输出，虚杆为直杆。反之，若在虚杆各处的二次导数f″(x)_k不全部等于0，则有D₆₂＝1的输出，虚杆为曲杆，且当f″(x)_k由零值变换为非零值处记为拐点，虚杆在此拐点处由刚性杆转换为柔性杆。

(九)直杆控制单元63

直杆控制单元63首先对接收到的信息D₆₂进行判断：

若D₆₂＝1，则说明虚杆的形态是弯曲的，由曲杆控制单元64进行控制，此时直杆控制单元63不输出任何信息；

若D₆₂＝0，则根据纵向力与虚杆长度关系

对接收到的D₈₂＝{v_F，a_F，S_F}以及D₆₁＝{f(x)_k，f′(x)_k，L_k}进行计算，求出当前状态下虚杆内部的纵向作用力F_l；其中：e表示曲杆的胡克系数，该胡克系数表征曲杆形变量与其内力大小的关系；e_a表示加速度因子，该加速度因子用于调节曲杆内力与前后车加速度大小的关系；e_v表示速度因子，速度因子用于调节曲杆内力与前后车速度大小的关系；l表示虚杆当前的长度，且

D_min表示设定的虚杆的最小安全距离。

(十)曲杆控制单元64

图5表示了两个机器人之间的虚杆在自主跟随机器人(后车)坐标系O_F-X_FY_F下的微分示意。G₁、G₂表示虚杆上任意相邻的两个点，Q表示G₁、G₂两点间圆弧对应的圆心，且圆心角为

圆心角

对应的弧长为dl。

在本发明中，利用弧长、角度与挠度之间的关系得到横向力与挠度的关系式

曲杆控制单元64首先对接收到的信息D₆₂进行判断：

若D₆₂＝0，则说明虚杆的形态是直的，由直杆控制单元63进行控制，此时曲杆控制单元64不输出任何信息。

若D₆₂＝1，则第一方面根据纵向力与虚杆长度关系

对接收到的D₈₂＝{v_F，a_F，S_F}以及D₆₁＝{f(x)_k，f′(x)_k，L_k}进行计算，求出当前时刻虚杆的纵向作用力F_l；第二方面根据横向力与挠度

进行计算，得到当前时刻虚杆的横向作用力F_n；其中：dx表示自变量x在X_F轴上的微分增量；ω表示虚杆在Y_F轴方向上的位移，即挠度，该挠度表征了虚杆的变形程度，并且在虚杆的各处均是可变的；dω表示对应dx的挠度的微分增量；E表示虚杆的弹性模量；I表示虚杆的惯性矩。

(十一)  线速度控制单元65

线速度控制单元65对接收到的信息F_l、

D₈₂＝{v_F,a_F,S_F}根据力与加速度的关系

得到自主跟随机器人(后车)在当前虚杆内的纵向力的作用下的加速度a_Fnew；

然后根据期望速度与虚杆长度关系

求出自主跟随机器人(后车)的期望线速度v_Fnew；其中m_F表示设定的自主跟随机器人(后车)质量。

在本发明中，线速度控制单元65输出的信息为自主跟随机器人(后车)在当前虚杆内的纵向力的作用下的加速度a_Fnew和期望线速度v_Fnew，采用数学集合形式表达为D₆₅＝{a_Fnew，v_Fnew}。该信息被传递给自主跟随机器人(后车)执行结构，自主跟随机器人(后车)按照上述信息进行跟随运动。

(十二)角速度控制单元66

角速度控制单元66对接收到的信息F_n和D₈₂＝{v_F，a_F，S_F}依据角速度与虚杆横向力之间的关系对横向力进行处理，进而得到自主跟随机器人(后车)在当前虚杆的横向力的作用下的期望角速度w。

在本发明中角速度控制单元66输出的信息为自主跟随机器人(后车)在当前虚杆内的横向力的作用下的期望角速度w，该角速度信息经过输出给自主跟随机器人(后车)执行结构，自主跟随机器人(后车)按照该信息进行跟随运动。

(十三)显示输出单元67

显示输出单元67对采集到的图像信息PCK进行输出。所述的图像信息PCK最终会显示在智能移动机器人上的LCD屏上，通过本发明中选用的中心处理器S3C2440芯片控制显示输出单元67对采集图像的输出方式。

本发明设计的应用于智能移动机器人在转弯时的自主跟踪，其跟踪过程为：

(1)初始时，前、后两车均处于静止状态，局部可柔度虚杆为自然直杆状态，该直杆的长度与真实的拖车系统静止时一样。

(2)当前车开始运动时局部可柔度虚杆被拉长，当长度大于虚杆所被设定的在该速度下的长度时，虚杆就会像弹簧一样在内部产生一个拉力，在该拉力的拖动下，后车也开始向前运动。

(3)由于后车的向前运动虽然虚杆的长度还在不断拉伸，但增加幅度越来越小，同时由于后车速度的不断增加，虚杆的长度会从后车速度增加到与前车相同后开始慢慢变短，而当比虚杆本身所设定的长度要短时又会产生斥力，推动后车减速运动，这样在虚杆纵向力的作用下，前后车最终会处于一个稳定状态，即前后车以相同的速度同时向前运动，并且两车之间始终保持着一个稳定的安全距离。

(4)当前车开始转弯时，参见图6所示，U₁表示刚性段转变为柔性段的拐点，U₂表示柔性段与刚柔可变段的衔接点。(I)中前车开始转弯，杆开始变形。(I)到(II)这段时间前车一直处于弯道行驶状态，后车仍在直道上行驶。杆是局部可柔的，此时的杆由两部分组成，一部分仍为刚性杆(拐点U₁之前的轨迹的直线部分)O_F～U₁；另一部分U₁～U₂段是柔性的可以弯曲，用于转弯，且柔度根据前车轨迹变化而变化，与前车的轨迹半径相对应，是可以不断变化的，局部可柔度使得杆的弯曲与前车的历史轨迹形状一致，从而使得后车的跟踪沿着前车的历史轨迹进行。这个柔性的部位一开始是紧紧挨着前车的，当前车转弯的时候，以前车为参考点这个弯曲由前车逐渐向后车移动；而从空间看这个弯曲的地方是不动的。(I)中虚杆的U₁～C段与(III)中虚杆的S₁～C在空间中是未发生变化的。其中(I)中C、(II)中U₁、(III)中U₁在空间中是同一个点，(II)中C与(III)中的U₂在空间中也是同一个点。虚杆的前端会随着前车的转弯运动而发生弯曲，但是在虚杆弯曲点的后端部分并不会因为前车的转弯运动而发生改变，后端部分依然是一条向前运动的直杆，只是当虚拟杆上的各点在前车的拉动运动下运动到前车拐弯的点时才开始发生弯曲，虚杆独特的柔韧性能够完美的适应前车的不同运动，确保无论前车做何种运动虚杆都能够完美的复制前车的运动轨迹。

(5)后车在虚杆的拖动下不断向前运动，由于后车的运动并不直接受到前车运动的影响，所以当前车发生转弯运动时后车并不会像现实的拖车系统一样立马进行转弯，而是会随着杆的末端继续向前运动，只有当杆的末端运动到前车开始转弯的位置时杆的末端发生弯曲，同时虚杆末端对后车作用力由原先的只有纵向力变为拉动后车进行转弯，这样就确保了虚拟拖车系统虽然没有人类的智能也能够成功确保后车完全复现前车的运动轨迹(实际上真实的拖车系统中，拖车是无法复现前车的轨迹的，之所以在转弯时能够避免危险的情况的出现是因为驾驶员在进行转弯运动时就已经把拖车的这段轨迹偏差考虑在其中了，这也是为什么拖车要比一般的小车要难以驾驶)。

(6)前车减速停车的过程实际上是其起步运动的反过程，虚杆的变化与起步运动相反，但是机理是一样的。

Claims (3)

1.一种适用于独立式自动跟踪的局部可柔度虚杆转弯控制系统，该局部可柔度虚杆转弯控制系统加载在智能移动机器人的主控制器内，所述的独立式自动跟踪是指自主跟随智能移动机器人复现目标智能移动机器人的前行轨迹，其特征在于：该局部可柔度虚杆转弯控制系统包括有激光测距仪(1)、GPS导航设备(2)、三轴陀螺仪(3)、码盘(4)、摄像头(5)、第一预处理单元(10)、第二预处理单元(20)、第三预处理单元(30)、第四预处理单元(40)、第一运动状态检测单元(7)、第二运动状态检测单元(8)和局部可柔虚杆自主跟踪系统(6)；

激光测距仪(1)、GPS导航设备(2)、三轴陀螺仪(3)、码盘(4)和摄像头(5)安装在自主跟随智能移动机器人上；

激光测距仪(1)用于检测自主跟随机器人(后车)前方180°范围内物体距其的位置信息D₁＝{P₁，P₂，…，P_i，…}；第一预处理单元(10)对D₁＝{P₁，P₂，…，P_i，…}依据距离与角度关系

进行处理，得到前车相对后车的位置信息ρ_C，θ_C；

GPS导航设备(2)用于测量自主跟随机器人(后车)当前的位置信息D₂＝(D_经度，D_纬度)；第二预处理单元(20)依据全局坐标转换关系

对D₂＝(D_经度，D_纬度)进行坐标映射，得到自主跟随机器人(后车)在全局坐标系O-XY下的位置

三轴陀螺仪(3)用于测量自主跟随机器人(后车)当前运动的角速度信息D₃＝(D₃₁，D₃₂，D₃₃)；第三预处理单元(30)对D₃＝(D₃₁，D₃₂，D₃₃)依据航向角关系Δβ＝D₃₃×t_s进行处理，得到自主跟随机器人(后车)当前航行的角度相对前一时刻t′的角度增量Δβ，t_s表示陀螺仪的采样时间；

码盘(4)用于测量自主跟随机器人(后车)运动过程以脉冲序列形式输出的距离信息D₄；第四预处理单元(40)对D₄利用脉冲-距离关系ΔS＝∑D₄×M进行处理，得到自主跟随机器人(后车)在采样时间间隔Δt＝t-t′内前进的位移ΔS；

摄像头(5)用于实时采集目标机器人(前车)前行的图像信息PCK；

第二运动状态检测单元(8)第一方面依据航位推算方法

对

Δβ和ΔS进行处理，来更新自主机器人在全局坐标系O-XY下的当前位置坐标；

表示自主跟随机器人(后车)前一时刻的位置坐标；β_t′表示自主跟随机器人(后车)前一时刻的航向角度大小；第二运动状态检测单元(8)第二方面利用位移与速度的关系

对ΔS进行处理，得到当前时刻自主跟随机器人(后车)的速度信息v_F；然后利用速度与加速度的关系

求出当前时刻自主跟随机器人(后车)的加速度信息a_F；v′_F表示前一时刻自主跟随机器人(后车)的速度信息；第二运动状态检测单元(8)第三方面进行位移ΔS的累积，即S_F＝∑ΔS；所述第二运动状态检测单元(8)一方面输出

给第一运动状态检测单元(7)，另一方面输出D₈₂＝{v_F，a_F，S_F}给局部可柔虚杆自主跟踪系统(6)；

第一运动状态检测单元(7)首先依据位姿转换关系式对接收的D_F-L＝{ρ_C，θ_C}和

进行处理，得到目标机器人(前车)在全局坐标系O-XY下的位置坐标

然后依据位移与速度的关系获得目标机器人(前车)在当前时刻的速度信息v_L；然后利用速度与加速度的关系求出当前时刻目标机器人(前车)的加速度信息a_L；

表示目标机器人(前车)前一时刻的位置坐标；v′_L表示前一时刻目标机器人(前车)的速度信息；所述第一运动状态检测单元(7)输出

给局部可柔虚杆自主跟踪系统(6)；

局部可柔虚杆自主跟踪系统(6)对接收的和D₈₂＝{v_F，a_F，S_F}进行复现跟随处理，得到自主跟随机器人(后车)在当前虚杆的横向力的作用下的期望角速度w、自主跟随机器人(后车)在当前虚杆内的纵向力的作用下的加速度a_Fnew和期望线速度v_Fnew；最后主控制器根据所述的w、a_Fnew和v_Fnew对自主跟随机器人(后车)的执行结构进行控制，实现沿目标智能移动机器人前行轨迹进行前行。

2.根据权利要求1所述的一种适用于独立式自动跟踪的局部可柔度虚杆转弯控制系统，其特征在于：局部可柔虚杆自主跟踪系统(6)包括有局部可柔度虚杆形态解析单元(61)、可柔度分析单元(62)、直杆控制单元(63)、曲杆控制单元(64)、线速度控制单元(65)、角速度控制单元(66)和显示输出单元(67)；

局部可柔度虚杆形态解析单元(61)对接收的

进行方程描述，获得虚杆在自主跟随机器人(后车)坐标系O_F-X_FY_F下的x′_FL≤x≤x_FL段的描述方程

然后对f(x)_k进行一阶求导得到f(x)_k在x′_FL≤x≤x_FL段的一阶导数方程f′(x)_k，对f(x)_k进行二阶求导得到f(x)_k在x′_FL≤x≤x_FL段的二阶导数方程f″(x)_k，然后利用曲线长度与一阶导数f′(x)_k的关系

求出虚杆在x′_FL≤x≤x_FL段的长度L_k；其中：

h＝x_gL-x′_gL；

$M_0=-\frac{2}{h}(\tan \left({\mathrm{\θ}}_C\right)+2\tan \left({\mathrm{\θ}}_C^{\′}\right)-3\frac{y_{\mathrm{gL}}-y_{\mathrm{gL}}^{\′}}{h});$

$M_1=-\frac{2}{h}(2\tan \left({\mathrm{\θ}}_C\right)+\tan \left({\mathrm{\θ}}_C^{\′}\right)-3\frac{y_{\mathrm{gL}}-y_{\mathrm{gL}}^{\′}}{h});$

x_FL＝(x_gL-x_gF)cosβ+(y_gL-y_gF)sinβ；

x′_FL＝(x′_gL-x′_gF)cosβ+(y′_gL-y′_gF)sinβ；

y_FL＝(y_gL-y_gF)cosβ-(x_gL-x_gF)sinβ；

y′_FL＝(y′_gL-y′_gF)cosβ-(x′_gL-x′_gF)sinβ；

θ′_C表示前一时刻目标机器人(前车)的航向角；

x_FL表示目标机器人(前车)上的点C映射在自主跟随机器人(后车)坐标系O_F-X_FY_F下X轴上的值；

y_FL表示目标机器人(前车)上的点C映射在自主跟随机器人(后车)坐标系O_F-X_FY_F下Y轴上的值；

f(x)_k表示在x′_FL≤x≤x_FL取值范围内的虚拟柔性曲杆的曲线方程，f(x)_k中的x为自变量，k表示跟随过程中的某一时间；

f′(x′_FL)_k表示f′(x)_k在x′_FL处的函数值；

f′(x_FL)_k表示f′(x)_k在x_FL处的函数值；

表示f′(x)_k在

处的函数值。

所述局部可柔度虚杆形态解析单元(61)输出信息有D₆₁＝{f(x)_k，f′(x)_k，L_k}。

可柔度分析单元(62)根据虚杆的二次导数关系式f″(x)_k＝0进行计算，分析当前状态下虚杆的形状。

若在虚杆各处的二次导数f″(x)_k全部等于O，则有D₆₂＝0的输出，虚杆为直杆。反之，若在虚杆各处的二次导数f″(x)_k不全部等于O，则有D₆₂＝1的输出，虚杆为曲杆，且当f″(x)_k由零值变换为非零值处记为拐点，虚杆在此拐点处由刚性杆转换为柔性杆。

直杆控制单元(63)首先对接收到的信息D₆₂进行判断：

若D₆₂＝1，则说明虚杆的形态是弯曲的，由曲杆控制单元(64)进行控制，此时直杆控制单元(63)不输出任何信息；

若D₆₂＝0，则根据纵向力与虚杆长度关系

对接收到的

D₈₂＝{v_F，a_F，S_F}以及D₆₁＝{f(x)_k，f′(x)_k，L_k}进行计算，求出当前状态下虚杆内部的纵向作用力F_l；其中：e表示曲杆的胡克系数，该胡克系数表征曲杆形变量与其内力大小的关系；e_a表示加速度因子，该加速度因子用于调节曲杆内力与前后车加速度大小的关系；e_v表示速度因子，速度因子用于调节曲杆内力与前后车速度大小的关系；l表示虚杆当前的长度，且

D_min表示设定的虚杆的最小安全距离。

曲杆控制单元(64)首先对接收到的信息D₆₂进行判断：

若D₆₂＝0，则说明虚杆的形态是直的，由直杆控制单元63进行控制，此时曲杆控制单元64不输出任何信息。

若D₆₂＝1，则第一方面根据纵向力与虚杆长度关系

对接收到的

D₈₂＝{v_F，a_F，S_F}以及D₆₁＝{f(x)_k，f′(x)_k，L_k}进行计算，求出当前时刻虚杆的纵向作用力F_l；第二方面根据横向力与挠度

进行计算，得到当前时刻虚杆的横向作用力F_n；其中：dx表示自变量x在X_F轴上的微分增量；ω表示虚杆在Y_F轴方向上的位移，即挠度，该挠度表征了虚杆的变形程度，并且在虚杆的各处均是可变的；dω表示对应dx的挠度的微分增量；E表示虚杆的弹性模量；I表示虚杆的惯性矩。

线速度控制单元(65)对接收到的信息F_l、

D₈₂＝{v_F，a_F，S_F}根据力与加速度的关系

得到自主跟随机器人(后车)在当前虚杆内的纵向力的作用下的加速度a_Fnew；

然后根据期望速度与虚杆长度关系

求出自主跟随机器人(后车)的期望线速度v_Fnew；其中m_F表示设定的自主跟随机器人(后车)质量；

所述线速度控制单元(65)输出信息有D₆₅＝{a_Fnew，v_Fnew}；

角速度控制单元(66)对接收到的信息F_n和D₈₂＝{v_F，a_F，S_F}依据角速度与虚杆横向力之间的关系对横向力进行处理，进而得到自主跟随机器人(后车)在当前虚杆的横向力的作用下的期望角速度w；

显示输出单元(67)对采集到的图像信息PCK进行传输出；所述的图像信息PCK最终会显示在智能移动机器人上的LCD屏上。

3.根据权利要求1所述的一种适用于独立式自动跟踪的局部可柔度虚杆转弯控制系统，其特征在于：主控制器中的中心处理器选用S3C2440芯片。

Images