CN102323827B - 具有力延时虚拟柔性曲杆的自主跟踪*** - Google Patents
具有力延时虚拟柔性曲杆的自主跟踪*** Download PDFInfo
- Publication number
- CN102323827B CN102323827B CN 201110117402 CN201110117402A CN102323827B CN 102323827 B CN102323827 B CN 102323827B CN 201110117402 CN201110117402 CN 201110117402 CN 201110117402 A CN201110117402 A CN 201110117402A CN 102323827 B CN102323827 B CN 102323827B
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- independently
- intelligent mobile
- curved bar
- prime
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
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°范围内物体距其的位置信息D1={P1,P2,…,Pi,…};第一预处理单元(10)对D1={P1,P2,…,Pi,…}依据距离与角度关系进行处理,得到前车相对后车的位置信息ρC,θC;
GPS导航设备(2)用于测量自主跟随机器人(后车)当前的位置信息D2=(D经度,D纬度);第二预处理单元(20)依据全局坐标转换关系对D2=(D经度,D纬度)进行坐标映射,得到自主跟随机器人(后车)在全局坐标系O-XY下的位置
三轴陀螺仪(3)用于测量自主跟随机器人(后车)当前运动的角速度信息D3=(D31,D32,D33);第三预处理单元(30)对D3=(D31,D32,D33)依据航向角关系Δβ=D33×ts进行处理,得到自主跟随机器人(后车)当前航行的角度相对前一时刻t′的角度增量Δβ,ts表示陀螺仪的采样时间;
码盘(4)用于测量自主跟随机器人(后车)运动过程以脉冲序列形式输出的距离信息D4;第四预处理单元(40)对D4利用脉冲-距离关系ΔS=∑D4×M进行处理,得到自主跟随机器人(后车)在采样时间间隔Δt=t-t′内前进的位移ΔS;
摄像头(5)用于实时采集目标机器人(前车)前行的图像信息PCK;
第二运动状态检测单元(8)第一方面依据航位推算方法Δβ和ΔS进行处理,来更新自主机器人在全局坐标系O-XY下的当前位置坐标;第二运动状态检测单元(8)第二方面利用位移与速度的关系对ΔS进行处理,得到当前时刻自主跟随机器人(后车)的速度信息vF;然后利用速度与加速度的关系求出当前时刻自主跟随机器人(后车)的加速度信息aF;第二运动状态检测单元(8)第三方面进行位移ΔS的累积,即SF=∑ΔS;所述第二运动状态检测单元(8)一方面输出给第一运动状态检测单元(7),另一方面输出D82={vF,aF,SF}给虚拟柔杆自主跟踪***(6);
第一运动状态检测单元(7)首先依据位姿转换关系式对接收的DF-L={ρC,θC}和进行处理,得到目标机器人(前车)在全局坐标系O-XY下的位置坐标然后依据位移与速度的关系获得目标机器人(前车)在当前时刻的速度信息vL;然后利用速度与加速度的关系求出当前时刻目标机器人(前车)的加速度信息aL;所述第一运动状态检测单元(7)输出给虚拟柔杆自主跟踪***(6);
虚拟柔杆自主跟踪***(6)对接收的和D82={vF,aF,SF}进行复现跟随处理,得到自主跟随机器人(后车)在当前虚杆内的纵向力的作用下的加速度aFnew和期望线速度vFnew、以及当前时刻的后车期望角速度w(xgF)k;最后主控制器根据所述的w(xgF)k、aFnew和vFnew对自主跟随机器人(后车)的执行结构进行控制,实现沿目标智能移动机器人前行轨迹进行前行。
本发明具有力延时的虚拟柔性曲杆的自主跟踪***的优点在于:
①本发明应用描述的虚拟柔性曲杆方程,求解出虚拟柔性曲杆内部沿虚杆切线方向的纵向力和与该纵向力垂直的横向力。纵向力的大小取决于前车速度、前车加速度、后车速度、后车加速度以及虚拟柔性曲杆的长度,该纵向力直接作用到后车上去,从而使后车的运动速度、加速度能够实时响应前车的运动变化;横向力的大小取决于曲杆形态和该点处前车线速度的大小,该横向力经过延时后作用到后车上,保证后车能够复现前车的轨迹。
②这样通过曲杆中纵向力与横向力的共同作用从而实现了后车对于前车的跟踪运动。通过法向力与纵向力的引入,不仅确保了后车能够实时的响应前车的运动变化从而使两车之间始终保持一个合理的安全距离,而且保证了前车在进行转弯运动的时候,后车不会立即跟随前车进行转弯,而是在纵向力的作用下运动到转弯点后才进行转弯运动,从而确保了后车的行车安全。
③引入力控制的概念,将曲杆中的纵向力Fl和横向力Fn作为驱动后车运动状态发生改变的条件,通过Fl和Fn的牵引作用,能够使后车更佳精确地复现前车的轨迹。
附图说明
图1是本发明具有力延时的虚拟柔性曲杆的自主跟踪***的信号流向结构框图。
图1A是本发明自主跟踪***中虚拟柔杆自主跟踪模块的信号流向结构框图。
图1B是目标机器人(前车)与自主跟随机器人(后车)的跟随状态示意图。
图2是本发明对激光测距仪测得扫描点对前车后端所在直线进行拟合的示意图。
图2A是本发明中对前车宽度直线获知前车后端中心点相对后车位置示意图。
图3是本发明前后两车在全局坐标系下的位姿关系图。
图4是本发明自主跟踪***处理得到的虚拟柔性曲杆形态示意图。
具体实施方式
下面将结合附图对本发明做进一步的详细说明。
本发明将具有力延时的虚拟柔性曲杆的自主跟踪***设置在现有智能移动机器人的主控制器上,所述主控制器的中心处理器采用三星公司生产的S3C2440芯片,并在该S3C2440芯片上应用Qt/embedded-4.4.3软件开发平台,设置了能够实现自主跟随机器人完全复现目标机器人的运动轨迹的自主跟踪***。该自主跟踪***加载在智能移动机器人的主控制器内,在本发明中涉及的独立式自动跟踪是指自主跟随智能移动机器人复现目标智能移动机器人的前行轨迹。
参见图3所示,目标智能移动机器人(前车)与自主跟随智能移动机器人(后车)之间相对跟随的距离记为DF-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所示,在本发明中,自主跟随机器人(后车)的坐标系记为OF-XFYFZF,XF轴与自主跟随机器人(后车)后轮的中轴线重合,YF轴与自主跟随机器人(后车)前进方向保持一致,OF是自主跟随机器人(后车)后轮中轴线的中心点,该中心点的正上方(即ZF轴)为激光测距仪1的安装位置。
在本发明中,自主跟随机器人(后车)上设置的激光测距仪1用于检测自主跟随机器人(后车)前方180°范围内物体距其的位置信息D1={P1,P2,…,Pi,…},P1表示激光测距仪1检测到的第1个扫描点,P2表示激光测距仪1检测到的第2个扫描点,Pi表示激光测距仪1检测到的第i个扫描点;
连接P1与OF两点的线段长度记为ρ1,该线段与XF轴的夹角记为θ1;
连接P2与OF两点的线段长度记为ρ2,该线段与XF轴的夹角记为θ2;
连接Pi与OF两点的线段长度记为ρi,该线段与XF轴的夹角记为θi;
在利用激光测距仪1进行前车与后车之间的距离测量时,对激光测距仪1测得的激光扫描点P1,P2,…,Pi,…采用最小二乘法拟合,得到前车后端在OF-XFYFZF坐标下的拟合直线ST。所述的激光扫描点P1,P2,…,Pi,…中,必然存在有目标机器人(前车)的后端两个边缘返回的激光扫描点分别记为A扫描点和B扫描点,连接A扫描点与B扫描点形成的线段记为AB,所述线段AB的长度即为前车的车宽。线段AB的中心点记为C点,则有:
连接A与OF两点的线段长度记为ρA,该线段与XF轴的夹角记为θA;
连接B与OF两点的线段长度记为ρB,该线段与XF轴的夹角记为θB;
参见图2A所示,在本发明中,自主跟随机器人(后车)的坐标系记为OF-XFYFZF,XF轴与自主跟随机器人(后车)后轮的中轴线重合,YF轴与自主跟随机器人(后车)前进方向保持一致,OF是自主跟随机器人(后车)后轮中轴线的中心点,该中心点的正上方(即ZF轴)为激光测距仪1的安装位置。
在利用激光测距仪1进行前车与后车之间的距离测量时,对激光测距仪1测得的激光扫描点采用最小二乘法拟合,得到前车后端在OF-XFYFZF坐标下的拟合直线ST。
自主跟随机器人(后车)上设置的GPS导航设备2用于测量自主跟随机器人(后车)当前的位置信息D2=(D经度,D纬度),其中D经度表示自主跟随机器人(后车)利用GPS导航设备测得的经度,D纬度表示自主跟随机器人(后车)利用GPS导航设备测得的纬度。
自主跟随机器人(后车)上设置的三轴陀螺仪3用于测量自主跟随机器人(后车)当前运动的角速度信息D3=(D31,D32,D33),其中D31为自主跟随机器人(后车)绕XF轴的角速度,D32为自主跟随机器人(后车)绕YF轴的角速度,D33为自主跟随机器人(后车)绕ZF轴的角速度。
自主跟随机器人(后车)上设置的码盘4用于测量自主跟随机器人(后车)运动过程以脉冲序列形式输出的距离信息D4;
自主跟随机器人(后车)上设置的摄像头5用于实时采集目标机器人(前车)前行的图像信息PCK。由于本发明中选用的中心处理器S3C2440芯片可以直接与摄像头的输出接口连接,故可以不用采集卡便实现了对采集到的图像进行在智能移动机器人上显示。
下面将对自主跟随机器人(后车)采集到的各路信息进行处理:
(一)第一预处理单元10
第一预处理单元10对接收到的D1={P1,P2,…,Pi,…}依据距离与角度关系进行处理,得到前车相对后车的位置信息DF-L=(ρC,θC);该位置信息ρC,θC表征了前后两车的相对跟随距离DF-L=(ρC,θC);
n表示线段AB上所有激光扫描点的个数,m表示求和公式中的变量;
ρm表示线段AB上扫描点Pm与OF两点的线段长度,该线段PmOF与XF轴的夹角记为θm。
(二)第二预处理单元20
参见图3所示,自主跟随机器人(后车)的全局坐标系记为O-XY,X轴上的数值代表的是地球的纬度,Y轴上的数值代表的是地球的经度。所述的经度、纬度的数值为GPS导航设备2采集到的自主跟随机器人(后车)位置信息。
第二预处理单元20依据全局坐标转换关系对接收到的位置信息D2=(D经度,D纬度)进行坐标映射,得到自主跟随机器人(后车)在全局坐标系O-XY下的位置其中:xgF表示自主跟随机器人(后车)上的点OF映射在全局坐标系O-XY下X轴上的值;ygF表示自主跟随机器人(后车)上的点OF映射在全局坐标系O-XY下Y轴上的值;表示自主跟随机器人(后车)当前时
刻的位置坐标;O经度度表示全局坐标系中原点O处的经度值;O纬度表示全局坐标系中原点O处的纬度值。
在本发明中,利用全局坐标系O-XY对自主跟随机器人(后车)的位置进行量化,便于不同位置在同一坐标基准下进行解算。
(三)第三预处理单元30
第三预处理单元30对接收的D3=(D31,D32,D33)依据航向角关系Δβ=D33×ts进行处理,得到自主跟随机器人(后车)当前航行的角度相对前一时刻t′的角度增量Δβ,ts表示陀螺仪的采样时间。
在本发明中,前进夹角关系Δβ=D33×ts是指当前时刻t与前一时刻t′的自主跟随机器人(后车)前进方向之间的夹角。
(四)第四预处理单元40
第四预处理单元40对接收的距离信息D4利用脉冲-距离关系ΔS=∑D4×M进行处理,得到自主跟随机器人(后车)在采样时间间隔Δt=t-t′内前进的位移ΔS;M为自主跟随机器人(后车)在码盘每输出一个脉冲间隔内前进的距离;
(五)第二运动状态检测单元8
βt′表示自主跟随机器人(后车)前一时刻的航向角度大小;
第二运动状态检测单元8第二方面利用位移与速度的关系对ΔS进行处理,得到当前时刻自主跟随机器人(后车)的速度信息vF;然后利用速度与加速度的关系求出当前时刻自主跟随机器人(后车)的加速度信息aF;v′F表示前一时刻自主跟随机器人(后车)的速度信息;
第二运动状态检测单元8第三方面进行位移ΔS的累积,即SF=∑ΔS,
在本发明中,第二运动状态检测单元8输出给虚拟柔杆自主跟踪***6的信息包括有当前时刻自主跟随机器人(后车)的速度信息vF、当前时刻自主跟随机器人(后车)的加速度信息aF和累积位移SF,采用数学集合形式表达为D82={vF,aF,SF},同时第二运动状态检测单元8输出给第一运动状态检测单元7的信息D81为自主跟随机器人(后车)当前时刻在全局坐标系O-XY下的位置坐标,表示为
(六)第一运动状态检测单元7
第一运动状态检测单元7首先依据位姿转换关系式对接收的DF-L={ρC,θC}和进行处理,得到目标机器人(前车)在全局坐标系O-XY下的位置坐标然后依据位移与速度的关系获得目标机器人(前车)在当前时刻的速度信息vL;然后利用速度与加速度的关系求出当前时刻目标机器人(前车)的加速度信息aL;
v′L表示前一时刻目标机器人(前车)的速度信息;
在本发明中,第一运动状态检测单元7输出给虚拟柔杆自主跟踪***6的信息包括有目标机器人(前车)在全局坐标系O-XY下的位置坐标当前时刻目标机器人(前车)的速度信息vL、当前时刻目标机器人(前车)的加速度信息aL,采用数学集合形式表达为
(七)虚拟柔性曲杆形态解析单元61
参见图4所示,两个机器人之间的虚拟曲杆在全局坐标系O-XY下形态如图示曲线表示。
虚拟柔性曲杆形态解析单元61对接收的进行方程描述,获得虚拟柔性曲杆在x′gL≤x≤xgL段的描述方程 然后对f(x)k进行一阶求导得到f(x)k在x′gL≤x≤xgL段的一阶导数方程f′(x)k,对f(x)k进行二阶求导得到f(x)k在x′gL≤x≤xgL段的二阶导数方程f″(x)k,然后利用曲线长度与一阶导数f′(x)k的关系求出虚拟曲杆在x′gL≤x≤xgL段的长度Lk;
其中h=xgL-x′gL;
θ′C表示前一时刻目标机器人(前车)的航向角;
f(x)k表示在x′gL≤x≤xgL取值范围内的虚拟柔性曲杆的曲线方程,f(x)k中的x为自变量,k表示跟随过程中的某一时间;
f′(x′gL)k表示f′(x)k在x′gL处的函数值;
f′(xgL)k表示f′(x)k在xgL处的函数值;
在本发明中,虚拟柔性曲杆形态解析单元61输出虚拟柔性曲杆内力解析单元62的信息为虚拟柔性曲杆在x′gL≤x≤xgL段的描述方程f(x)k、f(x)k的一阶导数f′(x)k、虚拟柔性曲杆的长度Lk,采用数学集合形式表达为D61-62={f(x)k,f′(x)k,Lk}。
(八)虚拟柔性曲杆内力解析单元62
其中e表示曲杆的胡克系数,该胡克系数表征曲杆形变量与其内力大小的关系;
ea表示加速度因子,该加速度因子用于调节曲杆内力与前后车加速度大小的关系;
ev表示速度因子,速度因子用于调节曲杆内力与前后车速度大小的关系;
l表示虚拟曲杆当前的长度,且
Dmin表示设定的虚拟曲杆的最小安全距离;
虚拟柔性曲杆内力解析单元62对接收到的
其中f′(x)k为f(x)k的一阶导数,f″(x)k为f(x)k的二阶导数,mL表示前车的质量;
在本发明中,虚拟柔性曲杆内力解析单元62输出的信息为虚拟柔性曲杆中的纵向力Fl和横向力Fn(x)k以及虚拟曲杆当前的长度l。
(九)线速度控制单元63
然后根据期望速度与虚杆长度关系
求出自主跟随机器人(后车)的期望线速度vFnew;
其中mF表示设定的自主跟随机器人(后车)质量;在本发明中,线速度控制单元63输出的信息为自主跟随机器人(后车)在当前虚杆内的纵向力的作用下的加速度aFnew和期望线速度vFnew,采用数学集合形式表达为D63={aFnew,vFnew}。该信息被传递给自主跟随机器人(后车)执行结构,自主跟随机器人(后车)按照上述信息进行跟随运动。
(十)角速度控制单元64
角速度控制单元64对接收到的信息Fn(x)k和D82={vF,aF,SF}依据角速度与虚杆横向力之间的关系对横向力进行处理,进而得到自主跟随机器人(后车)在当前虚杆内的x′gL≤x≤xgL段处横向力的作用下的期望角速度w(x)k,
在本发明中角速度控制单元64输出的信息为自主跟随机器人(后车)在当前虚杆内的横向力的作用下的期望角速度w(x)k,该角速度信息经过延时检测控制单元6
5的延时检测后输出给自主跟随机器人(后车)执行结构,自主跟随机器人(后车)按照该信息进行跟随运动。
(十一)延时检测控制单元65
延时检测控制单元65通过检测第二运动状态检测单元8的输出信息实时判断自主跟随机器人(后车)在虚拟柔性曲杆的拖动下运动到的取值范围段xgL(k-1)≤xgF≤xgL(k),然后通过角速度控制单元64运算得到的方程式计算出当前时刻的后车期望角速度w(xgF)k,该期望角速度w(xgF)k被传递给自主跟随机器人(后车)执行结构,自主跟随机器人(后车)按照该信息进行跟随运动。
其中,
xgL(k-1)表示在第k-1时刻目标机器人(前车)在全局坐标系O-XY下的横坐标;
xgL(k)表示在第k时刻目标机器人(前车)在全局坐标系O-XY下的横坐标。
在本发明中,目标机器人(前车)在进行转弯运动时产生的横向力Fn(xgL)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°范围内物体距其的位置信息D1={P1,P2,…,Pi,…};第一预处理单元(10)对D1={P1,P2,…,Pi,…}依据距离与角度关系进行处理,得到目标智能移动机器人相对自主跟随智能移动机器人的位置信息ρC,θC;P1表示激光测距仪(1)检测到的第1个扫描点,P2表示激光测距仪(1)检测到的第2个扫描点,Pi表示激光测距仪(1)检测到的第i个扫描点;连接C与OF两点的线段长度记为ρC,该线段与XF轴的夹角记为θC;n表示线段AB上所有激光扫描点的个数,m表示求和公式中的变量;ρm表示线段AB上扫描点Pm与OF两点的线段长度,该线段PmOF与XF轴的夹角记为θm,α表示线段AB的垂线与XF轴的夹角,且该垂线经过OF点;自主跟随智能移动机器人的坐标系记为OF-XFYFZF,OF是自主跟随智能移动机器人后轮中轴线的中心点;AB是目标智能移动机器人的车宽,C点表示线段AB的中心点;
GPS导航设备(2)用于测量自主跟随智能移动机器人当前的位置信息D2=(D经度,D纬度);第二预处理单元(20)依据全局坐标转换关系对D2=(D经度,D纬度)进行坐标映射,得到自主跟随智能移动机器人在全局坐标系O-XY下的位置xgF表示自主跟随智能移动机器人上的点OF映射在全局坐标系O-XY下X轴上的值;ygF表示自主跟随智能移动机器人上的点OF映射在全局坐标系O-XY下Y轴上的值;表示自主跟随智能移动机器人当前时刻的位置坐标;O经度表示全局坐标系中原点O处的经度值;O纬度表示全局坐标系中原点O处的纬度值;
三轴陀螺仪(3)用于测量自主跟随智能移动机器人当前运动的角速度信息D3=(D31,D32,D33);第三预处理单元(30)对D3=(D31,D32,D33)依据航向角关系Δβ=D33×ts进行处理,得到自主跟随智能移动机器人当前航行的角度相对前一时刻t′的角度增量Δβ,ts表示陀螺仪的采样时间;D31为自主跟随智能移动机器人绕XF轴的角速度,D32为自主跟随智能移动机器人绕YF轴的角速度,D33为自主跟随智能移动机器人绕ZF轴的角速度;
码盘(4)用于测量自主跟随智能移动机器人运动过程以脉冲序列形式输出的距离信息D4;第四预处理单元(40)对D4利用脉冲-距离关系ΔS=∑D4×M进行处理,得到自主跟随智能移动机器人在采样时间间隔Δt=t-t′内前进的位移ΔS;M为自主跟随智能移动机器人在码盘每输出一个脉冲间隔内前进的距离;
摄像头(5)用于实时采集目标智能移动机器人前行的图像信息PCK;
第二运动状态检测单元(8)第一方面依据航位推算方法 对 Δβ和ΔS进行处理,来更新自主跟随智能移动机器人在全局坐标系O-XY下的当前位置坐标; 表示自主跟随智能移动机器人前一时刻的位置坐标;βt′表示自主跟随智能移动机器人前一时刻的航向角度大小;第二运动状态检测单元(8)第二方面利用位移与速度的关系对ΔS进行处理,得到当前时刻自主跟随智能移动机器人的速度信息vF;然后利用速度与加速度的关系求出当前时刻自主跟随智能移动机器人的加速度信息aF;v′F表示前一时刻自主跟随智能移动机器人的速度信息;第二运动状态检测单元(8)第三方面进行位移ΔS的累积,即SF=∑ΔS;所述第二运动状态检测单元(8)一方面输出给第一运动状态检测单元(7),另一方面输出D82={vF,aF,SF}给虚拟柔杆自主跟踪***(6);
第一运动状态检测单元(7)首先依据位姿转换关系式 对接收的DF-L={ρC,θC}和 进行处理,得到目标智能移动机器人在全局坐标系O-XY下的位置坐标 β表示在全局坐标系O-XY下自主跟随智能移动机器人在当前时刻下的航行角度,即YF轴与X轴的夹角;然后依据位移与速度的关系 获得目标智能移动机器人在当前时刻的速度信息vL;然后利用速度与加速度的关系求出当前时刻目标智能移动机器人的加速度信息aL; 表示目标智能移动机器人前一时刻的位置坐标;v′L表示前一时刻目标智能移动机器人的速度信息;所述第一运动状态检测单元(7)输出 给虚拟柔杆自主跟踪***(6);
虚拟柔杆自主跟踪***(6)对接收的 和D82={vF,aF,SF}进行复现跟随处理,得到自主跟随智能移动机器人在当前虚杆内的纵向力的作用下的加速度aFnew和期望线速度vFnew、以及当前时刻的自主跟随智能移动机器人期望角速度w(xgF)k;最后主控制器根据所述的w(xgF)k、aFnew和vFnew对自主跟随智能移动机器人的执行结构进行控制,实现沿目标智能移动机器人前行轨迹进行前行。
2.根据权利要求1所述的具有力延时虚拟柔性曲杆的自主跟踪***,其特征在于:虚拟柔杆自主跟踪***(6)包括有虚拟柔性曲杆形态解析单元(61)、虚拟柔性曲杆内力解析单元(62)、线速度控制单元(63)、方向控制单元(64)、延时检测控制单元(65)和显示输出单元(66);
虚拟柔性曲杆形态解析单元(61)对接收的 进行方程描述,获得虚拟柔性曲杆在x′gL≤x≤xgL段的描述方程 然后对f(x)k进行一阶求导得到f(x)k在x′gL≤x≤xgL段的一阶导数方程f′(x)k,对f(x)k进行二阶求导得到f(x)k在x′gL≤x≤xgL段的二阶导数方程f″(x)k,然后利用曲线长度与一阶导数f′(x)k的关系 求出虚拟曲杆在x′gL≤x≤xgL段的长度Lk;其中h=xgL-x′gL; θ′C表示前一时刻目标智能移动机器人的航向角;f(x)k表示在x′gL≤x≤xgL取值范围内的虚拟柔性曲杆的曲线方程,f(x)k中的x为自变量,k表示跟随过程中的某一时间;f′(x′gL)k表示f′(x)k在x′gL处的函数值;f′(xgL)k表示f′(x)k在xgL处的函数值;表示f′(x)k在处的函数值;所述虚拟柔性曲杆形态解析单元(61)输出信息有D61-62={f(x)k,f′(x)k,Lk};
虚拟柔性曲杆内力解析单元(62)对接收到的 D82={vF,aF,SF}和D61-62={f(x)k,f′(x)k,Lk}根据力与虚拟曲杆长度关系 进行计算,求出当前状态下虚拟曲杆内部的纵向作用力Fl;其中e表示曲杆的胡克系数,该胡克系数表征曲杆形变量与其内力大小的关系;ea表示加速度因子,该加速度因子用于调节曲杆内力与目标智能移动机器人、自主跟随智能移动机器人之间加速度大小的关系;ev表示速度因子,速度因子用于调节曲杆内力与目标智能移动机器人、自主跟随智能移动机器人之间速度大小的关系;l表示虚拟曲杆当前的长度,且Dmin表示设定的虚拟曲杆的最小安全距离;虚拟柔性曲杆内力解析单元(62)对接收到的 D82={vF,aF,SF}和D61-62={f(x)k,f′(x)k,Lk}根据横向力与虚杆形态的关系 进行计算,得到虚拟柔性曲杆内部在x′gL≤x≤xgL段的横向力描述方程Fn(x)k;其中f′(x)k为f(x)k的一阶导数,f″(x)k为f(x)k的二阶导数,mL表示目标智能移动机器人的质量;
线速度控制单元(63)对接收到的信息Fl、 D82={vF,aF,SF}根据力与加速度的关系得到自主跟随智能移动机器人在当前虚杆内的纵向力的作用下的加速度aFnew,然后根据期望速度与虚杆长度关系 求出自主跟随智能移动机器人的期望线速度vFnew;其中mF表示设定的自主跟随智能移动机器人质量;所述线速度控制单元(63)输出的信息有D63={aFnew,VFnew};该信息被传递给自主跟随智能移动机器人执行结构,自主跟随智能移动机器人按照上述信息进行跟随运动;
角速度控制单元(64)对接收到的信息Fn(x)k和D82={vF,aF,SF}依据角速度与虚杆横向力之间的关系对横向力进行处理,进而得到自主跟随智能移动机器人在当前虚杆内的x′gL≤x≤xgL段处横向力的作用下的期望角速度w(x)k,
延时检测控制单元(65)通过检测第二运动状态检测单元(8)的输出信息 实时判断自主跟随智能移动机器人在虚拟柔性曲杆的拖动下运动到的取值范围段xgL(k-1)≤xgF≤xgL(k),然后通过角速度控制单元(64)运算得到的方程式计算出当前时刻的自主跟随智能移动机器人期望角速度w(xgF)k,该期望角速度w(xgF)k被传递给自主跟随智能移动机器人执行结构,自主跟随智能移动机器人按照该信息进行跟随运动;其中,xgL(k-1)表示在第k-1时刻目标智能移动机器人在全局坐标系O-XY下的横坐标;xgL(k)表示在第k时刻目标智能移动机器人在全局坐标系O-XY下的横坐标;
显示输出单元(66)对采集到的图像信息PCK进行输出。
3.根据权利要求1所述的具有力延时虚拟柔性曲杆的自主跟踪***,其特征在于:主控制器中的中心处理器选用S3C2440芯片。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN 201110117402 CN102323827B (zh) | 2011-05-06 | 2011-05-06 | 具有力延时虚拟柔性曲杆的自主跟踪*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN 201110117402 CN102323827B (zh) | 2011-05-06 | 2011-05-06 | 具有力延时虚拟柔性曲杆的自主跟踪*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN102323827A CN102323827A (zh) | 2012-01-18 |
CN102323827B true CN102323827B (zh) | 2013-04-24 |
Family
ID=45451577
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN 201110117402 Expired - Fee Related CN102323827B (zh) | 2011-05-06 | 2011-05-06 | 具有力延时虚拟柔性曲杆的自主跟踪*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN102323827B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104063153B (zh) * | 2014-05-04 | 2018-12-11 | 南京中兴软件有限责任公司 | 一种实现人机交互的方法和装置 |
CN105701292B (zh) * | 2016-01-13 | 2019-02-01 | 西北工业大学 | 一种机动目标转弯角速度的解析辨识方法 |
CN105929849B (zh) * | 2016-04-28 | 2018-10-23 | 东南大学 | 一种基于点镇定的轮式移动机器人目标跟踪控制方法 |
CN106647730B (zh) * | 2016-08-30 | 2019-10-29 | 北京九星智元科技有限公司 | 一种前方随行***及随行方法和智能移动平台 |
CN114848155B (zh) * | 2022-04-29 | 2023-04-25 | 电子科技大学 | 一种用于手术机器人延时测量的验证装置 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101201626A (zh) * | 2007-12-10 | 2008-06-18 | 华中科技大学 | 机器人自主定位*** |
CN101468664A (zh) * | 2007-12-24 | 2009-07-01 | 上海市电力公司电缆输配电公司 | 电缆隧道危险环境小型履带检查机器人*** |
CN101887037A (zh) * | 2010-04-27 | 2010-11-17 | 丹东奥龙射线仪器有限公司 | 轮式x射线探伤机器人装置 |
CN101980094A (zh) * | 2010-11-03 | 2011-02-23 | 北京理工大学 | 平衡装置、方法和两轮机器人 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4781000B2 (ja) * | 2005-04-25 | 2011-09-28 | 三菱電機株式会社 | サーボ制御装置 |
-
2011
- 2011-05-06 CN CN 201110117402 patent/CN102323827B/zh not_active Expired - Fee Related
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101201626A (zh) * | 2007-12-10 | 2008-06-18 | 华中科技大学 | 机器人自主定位*** |
CN101468664A (zh) * | 2007-12-24 | 2009-07-01 | 上海市电力公司电缆输配电公司 | 电缆隧道危险环境小型履带检查机器人*** |
CN101887037A (zh) * | 2010-04-27 | 2010-11-17 | 丹东奥龙射线仪器有限公司 | 轮式x射线探伤机器人装置 |
CN101980094A (zh) * | 2010-11-03 | 2011-02-23 | 北京理工大学 | 平衡装置、方法和两轮机器人 |
Non-Patent Citations (1)
Title |
---|
JP特开2006-302201A 2006.11.02 |
Also Published As
Publication number | Publication date |
---|---|
CN102323827A (zh) | 2012-01-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102183960B (zh) | 一种适用于独立式自动跟踪的局部可柔度虚杆转弯控制*** | |
Vivacqua et al. | Self-localization based on visual lane marking maps: An accurate low-cost approach for autonomous driving | |
CN107246868B (zh) | 一种协同导航定位***及导航定位方法 | |
US10579062B2 (en) | Scalable smooth reference path generator for autonomous driving vehicles | |
CN102323827B (zh) | 具有力延时虚拟柔性曲杆的自主跟踪*** | |
CN102372000B (zh) | 获取泊车位参数的装置和*** | |
US8392104B2 (en) | Avoidance maneuver calculation device, avoidance control device, vehicle having each device, avoidance maneuver calculating method, and avoidance controlling method | |
CN111907516B (zh) | 一种全自动泊车方法和*** | |
CN104061899B (zh) | 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法 | |
CN103822625B (zh) | 一种智能机器人寻线导航的方法与装置 | |
CN111026106A (zh) | 一种无人车室外驾驶*** | |
CN109823393A (zh) | 一种智能驾驶车辆轨迹跟踪控制方法 | |
CN104118430B (zh) | 一种基于滑模自抗扰控制的平行泊车***及泊车方法 | |
CN110060467A (zh) | 预测装置、预测方法及存储介质 | |
JP6607865B2 (ja) | 経路制御システムおよび経路制御方法 | |
CN206644887U (zh) | 基于myRIO平台的声控智能车 | |
CN111071249A (zh) | 基于障碍物运动预测的无人车紧急转向避障方法 | |
CN105774655B (zh) | 一种倾斜车位探测装置及方法 | |
CN111208814B (zh) | 用于自动车辆的、利用动态模型的、基于记忆的最优运动规划 | |
CN106950953A (zh) | 一种自动泊车路径跟踪及车辆控制***和方法 | |
CN104374391A (zh) | 一种车辆行驶轨迹计算***与计算方法 | |
CN103085815A (zh) | 一种识别驾驶员换道意图的方法 | |
CN113460033A (zh) | 一种自动泊车方法以及装置 | |
CN103608217A (zh) | 用于泊车引导的加装组件 | |
CN114993705A (zh) | 一种超车场景下紧急车道保持的实车测试*** |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20130424 Termination date: 20180506 |