Danfoss PLUS 1 Function Block Library User guide
User Manual
PLUS+1® GUIDE Software
Autonomous Control Library Function
Blocks
www.danfoss.com
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Revision history Table of revisions
Date Changed Rev
January 2020 First edition 0101
October 2021 Added new function blocks to the library 0102
December 2021 Two new function blocks ACL2.1 0103
2 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Contents
Library Introduction
Localization.........................................................................................................................................................................................6
Navigation...........................................................................................................................................................................................7
Perception...........................................................................................................................................................................................8
Errata.....................................................................................................................................................................................................9
Change Namespace Value............................................................................................................................................................ 9
ACL Acronyms.................................................................................................................................................................................10
Ackermann Yaw Rate Function Block
Inputs..................................................................................................................................................................................................11
Outputs..............................................................................................................................................................................................12
Angle to Curvature Function Block
Inputs..................................................................................................................................................................................................14
Parameters........................................................................................................................................................................................14
Outputs..............................................................................................................................................................................................14
Curvature to Angle Function Block
Inputs..................................................................................................................................................................................................15
Parameters........................................................................................................................................................................................16
Outputs..............................................................................................................................................................................................16
Edge Detection Function Block
Inputs..................................................................................................................................................................................................18
Parameters........................................................................................................................................................................................19
Outputs..............................................................................................................................................................................................20
Extract Ring Function Block
Inputs..................................................................................................................................................................................................21
Parameters........................................................................................................................................................................................21
Outputs..............................................................................................................................................................................................21
Feature Localization Function Block
Inputs..................................................................................................................................................................................................23
Parameters........................................................................................................................................................................................24
Outputs..............................................................................................................................................................................................25
Obstacle Avoidance Function Block
Inputs..................................................................................................................................................................................................27
Parameters........................................................................................................................................................................................27
Outputs..............................................................................................................................................................................................28
Obstacle Detection Function Block
LiDAR.................................................................................................................................................................................................. 29
Inputs..................................................................................................................................................................................................30
Parameters........................................................................................................................................................................................30
Outputs..............................................................................................................................................................................................31
Origin Function Block
Inputs..................................................................................................................................................................................................33
Parameters .......................................................................................................................................................................................34
Outputs..............................................................................................................................................................................................35
Path Follower Function Block
Inputs..................................................................................................................................................................................................37
Parameters .......................................................................................................................................................................................38
Outputs..............................................................................................................................................................................................39
Restart or Resume Path After ECU Power Loss....................................................................................................................40
Position Filter Function Block
Inputs..................................................................................................................................................................................................41
Outputs..............................................................................................................................................................................................42
Post Detection Function Block
Inputs..................................................................................................................................................................................................45
©
Danfoss | January 2020 AQ295075513101en-000101 | 3
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Contents
Parameters........................................................................................................................................................................................45
Outputs..............................................................................................................................................................................................45
Projected Path Function Block
Inputs..................................................................................................................................................................................................48
Parameters........................................................................................................................................................................................48
Outputs..............................................................................................................................................................................................49
Reflector Detection Function Block
Inputs..................................................................................................................................................................................................52
Parameters........................................................................................................................................................................................52
Outputs..............................................................................................................................................................................................53
Relative Position Function Block
Inputs..................................................................................................................................................................................................54
Outputs..............................................................................................................................................................................................55
Two Point Planner Function Block
Inputs..................................................................................................................................................................................................56
Parameters........................................................................................................................................................................................57
Outputs..............................................................................................................................................................................................58
UTM Conv Function Block
Inputs..................................................................................................................................................................................................59
Outputs..............................................................................................................................................................................................59
UTM Conv Zone Function Block
Inputs..................................................................................................................................................................................................61
Outputs..............................................................................................................................................................................................62
Wall Detection Function Block
Inputs..................................................................................................................................................................................................63
Parameters........................................................................................................................................................................................64
Outputs..............................................................................................................................................................................................65
Yaw Estimate Function Block
Inputs..................................................................................................................................................................................................67
Parameters........................................................................................................................................................................................67
Outputs..............................................................................................................................................................................................68
Customizable Service Screens
Ackermann Yaw Rate Service Tool Start Screen................................................................................................................. 69
Debug Signals............................................................................................................................................................................70
Angle to Curvature Service Tool Screen................................................................................................................................ 71
Debug Signals ...........................................................................................................................................................................71
Curvature to Angle Service Tool Screen................................................................................................................................ 72
Debug Signals............................................................................................................................................................................73
Edge Detection Service Tool Screen....................................................................................................................................... 74
Debug Signals............................................................................................................................................................................75
Extract Ring Service Tool Screen.............................................................................................................................................. 76
Inputs and Outputs..................................................................................................................................................................77
Scanned Data.............................................................................................................................................................................78
Feature Localization Service Tool Screen..............................................................................................................................79
Inputs............................................................................................................................................................................................ 80
Internals........................................................................................................................................................................................83
Outputs........................................................................................................................................................................................ 84
Obstacle Avoidance Service Tool Screen.............................................................................................................................. 85
Inputs ........................................................................................................................................................................................... 86
Internals........................................................................................................................................................................................87
Outputs........................................................................................................................................................................................ 88
Obstacle Detection Service Tool Screen................................................................................................................................89
Inputs............................................................................................................................................................................................ 89
Internals and Outputs ............................................................................................................................................................ 90
4 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Contents
Origin Service Tool Screen..........................................................................................................................................................91
Debug Signals............................................................................................................................................................................92
Path Follower Service Tool Start Screen................................................................................................................................ 94
Debug Signals............................................................................................................................................................................95
Waypoints Info...........................................................................................................................................................................96
Control Points Info................................................................................................................................................................... 97
Position Filter Service Tool Start Screen................................................................................................................................ 98
Debug Signals............................................................................................................................................................................99
Standard Deviation................................................................................................................................................................100
Post Detection Service Tool Start Screen............................................................................................................................101
Debug Signals......................................................................................................................................................................... 102
Projected Path Service Tool Screen...................................................................................................................................... 103
Inputs..........................................................................................................................................................................................103
Internals and Outputs...........................................................................................................................................................104
Coordinates and Score.........................................................................................................................................................105
Reflector Detection Service Tool Screen.............................................................................................................................106
Inputs..........................................................................................................................................................................................106
Outputs......................................................................................................................................................................................107
Features Location...................................................................................................................................................................108
Standard Deviation................................................................................................................................................................108
Relative Position Service Tool Screen...................................................................................................................................109
Debug Signals......................................................................................................................................................................... 110
Two Point Planner Service Tool Screen............................................................................................................................... 111
Inputs .........................................................................................................................................................................................112
Waypoints Info........................................................................................................................................................................ 113
Control Points Info.................................................................................................................................................................114
Internal Signals........................................................................................................................................................................115
Outputs......................................................................................................................................................................................116
UTM Conversion Service Tool Start Screen........................................................................................................................117
Debug Signals......................................................................................................................................................................... 117
UTM Conversion Zone Service Tool Start Screen.............................................................................................................118
Debug Signals......................................................................................................................................................................... 119
Wall Detect Service Tool Start Screen.................................................................................................................................. 120
Debug Signals......................................................................................................................................................................... 121
Yaw Estimate Service Tool Screen.........................................................................................................................................122
Inputs .........................................................................................................................................................................................123
Outputs......................................................................................................................................................................................123
©
Danfoss | January 2020 AQ295075513101en-000101 | 5
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Library Introduction
The autonomous control function block library offers a quick and easy way to develop PLUS+1® GUIDE
applications that provide the foundation for autonomous machine control systems.
Using the function blocks in this library, developers can create applications that allow machines to
navigate environments without an operator.
Autonomous machine applications are comprised of several core sub-systems built with the autonomous
control function blocks:
•
Localization: One of the core functions of the Autonomous Control Library is to provide accurate
positioning information to Navigation sub-systems. The function blocks listed below aid in
localization.
•
Navigation: The Path_Follower function block helps autonomous machines reach their destination.
•
Perception: Radar systems, laser-based (LiDAR) systems, and ultrasonic sensors can help the
autonomous machine avoid obstacles in its path.
Localization
The physical location of the machine, over time, must be determined so the machine can operate
autonomously.
When processed by a Position Filter, data from a Global Navigation Satellite System (GNSS), Inertial
Measurement Units (IMU), steering angle sensors, and wheel speed sensors can produce a constant and
reliable estimate of a machine's location.
The Autonomous Control function block library uses a Position_Filter to provide a method to combine
GNSS data, IMU data, and odometry data. The Position_Filter function block processes the combined
data to produce an improved estimate of the autonomous machine's position and orientation over time.
To produce location estimates, the Position Filter requires sensor data to be formatted to fit standard
conventions. For example, raw GPS data is conventionally reported in latitude and longitude. The data
must be transformed into the Universal Transverse Mercator (UTM) coordinate system.
The Autonomous Control Library supplies the UTM_Conv, Origin and the Relative_Pos function blocks
to format the data properly for the Position Filter to use.
The UTM Conversion blocks (with and without zone selection) are used to convert from latitude and
longitude to the UTM coordinate frame. The Origin function block captures the starting UTM position of
the machine.
The Relative_Pos is stored as X (East being positive) and Y (North being positive) distance from the
Origin in millimeters. This simplifies operations for downstream blocks, so calculations do not need to be
done on raw latitude and longitude values.
The Ackermann_Yaw_Rate function block uses steering and speed sensor data to form a machinecentric odometry pair. Sensor standard deviation is used in the covariance matrix in the Position Filter.
UTM is the coordinate information of the latitude and longitude information of the earth into a plane
coordinate system. The East and West directions are divided into 60 zones, with each zone being 6
degrees. Further, each zone is divided into a band every 8 degrees in the latitude direction, using the
labels C to X. The letters I and O are not used, to avoid their potential confusion with the numbers one (1)
and zero (0). A, B, Y, and Z represent two polar regions.
6 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Library Introduction
UTM grid zones on a projected map of the world © Wikimedia Commons
Navigation
The Path_Follower function block compares the current machine position to the desired path and
provides a steering curvature command to bring the machine to the path.
The path is made up of one or more connected Bézier curves. The Bézier curves connect the waypoints to
make the path.
By using Bézier curves, the number of points used to define the path is reduced. This lowers computation
time and needed memory, while retaining the ability to navigate a complex path.
The Path_Follower function block uses a configurable look-ahead distance that determines how the
machine calculates its steering correction commands to reach the intended path. It also allows for
looping and non-looping paths and contains a Search_Path feature to locate the nearest point on the
path when the machine is not on the path.
The figure that follows shows a basic path, as well as the components that make up the waypoints on the
path, such as the Relative X Coordinate, Relative Y Coordinate, Bearing, Backward Radius, and Forward
Radius. The Bearing is the angle at which the machine should pass through the waypoint. The curve
between two waypoints is weighted by the length of the Forward Radius of the first point and the
Backward Radius of the second point.
©
Danfoss | January 2020 AQ295075513101en-000101 | 7
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Library Introduction
Item Description
1 Origin
2 Bearing
3 Backward Radius
4 Forward Radius
5 Waypoint
6 East-North-Up (ENU) Notation.
0 (East), 90 (North), 180 (West), 270 (South)
Perception
An autonomous machine requires sensors to detect and avoid obstacles during navigation.
CAN-based sensors can easily integrate with the Autonomous Control Library. Radar systems are well
suited to detect obstacles for autonomous machines due to their ability to operate in harsh conditions.
LiDAR systems can also provide some obstacle data on CAN, due to their more accurate distance
readings.
Ultrasonic sensors can detect objects that are in close range to the autonomous machine. With their
conical detection zone and an offering of a scalar distance value to the nearest object, these sensors are
useful for emergency braking and in safety curtain scenarios.
8 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Library Introduction
Errata
You can successfully compile applications that use the blocks in PLUS+1® GUIDE version 12.2 and later.
Change Namespace Value
To successfully compile your application, change the namespace value for function blocks that are used
more than once in an application.
Callout Description
1 Page Name
2 Namespace
1. Enable Query/Change mode.
Select Edit > Query/Change.
•
Or, press Q.
•
2. Click on the function block whose namespace you want to modify.
The Edit Page dialog box opens.
3. In the Namespace field, enter a unique Namespace value.
©
Danfoss | January 2020 AQ295075513101en-000101 | 9
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Library Introduction
Namespace values are case-sensitive.
•
To save controller memory, use a short namespace value.
•
4. Click OK.
5. Repeat these steps to enter unique namespace values for other identical function blocks.
ACL Acronyms
Acronyms used in the library user manual are described.
Acronym Meaning
ACL Autonomous Control Library
CAN Controller Area Network
ECU Electronic Control Unit
EKF Extended Kalman Filter
ENU East/North/Up
GNSS Global Navigation Satellite System
GPS Global Positioning System
IMU Inertial Measurement Unit
LiDAR Light Detection and Ranging
UTM Universal Transverse Mercator
10 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Ackermann Yaw Rate Function Block
The Ackermann Yaw Rate function block provides yaw rate information to the Position Filter.
The function block converts machine speed and steering angle into a velocity and yaw rate machinecentric odometry pair.
The pair consists of the linear velocity (meters/second) and the angular velocity (degrees/second). The
values are defined relative to a coordinate frame which has its origin on the center of the rear axle with
the X-axis pointing forward along the machine, Y pointing left along the axle and Z pointing upwards.
•
The X-axis points forward along the machine.
•
The Y-axis points left along the axle.
•
The Z-axis points up.
Input data types must exactly match the indicated type to successfully compile.
The checkpoints page includes advanced checkpoints for each input, output, and internal signal. These
require a unique name space to prevent multiple checkpoints with the same name. See the topic Change
Namespace Value for more information about creating unique namespaces.
Sensor variance—the noise observed in sensor data—can be manually set if it is not provided by the
sensor.
The standard deviation of a sensor characterizes the amount of noise in the sensor. This is obtained from
sensor documentation of manually calculating from a log of steady-state sensor data.
Inputs
Inputs to the Ackermann Yaw Rate function block are described.
Item Type Range Description [Unit]
Chkpt BOOL T/F Enables advanced checkpoints with namespace for each Diag signal.
T: Include checkpoints when compiled.
F: Do not include checkpoints when compiled.
Str_Ang_Std_Dev U32 1-4294967295 The standard deviation of the steering angle.
[0.01 degree]
VelX_Std_Dev U32 1-4294967295 The standard deviation of VelX.
[mm/s]
VelX S32 -25000-25000 The linear velocity of the machine.
[mm/s]
Str_Ang
S16
-7000-7000 The angle between the front of the machine and the steered wheel direction.
Negative values are to the right. Positive values are to the left.
[0.01 degree]
©
Danfoss | January 2020 AQ295075513101en-000101 | 11
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Ackermann Yaw Rate Function Block
Item Type Range Description [Unit]
Wheelbase
Updated BOOL T/F TRUE when there is new data.
Outputs
Item Type Range Description [Unit]
Diag BUS ——
Fault U16 —— Bitwise code where multiple items can be reported at a time.
Yaw_Rate BUS ——
Yaw_Rate_Std_Dev U32 1-4294967295 The standard deviation of Yaw_Rate.
U16 300-20000 The distance between the centers of the front and rear wheels.
[mm]
T: New data is ready.
F: New data is not ready.
Outputs of the Ackermann Yaw Rate function block are described.
Provides diagnostic values for troubleshooting.
0x0000: No fault.
0x8001: Input value too low.
0x8002: Input value too high.
This bus contains Yaw Rate and its standard deviation data.
Yaw_Rate S32 -1312080-1312080 The angular velocity of the machine relative to the machine's vertical axis.
[0.01 deg/s]
[0.01 deg/s]
Updated BOOL T/F TRUE when new data is available from the conversion.
T: New data is available.
F: New data is not available.
12 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Angle to Curvature Function Block
The Angle to Curvature function block converts a steering angle to curvature.
©
Danfoss | January 2020 AQ295075513101en-000101 | 13
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Angle to Curvature Function Block
Inputs
The following table describes the inputs of the Angle to Curve function block.
Item Type Range Description [Unit]
Chkpt BOOL T/F Enables advanced checkpoints with
namespace for each Diag signal.
T: Include checkpoints when
compiled.
F: Do not include checkpoints when
compiled.
Str_Ang
Parameters
S16 -9000 to 9000 The angle between the front of the
machine and the steered wheel
direction.
Negative values are to the right.
Positive values are to the left.
[0.01 deg]
The following table describes the parameters of the Angle to Curve function block.
Item Type Range Description [Unit]
Para BUS
Wheelbase
U16 300 to 20000 The distance between the centers of
--
Adjust configuration values here, or
replace them with signals routes
from the application through the
Para BUS.
the front and rear wheels.
[mm]
Default: 5000
Outputs
The following table describes outputs for the function block.
Item Type Range Description [Unit]
Diag BUS —— Bus containing diagnostic values for
troubleshooting. In addition, all
inputs, parameters, and output
signals are contained inside of the
bus.
Status U16 —— Bitwise code where multiple items
can be reported at a time.
0x0000: Status OK.
0x8008: At least one parameter is
out of range.
Fault U16 —— Bitwise code where multiple items
can be reported at a time.
0x0000: No fault.
0x8001: Input value too low.
0x8002: Input value too high.
Curvature
S32
-2,147,483,648 to 2,147,483,647
Curvature calculated based on the
steering angle and wheelbase of the
machine.
Negative values are right curves.
Positive values are left curves
[0.01/km]
14 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Curvature to Angle Function Block
The Curvature_to_Angle function block converts curvature to steering angle.
To get the steering angle from the Curvature_to_Angle function block you provide a curvature input in
0.01/km and a wheelbase parameter in mm.
Input data types must exactly match the indicated type to successfully compile.
Inputs
The following table describes inputs for the Curve to Angle function block.
Item Type Range Description [Unit]
Curvature
Chkpt BOOL T/F Enables Advanced Checkpoints with
S32
-2,147,483,648 to 2,147,483,647
Curvature calculated based on the
steering angle and wheelbase of the
machine.
Negative values are right curves.
Positive values are left curves
[0.01/km]
Namespace for each Diag signal.
T: Include checkpoints when
compiled.
F: Do not include checkpoints when
compiled.
©
Danfoss | January 2020 AQ295075513101en-000101 | 15
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Curvature to Angle Function Block
Parameters
The following table describes parameters for the Curve to Angle function block.
Item Type Range Description [Unit]
Para BUS —— Adjust configuration values here, or
replace them with signals routed
from the application through the
Para BUS.
Wheelbase
Outputs
Item Type Range Description [Unit]
Diag BUS —— Bus containing diagnostic data for
Status U16 —— Reports the status of the function
Fault U16 —— Reports the fault status of the
Str_Angle
U16 300 to 20000 The distance between the centers of
the front and rear wheels.
Default: 5000
[mm]
The following table describes outputs for the Curve to Angle function block.
the function block.
block.
0x0000: Status OK.
0x8008: At least one parameter is
out of range.
function block.
0x0000: No fault.
0x8001: Input value too low.
0x8002: Input value too high.
S16 -9000 to 9000 The angle between the front of the
machine and the steered wheel
direction.
Negative values are right curves.
Positive values are left curves
[0.01 deg]
16 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Edge Detection Function Block
The Edge Detect function block analyzes the incoming LiDAR scan and parses the data to look for a an
edge between two continuous features.
The edge appears as a discontinuity in the LiDAR scan. This can be used to find any generic edge of a
smooth feature such as a wall or garage door, and it is able to parse the scan from either direction, limit
the range of the scan and the size of the feature to help find more specific features.
Input data types must exactly match the indicated type to successfully compile.
The following table describes the limitations of the function block.
©
Danfoss | January 2020 AQ295075513101en-000101 | 17
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Edge Detection Function Block
Item Description
Parse Direction The parsing direction is determined by the values chosen for the start and
stop angles. If the start angle is less than the stop angle, the parsing occurs
counter-clockwise. If the start angle is greater than the stop angle, the
parsing occurs clockwise.
Falling Edge The algorithm is designed to find a falling edge within the LiDAR scan
where the distance return values are suddenly much farther away or
disappear. This is not designed to find an interior corner where the returns
appear closer.
Inputs
Inputs to the Edge Detection function block are described.
The Parsing Direction is determined by the values chosen for the Start and Stop angles. If the Start Angle
is less than the Stop Angle the parsing occurs from right to left. If the Start Angle is greater then the Stop
angle the parsing occurs from left to right.
The algorithm is designed to find a Falling Edge within the LiDAR scan where the returns are suddenly
much farther away or disappear. This is not designed to find an interior corner where the returns appear
closer.
18 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Edge Detection Function Block
Item Type Range Description
Locker_ID
Chkpt BOOL T/F Enables Advanced Checkpoints with
Parameters
Item Type Range Description [Unit]
Para BUS —— Adjust configuration values here, or
Start_Angle S32 -18000-18000 Specifies which beam of the LiDAR
Stop_Angle S32 -18000-18000 Specifies which beam of the LiDAR
Min_Feature_Size U16 1-60000 Determines how large of an object is
S8 -1-99 Non-negative ID gives location for
point cloud data to be retrieved
from.
-1: Will not process new LiDAR scan.
Namespace for each Diag signal.
T: Include checkpoints when
compiled.
F: Do not include checkpoints when
compiled.
The Edge Detection function block's operating characteristics are set by para bus input signals.
replace them with signals routed
from the application through the
Para Bus.
scan to use to start parsing for an
edge.
Default: 0
[0.01 deg]
scan to use to stop parsing for an
edge.
Default: 18000
[0.01 deg]
required to be identified as a
continuous surface Determines how
large of an object is required to be
identified as a continuous surface
before finding the falling edge. This
can be used to filter out noise from
the sensor or ignore very small
objects.
This can be used to filter out noise
from the sensor or ignore very small
objects.
Default: 1000
[mm]
Ring
U16 0-65535 For 3D LiDAR which ring, or
horizontal row of the scan to use.
For 2D LiDAR set to zero.
Default: 0
Parse Direction: The parsing direction is determined by the values chosen for the Start and Stop Angles. If
the Start Angle is less than the Stop angle the parsing will occur from right to left. If the Start Angle is
greater than the Stop Angle the parsing will occur from left to right.
Edge Detection: The algorithm is designed to find a 'falling edge' within the LiDAR scan where the returns
are suddenly much farther away or disappear. This is not designed to find an interior corner where the
returns would appear closer.
©
Danfoss | January 2020 AQ295075513101en-000101 | 19
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Edge Detection Function Block
Outputs
Outputs of the Edge Detection function block are described.
Item Type Range Description [Unit]
Diag U16 —— This bus provides diagnostic values
for troubleshooting and information
about the current status of the
sensor.
Status U16 —— Bitwise code where multiple items
can be reported at a time.
0x0000: Status OK.
0x8008: At least one parameter is
out of range.
Fault U16 —— Reports the fault status of the
function block.
Bitwise code where multiple items
can be reported at a time.
0x0000: No fault.
0x8001: Input value too low.
0x8002: Input value too high.
Edge BUS —— The Edge bus contains the updated
information about the location of
the detected edge.
Updated BOOL T/F Indicates that new information is
available from the block.
T: Edge detected.
F: Edge not detected.
Distance U32 1-4294967295 Distance from the edge in radial
coordinates.
[mm]
Angle S16 -18000-18000 Angle to the edge in radial
coordinates.
[0.01 deg]
Edge_X S32 -2147483648-2147483647 X coordinate of the edge relative to
the scanner.
[mm]
Edge_Y S32 -2147483648-2147483647 Y coordinate of the edge relative to
the scanner.
[mm]
20 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Extract Ring Function Block
The Extract_Ring function block is used to extract a ring of information from the latest LiDAR point cloud
object found inside a data locker.
Input data types must exactly match the indicated type to successfully compile.
Inputs
Inputs to the Extract Ring function block are described.
Item Type Range Description [Unit]
Locker_ID
Chkpt BOOL T/F Enables advanced checkpoints with namespace for each Diag signal.
S8 -1-99 Location where LiDAR scan data is stored.
T: Include checkpoints when compiled.
F: Do not include checkpoints when compiled.
Parameters
Parameters to the Extract Ring function block are described.
Item Type Range Description [Unit]
Para BUS —— Adjust configuration values here, or replace them with signals routed
from the application through the Para bus.
Ring
U16 0 - Number of rings - 1. Ring to extract data from point cloud. For 2D LiDAR set ring to 0
Default: 0
Outputs
Outputs of the Extract Ring are described.
Item Type Range Description [Unit]
Diag BUS —— This bus provides diagnostic values for troubleshooting and
information about the current status of the sensor.
Status
©
Danfoss | January 2020 AQ295075513101en-000101 | 21
U16 —— Bitwise code where multiple items can be reported at a time.
0x0000: Status OK.
0x8008: At least one parameter is out of range.
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Extract Ring Function Block
Item Type Range Description [Unit]
Fault
Scan
Updated
Num_Points
U16 —— Bitwise code where multiple items can be reported at a time.
0x0000: No fault.
0x8001: Input value too low.
0x8002: Input value too high.
BUS —— The output data of the read from the Data Locker.
BOOL —— True if new data was processed.
U16 0-2048 Number of points in the row.
X (Array[
2048]S
32)
Y (Array[
2048]S
32)
Z (Array[
2048]S
32)
I (Array[
2048]U
16)
-2147483648-2147483647 X coordinate of point in Cartesian coordinates.
[mm]
-2147483648-2147483647 Y coordinate of point in Cartesian coordinates.
[mm]
-2147483648-2147483647 Z coordinate of point in Cartesian coordinates.
[mm]
0-10000
intensity values of points.
22 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Feature Localization Function Block
The Feature Localization function block is used to update the location of the machine within a known
environment. This block compares the Map of known features with Detected features and estimates
machine position based on the result of comparison.
Input data types must exactly match the indicated type to successfully compile.
The checkpoints page includes advanced checkpoints for each input, output, and internal signal. These
require a unique name space to prevent multiple checkpoints with the same name. See the topic Change
Namespace Value for more information about creating unique namespaces.
Inputs
Inputs to the Feature Localization function block are described.
Item Type Range Description [Unit]
Map_Features BUS —— These features correspond to
detectable objects in the
environment. They must be in a Map
or global reference frame.
(corresponding to the origin for the
Position).
X (Array[100]S32) -2147483648-2147483647 X components of the center of
detectable features.
[mm]
Y (Array[100]S32) -2147483648-2147483647 Y components of the center of
detectable features.
[mm]
Num_Features U16 0-100 Number of features in the arrays.
Detected_Features BUS —— These are the features detected by
other blocks which should
correspond to Map_Features. They
must be in the machine reference
frame.
Updated
BOOL T/F TRUE when there is new X, Y, and
features to be used for localization.
T: The data is new and valid.
F: The data is not valid, do not use.
©
Danfoss | January 2020 AQ295075513101en-000101 | 23
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Feature Localization Function Block
Item Type Range Description [Unit]
X
(Array[S32]100) -2147483648-2147483647 X components of center of features
found.
[mm]
Y
(Array[S32]100) -2147483648-2147483647 Y components of center of features
found.
[mm]
Num_Features
Position
Chkpt BOOL T/F Enables advanced checkpoints with
U16 0-100 Number of features in the arrays.
BUS —— This is the position of the machine in
the map or global reference frame.
X S32 -2147483648-2147483647 The Cartesian X component in the
map frame.
[mm]
Y S32 -2147483648-2147483647 The Cartesian Y component in the
map frame.
[mm]
Yaw S32 -72000-72000 The Cartesian Yaw component in
the map frame. The angle used to
describe the machine's heading
using the ENU reference frame.
[0.01 deg]
namespace for each Diag signal.
T: Include checkpoints when
compiled.
F: Do not include checkpoints when
compiled.
Parameters
Parameters to the Feature Localization function block are described.
Item Type Range Description [Unit]
Para BUS —— Adjust configuration values here, or
replace them with signals routed
from the application through the
Para bus.
Distance_Error
Num_Checks
Sensor_Range_Std_Dev
Sensor_Angular_Std_Dev
U16 1-1000 The distance within which to match
the feature. Features will not be
matched if it is farther than this
distance.
Default: 500
[mm]
U16 2-100 Maximum number of matched map
features. Once these many features
are matched, farther features will
not be checked. Lowering this can
improve the processing time.
Default: 100
U16 1-1000 Standard deviation of LiDAR range.
Default: 10
[mm]
U16 1-1000 Angular standard deviation or
angular resolution of LiDAR.
Default: 20
[0.01 deg]
24 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Feature Localization Function Block
Outputs
Outputs of the Feature Localization function block are described.
Item Type Range Description [Unit]
Diag U16 —— This bus provides diagnostic values
for troubleshooting and information
about the current status of the
sensor.
Status U16 —— Reports the status of the function
block.
0x0000: Status OK.
0x8008: At least one parameter is
out of range.
Fault U16 —— Bitwise code where multiple items
can be reported at a time.
0x0000: Status OK.
0x8001: Input value too low.
0x8002: Input value too high.
Lost
Pos_XY
X_Std_Dev
Y_Std_Dev
Updated BOOL T/F T: Indicates new and valid data,
Yaw BUS —— This bus provides machine yaw and
Yaw_Std_Dev U32 1-4294967295 The standard deviation of the Yaw
Updated BOOL T/F T: Indicates new and valid data,
BOOL T/F T: No Detected_Features matched
with Map_Features, within
Distance_Error.
F: One or more features matched.
BUS —— This bus provides estimated
machine position in the map and its
standard deviation. If no update, this
bus retains last updated data.
X
S32 -2147483648-2147483647 The estimated X position of the
machine in Map frame.
[mm]
Y
S32 -2147483648-2147483647 The estimated Y position of the
U32 1-4294967295 The standard deviation of the X
U32 1-4294967295 The standard deviation of the Y
Yaw
S32 -72000-72000 The angle used to describe the
machine in Map frame.
[mm]
value.
[mm]
value.
[mm]
without any status/fault errors.
F: Not valid data, no updated
detected features, no matches.
its standard deviation in the map
frame.
machine's heading using the ENU
reference frame.
[0.01 degree]
value.
[0.01 degree]
without any status/fault errors.
F: Not valid data.
©
Danfoss | January 2020 AQ295075513101en-000101 | 25
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Obstacle Avoidance Function Block
The Obstacle_Avoidance function block simplifies the evaluation of an incoming LiDAR scan and
determines the best option.
The LiDAR scanner is typically placed at the front of a machine, but the Sensor_Offset parameters allow
for a variety of sensor placement. The function block places 15 zones radially within the defined
Max_Angle. This primary output is the Best_Angle, which is calculated based on the selected Mode. You
can use the raw zone scores for a more customized interpretation.
Input data types must exactly match the indicated type to successfully compile.
This is a safety block.
The checkpoints page includes advanced checkpoints for each input, output, and internal signal. These
require a unique name space to prevent multiple checkpoints with the same name.
26 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Obstacle Avoidance Function Block
Inputs
Inputs to the Obstacle Avoidance function block are described.
Item Type Range Description [Unit]
Mode U8 —— Selection of Best Angle calculation.
0: Raw, no additional processing of zone scores, Best_Angle =
lowest score, a tie goes to the Target_Angle.
1: Weighted, scaling and addition factors are applied to each zone
based on how far away they are from the Target_Angle.
2: Centered, designed for following a corridor and weighted to find
the most open space in the middle.
3: Nearest Acceptable, next closest angle to Target_Angle that
scores below the threshold unless Target_Angle is below the
threshold.
Target_Angle S16 -18000-18000 Desired angle for machine to drive.
[0.01 deg]
Locker_ID U8 -1-99 Location where LiDAR scan data is stored.
Chkpt BOOL T/F Enables Advanced Checkpoints with Namespace for each Diag
signal.
T: Include checkpoints when compiled.
F: Do not include checkpoints when compiled.
Parameters
The following table describes parameters for the Obstacle Avoidance function block.
Item Type Range Description [Unit]
Para BUS —— Adjust configuration values here, or replace them with signals routed
from the application through the Para bus.
Max_Angle U16 0-36000 Magnitude of max left to max right angle.
Default: 3000
[0.01 deg]
Width U16 1-20000 The width of a zone.
Default: 1000
[mm]
Threshold U16 1-10000 Higher limit of points for a zone to be invalid. Used only in mode 3.
Default: 10
[number of points]
Weight_Scale S16 -25000-25000 Scaling factor that is used to multiple the score of the zone. A
negative value steers toward the highest scoring zone.
Default: 1
[0.001]
Weight_Add U16 0-1000 Factor (Integer) that is adding to each zone after scaling factor is
applied.
Default: 0
Min_Distance U16 0-65000 Distance between the steering point of the machine and the start of
the zone.
Default: 0
[mm]
Max_Distance U16 1000-65000 Distance between the steering point of the machine and the end of
the zone.
Default: 1000
[mm]
©
Danfoss | January 2020 AQ295075513101en-000101 | 27
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Obstacle Avoidance Function Block
Item Type Range Description [Unit]
Min_Height S32 -50000-50000 Minimum height of the zone with respect to the position of the
LiDAR scanner.
Default: 0
[mm]
Max_Height S32 -50000-50000 Maximum height of the zone with respect to the position of the
LiDAR scanner.
Default: 1000
[mm]
Sensor_Offset_[X,Y,Z] S32 —— Offset of the sensor from the desired sensor frame.
Default: 0
[mm]
Sensor_Orientation S16 -18000-18000 Orientation of the LiDAR sensor from the desired sensor frame.
Default: 0
[0.01 deg]
Outputs
Outputs of the Obstacle Avoidance function block are described.
Item Type Range Description [Unit]
Diag BUS —— This bus provides diagnostic values for troubleshooting and
information about the current status of the sensor.
Status U16 —— The status of the function block.
Bitwise code where multiple items can be reported at a time.
0x0000: Status OK.
0x8008: At least one parameter is out of range.
Fault U16 —— Bitwise code where multiple items can be reported at a time.
*Non-Standard
0x0000: No fault.
0x0001: Target_Angle or Mode value too low.
0x0002: Tarteg_Angle or Mode value too high.
0x0004: Locker_ID is invalid.
0x0008: Min_Distance is greater than Max_Distance.
0x0010: Min_Height is greater than Max_Height.
Outputs BUS —— This bus provides zone scores and resulting angle information.
Updated BOOL T/F New information is available from the block namespace for each
Diag signal.
T: New data is available.
F: New data is not available.
Scores
Total_Valid_Points
Zone_Angles
Best_Score
Best_Angle
(Array[
15]U32
)
U32 0-4294967295 Number of valid LiDAR scan points used in the analysis.
(Array[
15]S16)
U32 0-4294967295 Least points count in a single zone out of all the 15 zones.
S16 -32768-32767 Angle of the zone with the best score.
0-4294967295 Reports the number of points in each zone from the LiDar scan data.
[Count]
[Count]
-32768-32767 Array of the calculated orientation of each zone.
[Count]
[Count]
[0.01 deg]
28 | © Danfoss | January 2020 AQ295075513101en-000101
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Obstacle Detection Function Block
The Obstacle_Detection function block analyzes the incoming LiDAR scan and evaluates each zone
created to determine the number of points that fall inside each zone.
LiDAR
The output is the number of obstacle points falling within the cuboid zone using point cloud data from a
sensor. A point cloud is a set of data points in space where each point is defined by its (X,Y,Z) coordinate.
This function block is extendable into Obstacle Avoidance, Projected Path or Safety Stop function blocks
by processing output data.
Use cases for this include all machines, indoor and outdoor navigation, and safety stop blocks.
Input data types must exactly match the indicated type to successfully compile.
The primary input to the Obstacle Detection block is the LiDAR scanner interface.
The internal algorithm takes into consideration the user-specified parameters and creates a cuboid zone.
Parameters include, X, Y, Z, Yaw, Length, Width, Min-Height, Max-Height. Then all the points in the point
cloud are checked if they are within the cuboid or not.
©
Danfoss | January 2020 AQ295075513101en-000101 | 29
User Manual
PLUS+1® Function Block Library—Autonomous Control Function Blocks
Obstacle Detection Function Block
Inputs
Inputs to the Obstacle Detection function block are described.
Item Type Range Description [Unit]
Locker_ID
Chkpt BOOL T/F Enables Advanced Checkpoints with
S8 -1-99 Location where LiDAR scan data is
stored.
Namespace for each Diag signal.
T: Include checkpoints when
compiled.
F: Do not include checkpoints when
compiled.
Parameters
The following table describes parameters for the Obstacle Detection function block.
Item Type Range Description [Unit]
Para BUS —— Adjust configuration values here, or
replace them with signals routed
from the application through the
Para bus.
X (ARRAY[100]S32) -2147483648-2147483647 X component of Cartesian location
of the center of the zone.
Default: zeros (100)
[mm]
Y (ARRAY[100]S32) -2147483648-2147483647 Y component of Cartesian location
of the center of the zone.
Default: zeros (100)
[mm]
30 | © Danfoss | January 2020 AQ295075513101en-000101
Loading...