CN114312988B - 一种基于偏航角与转向系统转角转换模型的轨迹控制方法 - Google Patents

Abstract

本发明公开了一种基于偏航角与转向系统转角转换模型的轨迹控制方法，该方法可应用于低成本农业驾驶机器人自动驾驶，具体方法是建立前轮转角与转向电机转角（方向盘转角）的关系模型和农机偏航角与前轮转角的关系模型，在这两个模型的基础上，致力于简化已知的偏航角与待定的转向系统转角（包括前轮转角、方向盘转向电机转角和方向盘转角）的非线性关系，基于多传感器融合下的路径规划，实现控制无人驾驶农机轨迹实时校正，达到解决实际农机自动驾驶过程中在完成既规划路线过程中转向系统转动角度问题的目的。本发明适用于在广阔平坦的无人区域作业的农机，且可以使农机较为稳定且平稳地进行作业。

Description

一种基于偏航角与转向系统转角转换模型的轨迹控制方法

技术领域

本发明属于自动驾驶领域，具体涉及一种基于偏航角与转向系统转角转换模型的轨迹控制方法。

背景技术

近年来，自动驾驶领域的热度持续上升，无人驾驶汽车是最受关注和期望的未来，国内外车企抑或是其他领域的企业开拓市场、抢占先机。有一些代表性较强的无人驾驶汽车研究团队，如国外有Waymo、Argo、谷歌、特斯拉，中国有百度、华为、大疆、蔚来等驾驶汽车研究团队。其中比较特别的是特斯拉，它的特点在于基于纯视觉的自动驾驶。而百度涉及Robotaxi，Robobus，物流小车，乘用车(威马)，泊车等，已经开源的机器人开发平台apollo，车路协同，智能座舱，计算平台等。

在整个驾驶的过程中，大部分时间都可以由机器来辅助驾驶，但是人依然需要负有最终的驾驶责任，尽管开启了自动驾驶模式，车内人员的手还是要随时做好接管驾驶这辆车的准备。车上人员仅仅承担一个安全员的角色，除了在特殊或紧急情况下，都不需要对车辆行驶进行干预。即使是对于一个成熟的驾驶员，在车辆行驶过程中对方向盘的把控也是尤为重要的，对于自动驾驶的农机也是如此。现有技术多是对于方向盘转角和前轮转角的研究，最为经典的当属阿尔曼车辆模型，阿克曼车辆的设计可以使得车辆在转弯时，前外轮转角比前内轮转角大2到4度，使得四个轮子路径的圆心大致交于后轴延长线上，从而使车辆顺畅拐弯。然而实际上，因为方向盘转角和前轮转角关系本身是一个非常复杂的非线性关系，它会受到车辆设计结构、车辆速度、车辆转弯半径、偏航角等各个因素的影响，所以阿克曼车辆模型在方向盘转角和前轮转角关系上依旧存在着比较模糊的界限。

本发明提出了一种基于偏航角与转向系统转角转换模型的轨迹控制方法，不仅建立了前轮转角与转向电机转角(方向盘转角)的关系模型，还建立了偏航角与前轮转角关系模型，在这两个模型的基础上，简化已知的偏航角与待定的转向系统转角(包括前轮转角、方向盘转向电机转角和方向盘转角)的非线性关系，这就为无人驾驶过程提供了至少两种便利：其一是便于控制车辆转向，其二是便于按照既规划路径去实时控制车辆到达指定的目标点。

发明内容

本发明所涉及的无人驾驶是以农机为平台实行的，且应用场景为农田环境，故在此说明一些实际应用时所涉及的硬件设备及相关的要素。

(1)定位系统：定位系统是利用双天线组合导航设备来实现车辆定位定向的，经纬度数值的精确度可达到小数点后8位，经计算知该定位系统的定位精度达到了厘米级，同时航向角的误差在1°左右。

(2)转向系统：转向系统包括方向盘、转向轴、转向中间轴、转向油管、转向油泵、转向油罐、转向节臂、转向横拉杆、转向摇臂、整体式转向器、转向直拉杆、转向减振器等组成。在此只考虑方向盘，因为只要驱动方向盘向左或向右转一定的角度，车轮就会执行相应的动作，从而引起轨迹的变化。

(3)控制系统：主要是由转向电机、链条等组成，通过转向电机驱动链条，带动方向盘的转动，进而控制前轮转动。控制部分最为关键的就是转向电机的转动角度。

(4)反馈系统：左前轮和右前轮在基于实际应用的设计时是非平行设计的，所以作为反馈值的车前轮转角的获取是通过在左前轮这一侧安装一个维特智能牌的角度传感器实时返回左前轮的转角来实现的。

根据以上情况，为解决自动驾驶中控制部分中问题，本发明公开了一种基于偏航角与转向系统转角转换模型的轨迹控制方法，建立前轮转角与转向电机转角(方向盘转角)的关系模型和偏航角与前轮转角关系模型，在这两个模型的基础上，基于多传感器融合下的路径规划，实现控制无人驾驶农机轨迹实时校正，达到解决实际农机自动驾驶过程中在完成既规划路线过程中转向系统转动角度问题的目的。

为达到上述目的，本发明的技术方案如下：

一种基于偏航角与转向系统转角转换模型的轨迹控制方法，包括以下步骤：

(1)初始状态时，车轮与车身方向一致，此时定义车辆的方向盘、转向电机以及车轮的角度均为0，并以此状态为基准；车辆左转时，定义为负方向，此时车辆的方向盘、转向电机以及车轮转动的相对偏角为负；车辆右转时，定义为正方向，此时车辆的方向盘、转向电机以及前轮转动的相对偏角为正。

(2)由于农机在运作时，走过的轨迹方向转动幅度都是比较小的，在这样的转动范围内，可以认为控制方向盘的转向电机的转角与前轮转动的角度是成正比的。

τ为传动比，α₁为方向盘向右转动极限时转向电机转动的度数，α₂为方向盘向左转动极限时转向电机转动的度数，β₁为车轮向右转动极限角度，β₂为车轮向左转动极限角度。

(3)由于左前轮和右前轮在基于实际应用的设计时是非平行设计的，在初始状态时，左前轮和右前轮都存在固有轮外倾角，而固有轮外倾角对前轮转角与方向盘转角(转向电机转角) 关系模型有一定影响，因此设立了左前轮和右前轮固有轮外倾角影响因子，通过足够多次实测可以分别解算出左前轮和右前轮固有轮外倾角影响因子。

(4)基于步骤(1),(2)和(3)，前轮转角与转向电机转角(方向盘转角)关系模型为：

车辆左转时，θ_L为左前轮向左转的角度，φ_L为转向电机向左转的度数，p₁为左前车轮固有轮外倾角影响因子，λ_L为左前车轮固有轮外倾角；车辆右转时，θ_R为右前轮向右转的角度，φ_R为转向电机向右转的度数，p₂为右前车轮固有轮外倾角影响因子，λ_R为右前车轮固有轮外倾角。

(5)农机在运作过程中，速度是比较慢的，为5km/h，这使得方向控制系统有足够的时间去控制和调整农机的实时位置和方向以达到下一个轨迹点。同时由于农机并非一个质点，它有着较为庞大的体型，因此需要给每一个轨迹点设定一个认可范围，一旦农机定位到了轨迹点认可范围内，就认为农机已经到达该点，从而继续前往下一个轨迹点，从起始点开始，依次迭代，直到到达最终目标点。这个范围是基于足够多次的实测，直到达到理想效果才能最终确定。

(6)由于从组合导航设备结算的航向角是以正北方向为基准，而轨迹点是以经纬度来定位的，在整条路径上纬度差方向是指向正北方向的，且经度差方向与纬度差方向是垂直的。

(7)将偏航角定义为沿农机机头方向在水平面上的投影与预定轨迹的切线方向之间的夹角，实际上这个偏航角是需要通过计算获取的。航向角参数是可以通过组合导航设备直接计算获取的，设备解算出的航向角是当前航向与正北方向的夹角，而偏航角的计算就需要用到航向角。

(8)农机实时位置的偏航角γ是可以实时确定的，γ是在一定范围内的，本农机大致是在-115°到115°之间，如果偏航角多次超出这个范围，这种情况一般是路径规划环节出现偏差，需要重新规划。经过归纳，在可行范围内最终总结为γ＝β-α或γ＝β-α+360°。其中α为轨迹偏角，β为航向角。

(9)基于步骤(5)、(6)、(7)和(8)提出一种实时校正逐步逼近的方法以实现到达下一个轨迹点的认可范围内的目的。偏航角在-115°到0°时,定义偏航角为γ_L,左偏航影响因子为ξ_L(0<ξ_L<1)；定义偏航角在0°到115°时，偏航角为γ_R,右偏航影响因子为ξ_R(0<ξ_R< 1)。同样地，ξ_L和ξ_R这两个偏航影响因子都是基于足够多次实测获取的。偏航角与前轮转角关系模型为：

其中θ_L，θ_R分别为步骤(3)中的左前轮向左转动的角度和右前轮向右转动的角度。

(10)组合导航设备获取经度、纬度、航向等信息的频率设置为100Hz，即一秒内可以获取定位定向数据100次。组合导航每解算并导出一次定位定向的数据时，都将农机的实时位置和航向与规划的下一个轨迹点位置和方向进行比对，每比对一次，都通过控制算法解算出控制指令，包括控制方向与控制角度，这样便可做到实时控制。

本发明的有益效果为：

本发明所述的一种基于偏航角与转向系统转角转换模型的轨迹控制方法，建立了前轮转角与转向电机转角(方向盘转角)的关系模型和偏航角与前轮转角关系模型，在这两个模型的基础上，简化已知的偏航角与待定的转向系统转角(包括前轮转角、方向盘转向电机转角和方向盘转角)的非线性关系，基于多传感器融合下的路径规划，实现控制无人驾驶农机轨迹实时校正，达到解决实际农机自动驾驶过程中在完成既规划路线过程中转向系统转动角度问题的目的。本发明适用于在广阔平坦的无人区域作业的农机，且可以使农机较为稳定且平稳地进行作业。

附图说明

图1为本发明的整个系统流程图；

图2为本发明实际可能下的偏航角运算模型图；

图3为本发明的控制转向系统的控制流程图；

图4为本发明的反馈部分流程图；

图5为本发明所述的农机实物图；

图6为本发明的转向系统中的方向盘与转向电机实物图；

图7为本发明的转向系统中的左前轮与角度传感器实物图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

如图所示，本发明所述的一种基于偏航角与转向系统转角转换模型的轨迹控制方法，包括以下步骤：

1、建立前轮转角与转向电机转角(方向盘转角)关系模型：

1)在前轮转角在-60°到60°范围内，确定控制方向盘的转向电机的转角与车轮转动的角度是关系：

τ为传动比，α₁为方向盘向右转动极限时转向电机转动的度数，α₂为方向盘向左转动极限时转向电机转动的度数，β₁为车轮向右转动极限角度，β₂为车轮向左转动极限角度。经过足够多次实际测量，最终获得实际可行的传动比τ。

2)设立左前轮和右前轮固有轮外倾角影响因子。建立前轮转角与转向电机转角(方向盘转角) 关系模型：

车辆左转时，θ_L为左前轮向左转的角度，φ_L为转向电机向左转的度数，p₁为左前车轮固有轮外倾角影响因子，λ_L为左前车轮固有轮外倾角；车辆右转时，θ_R为右前轮向右转的角度，φ_R为转向电机向右转的度数，p₂为右前车轮固有轮外倾角影响因子，λ_R为右前车轮固有轮外倾角。经过足够多次实际测量，最终获得实际可行的固有轮外倾角影响因子p₁和p₂。

2、建立偏航角与前轮转角关系模型：

1)利用纬度差的方向与航向角的基准线是相同的，基于此根据经纬度和航向角来计算偏航角；并在实际可行且可考虑范围为，对于偏航角γ的确定进行讨论：γ＝β-α或γ＝β-α+360°，其中α为轨迹偏角，β为航向角；

2)对偏航角γ进行分开讨论，根据其数值划分为γ_L和γ_R；

3)设立左偏航影响因子ξ_L和右偏航影响因子ξ_R两个偏航影响因子，偏航角与前轮转角关系模型为：

其中θ_L，θ_R分别为左前轮向左转动的角度和右前轮向右转动的角度，经过足够多次实际测量，最终获得实际可行的左偏航影响因子ξ_L和右偏航影响因子ξ_R。

3、结合前轮转角与转向电机转角(方向盘转角)关系模型和偏航角与前轮转角关系模型，基于路径规划算法预先规划好的路径和组合导航设备的实时定向定位数据，组合导航解算并导出当前定位定向的数据，将其与规划的下一个轨迹点位置和方向进行比对，通过控制算法解算出控制指令，包括控制方向与控制角度，进而控制转向电机和农机前轮进行一次动作。

4、重复步骤3，直到农机到达下一个轨迹点，继续控制农机到下一个轨迹点。

5、重复步骤4，直到农机达到既规划路径的终点。

需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。

Claims (5)

1.一种基于偏航角与转向系统转角转换模型的轨迹控制方法，其特征在于：该方法应用的主体为农机，应用场景为农田环境，且包括以下步骤：

(1)建立前轮转角与转向电机转角关系模型：

1)在前轮转角在-60°到60°范围内，控制方向盘的转向电机的转角与车轮转动的角度关系如下：

τ为传动比，α₁为方向盘向右转动极限时转向电机转动的度数，α₂为方向盘向左转动极限时转向电机转动的度数，β₁为车轮向右转动极限角度，β₂为车轮向左转动极限角度；

2)设立左前轮和右前轮固有轮外倾角影响因子；建立前轮转角与转向电机转角关系模型：

车辆左转时，θ_L为左前轮向左转的角度，φ_L为转向电机向左转的度数，p₁为左前车轮固有轮外倾角影响因子，λ_L为左前车轮固有轮外倾角；车辆右转时，θ_R为右前轮向右转的角度，φ_R为转向电机向右转的度数，p₂为右前车轮固有轮外倾角影响因子，λ_R为右前车轮固有轮外倾角；

(2)建立偏航角与前轮转角关系模型：

1)利用纬度差的方向与航向角的基准方向是相同的，基于此，根据经纬度和航向角来计算偏航角；并在实际可行且可考虑范围为，对于偏航角γ的确定进行讨论：γ＝β-α或γ＝β-α+360°，其中α为轨迹偏角，β为航向角；

2)根据数值对偏航角γ进行划分，偏航角在-115°到0°时,定义为第一偏航角γ_L，偏航角在0°到115°时，定义为第二偏航角γ_R；

3)设立左偏航影响因子ξ_L和右偏航影响因子ξ_R两个偏航影响因子；偏航角与前轮转角关系模型为：

其中θ_L，θ_R分别为左前轮向左转动的角度和右前轮向右转动的角度；

(3)结合前轮转角与转向电机转角关系模型和偏航角与前轮转角关系模型，基于路径规划算法预先规划好的路径和组合导航设备的实时定向定位数据，组合导航解算并导出当前定位定向的数据，将其与规划的下一个轨迹点位置和方向进行比对，通过控制算法解算出控制指令，包括控制方向与控制角度，进而控制转向电机和农机前轮进行一次动作；

(4)重复步骤(3)，直到农机到达下一个轨迹点，继续控制农机到下一个目标轨迹点；

(5)重复步骤(4)，直到农机达到既规划路径的终点，工作结束。

2.根据权利要求1所述的一种基于偏航角与转向系统转角转换模型的轨迹控制方法，其特征在于：所述农机带有定位系统：定位系统是利用双天线组合导航设备来实现车辆定位定向的，经纬度数值的精确度达到小数点后8位，经计算知该定位系统的定位精度达到了厘米级，同时航向角的误差在1°左右。

3.根据权利要求1所述的一种基于偏航角与转向系统转角转换模型的轨迹控制方法，其特征在于：所述农机带有转向系统：转向系统包括方向盘、转向轴、转向中间轴、转向油管、转向油泵、转向油罐、转向节臂、转向横拉杆、转向摇臂、整体式转向器、转向直拉杆、转向减振器。

4.根据权利要求1所述的一种基于偏航角与转向系统转角转换模型的轨迹控制方法，其特征在于：所述农机带有控制系统：包括转向电机、链条，通过转向电机驱动链条，带动方向盘的转动，进而控制前轮转动。

5.根据权利要求1所述的一种基于偏航角与转向系统转角转换模型的轨迹控制方法，其特征在于：所述农机带有反馈系统：左前轮和右前轮在基于实际应用的设计时是非平行设计的，所以作为反馈值的车前轮转角的获取是通过在左前轮这一侧安装一个维特智能牌的角度传感器实时返回左前轮的转角来实现的。