防撞功能
Collision Prevention may be used to automatically slow and stop a vehicle before it can crash into an obstacle. It can be enabled for multicopter vehicles when using acceleration-based Position mode (or VTOL vehicles in MC mode).
综述
The vehicle restricts the current velocity in order to slow down as it gets closer to obstacles and adapts the acceleration setpoint in order to disallow collision trajectories. 为了远离（或与之平行的）障碍物，用户必须使无人机/无人车朝向不靠近障碍物的设定点移动。 如果存在一个”更好”的设定点，这个设定点在请求设定点的任何一侧，并且在固定的间隙内，算法将对设定点方向做最小的调整。
Collision prevention prevents motion in directions where no sensor data is available (i.e. if you have no rear-sensor data, you will not be able to fly backwards). It may also restrict vehicle maximum speed if the sensor range isn't large enough!
Multiple sensors can be used to get information about, and prevent collisions with, objects around the vehicle. If multiple sources supply data for the same orientation, the system uses the data that reports the smallest distance to an object.
The feature requires obstacle information from a distance sensor. The sensor may be connected to the flight controller (recommended). It can also be used with distance information provided by an external system via MAVLink, but this is currently untested (see companion setup below).
The rangefinders that have been tested when directly connected to PX4 are listed below, along with setup instructions. The PX4 Configuration specific to collision-prevention is the same for all sensors and independent of whether data is provided by a connected sensor or MAVLink.
TIP
- If high flight speeds are critical, consider disabling collision prevention when not needed.
- Ensure that you have sensors/sensor data in all directions that you want to fly, including backwards (when collision prevention is enabled).
Supported Rangefinders
Collision Prevention has been tested with the following rangefinders attached to PX4 (not for providing data supplied over MAVLink).
兰宝 PSK-CM8JL65-CC5
At time of writing PX4 allows you to use the Lanbao PSK-CM8JL65-CC5 IR distance sensor for collision prevention "out of the box", with minimal additional configuration:
- Attach and configure the sensor.
- Set the sensor orientation using SENS_CM8JL65_R_0.
- Configure and enable collision prevention
LightWare LiDAR SF45 Rotating Lidar
PX4 v1.14 (and later) supports the LightWare LiDAR SF45 rotating lidar which provides 320 degree sensing.
The SF45 must be connected via a UART/serial port and configured as described below:
LightWare Studio configuration:
- In the LightWare Studio app enable scanning, set the scan angle, and change the baud rate to
921600.
- In the LightWare Studio app enable scanning, set the scan angle, and change the baud rate to
Add lightware_sf45_serial driver to PX4 firmware:
- Open menuconfig
- Under drivers > Distance sensors select
lightware_sf45_serial. - Recompile and upload to the flight controller.
Set the following parameters via QGC to configure the driver:
- SENS_EN_SF45_CFG: Set to the serial port you have the sensor connected to. Make sure GPS or Telemetry are not enabled on this port.
- SF45_ORIENT_CFG: Set the orientation of the sensor (facing up or down)
- SF45_UPDATE_CFG: Set the update rate
- SF45_YAW_CFG: Set the yaw orientation
PX4 will emit the OBSTACLE_DISTANCE message when collision prevention is enabled. In QGroundControl you should this in the MAVLink console if collision prevention is configured correctly and active.
The obstacle overlay in QGC will look like this:
Other Rangefinders
其他传感器的使能需要修改驱动代码来设置传感器方向和视觉范围。
- Attach and configure the distance sensor on a particular port (see sensor-specific docs) and enable collision prevention using CP_DIST.
- 修改驱动程序以设置方向。 This should be done by mimicking the
SENS_CM8JL65_R_0parameter (though you might also hard-code the orientation in the sensor module.yaml file to something likesf0x start -d ${SERIAL_DEV} -R 25- where 25 is equivalent toROTATION_DOWNWARD_FACING). - Modify the driver to set the field of view in the distance sensor UORB topic (
distance_sensor_s.h_fov).
TIP
You can see the required modifications from the feature PR. 请回馈你的更改！
PX4 配置
Collision Prevention is enabled on PX4 by setting the parameter for minimum allowed approach distance in QGroundControl to a positive value (CP_DIST). Note that this value is the distance to the sensors, not the outside of your vehicle or propellers (set a safe margin!).
In addition you will need to tune the sendor delay and angle change tuning. You may choose to enable CP_GO_NO_DATA in order to allow unprotected movement in directions where there is no rangefinder data/sensor.
CP_DELAY Delay Tuning
There are two main sources of delay which should be accounted for: sensor delay, and vehicle velocity setpoint tracking delay. Both sources of delay are tuned using the CP_DELAY parameter.
The sensor delay for distance sensors connected directly to the flight controller can be assumed to be 0. 对于外部视觉系统，传感器延迟可能高达 0.2秒。
Vehicle velocity setpoint tracking delay can be measured by flying at full speed in Position mode, then commanding a stop. 然后可以从日志中测量实际速度和速度设置值之间的延迟。 跟踪延迟通常在 0.1 至 0.5秒之间，取决于机身尺寸和调试情况。
TIP
If vehicle speed oscillates as it approaches the obstacle (i.e. it slows down, speeds up, slows down) the delay is set too high.
CP_GUIDE_ANG Guidance Tuning
取决于机身，环境类型和飞行员技能，可能需要不同数量的制导。 Setting the CP_GUIDE_ANG parameter to 0 will disable the guidance, resulting in the vehicle only moving exactly in the directions commanded. 增大此参数将使无人机选择最佳方向来避开障碍物，从而更容易飞过狭窄的间隙，并与物体周围保持最小间距。
如果该参数设置太小，机身在靠近障碍物时可能会感觉“卡住”， 因为只允许以最小距离远离障碍物移动。 如果该参数设置太大，机身可能会感觉它朝着飞手未指示的方向“滑动”远离障碍物。 从测试来看，尽管不同的车辆可能有不同的要求，但是 30度是一个很好的平衡点。
INFO
The guidance feature will never direct the vehicle in a direction without sensor data. 如果只有一个距离传感器指向前方时无人机感到“卡住”，这可能是因为由于缺乏信息，制导无法安全地调整方向。
参数
All relevant parameters are listed below:
| 参数 | 描述 |
|---|---|
| CP_DIST | Minimum allowed distance from the sensor (the closest distance that the vehicle can approach the obstacle). Set negative to disable collision prevention. |
| CP_DELAY | Sensor and velocity setpoint tracking delay. See Delay Tuning below. |
| CP_GUIDE_ANG | Angle (to both sides of the commanded direction) within which the vehicle may deviate if it finds fewer obstacles in that direction. See Guidance Tuning below. |
| CP_GO_NO_DATA | Set to 1 to allow the vehicle to move in directions where there is no sensor coverage (default is 0/False). |
| MPC_POS_MODE | Set to Acceleration based (default). Collision prevention is disabled for Direct velocity and Smoothed velocity. |
算法描述
The data from all sensors are fused into an internal representation of 72 sectors around the vehicle, each containing either the sensor data and information about when it was last observed, or an indication that no data for the sector was available. When the vehicle is commanded to move in a particular direction, all sectors in the hemisphere of that direction are checked to see if the movement will bring the vehicle closer than allowed to any obstacles. 如果是这样，无人机的速度就会受到限制。
The Algorithm then can be split intwo two parts, the constraining of the acceleration setpoint coming from the operator, and the compensation of the current velocity of the vehicle.
INFO
If there is no sensor data in a particular direction, movement in that direction is restricted to 0 (preventing the vehicle from crashing into unseen objects). If you wish to move freely into directions without sensor coverage, this can be enabled by setting CP_GO_NO_DATA to 1.
Acceleration Constraining
For this we split out Acceleration Setpoint into two components, one parallel to the closest distance to the obstacle and one normal to it. Then we scale each of these components according the the figure below. 
Velocity compensation
This velocity restriction takes into account the jerk-optimal velocity controller via MPC_JERK_MAX and MPC_ACC_HOR. Whereby The current velocity is compared with the maximum allowed velocity so that we are still able to break based on the maximal allowed jerk, acceleration and delay. from this we are able to use the proportional gain of the acceleration controller(MPC_XY_VEL_P_ACC) to transform it into an acceleration.
Delay
The delay associated with collision prevention, both in the vehicle tracking velocity setpoints and in receiving sensor data from external sources, is conservatively estimated via the CP_DELAY parameter. This should be tuned to the specific vehicle.
If the sectors adjacent to the commanded sectors are 'better' by a significant margin, the direction of the requested input can be modified by up to the angle specified in CP_GUIDE_ANG. 这有助于微调用户输入，以“引导”机身绕过障碍物，而不是卡在障碍物上。
航程数据丢失
If the autopilot does not receive range data from any sensor for longer than 0.5s, it will output a warning No range data received, no movement allowed. 这会导致强制将 xy 的速度设置为 0。 After 5 seconds of not receiving any data, the vehicle will switch into HOLD mode. If you want the vehicle to be able to move again, you will need to disable Collision Prevention by either setting the parameter CP_DIST to a negative value, or switching to a mode other than Position mode (e.g. to Altitude mode or Stabilized mode).
如果连接了多个传感器，但是其中有一个传感器失去连接，仍然能够在有传感器数据上报的视野（FOV）范围内飞行。 故障传感器的数据会失效，并且该传感器覆盖的区域会被视为未覆盖区域，意味着无法移动到该区域。
WARNING
Be careful when enabling CP_GO_NO_DATA=1, which allows the vehicle to fly outside the area with sensor coverage. 如果多个传感器中有一个失去连接，故障传感器所覆盖的区域将被视为未覆盖，可以在该区域移动不受限制。
Companion Setup
WARNING
The companion implementation/setup is currently untested (the original companion project was unmaintained and has been archived).
If using a companion computer or external sensor, it needs to supply a stream of OBSTACLE_DISTANCE messages, which should reflect when and where obstacle were detected.
The minimum rate at which messages must be sent depends on vehicle speed - at higher rates the vehicle will have a longer time to respond to detected obstacles. Initial testing of the system used a vehicle moving at 4 m/s with OBSTACLE_DISTANCE messages being emitted at 10Hz (the maximum rate supported by the vision system). 在更高的速度或更低的距离信息更新频率下，该系统应该也能达到不错的效果。
Gazebo Simulation
Collision Prevention can be tested using Gazebo with the x500_lidar_2d model. To do this, start a simulation with the x500 lidar model by running the following command:
sh
make px4_sitl gz_x500_lidar_2dNext, adjust the relevant parameters to the appropriate values and add arbitrary obstacles to your simulation world to test the collision prevention functionality.
The diagram below shows a simulation of collision prevention as viewed in Gazebo.
Sensor Data Overview (Implementation Details)
Collision Prevention has an internal obstacle distance map that divides the plane around the drone into 72 Sectors. Internally this information is stored in the obstacle_distance UORB topic. New sensor data is compared to the existing map, and used to update any sections that has changed.
The angles in the obstacle_distance topic are defined as follows:
The data from rangefinders, rotary lidars, or companion computers, is processed differently, as described below.
Rotary Lidars
Rotary Lidars add their data directly to the obstacle_distance uORB topic.
Rangefinders
Rangefinders publish their data to the distance_sensor uORB topic.
This data is then mapped onto the obstacle_distance topic. All sectors which have any overlap with the orientation (orientation and q) of the rangefinder, and the horizontal field of view (h_fov) are assigned that measurement value. For example, a distance sensor measuring from 9.99° to 10.01° the measurements will get added to the bin's corresponding to 5° and 10° covering the arc from 2.5° and 12.5°
INFO
the quaternion q is only used if the orientation is set to ROTATION_CUSTOM.
机载电脑
Companion computers update the obstacle_distance topic using ROS2 or the OBSTACLE_DISTANCE MAVLink message.