CN102323827B - 具有力延时虚拟柔性曲杆的自主跟踪系统 - 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下的当前位置坐标；第二运动状态检测单元(8)第二方面利用位移与速度的关系

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

求出当前时刻自主跟随机器人(后车)的加速度信息a_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；所述第一运动状态检测单元(7)输出给虚拟柔杆自主跟踪系统(6)；

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

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

本发明具有力延时的虚拟柔性曲杆的自主跟踪系统的优点在于：

①本发明应用描述的虚拟柔性曲杆方程，求解出虚拟柔性曲杆内部沿虚杆切线方向的纵向力和与该纵向力垂直的横向力。纵向力的大小取决于前车速度、前车加速度、后车速度、后车加速度以及虚拟柔性曲杆的长度，该纵向力直接作用到后车上去，从而使后车的运动速度、加速度能够实时响应前车的运动变化；横向力的大小取决于曲杆形态和该点处前车线速度的大小，该横向力经过延时后作用到后车上，保证后车能够复现前车的轨迹。

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

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

附图说明

图1是本发明具有力延时的虚拟柔性曲杆的自主跟踪系统的信号流向结构框图。

图1A是本发明自主跟踪系统中虚拟柔杆自主跟踪模块的信号流向结构框图。

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

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

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

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

图4是本发明自主跟踪系统处理得到的虚拟柔性曲杆形态示意图。

具体实施方式

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

本发明将具有力延时的虚拟柔性曲杆的自主跟踪系统设置在现有智能移动机器人的主控制器上，所述主控制器的中心处理器采用三星公司生产的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。

参见图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，…}依据距离与角度关系

进行处理，得到前车相对后车的位置信息D_F-L＝(ρ_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输出给虚拟柔杆自主跟踪系统6的信息包括有当前时刻自主跟随机器人(后车)的速度信息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输出给虚拟柔杆自主跟踪系统6的信息包括有目标机器人(前车)在全局坐标系O-XY下的位置坐标

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

(七)虚拟柔性曲杆形态解析单元61

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

虚拟柔性曲杆形态解析单元61对接收的进行方程描述，获得虚拟柔性曲杆在x′_gL≤x≤x_gL段的描述方程

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

求出虚拟曲杆在x′_gL≤x≤x_gL段的长度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});$

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

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

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

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

表示

处的函数值；

在本发明中，虚拟柔性曲杆形态解析单元61输出虚拟柔性曲杆内力解析单元62的信息为虚拟柔性曲杆在x′_gL≤x≤x_gL段的描述方程f(x)_k、f(x)_k的一阶导数f′(x)_k、虚拟柔性曲杆的长度L_k，采用数学集合形式表达为D_61-62＝{f(x)_k，f′(x)_k，L_k}。

(八)虚拟柔性曲杆内力解析单元62

虚拟柔性曲杆内力解析单元62对接收到的

D₈₂＝{v_F，a_F，S_F}和D_61-62＝{f(x)_k，f′(x)_k，L_k}根据力与虚拟曲杆长度关系

进行计算，求出当前状态下虚拟曲杆内部的纵向作用力F_l；

其中e表示曲杆的胡克系数，该胡克系数表征曲杆形变量与其内力大小的关系；

e_a表示加速度因子，该加速度因子用于调节曲杆内力与前后车加速度大小的关系；

e_v表示速度因子，速度因子用于调节曲杆内力与前后车速度大小的关系；

l表示虚拟曲杆当前的长度，且

D_min表示设定的虚拟曲杆的最小安全距离；

虚拟柔性曲杆内力解析单元62对接收到的

D₈₂＝{v_F，a_F，S_F}和D_61-62＝{f(x)_k，f′(x)_k，L_k}根据横向力与虚杆形态的关系

进行计算，得到虚拟柔性曲杆内部在x′_gL≤x≤x_gL段的横向力描述方程F_n(x)_k。

其中f′(x)_k为f(x)_k的一阶导数，f″(x)_k为f(x)_k的二阶导数，m_L表示前车的质量；

在本发明中，虚拟柔性曲杆内力解析单元62输出的信息为虚拟柔性曲杆中的纵向力F_l和横向力F_n(x)_k以及虚拟曲杆当前的长度l。

(九)线速度控制单元63

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

根据力与加速度的关系

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

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

$v_{\mathrm{Fnew}}=\sqrt{v_F^2+2\frac{e\frac{a_L{v}_L}{a_F}[l-e_a(a_L-a_F)-e_v(v_L-v_F)-D_{\min }]}{m_F}\×(l-e_a(a_L-a_F)-e_v(v_L-v_F)-D_{\min })}$

求出自主跟随机器人(后车)的期望线速度v_Fnew；

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

(十)角速度控制单元64

角速度控制单元64对接收到的信息F_n(x)_k和D₈₂＝{v_F，a_F，S_F}依据角速度与虚杆横向力之间的关系

对横向力进行处理，进而得到自主跟随机器人(后车)在当前虚杆内的x′_gL≤x≤x_gL段处横向力的作用下的期望角速度w(x)_k，

在本发明中角速度控制单元64输出的信息为自主跟随机器人(后车)在当前虚杆内的横向力的作用下的期望角速度w(x)_k，该角速度信息经过延时检测控制单元6

5的延时检测后输出给自主跟随机器人(后车)执行结构，自主跟随机器人(后车)按照该信息进行跟随运动。

(十一)延时检测控制单元65

延时检测控制单元65通过检测第二运动状态检测单元8的输出信息

实时判断自主跟随机器人(后车)在虚拟柔性曲杆的拖动下运动到的取值范围段x_gL(k-1)≤x_gF≤x_gL(k)，然后通过角速度控制单元64运算得到的方程式

计算出当前时刻的后车期望角速度w(x_gF)_k，该期望角速度w(x_gF)_k被传递给自主跟随机器人(后车)执行结构，自主跟随机器人(后车)按照该信息进行跟随运动。

其中，

x_gL(k-1)表示在第k-1时刻目标机器人(前车)在全局坐标系O-XY下的横坐标；

x_gL(k)表示在第k时刻目标机器人(前车)在全局坐标系O-XY下的横坐标。

在本发明中，目标机器人(前车)在进行转弯运动时产生的横向力F_n(x_gL)_k，并没有立即作用到自主跟随机器人(后车)上，而是延迟到后车运动到该横向力产生的位置时才开始产生作用，这样就确保了自主跟随机器人(后车)对于目标机器人(前车)的转弯运动进行延时复现。

(十二)显示输出单元66

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

本发明设计的应用于智能移动机器人的具有力延时的虚拟柔性曲杆的自主跟踪系统，进行跟踪的过程为：

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

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

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

(4)对于曲线运动，当前车开始转弯时，虚拟曲杆的前端会随着前车的转弯运动而发生弯曲，但是在曲杆弯曲点的后端部分并不会因为前车的转弯运动而发生改变，后端部分依然是一条向前运动的直杆，只是当虚拟杆上的各点在前车的拉动运动下运动到前车拐弯的点时才开始发生弯曲，虚杆独特的柔韧性能够完美的适应前车的不同运动，确保无论前车做何种运动曲杆都能够完美的复制前车的运动轨迹。

(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；P₁表示激光测距仪（1）检测到的第1个扫描点，P₂表示激光测距仪（1）检测到的第2个扫描点，P_i表示激光测距仪（1）检测到的第i个扫描点；连接C与O_F两点的线段长度记为ρ_C，该线段与X_F轴的夹角记为θ_C；n表示线段AB上所有激光扫描点的个数，m表示求和公式中的变量；ρ_m表示线段AB上扫描点P_m与O_F两点的线段长度，该线段P_mO_F与X_F轴的夹角记为θ_m，α表示线段AB的垂线与X_F轴的夹角，且该垂线经过O_F点；自主跟随智能移动机器人的坐标系记为O_F-X_FY_FZ_F，O_F是自主跟随智能移动机器人后轮中轴线的中心点；AB是目标智能移动机器人的车宽，C点表示线段AB的中心点；

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

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

x_gF表示自主跟随智能移动机器人上的点O_F映射在全局坐标系O-XY下X轴上的值；y_gF表示自主跟随智能移动机器人上的点O_F映射在全局坐标系O-XY下Y轴上的值；

表示自主跟随智能移动机器人当前时刻的位置坐标；O_经度表示全局坐标系中原点O处的经度值；O_纬度表示全局坐标系中原点O处的纬度值；

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

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

摄像头（5）用于实时采集目标智能移动机器人前行的图像信息PCK；

第二运动状态检测单元（8）第一方面依据航位推算方法 $\left[\begin{array}{c}{x}_{\mathrm{gF}}\\ y_{\mathrm{gF}}\end{array}\right]=\left[\begin{array}{c}{x}_{\mathrm{gF}}^{\′}+\mathrm{\Δ}S\cos ({\mathrm{\β}}_{t^{\′}}+\frac{\mathrm{\Δ\β}}{2})\\ y_{\mathrm{gF}}^{\′}+\mathrm{\Δ}S\sin ({\mathrm{\β}}_{t^{\′}}+\frac{\mathrm{\Δ\β}}{2})\end{array}\right]$ 对 $\left[\begin{array}{c}{x}_{\mathrm{gF}}^{\′}\\ y_{\mathrm{gF}}^{\′}\end{array}\right],$ Δβ和ΔS进行处理，来更新自主跟随智能移动机器人在全局坐标系O-XY下的当前位置坐标； $\left[\begin{array}{c}{x}_{\mathrm{gF}}^{\′}\\ y_{\mathrm{gF}}^{\′}\end{array}\right]$ 表示自主跟随智能移动机器人前一时刻的位置坐标；β_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）首先依据位姿转换关系式 $\left\{\begin{array}{c}{x}_{\mathrm{gL}}={\mathrm{\ρ}}_C\cos {\mathrm{\θ}}_C\cos \mathrm{\β}-{\mathrm{\ρ}}_C\sin {\mathrm{\θ}}_C\sin \mathrm{\β}+x_{\mathrm{gF}}\\ y_{\mathrm{gL}}={\mathrm{\ρ}}_C\cos {\mathrm{\θ}}_C\sin \mathrm{\β}+{\mathrm{\ρ}}_C\sin {\mathrm{\θ}}_C\cos \mathrm{\β}+y_{\mathrm{gF}}\end{array}\right.$ 对接收的D_F-L＝{ρ_C,θ_C}和 $D_{81}=\left[\begin{array}{c}{x}_{\mathrm{gF}}\\ y_{\mathrm{gF}}\end{array}\right]$ 进行处理，得到目标智能移动机器人在全局坐标系O-XY下的位置坐标 $\left[\begin{array}{c}{x}_{\mathrm{gL}}\\ y_{\mathrm{gL}}\end{array}\right];$ β表示在全局坐标系O-XY下自主跟随智能移动机器人在当前时刻下的航行角度，即Y_F轴与X轴的夹角；然后依据位移与速度的关系 $v_L=\frac{\sqrt{{(x_{\mathrm{gL}}-x_{\mathrm{gL}}^{\′})}^2+{(y_{\mathrm{gL}}-y_{\mathrm{gL}}^{\′})}^2}}{\mathrm{\Δt}}$ 获得目标智能移动机器人在当前时刻的速度信息v_L；然后利用速度与加速度的关系

求出当前时刻目标智能移动机器人的加速度信息a_L； $\left[\begin{array}{c}{x}_{\mathrm{gL}}^{\′}\\ y_{\mathrm{gL}}^{\′}\end{array}\right]$ 表示目标智能移动机器人前一时刻的位置坐标；v′_L表示前一时刻目标智能移动机器人的速度信息；所述第一运动状态检测单元（7）输出 $D_7=\{\left[\begin{array}{c}{x}_{\mathrm{gL}}\\ y_{\mathrm{gL}}\end{array}\right],v_L,a_L\}$ 给虚拟柔杆自主跟踪系统（6）；

虚拟柔杆自主跟踪系统（6）对接收的 $D_7=\{\left[\begin{array}{c}{x}_{\mathrm{gL}}\\ y_{\mathrm{gL}}\end{array}\right],v_L,a_L\}$ 和D₈₂＝{v_F,a_F,S_F}进行复现跟随处理，得到自主跟随智能移动机器人在当前虚杆内的纵向力的作用下的加速度a_Fnew和期望线速度v_Fnew、以及当前时刻的自主跟随智能移动机器人期望角速度w(x_gF)_k；最后主控制器根据所述的w(x_gF)_k、a_Fnew和v_Fnew对自主跟随智能移动机器人的执行结构进行控制，实现沿目标智能移动机器人前行轨迹进行前行。

2.根据权利要求1所述的具有力延时虚拟柔性曲杆的自主跟踪系统，其特征在于：虚拟柔杆自主跟踪系统（6）包括有虚拟柔性曲杆形态解析单元（61）、虚拟柔性曲杆内力解析单元（62）、线速度控制单元（63）、方向控制单元（64）、延时检测控制单元（65）和显示输出单元（66）；

虚拟柔性曲杆形态解析单元（61）对接收的 $D_7=\{\left[\begin{array}{c}{x}_{\mathrm{gL}}\\ y_{\mathrm{gL}}\end{array}\right],v_L,a_L\}$ 进行方程描述，获得虚拟柔性曲杆在x′_gL≤x≤x_gL段的描述方程 $\begin{array}{c}{f\left(x\right)}_k=\frac{M_0}{6h}{(x_{\mathrm{gL}}-x)}^3+\frac{M_1}{6h}{(x-x_{\mathrm{gL}}^{\′})}^3+\\ (\frac{y_{\mathrm{gL}}^{\′}}{h}-\frac{M_0}{6}h)(x_{\mathrm{gL}}-x)+(\frac{y_{\mathrm{gL}}}{h}-\frac{M_1}{6}h)(x-x_{\mathrm{gL}}^{\′}),\end{array}$ 然后对f(x)_k进行一阶求导得到f(x)_k在x′_gL≤x≤x_gL段的一阶导数方程f′(x)_k，对f(x)_k进行二阶求导得到f(x)_k在x′_gL≤x≤x_gL段的二阶导数方程f″(x)_k，然后利用曲线长度与一阶导数f′(x)_k的关系 $L_k=\frac{x_{\mathrm{gL}}-x_{\mathrm{gL}}^{\′}}{6}[\sqrt{1+f^{\′}{{\left(x_{\mathrm{gL}}^{\text{\′}}\right)}_k}^2}+4\sqrt{1+f^{\′}{{\left(\frac{x_{\mathrm{gL}}^{\′}+x_{\mathrm{gL}}}{2}\right)}_k}^2}+\sqrt{1+f^{\′}{{\left(x_{\mathrm{gL}}\right)}_k}^2}],$ 求出虚拟曲杆在x′_gL≤x≤x_gL段的长度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}(\tan \left({\mathrm{\θ}}_C\right)+2\tan \left({\mathrm{\θ}}_C^{\′}\right)-3\frac{y_{\mathrm{gL}}-y_{\mathrm{gL}}^{\′}}{h});$ θ′_C表示前一时刻目标智能移动机器人的航向角；f(x)_k表示在x′_gL≤x≤x_gL取值范围内的虚拟柔性曲杆的曲线方程，f(x)_k中的x为自变量，k表示跟随过程中的某一时间；f′(x′_gL)_k表示f′(x)_k在x′_gL处的函数值；f′(x_gL)_k表示f′(x)_k在x_gL处的函数值；

表示f′(x)_k在

处的函数值；所述虚拟柔性曲杆形态解析单元（61）输出信息有D_61-62＝{f(x)_k,f′(x)_k,L_k}；

虚拟柔性曲杆内力解析单元（62）对接收到的 $D_7=\{\left[\begin{array}{c}{x}_{\mathrm{gL}}\\ y_{\mathrm{gL}}\end{array}\right],v_L,a_L\},$ D₈₂＝{v_F,a_F,S_F}和D_61-62＝{f(x)_k,f′(x)_k,L_k}根据力与虚拟曲杆长度关系 $F_l=e\frac{a_L{v}_L}{a_F{v}_F}[l-e_a(a_L-a_F)-e_v(v_L-v_F)-D_{\min }]$ 进行计算，求出当前状态下虚拟曲杆内部的纵向作用力F_l；其中e表示曲杆的胡克系数，该胡克系数表征曲杆形变量与其内力大小的关系；e_a表示加速度因子，该加速度因子用于调节曲杆内力与目标智能移动机器人、自主跟随智能移动机器人之间加速度大小的关系；e_v表示速度因子，速度因子用于调节曲杆内力与目标智能移动机器人、自主跟随智能移动机器人之间速度大小的关系；l表示虚拟曲杆当前的长度，且

D_min表示设定的虚拟曲杆的最小安全距离；虚拟柔性曲杆内力解析单元（62）对接收到的 $D_7=\{\left[\begin{array}{c}{x}_{\mathrm{gL}}\\ y_{\mathrm{gL}}\end{array}\right],v_L,a_L\},$ D₈₂＝{v_F,a_F,S_F}和D_61-62＝{f(x)_k,f′(x)_k,L_k}根据横向力与虚杆形态的关系 $F_n{\left(x\right)}_k=\frac{a_L}{a_F}\×m_L{v}_L^2\×\left|\frac{f^{\″}{\left(x\right)}_k}{{(1+f^{\′}{\left(x\right)}_k^2)}^{\frac{3}{2}}}\right|$ 进行计算，得到虚拟柔性曲杆内部在x′_gL≤x≤x_gL段的横向力描述方程F_n(x)_k；其中f′(x)_k为f(x)_k的一阶导数，f″(x)_k为f(x)_k的二阶导数，m_L表示目标智能移动机器人的质量；

线速度控制单元（63）对接收到的信息F_l、 $D_7=\{\left[\begin{array}{c}{x}_{\mathrm{gL}}\\ y_{\mathrm{gL}}\end{array}\right],v_L,a_L\},$ D₈₂＝{v_F,a_F,S_F}根据力与加速度的关系

得到自主跟随智能移动机器人在当前虚杆内的纵向力的作用下的加速度a_Fnew，然后根据期望速度与虚杆长度关系 $v_{\mathrm{Fnew}}=\sqrt{v_F^2+2\frac{e\frac{a_L{v}_L}{a_F}[l-e_a(a_L-a_F)e_v(v_L-v_F)-D_{\min }}{m_F}\×(l-e_a(a_L-a_F)-e_v(v_L-v_F)-D_{\min })}$ 求出自主跟随智能移动机器人的期望线速度v_Fnew；其中m_F表示设定的自主跟随智能移动机器人质量；所述线速度控制单元（63）输出的信息有D₆₃＝{a_Fnew,V_Fnew}；该信息被传递给自主跟随智能移动机器人执行结构，自主跟随智能移动机器人按照上述信息进行跟随运动；

角速度控制单元（64）对接收到的信息F_n(x)_k和D₈₂＝{v_F,a_F,S_F}依据角速度与虚杆横向力之间的关系

对横向力进行处理，进而得到自主跟随智能移动机器人在当前虚杆内的x′_gL≤x≤x_gL段处横向力的作用下的期望角速度w(x)_k，

延时检测控制单元（65）通过检测第二运动状态检测单元（8）的输出信息 $D_{81}=\left[\begin{array}{c}{x}_{\mathrm{gF}}\\ y_{\mathrm{gF}}\end{array}\right],$ 实时判断自主跟随智能移动机器人在虚拟柔性曲杆的拖动下运动到的取值范围段x_gL(k-1)≤x_gF≤x_gL(k)，然后通过角速度控制单元（64）运算得到的方程式

计算出当前时刻的自主跟随智能移动机器人期望角速度w(x_gF)_k，该期望角速度w(x_gF)_k被传递给自主跟随智能移动机器人执行结构，自主跟随智能移动机器人按照该信息进行跟随运动；其中，x_gL(k-1)表示在第k-1时刻目标智能移动机器人在全局坐标系O-XY下的横坐标；x_gL(k)表示在第k时刻目标智能移动机器人在全局坐标系O-XY下的横坐标；

显示输出单元（66）对采集到的图像信息PCK进行输出。

3.根据权利要求1所述的具有力延时虚拟柔性曲杆的自主跟踪系统，其特征在于：主控制器中的中心处理器选用S3C2440芯片。

Images