具有力延时虚拟柔性曲杆的自主跟踪系统

摘要

本发明公开了一种具有力延时的虚拟柔性曲杆的自主跟踪系统，该系统包括有激光测距仪、GPS导航设备、三轴陀螺仪、码盘、摄像头、第一预处理单元、第二预处理单元、第三预处理单元、第四预处理单元、第一运动状态检测单元、第二运动状态检测单元和虚拟柔杆自主跟踪系统。该系统首先通过激光雷达对目标机器人进行检测，得到目标机器人相对于自主跟随机器人的距离与位姿，利用自主跟随机器人的自我定位信息得到目标机器人在全局坐标系中的位置坐标，由目标机器人当前点以及历史点即可通过虚拟柔性曲杆模型得到目标机器人运动的轨迹曲线，进而求出当前状态下虚拟柔性曲杆的长度；利用虚拟柔性曲杆的形态与力延时模型的结合得到目标机器人在当前运动点处对自主跟随机器人的作用力。

主权项

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}}^{\prime }+\Delta S\cos ({\beta }_{t^{\prime }}+\frac{\mathrm{\Delta \beta }}{2})\\ y_{\mathrm{gF}}^{\prime }+\Delta S\sin ({\beta }_{t^{\prime }}+\frac{\mathrm{\Delta \beta }}{2})\end{array}\right]$对$\left[\begin{array}{c}{x}_{\mathrm{gF}}^{\prime }\\ y_{\mathrm{gF}}^{\prime }\end{array}\right],$Δβ和ΔS进行处理，来更新自主跟随智能移动机器人在全局坐标系O-XY下的当前位置坐标；$\left[\begin{array}{c}{x}_{\mathrm{gF}}^{\prime }\\ y_{\mathrm{gF}}^{\prime }\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}}={\rho }_C\cos {\theta }_C\cos \beta -{\rho }_C\sin {\theta }_C\sin \beta +x_{\mathrm{gF}}\\ y_{\mathrm{gL}}={\rho }_C\cos {\theta }_C\sin \beta +{\rho }_C\sin {\theta }_C\cos \beta +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}}^{\prime })}^2+{(y_{\mathrm{gL}}-y_{\mathrm{gL}}^{\prime })}^2}}{\mathrm{\Delta t}}$获得目标智能移动机器人在当前时刻的速度信息v_L；然后利用速度与加速度的关系求出当前时刻目标智能移动机器人的加速度信息a_L；$\left[\begin{array}{c}{x}_{\mathrm{gL}}^{\prime }\\ y_{\mathrm{gL}}^{\prime }\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对自主跟随智能移动机器人的执行结构进行控制，实现沿目标智能移动机器人前行轨迹进行前行。