system: [SYSTEM]

user: 3. 无人车避障及路径规划设计 3.1. 无人车建模 车辆运行路径的动态调整是实现自动巡航功能的核心，在获得车辆精准的位置信息后，如何使车辆能够精确地沿着预设的路线进行巡航，是下一步需要研究的问题。要实现对无人车的控制分析，首先需要对控制对象进行建模，下面对本文中使用的无人车进行建模分析。 1) 无人车建模 这里我们参考自行车模型 [ 10 ] 对无人车的运动进行简化，主要有以下两个假设： ① 忽略车辆在垂直(z轴)方向的运动，可以理解为在高处以俯视的角度对车辆进行观察。 ② 假设车辆的前面两个轮胎具有相同的角度和转速，同样后面两个轮胎也是如此，这样就可以把车辆的结构看作自行车，前后的轮胎就可以各用一个轮胎来描述。与自行车原理相同，假设车辆的转向完全由前轮控制。这样无人车模型就可以简化为如图4所示的自行车模型。 图4. 自行车模型示意图 在该模型中，因为我们使用的车辆后轮不能旋转，这里设置后轮的转向角控制输入 δ f = 0，也就是说方向盘的控制输入都反映到前轮的转角上，因此整个系统的控制量可以简化为(a， δ f )，其中a代表车辆的加速度。加速度的正负分别代表无人车处于加速或减速阶段， δ f 代表当前轮胎的转角，这样就使用两个变量描述车辆的控制输入。使用四个状态变量描述车辆当前的状态，(x， y)表示车辆当前位置； ψ 表示当前车辆的偏航角，逆时针方向为正；v表示当前的车速； β 表示车辆当前的质心侧偏角，即小车质心的速度相对于无人车纵轴的角度。使用 l r 和 l f 表示前轮和后轮到车辆重心的距离，根据动力学原理，无人车的各个状态变量的更新公式如下所示： x t + 1 = x t + v t cos ( ψ t + β ) * d t (1) y t + 1 = y t + v t sin ( ψ t + β ) * d t (2) ψ t + 1 = ψ t + v t l r sin ( β ) * d t (3) v t + 1 = v t + a * d t (4) β = tan − 1 ( l r l f + l r t a n ( δ f ) ) (5) 基于该运动学自行车模型，在给定一个时刻的控制输入后，可以通过计算求得在dt时间以后车辆的状态信息(x， y， ψ ， v)，这样就可以使用该模型结合串级PID控制算法实现对控制量(a， δ f )的动态调整，从而实现对车辆状态的实时调整，进而实现自动巡航功能。 3.2. 避障及路径规划设计 因为无人车不具备垂直方向的运动能力，这里我们仅考虑在二维空间中的避障策略。由前文超声波的布局方式可知，超声波的探测区域可以被简化为一个扇形区域。超声波的最大测量距离为4 m，但是实验中发现在3 m到4 m范围内的检测数据容易出现波动，因此这里将超声波的探测区域简化为半径r = 3 m、角度 θ = 45˚的扇形区域，如下图5所示。 图5. 检测区域划分示意 下面首先对检测区域进行划分，根据检测到的距离将检测区域划分为F、M、N为三个扇形区域，分别表示距离无人车2米到3米之间的区域、1米到2米之间的区域、0到1米之间的区域。根据超声波传感器的检测角度，可以将检测区域划分为九个检测区域，分别表示为F1、F2、F3、M1、M2、M3和N1、N2、N3九个检测区域，角标代表传感器编号，1、2、3号传感器代表在车身上安装的U1、U2、U3超声波传感器，分别用来检测无人车的左侧、前方、右侧区域。这里将F、M、N分别定义为减速区、避障区和停止区，在不同的距离范围内无人车采取不同的避障操作。无人车的避障实现流程如图6所示。当检测到障碍物的时候，首先对障碍物所处区域进行判断，然后判断障碍物的方位，根据障碍物的方位执行相应的避障操作指令。 下面对障碍物位于M区域的情况进行简要分析，当障碍物由1号超声波检测到的时候，表明障碍物在M1区域位于无人车的左侧，此使应让无人车执行小角度右转操作；当障碍物由2号超声波检测到的时候，此时障碍物位于M2区域，障碍物为无人车的正前方，考虑到操场的实际情况，这里执行大角度左转操作；同理，当障碍物被3号超声波检测到的时候，障碍物位于无人车右侧，此时执行小角度左转。 真实的场景下可能会遇到体积比较大的障碍物，同时会被多个超声波检测到。这里针对该种情况进行讨论，当障碍物由1号和2号超声波同时检测到，此时障碍物跨越了M1和M2区域位于无人车的左前方，这时候仅执行小角度的右转已经不能满足避障的要求，需要使无人车执行大角度右转来躲避障碍物。同样，当障碍物被2号和3号传感器同时检测到的时候，执行大角度左转来躲避障碍物。更极端的一种情况，障碍物同时被三个传感器检测到，这时仅执行转向操作已经不能实现较好的避障，此时无人车执行和在刹车区域时相同的操作，首先停车，然后执行倒车指令，进入避障区域重新执行避障操作。 图6. 避障实现流程 在避障策略的研究中，主要通过减速及转向来实现对障碍物的躲避。这就会产生另外一个问题，即躲避障碍物后，如何快速回到最初设定的航线上继续执行巡航任务。 这里使用串级PID算法来对无人车的运动状态进行动态调整。在串级PID中将角度控制PID算法和角速度控制PID算法进行串联，经过串联后的PID控制器可以控制多个变量，系统的抗干扰性得到增强，无人车的适应能力得到进一步的提升。在执行避障操作后，无人车的行进路线已经偏离了最初的航线，根据PID原理，此时的误差值会突然变大，同时PID针对控制量 ( a ， δ f ) 不断进行优化，优化过程中无人车逐渐回到最初航线上，无人车中使用的串级PID的原理框图如图7所示。 图7. 串级PID原理框图 3.3. 自定义主题添加 在进行避障及路径规划实验前，需要对数据采集模块发送过来的超声波数据进行处理。由于PX4采用uORB (微对象请求代理器)通信机制，因此通过串口获得的数据并不能直接利用，需要以发布主题、订阅主题的形式进行消息的传递。uORB是飞控程序中进行消息传递的异步消息机制。它在Pixhawk系统占有重要地位，负责整个系统的数据传输任务，飞控中所有的传感器数据、GPS、PPM信号等数据都要通过uORB传输到系统中各个模块进行计算处理。 Pixhawk的核心为NuttX实时ARM操作系统，uORB允许多个进程打开同一个文件，进程之间通过该文件节点进行数据的交互与共享。每个进程都可以订阅或者发布主题，系统中可以存在多个发布者，一个进程也可以同时订阅多个主题，但是在一条总线上始终只能有一条消息。飞控中所有的数据都通过这种主题的发布和订阅方式来完成数据交换，如图8所示。下面我们需要通过自定义主题的方式，将数据采集模块发送过来的超声波信息进行发布，供其他功能模块订阅来实现信息的交换。 图8. 数据交换流程 1) 首先需要在Firmware/msg目录下新建一个名为read_uart_sensor.msg文件，该文件定义Pixhawk串口接收到所有超声波数据的结构，然后在msg/CMakeLists.txt下添加该文件名。添加完成后重新编译，会在Firmware/src/modules/uORB/topics目录下生成read_uart_sensor.h头文件。 2) 在Firmware/src/modules目录下创建文件夹并命名为read_uart_sensor作为超声波主题模块的存放位置。在该文件夹下创建read_uart_sensor.c文件和CMakeLists.txt文件。 在read_uart_sensor.c文件中对发布主题进行定义，主要包括定义主题、公告主题和发布主题三个过程。程序中，orb_advertise()函数实现公告操作，在使用orb_publish()发布新数据前需要先执行公告操作，公告成功后函数返回的处理值提供给orb_publish()函数，在主题发布时进行调用。在对read_uart_sensor.c文件进行定义后，需要对CMakeLists.txt文件进行配置，主要是对编译中的参数进行定义。 3) 完成前面的操作后需要在Firmware/cmake/configs/nuttx/nuttx_px4fmu-v2_default.cmake文件中注册自定义的read_uart_sensor模块。 4) 将超声波主题添加到启动脚本中。要实现上电后就可以在Pixhawk控制器中获得超声波数据，需要将该模块添加到系统的启动脚本文件中。添加指令read_uart_sensor start/dev/ttyS2。这样就可以实现当数据采集模块与TELEM2端口(/dev/ttyS2)相连的时候，自动启动超声波模块。 至此已经完成了超声波自定义主题的发布过程，整体的发布流程如图9所示。 在完成主题的发布后，下一步需要订阅该主题，获得超声波数据数据。orb_subscribe()函数用来订阅数据，获得的数据保存在int sensor_sub_fd结构体中；orb_set_interval()函数用来设置获取数据的时间间隔；orb_check()函数用来检查上次读取主题后，主题是否更新过。最后对修改后的源码重新编译，并上传编译后的固件。经过以上操作，在nsh命令行中就可以通过输入指令，查看当前数据的订阅结果。