CN107444491B

CN107444491B - 一种机动车轨迹控制方法及机动车导航坐标系建立方法

Info

Publication number: CN107444491B
Application number: CN201710678122.XA
Authority: CN
Inventors: 郑杨; 郑兴
Original assignee: Sihong Zhengxing Medical Technology Co ltd
Current assignee: Sihong Zhengxing Medical Technology Co ltd
Priority date: 2017-08-10
Filing date: 2017-08-10
Publication date: 2023-12-05
Anticipated expiration: 2037-08-10
Also published as: CN107444491A

Abstract

本发明公开了一种机动车轨迹控制方法及机动车导航坐标系建立方法，任选两个参考点，计算两点之间的距离；以任一点为原点，以两点的连线为X轴，建立XY轴坐标系；基于建立的坐标系，机动车的激光导航设备以角度θ发出直线激光，机动车沿车身偏角直线倒车，当直线激光扫描到非原点的参考点时，将方向盘打至固定位置，改变行驶轨迹。本发明在任何车库、停车场、空地都可以使用，使用范围广泛；受周围环境干扰小，可以在任意角度和位置停车；精确、安全、安装及拆卸方便，可根据车型选择合适的卡具，直接卡在后视镜上，自带电源，不用改汽车线路；外形美观，不产生额外风阻，保护壳与外后视镜融为一体，可放平或缩至保护壳内。

Description

一种机动车轨迹控制方法及机动车导航坐标系建立方法

技术领域

本发明属于汽车导航技术领域，尤其涉及一种机动车轨迹控制方法及机动车导航坐标系建立方法。

背景技术

随着汽车产业的发展，我国汽车保有量和相关的事故迅速增长，据有关数据统计，40％事故发生与泊车有关。驾校必考项目“倒车入库”一向被誉为“考试杀手”，即使拿到驾照，新手驾驶员在现实中经常要遇到更加复杂的倒车情况。由于没有合适的参照物，或难以观察到车辆后方环境，空间感知能力不足，很容易导致车尾碰撞、车身刮蹭甚至更严重的事故发生。

目前车载泊车系统主要依靠两种设备：距离探测器和实时影像。距离探测器主要包括超声波雷达、微波雷达和激光雷达三种。其中，超声波雷达有效测距只有几米，只能作为倒车雷达应用，低矮的障碍物难以探测，并且使用者无法直观的判断障碍物相对于车的距离和位置。微波雷达具有受雨雾天气影响小、有效距离可达100米左右的优点，但其发散角太大，测量目标距离和位置时精度不高，实际应用中主要用于测速、测距。激光雷达具有测量精度高，测距远，性能优异等特点，适用于汽车测速、测距和目标定位。激光雷达结构相对简单，具有高单色性、高方向性、相干性好、测量精度较高、探测距离远等特点，受到了广泛的关注，可应用于自动巡航(ACC)、碰撞预警、车道偏离预警等方面，前景非常广阔。车载实时影像一般采用VGA摄像头，其图像像素小、清晰度差、镜头视角小、存在盲区，并且在晚上或逆光状态下图像质量极差，无法满足使用要求。

车载泊车系统中除综合应用上述设备，还增加了许多设备，如辅助制动、油门辅助控制、方向盘转向辅助、轮胎偏转角测量装置、车速测量装置等。虽然方便但启用的条件却很繁琐，必须让雷达精准的检测到车位位置。以上装置都是固定式安装，出厂就安装的一般为高端车，售价自然不扉。对于广大的中低端车，如果要出厂后加装，则对汽车的电路和外形改动较大，有故障隐患。

现有的车载泊车系统在计算路径时没有建立统一坐标系，不能预先制定完整的行车路线，而是靠车载感应器与障碍物之间的即时反馈来引导泊车，一旦反馈中断，泊车也将停止。由于无法事先精确提供车辆需要的转弯角度和行驶距离，泊车停止后要重新回到起始位置，等满足车载电脑的预设条件后，才能开始第二次自动泊车。泊车中止时驾驶员需要继续完成泊车或重新开回原来位置，反而更困难。

非车载的泊车辅助系统主要采用在停车位安装指示装置，通过声光电等方式指导车辆进入，虽然精度高，但是成本较高，无法在全国所有停车位上都安装这样的系统。对于新手司机来说，在没有安装这类辅助系统的停车位会出现事故。

目前还没有可以在泊车和正常行车时通用的车载系统。高速行车时对车距的判断需要靠驾驶员目测估计，判断不准时容易造成追尾。经常有驾驶员由于打瞌睡、捡手机、扭头造成目光短暂地脱离正前方，或方向盘打偏，在以100公里每小时的车速下，1秒钟就会行驶约30米，很容易发生追尾或撞到路边护栏。这些问题可以通过在汽车上安装汽车雷达来提前预警，而目前的泊车系统都是在低速时运行，在高速行车时毫无用处。

发明内容

发明目的：针对以上问题，本发明提出一种机动车轨迹控制方法及机动车导航坐标系建立方法。

技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种机动车导航坐标系建立方法，任选两个参考点，计算两点之间的距离；以任一点为原点，以两点的连线为X轴，建立XY轴坐标系。

所述机动车导航坐标系建立方法在机动车轨迹控制中的应用。

一种机动车轨迹控制方法，基于建立的坐标系，机动车的激光导航设备以角度θ发出直线激光，机动车沿车身偏角直线倒车，当直线激光扫描到参考点时，将方向盘打至固定位置，改变行驶轨迹。

进一步地，固定位置包括方向盘打半圈、方向盘打一圈和方向盘打死。

应用于倒车入库，具体包括以下步骤：

(1)建立以车库门两边连线为横坐标的坐标系，计算机动车的坐标、终点位置、车库门宽度和车库门与机动车的距离及角度；

(2)计算角度θ，再旋转激光旋转平台至角度θ，使射出的直线激光在车库门前方地面照射出一条直线；

(3)方向盘拨至0度，直线倒车至直线激光照射到车库门近侧门框；

(4)向右打方向盘打至固定位置，旋转激光旋转平台至角度为0度使直线激光与车身平行，继续倒车至直线激光与车库内框平行；

(5)回复方向盘至0度，沿与车体平行的直线激光直线倒车入库。

所述步骤(1)中建立坐标系具体包括以下步骤：

(1)打开激光导航系统，升至最大高度；

(2)旋转激光旋转平台，使射出的直线激光照射到车库门近侧门框，测量出旋转角度为a1；如果没有车库门框，激光雷达测量车库门在地面上的近点距离；

(4)通过激光雷达测量出距离车库门近侧门框的距离或距离车库门在地面上近点的距离L1；

(5)旋转激光旋转平台，使射出的直线激光偏转后照射到车库门远侧门框或在地面上的远点，测量出旋转角度为a2；

(6)通过激光雷达测量出距离车库门近侧门框的距离或距离车库门在地面上远点的距离L2；

(7)将参数a1、a2、L1、L2输入车内控制单元，并提前在车内控制单元中输入固有参数，建立以车库门两边连线为横坐标的坐标系。

应用于侧方位停车，具体包括以下步骤：

(1)以后方障碍物的前方内侧点O和外侧点B为X轴建立坐标系；以前方障碍物后外侧拐角P点为碰撞危险点；

(2)调整车前后距离，调整车外执行单元使右侧直线激光照射到O点，向前直线行驶少量距离后该激光刚好扫到P点，再调整车外执行单元使右侧直线激光照射到O点，计算并控制左侧直线激光转动成需要的偏转角度θ1；

(3)将方向盘打至固定位置倒车，当以θ1角度发射的直线激光扫射到P点，计算并控制左侧直线激光转动成需要的偏转角度θ2；

(4)直线倒车，当以θ2角度发射的直线激光扫射到P点；

(5)以最大转角倒车，完成侧方位停车。

应用于计算方向盘打至固定位置时车辆环行内半径，具体包括以下步骤：

(1)在地面同一水平面上任意选两点O和B；

(2)以O点为原点，以OB线为X轴，建立XY轴坐标系；并得出激光导航系统在坐标系中的具体值；

(3)以方向盘打至固定位置的状态行驶到任意E、C两点，计算出这两点的坐标，则此时已知同一圆上3点坐标，求该圆的半径即为最小环行内半径。

应用于会车，具体包括以下步骤：

(1)机动车正常行驶中，旋转激光旋转平台至角度为0度使直线激光与车身平行；

(2)观察直线激光是否照射到对面车辆。

应用于高速行驶，具体包括以下步骤：

(1)机动车正常行驶中，将激光雷达调整到面向正前方，测量与前后车之间车间距；

(2)激光雷达向前方探测，以正前方左右以一定的偏角做扇形扫描，计算扫描区域内前后车之间距离，小于一定值时报警；

(3)如驾驶员在规定时间内回正方向或按按钮取消报警，激光雷达重新测量；如驾驶员超过规定时间没有采取任何措施，会电击驾驶员手指。

有益效果：与现有技术相比，本发明具有以下优点：(1)在任何车库、停车场、空地都可以使用，使用范围广泛；受周围环境干扰小，可以在任意角度和位置停车；(2)只需要驾驶员在几个关键点做直线行驶或最大转角行驶，简单易学，尤其适合经验不丰富的新手；即使轮胎胎压变化或汽车老化，最小转弯半径发生变化，也可定期在空地上借助车旁任意两点建立坐标系来计算目前汽车的最小转弯半径；(3)在通过精确测量建立的坐标系中，车辆初始坐标、终点坐标、关键点坐标都可以经预设的计算公式自动算出，结果精确误差小，可减少倒车时的角度反复调整过程；(4)计算时可预先输入安全距离，按照激光指示由驾驶员操作，让复杂情况泊车时更加安全；(5)安装及拆卸方便，可根据车型选择合适的卡具，直接卡在后视镜上，自带电源，不用改汽车线路；(6)外形美观，不产生额外风阻，保护壳与外后视镜融为一体，可放平或缩至保护壳内；(7)测量车间距，防止追尾和撞护栏；防止疲劳打瞌睡，除报警之外还有电刺激提醒功能。

附图说明

图1是本发明所述的机动车激光导航系统示意图；

图2是卡具安装在后视镜轴上的未启动状态示意图；

图3是卡具安装在后视镜轴上的启动状态示意图；

图4是卡具安装在后视镜前壳上的启动状态示意图；

图5是卡具安装在后视镜前壳上的未启动状态示意图；

图6是本发明的机动车导航坐标系示意图；

图7是本发明的机动车激光导航系统进行倒车入库时的原理图；

图8是倒车入库的计算坐标系示意图；

图9是固有参数示意图；

图10是方向盘打至固定位置时车辆环行内半径的测量方法示意图；

图11是本发明的机动车激光导航系统进行侧方位停车时的原理图；

图12是本发明的机动车激光导航系统进行高速行驶时测距的原理图。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步的说明。

如图1所示是本发明所述的机动车激光导航系统，包括车内控制单元、车外执行单元和卡具，卡具用于安装车外执行单元。

车内控制单元包括电源、开关和车内主控电路，车内主控电路包括车内信号接收单元、车内信号发射单元、直线激光控制单元、升降控制单元、水平旋转控制单元、垂直旋转控制单元、车内显示单元、车内电流刺激单元和车内语音提示单元。车内控制单元可以安装在方向盘上，空调出风口等方便触及的地方。

车内控制单元内有进行数学计算的运算电路，电路内有内置的公式；车内控制单元为安装在手机上的APP或借助手机运算和显示功能的设备；车内控制单元有警告驾驶员的装置。

车外执行单元包括电源12、开关和车外单片机主控电路，车外单片机主控电路包括车外信号接收单元13、车外信号发射单元14、直线激光发射器1、升降单元2、水平旋转控制单元3、垂直旋转控制单元4、激光雷达发射器5、激光雷达接收器6和水平旋转角度测量单元。车外执行单元可以遥控控制，也可以直接手动控制。车外执行单元有水平仪，安装时使垂直升降单元2与水平地面垂直。

激光导航系统的卡具根据车型不同及安装位置的不同专门配置，可粘贴、焊接、捆缚等多种方式，以外后视镜卡具为优先，卡具与外后视镜颜色、结构相仿，向车头方向突出，突出部与外后视镜之间形成空腔，用于安装车外执行单元。

车外执行单元通过卡具安装在后视镜7轴上，如图2所示是未启动状态，如图3所示是开启状态。卡具为环形卡具8，固定于后视镜轴上，车外执行单元固定于卡具上。

车外执行单元通过卡具安装在后视镜前壳上，如图4和图5所示。卡具为双环形卡具9，一环固定于后视镜轴上，一环固定于与后视镜相似的前凸外壳10上，外壳上方开孔11，用于固定车外执行单元。前凸外壳10内有电源12和车外单片机主控电路。

在一个实施例中，车外执行单元通过卡具安装在车顶，卡具为圆盘型，底面通过吸盘固定于车顶上，顶面有卡槽或磁铁，与车外执行单元连接。车外单片机主控电路不再包括升降单元2，车内控制单元也不再包括升降控制单元。

在另一个实施例中，车外执行单元通过吸盘直接安装在车体上。车外单片机主控电路不再包括升降单元2，车内控制单元也不再包括升降控制单元。

机动车激光导航系统的电源由汽车电瓶提供或自带电池，驾驶员在车库门附近的初始位置停车，通过车内控制单元调整安装在机动车身上的的激光雷达和电子量角仪，精确地测算出仪器与车库门两条门边的距离和角度，结合特定车型的轴距、最小转弯半径、激光导航系统与地面高度等固有参数，经过安装在车内控制单元上的配套软件，建立以车库门两条门边连线为X轴的坐标系，计算车辆的坐标，并计算指示激光的预定偏转角度，并由激光指示器投射出指示激光束。机动车从初始位置沿直线后退，等指示激光扫射到车库近端门边时，将方向盘打至固定位置，固定位置包括方向盘打半圈、方向盘打一圈、方向盘打死，转向至车身与车库门边连线垂直后，再直线倒车入口。

本发明也可以用于汽车侧方位停车和在正常行驶中测量车间距，并在间距过小或可能撞护栏时报警。

有了导航装置，还需建立机动车激光导航坐标系，并以此坐标系为基础进行倒车入库和侧方位停车的计算。以下结合附图对机动车导航坐标系的建立方法作详细的陈述。

如图6所示是本发明所述的机动车导航坐标系，停车位入口两边在地面垂直投影分别是O点和B点，O点为左投影点，B点为右投影点。设机动车起始位置时，激光导航设备在地面的垂直投影点为A点。AC是直线激光在地面的垂直投影，AL是车身的延长线。

机动车导航坐标系的建立方法具体包括以下步骤：

(1)设水平旋转角与车身延长线AL平行时为0度，测量O点和B点时偏转角度为α和β；

(2)在三角形ABO中利用余弦定理计算出停车位入口宽度OB和∠AOB：

其中，l＝AO，h＝AB，α＝∠OAL，β＝∠BAL；由cos值对应表，得知γ＝∠AOB的大小；

(3)以O点为原点，以OB线为X轴，建立XY轴坐标系；

(4)得出A点在坐标系中的具体值(Ax、Ay)为(AO×cosγ，AO×sinγ)，车身与停车位入口的偏角∠ALB＝∠AOB-∠LAB＝γ-α。

如图7所示是使用本发明的机动车激光导航系统进行倒车入库时的原理图，如图8所示是倒车入库的计算坐标系。

停车位入口两点分别是O和B，F点和G点是机动车转弯的关键点,预留车身与车库门的安全距离JB＝s。D点为机动车以最大转角转弯时最小环形内半径r的圆心。E点是机动车后轮轴心垂直投影在关键点F时激光导航设备在地面的投影，EB平行于AC，MD平行于X轴。G点是机动车车身刚转角至与OB垂直时，后轮轴心在地面的投影，GJ为机动车车身线，与Y轴平行。设机动车两边距离停车位边的安全距离都为s。

设机动车的激光导航设备垂直投影在A点时以角度θ发出直线激光在地面的垂直投影形成直线AC。机动车沿此时的车身偏角直线倒车，当激光导航设备投影到关键点E点时，AC线正好平移到与BE重合，直线激光扫描到停车位的B点。此时将方向盘打死，转向至车身与OB连线垂直后，可直线倒车入口。

求角度θ的大小具体步骤如下：

(1)由附图可知：

Dx＝OB-s+r-g

AL＝Ay/sin∠ALO＝AO×sinγ/sin(γ-β)

Gx＝Dx＝OB-s+r-g

KL＝AK/tan∠ALO＝Ay/tan(γ-β)

Lx＝Ax–KL＝Ax–Y_A/tan(γ-β)

(2)直线AL与FD相垂直，其斜率互为负倒数，k_AL×k_FD＝-1。

Fy-Dy/Fx-Dx＝-(Ax-Lx/Ay)

(Fx-Dx)²+(Fy-Dy)²＝(r-g)²

直角三角形FDM和直角三角形HDF有共同角，直角三角形FDM和直角三角形KAL有平行边，所以这三个直角三角形都是相似三角形。

FH/FD＝LK/AL＝cos∠ALO

(Fy-Dy)/(r-g)＝cos(γ-β)

(3)根据圆的一般方程式得出：

(Fx-Dx)²+(Fy-Dy)²＝(r-g)²

Fy-Dy/Fx-Dx＝-(Ax-Lx/Ay)

(Fy-Dy)/(r-g)＝cos(γ-β)

(Fx-Dx)²+(Fy-Dy)²＝(r-g)²

解方程可以得到F点坐标(Fx、Fy)和D点纵坐标Dy。

(4)设E点坐标为(Ex、Ey)，得方程：

(Ex-Dx)²+(Ey-Dy)²＝R²＝(r-g)²+f²

Ey＝LE×sin∠ALO得出：Ey²＝[(Ex-Lx)²+Ey²]×sin(γ-β)

解方程可以得到E点坐标(Ex、Ey)，已知根据三角函数得出∠LEB的大小，即为激光指示器在起始点的偏转角度θ。

倒车入库的导航方法具体包括以下步骤：

(1)激光导航系统安装在左侧或右侧后视镜上均可，以安装在驾驶员左侧为例，通过车内控制单元上的开关打开激光导航系统，升高至最大高度。

(2)车内控制单元上有两个拨轮，可控制激光旋转平台的水平和垂直旋转，上面还有液晶屏可显示数据，还包括确定键、扬声器等，单手触摸就可以完成。

(3)通过车内控制单元上的拨轮旋转激光旋转平台，使射出的直线激光1照射到车库门近侧门框，设为直线激光1，通过电子量角器测量出旋转角度为a1；如果没有车库门框，则再通过车内控制单元上的拨轮使激光雷达测量车库门在地面上的近点距离；直线激光1与激光雷达的点状激光2在同一垂直平面内。

(4)通过激光雷达测量出距离车库门近侧门框的距离或距离车库门在地面上近点的距离L1。

(5)通过车内控制单元旋转激光旋转平台，使射出的直线激光3照射到车库门远侧门框或在地面上的远点，设为直线激光2，测量出旋转角度为a2。

(6)通过激光雷达测量出距离车库门近侧门框的距离或距离车库门在地面上远点的距离L2。

(7)提前在车内控制单元中输入特定车型的轴距、最小转弯半径、导航系统与地面高度等固有参数并保存方便下次使用，并同时将a1、a2、L1、L2输入。

(8)通过软件建立以车库门两边连线为横坐标的坐标系，计算机动车的坐标、终点位置、车库门宽度，与机动车之间距离及角度等数据，可根据需要可以在显示设备中建立模拟路径方便直观显示。

(9)计算出此次的导航角度a3，再旋转激光旋转平台至角度a3，使射出的直线激光在车库门前方地面照射出一条直线，设为直线激光3。

(10)方向盘拨至0度，直线倒车至直线激光3照射到车库门近侧门框；因为直线激光3与地面垂直，车库门一般也与地面垂直，所以此时直线激光3也会照射在车库门在地面上的近点。

(11)通过车内控制单元旋转激光旋转平台，使射出的直线激光角度归零，即与车身平行，设为直线激光4，液晶屏显示旋转角度为0。

(12)向右打方向盘打至固定位置，继续倒车至直线激光4与车库内框平行。

(13)回复方向盘至0角度，沿与车体平行的直线激光4直线倒车入库。

如图9所示是固有参数的示意图，其中，R1为汽车最小转弯半径；R2为汽车环行外半径；r为汽车环行内半径；R为激光导航设备环行半径；a为汽车长度；b为前悬尺寸；c为后悬尺寸；d为汽车宽度；e为轴距；f为激光导航设备距后轴垂直距离；g为激光导航设备距车身垂直距离。

其中，

如图10所示是方向盘打至固定位置时车辆环行内半径的测量方法，利用车辆旁边任意两点O和B，通过建立坐标系，可随时测量车辆目前实际环行内半径r以及激光装置的最小转弯半径R。

假设机动车在起点时激光装置在地面的垂直投影为A点，机动车以最大转弯角度行驶，经过两点，激光装置在地面的垂直投影分别为E点和C点。

具体计算方法：

1、任意选两点O和B，在三角形ABO中利用余弦定理计算出宽度OB和∠AOB，由cos值对应表，得知∠AOB＝γ的大小。

2、以O点为原点，以OB线为X轴，建立XY轴坐标系；并得出A点在坐标系中的具体值(Ax、Ay)(AO×cosγ，AO×sinγ)。

3、以方向盘打至固定位置的状态行驶到任意E、C两点，以上述方式计算出这两点的坐标。则此时已知同一圆上3点坐标，求该圆的半径。

设圆心坐标为(x₀，y₀)，半径为R，三点坐标分别是(x₁，y₁)(x₂，y₂)(x₃，y₃)。根据圆的一般解析式，3点到圆心的距离都是R，可列出方程如下：

(x₁-x₀)²+(y₁-y₀)²＝R²

(x₂-x₀)²+(y₂-y₀)²＝R²

(x₃-x₀)²+(y₃-y₀)²＝R²

解出方程后可以得出圆心坐标(x₀，y₀)和半径R。根据激光设备安装的位置，可以根据R计算出方向盘打至固定位置时车辆环行内半径r。

如图11所示是使用本发明的机动车激光导航系统进行侧方位停车时的原理图，原理同倒车入库，其导航方法具体包括以下步骤：

(1)以后方障碍物的前方内侧点O和外侧点B为X轴建立坐标系，以前方障碍物后外侧拐角P点为碰撞危险点，计算激光导航系统从初始点A点到目标点F所经过的路径，并分别计算出车头轨迹、车尾轨迹和车身轨迹，确保与前后障碍物及P点相交，初始点车身与Y轴平行或保持小角度即可，计算设置安全距离。

(2)起始点调整车前后距离，调整车外执行单元使右侧直线激光照射到O点，向前直线行驶少量距离后该激光刚好扫到P点，再调整车外执行单元使右侧直线激光照射到O点，计算并控制左侧直线激光转动成需要的偏转角度θ1。

(3)将方向盘打至固定位置倒车，当激光导航系统从A点移动到C点，从C点以θ1角度发射的直线激光正好扫射到P点，计算并控制左侧直线激光转动成需要的偏转角度θ2。

(4)直线倒车，当激光导航设备从C点移动到E点，从E点以θ2角度发射的直线激光正好扫射到P点。

(5)以最大转角倒车，当激光导航设备从E点移动到F点，机动车完成侧方位停车。

应用于会车，具体包括以下步骤：

(2)观察直线激光是否照射到对面车辆。

如图12所示是使用本发明的机动车激光导航系统进行高速行驶时测距的原理图，其导航方法具体包括以下步骤：

(1)在汽车在正常行驶中，通过车内控制单元将激光雷达调整到面向正前方，测量与前后车之间车间距。

(2)行车时激光雷达向前方100米探测，以正前方左右各5°的偏角做扇形扫描，可计算扫描区域内前后车之间距离，小于一定值时会报警，防止追尾。具体探测距离和扫描角度可根据情况自由设定，但照射高度不会超过前车的后视镜，不会直接或间接引起前车驾驶员炫目。

(3)正常行车非转弯时车辆一般与车道线平行，以优选参数设定后，如果机动车在三车道的中间车道行驶。与车道线偏移角度超过5°时测距仪会扫描到护栏，此时护栏与机动车之间距离为100米，此时报警，提示驾驶员防止撞护栏。如果不改变方向和减速，以100公里每小时的车速，3秒后会发生碰撞。

(4)如驾驶员在两秒钟后回正方向或按按钮取消报警，激光雷达重新测量。如果驾驶员疲劳打瞌睡忽视警报没有采取任何措施，报警后的第三秒会直接电击驾驶员手指，强制提醒危险来临。此功能需要驾驶员将电击环套在手指上才会受到电击，其电击参数可根据需要设定。

以上所述仅为本发明的优选实施方式而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化，在本发明的原理和技术思想的范围内，对这些实施方式进行多种变化、修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims

1.一种机动车轨迹控制方法，其特征在于：包括：

建立机动车导航坐标系，机动车导航坐标系建立包括：任选两个参考点，计算两点之间的距离；以任一点为原点，以两点的连线为X轴，建立XY轴坐标系；

机动车的激光导航设备以角度θ发出直线激光，机动车沿车身偏角直线倒车，当直线激光扫描到参考点时，将方向盘打至固定位置，改变行驶轨迹；将方向盘打至固定位置包括方向盘打半圈、方向盘打一圈和方向盘打死；

所述机动车轨迹控制方法能够应用于倒车入库和侧方位停车；

应用于倒车入库，具体包括以下步骤：

（1）建立以车库门两边连线为横坐标、以车库门左门框在地面上的垂直投影为原点的坐标系，计算机动车的坐标、终点位置、车库门宽度和车库门与机动车的距离及角度；

所述步骤（1）中建立坐标系具体包括以下步骤：

打开激光导航系统，升至最大高度；

旋转激光旋转平台，使射出的直线激光照射到车库门近侧门框，测量出旋转角度为a1；如果没有车库门框，激光雷达测量车库门在地面上的近点距离；

通过激光雷达测量出距离车库门近侧门框的距离或距离车库门在地面上近点的距离L1；

旋转激光旋转平台，使射出的直线激光偏转后照射到车库门远侧门框或在地面上的远点，测量出旋转角度为a2；

通过激光雷达测量出距离车库门远侧门框的距离或距离车库门在地面上远点的距离L2；

将参数a1、a2、L1、L2输入车内控制单元，并提前在车内控制单元中输入固有参数，建立以车库门两边连线为横坐标的坐标系；

（2）计算角度θ，再旋转激光旋转平台至角度θ，使射出的直线激光在车库门前方地面照射出一条直线；

（3）方向盘拨至0度，直线倒车至直线激光照射到车库门近侧门框；

（4）向右打方向盘打至固定位置，旋转激光旋转平台至角度为0度使直线激光与车身平行，继续倒车至直线激光与车库内框平行；

（5）恢复方向盘至0度，沿与车体平行的直线激光直线倒车入库；

应用于侧方位停车，具体包括以下步骤：

以后方障碍物的前方内侧点O和外侧点B为X轴建立坐标系；以前方障碍物后外侧拐角P点为碰撞危险点；

调整车前后距离，调整车外执行单元使右侧直线激光照射到O点，向前直线行驶少量距离后该激光刚好扫到P点，再调整车外执行单元使右侧直线激光照射到O点，计算并控制右侧直线激光转动成需要的偏转角度θ1；

将方向盘打至固定位置倒车，当以θ1角度发射的直线激光扫射到P点，计算并控制右侧直线激光转动成需要的偏转角度θ2；

直线倒车，当以θ2角度发射的直线激光扫射到P点；

以最大转角倒车，完成侧方位停车。

2.根据权利要求1所述的机动车轨迹控制方法，其特征在于：还能够应用于计算方向盘打至固定位置时车辆环行内半径，具体包括以下步骤：

在地面同一水平面上任意选两点O和B；

以O点为原点，以OB线为X轴，建立XY轴坐标系；并得出激光导航系统在坐标系中的具体值；

以方向盘打至固定位置的状态行驶到任意E、C两点，计算出这两点的坐标，则此时已知同一圆上3点坐标，求该圆的半径即为方向盘打至固定位置时车辆环行内半径。

3.根据权利要求1所述的机动车轨迹控制方法，其特征在于：还能够应用于会车，具体包括以下步骤：

机动车正常行驶中，旋转激光旋转平台至角度为0度使直线激光与车身平行；

观察直线激光是否照射到对面车辆。

4.根据权利要求1所述的机动车轨迹控制方法，其特征在于：还能够应用于高速行驶，具体包括以下步骤：

机动车正常行驶中，将激光雷达调整到面向正前方，测量与前后车之间车间距；

激光雷达向前方探测，以正前方左右以一定的偏角做扇形扫描，计算扫描区域内前后车之间距离，小于一定值时报警；

如驾驶员在规定时间内回正方向或按按钮取消报警，激光雷达重新测量。