无人车路径点遍历控制器、控制系统及方法与流程

1.本发明属于自动控制技术领域，涉及无人车自动运动控制技术，具体地说，涉及一种无人车路径点遍历控制器、控制系统及方法。


背景技术：

2.目前，轮式无人车在社会各个领域中发挥着重要作用。为实现轮式无人车的自动运动控制，需对轮式无人车自主运动的建图、定位、路径规划以及路径点遍历运动控制器进行相应设计，以实现轮式无人车的自主移动。其中，路径点遍历运动控制器是顶层路径规划算法与底层轮式无人车运动控制之间的桥梁，如何使轮式无人车按照上层规划出的运动路径进行运动，同时保持运动的连续性，是一款路径点遍历运动控制器的设计出发点。目前已有路径点遍历运动控制器的相关专利研究。
3.陈海波提出一种移动机器人的运动控制方法及装置（专利申请号：cn201910119532.x），公开了一种移动机器人的运动控制方法和装置，其中，移动机器人的运动控制方法，包括：获取所述移动机器人的运动路径；将所述运动路径拆分为多个运动点；计算所述运动路径在各个所述运动点处的曲率；根据所述移动机器人当前所处的运动点处的曲率控制所述移动机器人向下一个所述运动点移动，直至所述移动机器人到达所述运动路径的终点。该运动控制方法及装置通过曲率对机器人运动进行控制，但是在实际运动控制过程中，机器人的运动曲率很难通过装置直接进行测量，在控制过程中存在很大局限性，控制精度差。
4.邱斌斌等提出基于离散时间神经动力学的移动机器人重复运动控制方法及系统（专利申请号：cn202210713454.8），提出一种基于离散时间神经动力学的移动机器人重复运动控制方法及系统，所述方法包括构建移动机器人的位置层运动学方程、速度层运动学方程和物理极限约束，并将所述物理极限约束转换为速度层双端不等式约束；构建基于时变二次型优化问题的移动机器人的重复运动控制方案；构建连续时间零化神经动力学模型；根据五步显式线性多步法则与所述连续时间零化神经动力学模型，构建离散时间零化神经动力学模型，并计算移动机器人的控制变量，对移动机器人进行重复运动控制。该控制方法存在一定缺陷，一方面是对控制器的芯片性能有一定要求，另一方面降低了控制算法的实时性。
5.彭涛等提出一种基于距离权重的移动机器人全局路径平滑方法（专利申请号：cn202210548004.8）该方法首先构建栅格地图并设定移动机器人的安全距离，在地图上指定全局路径规划起点和终点，并根据设置的安全距离生成全局路径点；然后进行全局路径点密化处理，并设定平滑权重；最后从起点开始依次选择待平滑点，参考地图精度和安全距离，根据全局路径点的顺序分别向前和向后选取等量的路径点作为路径参考点，根据选取的路径参考点和设定的平滑权重调整待平滑点的位置，调整后的路径点位置即平滑路径的路径点位置。该方法只是对路径点进行连接及平滑处理，但实际机器人受制于物理尺寸，往往不能按照规划的平滑路径进行移动，会存在局部脱轨的现象，控制精度差。


技术实现要素：

6.本发明针对现有技术存在的控制精度差等上述问题，提供一种无人车路径点遍历控制器、控制系统及方法，能够实现无人车对全局路径点的遍历控制完整性，同时保持无人车在路径点位置的航向角一致性，在完成遍历路径点时，实现对无人车位置、姿态量的并行控制，使运动轨迹平稳圆滑，控制精度高。
7.本发明第一方面，提供了一种无人车路径点遍历控制器，设有：坐标映射模块，通过坐标系转换函数模型对无人车位姿差值进行坐标转换得到映射后无人车位姿差值；所述映射后无人车位姿差值包括映射后x轴方向位置差值、映射后y轴方向位置差值和映射后航向角差值；线速度控制模块，根据映射后x方向位置差值通过线速度控制模型得到速度增益值；角速度控制模块，根据映射后y轴方向位置差值、映射后航向角差值通过角速度控制模型得到航向角增益值；运动学模块，根据速度增益值和航向角增益通过运动学模型得到无人车的运动控制速度，以根据该运动控制速度控制无人车前进和转向；所述运动学模型表示为：式中，为无人车的运动控制线速度，为无人车的运动控制线角速度，为无人车预设线速度，为无人车预设角速度，为速度增益值，为航向角增益值。
8.在一种可能实现的方式中，所述无人车位姿差值为无人车位姿与路径点位姿的差值，所述无人车位姿差值包括x轴方向位置差值、y轴方向位置差值和航向角差值。
9.在一种可能实现的方式中，所述坐标系转换函数模型表示为：式中，为坐标系转换函数，为初始航向角，为最近目标点的目标航向角；通过坐标系转换函数坐标转换得到映射后无人车的位置差值为：式中，为映射后x轴方向位置差值，为映射后y轴方向位置差值，为映射后航向角差值，为无人车初始位置x轴坐标，为最近目标点位置x轴坐标，为无人车初始位置y轴坐标，为最近目标点位置y轴坐标。
10.在一种可能实现的方式中，所述线速度控制模型的构建方法为：无人车当前位置姿态与目标位置姿态的误差只有x轴方向位置差值时，即无人车目标位置y方向与无人车姿态无差值，无人车进行变速直线运动以使无人车运行至目标点，则以映射后x方向位置差值为输入，构建线速度控制模型，表示为：式中，表示线速度控制模型的增益系数，为常数。
11.在一种可能实现的方式中，所述角速度控制模型的构建方法为：无人车当前位置与目标位置x方向位置误差为0，存在y方向位置差值和航向角差值，无人车以变角速度进行运动以使无人车的航向角收敛至目标值，同时运动过程中无人车x方向差值不会维持为0，则以映射后无人车y方向差值和映射后航向角为输入，构建角速度控制模型，表示为：式中，、表示角速度控制模型的增益系数，为常数。
12.根据本发明第二方面，提供了一种无人车路径点遍历控制系统，包括：定位装置，用于获取无人车当前位置和目标路径点位置；计算模块，用于根据无人车当前位置和目标路径点位置计算无人车位姿差值；控制器，采用如第一方面中任意一项所述无人车路径点遍历控制器，控制器根据无人车位姿差值调节无人车当前位置运动控制速度，以该运动控制速度为当前控制值，传输至无人车的前进电机和转向电机，调整无人车的位姿，以遍历所有路径点。
13.根据本发明第三方面，一种无人车路径点遍历控制方法，中任意一项所述无人车路径点遍历控制器，其具体步骤为：s1、获取无人车当前位置和目标路径点位置；s2、根据无人车当前位置和目标路径点位置计算无人车位姿差值；s3、对无人车位姿差值进行坐标转换得到映射后无人车位姿差值；所述映射后无人车位姿差值包括映射后x轴方向位置差值、映射后y轴方向位置差值和映射后航向角差值；s4、根据映射后x轴方向位置差值计算速度增益值；根据映射后y轴方向位置差值、映射后航向角差值计算航向角增益值；s5、根据速度增益值和航向角增益计算无人车的运动控制速度；s6、将运动控制速度为当前控制值，传输至无人车的前进电机和转向电机，调整无人车的位姿，以遍历所有路径点。
14.与现有技术相比，本发明的优点和积极效果在于：本发明无人车路径点遍历控制器、控制系统及方法，够实现无人车对全局路径点的遍历控制完整性，同时保持无人车在路径点位置的航向角一致性，在完成遍历路径点时，实现对无人车位置、姿态量的并行控制，能够在运动的过程中对线速度与角速度实时动态调节，使运动轨迹平稳圆滑，同时满足所有路径点位置与姿态角度的要求，控制精度高。
附图说明
15.图1为本发明实施例所述无人车路径点遍历控制器的结构框图；图2为本发明实施例所述无人车路径点遍历控制器路径点遍历示意图；图3为本发明实施例所述无人车路径点遍历控制系统的结构框图；图4为本发明实施例所述无人车路径点遍历控制方法的流程图。
16.图中，1、坐标映射模块，2、线速度控制模块，3、角速度控制模块，4、运动学模块，5、定位装置，6、计算模块、7、控制器，8、无人车，9、前进电机，10、转向电机，11、计算机。
具体实施方式
17.下面，通过示例性的实施方式对本发明进行具体描述。然而应当理解，在没有进一步叙述的情况下，一个实施方式中的元件、结构和特征也可以有益地结合到其他实施方式中。附图中相同的附图标记表示功能相同或相似的元件。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
18.在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。
19.另外，为了更好的说明本公开，在下文的具体实施方式中给出了众多的具体细节。本领域技术人员应当理解，没有某些具体细节，本公开同样可以实施。在一些实例中，对于本领域技术人员熟知的方法、手段、元件和电路未作详细描述，以便于凸显本公开的主旨。
20.在移动无人车控制领域，通常使用运动路径规划算法如rrt等，对无人车自起始点至目标点进行运动路径规划。使用定位硬件如uwb或gps，获取无人车位置数据以及姿态角数据。在无人车获取自身在地图中的位置姿态信息后，按照顶层规划的路径点位置，使无人车按照顺序逐个通过各路径点，实现自起始点至目标点的移动。现有路径点遍历方法存在实时性差、控制精度低的问题。为了解决上述问题，本发明提供了一种无人车路径点遍历控制器、控制系统及方法，能够实现无人车对全局路径点的遍历控制完整性，同时保持无人车在路径点位置的航向角一致性，在完成遍历路径点时，实现对无人车位置、姿态量的并行控制，使运动轨迹平稳圆滑，控制精度高。
21.控制无人车遍历路径点时，从以下两个方面考虑：在位置控制方面，无人车的输入线速度与角速度表示为：式中，表示差速车获得的线速度与角速度，包含三个物理量、、，表示无人车与下一个路径点之间x轴方向位置差值、表示无人车与下一个路径点之间y轴方向位置差值，表示无人车与下一个路径点之间航向角差值。为控制系数，为常数。
22.通过lyapuniv函数证明当时，，差速车的位姿与
目标点位姿的差值为0，则。
23.在位置姿态角方面，以航向角误差对无人车的角速度进行控制，即以航向角差值作为角速度的输入控制量。以pid比例控制为例，将作为输入量输入pid控制模型，输出量为差速车线速度与角速度指令，、为控制系数，公式如下式所示：本发明提供的无人车路径点遍历控制器、控制系统及方法，基于上述两个方面，以位置坐标误差为输入量控制线速度，以航向角误差作为输入量控制角速度。即当差速车与目标点位置较远时，线速度值较大，随着差速车靠近目标点位，差速车线速度值逐步减小，至目标点时线速度为零。在靠近过程中，若差速车与目标姿态间的航向角较大，则差速车的角速度较大，随着差速车运动，航向角差值逐渐变小，至目标点时，航向角差值为零。
24.以下结合实施例对无人车路径点遍历控制器、控制系统及方法进行详细说明。
25.如图1所示，本发明实施例的无人车路径点遍历控制器，设有：坐标映射模块1，通过坐标系转换函数模型对无人车位姿差值进行坐标转换得到映射后无人车位姿差值；所述映射后无人车位姿差值包括映射后x轴方向位置差值、映射后y轴方向位置差值和映射后航向角差值；线速度控制模块2，根据映射后x方向位置差值通过线速度控制模型得到速度增益值；角速度控制模块3，根据映射后y轴方向位置差值、映射后航向角差值通过角速度控制模型得到航向角增益值；运动学模块4，根据速度增益值和航向角增益通过运动学模型得到无人车的运动控制速度，以根据该运动控制速度控制无人车前进和转向；所述运动学模型表示为：式中，为无人车的运动控制线速度，为无人车的运动控制线角速度，为无人车预设线速度，为无人车预设角速度，为速度增益值，为航向角增益值。
26.在一种可能实现的方式中，所述无人车位姿差值为无人车位姿与路径点位姿的差值，所述无人车位姿差值包括x轴方向位置差值、y轴方向位置差值和航向角差值。
27.在一种可能实现的方式中，所述坐标系转换函数模型表示为：
式中，为坐标系转换函数，为初始航向角，为最近目标点的目标航向角；通过坐标系转换函数坐标转换得到映射后无人车的位置差值为：式中，为映射后x轴方向位置差值，为映射后y轴方向位置差值，为映射后航向角差值，为无人车初始位置x轴坐标，为最近目标点位置x轴坐标，为无人车初始位置y轴坐标，为最近目标点位置y轴坐标。
28.在一种可能实现的方式中，所述线速度控制模型的构建方法为：无人车当前位置姿态与目标位置姿态的误差只有x轴方向位置差值时，即无人车目标位置y方向与无人车姿态无差值，无人车进行变速直线运动以使无人车运行至目标点，则以映射后x方向位置差值为输入，构建线速度控制模型，表示为：式中，表示线速度控制模型的增益系数，为常数。
29.在一种可能实现的方式中，所述角速度控制模型的构建方法为：无人车当前位置与目标位置x方向位置误差为0，存在y方向位置差值和航向角差值，无人车以变角速度进行运动以使无人车的航向角收敛至目标值，同时运动过程中无人车x方向差值不会维持为0，则以映射后无人车y方向差值和映射后航向角为输入，构建角速度控制模型，表示为：式中，、表示角速度控制模型的增益系数，为常数。
30.上述控制器通过调节增益系数、、对无人车的运动控制效果进行调节，实现无人车遍历所有路径点的任务。
31.图2所示为无人车路径点遍历控制器路径点遍历示意图。图中，为初始目标点位置，为最近目标点位置，为新目标点位置。通过上述控制器遍历无人车路径点，通过调节、、的大小以调节线速度和角速度。例如：当无人车与目标点位置较远时，调大增益系数，逐渐增大线速度，使无人车加速直线运动，随着无人车靠近目标点位，调小增益系数，逐渐减小无人车车线速度值，至目标点时线速度为零。在靠近目标点位置过程中，若无人车与目标姿态间的航向角较大，则调大增益系数、，逐渐增大角速度，
随着无人车运动，航向角差值逐渐变小，则调小增益系数、，逐渐减小角速度，至目标点时，航向角差值为零。
32.需要说明的是，通过调节增益系数、、，可并行控制进行无人车位置、姿态角，在运动的过程中对线速度与角速度实时动态调节，使运动轨迹平稳圆滑，同时满足所有路径点位置与姿态角度的要求。
33.图3是根据示例性实施例示出的一种无人车路径点遍历控制系统，包括：定位装置5，用于获取无人车8当前位置和目标路径点位置；计算模块6，用于根据无人车8当前位置和目标路径点位置计算无人车位姿差值；控制器7，根据无人车位姿差值调节无人车8当前位置运动控制速度，以该运动控制速度为当前控制值，传输至无人车8的前进电机9和转向电机10，调整无人车8的位姿，以遍历所有路径点。控制器采用上述无人车路径点遍历控制器。
34.在示例性的实施例中，继续参见图3，控制系统还包括计算机11，所述计算模块6设于计算机11内。
35.在示例性的实施例中，定位装置采用uwb或gps。
36.图4是根据示例性实施例提出的一种无人车路径点遍历控制方法，采用车路径点遍历控制系统，其具体步骤为：s1、通过定位装置获取无人车当前位置和目标路径点位置。
37.s2、通过计算模块根据无人车当前位置和目标路径点位置计算无人车位姿差值。
38.s3、控制器对无人车位姿差值进行坐标转换得到映射后无人车位姿差值；所述映射后无人车位姿差值包括映射后x轴方向位置差值、映射后y轴方向位置差值和映射后航向角差值。
39.s4、控制器根据映射后x轴方向位置差值计算速度增益值；根据映射后y轴方向位置差值、映射后航向角差值计算航向角增益值。
40.s5、控制器根据速度增益值和航向角增益计算无人车的运动控制速度。
41.s6、控制器将运动控制速度为当前控制值，传输至无人车的前进电机和转向电机，调整无人车的位姿，以遍历所有路径点。
42.附图中的流程图和框图显示了根据本公开的多个实施例的控制器、控制系统及方法产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或指令的一部分，所述模块、程序段或指令的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
43.上述实施例用来解释本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明做出的任何修改和改变，都落入本发明的保护范围。

技术特征：
1.一种无人车路径点遍历控制器，其特征在于，所述控制器设有：坐标映射模块，通过坐标系转换函数模型对无人车位姿差值进行坐标转换得到映射后无人车位姿差值；所述映射后无人车位姿差值包括映射后x轴方向位置差值、映射后y轴方向位置差值和映射后航向角差值；线速度控制模块，根据映射后x方向位置差值通过线速度控制模型得到速度增益值；角速度控制模块，根据映射后y轴方向位置差值、映射后航向角差值通过角速度控制模型得到航向角增益值；运动学模块，根据速度增益值和航向角增益通过运动学模型得到无人车的运动控制速度，以根据该运动控制速度控制无人车前进和转向；所述运动学模型表示为：式中，为无人车的运动控制线速度，为无人车的运动控制线角速度，为无人车预设线速度，为无人车预设角速度，为速度增益值，为航向角增益值。2.如权利要求1所述的无人车路径点遍历控制器，其特征在于，所述无人车位姿差值为无人车位姿与路径点位姿的差值，所述无人车位姿差值包括x轴方向位置差值、y轴方向位置差值和航向角差值。3.如权利要求2所述的无人车路径点遍历控制器，其特征在于，所述坐标系转换函数模型表示为：式中，为坐标系转换函数，为初始航向角，为最近目标点的目标航向角；通过坐标系转换函数坐标转换得到映射后无人车的位置差值为：式中，为映射后x轴方向位置差值，为映射后y轴方向位置差值，为映射后航向角差值，为无人车初始位置x轴坐标，为最近目标点位置x轴坐标，为无人车初始位置y轴坐标，为最近目标点位置y轴坐标。4.如权利要求3所述的无人车路径点遍历控制器，其特征在于，所述线速度控制模型的构建方法为：无人车当前位置姿态与目标位置姿态的误差只有x轴方向位置差值时，即无人车目标位置y方向与无人车姿态无差值，无人车进行变速直线运动以使无人车运行至目标点，则以映射后x方向位置差值为输入，构建线速度控制模型，表示为：
式中，表示线速度控制模型的增益系数，为常数。5.如权利要求3所述的无人车路径点遍历控制器，其特征在于，所述角速度控制模型的构建方法为：无人车当前位置与目标位置x方向位置误差为0，存在y方向位置差值和航向角差值，无人车以变角速度进行运动以使无人车的航向角收敛至目标值，同时运动过程中无人车x方向差值不会维持为0，则以映射后无人车y方向差值和映射后航向角为输入，构建角速度控制模型，表示为：式中，、表示角速度控制模型的增益系数，为常数。6.一种无人车路径点遍历控制系统，其特征在于，包括：定位装置，用于获取无人车当前位置和目标路径点位置；计算模块，用于根据无人车当前位置和目标路径点位置计算无人车位姿差值；控制器，采用如权利1至5任意一项所述无人车路径点遍历控制器，控制器根据无人车位姿差值调节无人车当前位置运动控制速度，以该运动控制速度为当前控制值，传输至无人车的前进电机和转向电机，调整无人车的位姿，以遍历所有路径点。7.一种无人车路径点遍历控制方法，其特征在于，其具体步骤为：s1、获取无人车当前位置和目标路径点位置；s2、根据无人车当前位置和目标路径点位置计算无人车位姿差值；s3、对无人车位姿差值进行坐标转换得到映射后无人车位姿差值；所述映射后无人车位姿差值包括映射后x轴方向位置差值、映射后y轴方向位置差值和映射后航向角差值；s4、根据映射后x轴方向位置差值计算速度增益值；根据映射后y轴方向位置差值、映射后航向角差值计算航向角增益值；s5、根据速度增益值和航向角增益计算无人车的运动控制速度；s6、将运动控制速度为当前控制值，传输至无人车的前进电机和转向电机，调整无人车的位姿，以遍历所有路径点。

技术总结
本发明属于自动控制技术，涉及一种无人车路径点遍历控制器、控制系统及方法，控制器设有：坐标映射模块，通过坐标系转换函数模型对无人车位姿差值进行坐标转换得到映射后无人车位姿差值；映射后无人车位姿差值包括映射后X轴方向位置差值、映射后Y轴方向位置差值和映射后航向角差值；线速度控制模块，根据映射后X方向位置差值通过线速度控制模型得到速度增益值；角速度控制模块，根据映射后Y轴方向位置差值、映射后航向角差值通过角速度控制模型得到航向角增益值；运动学模块，根据速度增益值和航向角增益通过运动学模型得到无人车的运动控制速度。本发明在完成遍历路径点时，实现无人车位置、姿态量并行控制，使运动轨迹平滑，控制精度高。控制精度高。控制精度高。


技术研发人员：隋明君 刘韶庆 李博睿 王文钦
受保护的技术使用者：国家高速列车青岛技术创新中心
技术研发日：2023.03.31
技术公布日：2023/7/21