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

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

Danfoss PLUS 1 Function Block Library User guide

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.

AQ295075513101en-000101 | 7

User Manual

PLUS+1® Function Block Library—Autonomous Control Function Blocks

Library Introduction

Item Description

1Origin

2Bearing

3Backward Radius

4Forward Radius

5Waypoint

6East-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.

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]

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	U16	300-20000	The distance between the centers of the front and rear wheels.
			[mm]

Updated	BOOL	T/F	TRUE when there is new data.
			T: New data is ready.
			F: New data is not ready.

Outputs

Outputs of the Ackermann Yaw Rate function block are described.

Item	Type	Range	Description [Unit]

Diag	BUS	——	Provides diagnostic values for troubleshooting.

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.

Yaw_Rate	BUS	——	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]

Yaw_Rate_Std_Dev	U32	1-4294967295	The standard deviation of Yaw_Rate.
			[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.

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	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]

Parameters

The following table describes the parameters of the Angle to Curve function block.

Item	Type	Range	Description [Unit]

Para	BUS	--	Adjust configuration values here, or
			replace them with signals routes
			from the application through the
			Para BUS.

Wheelbase	U16	300 to 20000	The distance between the centers of
			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	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]

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.

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	U16	300 to 20000	The distance between the centers of
			the front and rear wheels.
			Default: 5000
			[mm]

Outputs

The following table describes outputs for the Curve to Angle function block.

Item	Type	Range	Description [Unit]

Diag	BUS	——	Bus containing diagnostic data for
			the function block.

Status	U16	——	Reports the status of the function
			block.
			0x0000: Status OK.
			0x8008: At least one parameter is
			out of range.

Fault	U16	——	Reports the fault status of the
			function block.
			0x0000: No fault.
			0x8001: Input value too low.
			0x8002: Input value too high.

Str_Angle	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.

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	S8	-1-99	Non-negative ID gives location for
			point cloud data to be retrieved
			from.
			-1: Will not process new LiDAR scan.

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 Edge Detection function block's operating characteristics are set by para bus input signals.

Item	Type	Range	Description [Unit]

Para	BUS	——	Adjust configuration values here, or
			replace them with signals routed
			from the application through the
			Para Bus.

Start_Angle	S32	-18000-18000	Specifies which beam of the LiDAR
			scan to use to start parsing for an
			edge.
			Default: 0
			[0.01 deg]

Stop_Angle	S32	-18000-18000	Specifies which beam of the LiDAR
			scan to use to stop parsing for an
			edge.
			Default: 18000
			[0.01 deg]

Min_Feature_Size	U16	1-60000	Determines how large of an object is
			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.

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	S8	-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

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	U16	——	Bitwise code where multiple items can be reported at a time.
			0x0000: Status OK.
			0x8008: At least one parameter is out of range.

AQ295075513101en-000101 | 21

User Manual

PLUS+1® Function Block Library—Autonomous Control Function Blocks

Extract Ring Function Block

Item	Type	Range	Description [Unit]

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.

Scan	BUS	——	The output data of the read from the Data Locker.

Updated	BOOL	——	True if new data was processed.

Num_Points	U16	0-2048	Number of points in the row.

X	(Array[	-2147483648-2147483647	X coordinate of point in Cartesian coordinates.
	2048]S		[mm]
	32)

Y	(Array[	-2147483648-2147483647	Y coordinate of point in Cartesian coordinates.
	2048]S		[mm]
	32)

Z	(Array[	-2147483648-2147483647	Z coordinate of point in Cartesian coordinates.
	2048]S		[mm]
	32)

I	(Array[	0-10000	intensity values of points.
	2048]U
	16)

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.

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.

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	U16	0-100	Number of features in the arrays.

Position	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]

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

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	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]

Num_Checks	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

Sensor_Range_Std_Dev	U16	1-1000	Standard deviation of LiDAR range.
			Default: 10
			[mm]

Sensor_Angular_Std_Dev	U16	1-1000	Angular standard deviation or
			angular resolution of LiDAR.
			Default: 20
			[0.01 deg]

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	BOOL	T/F	T: No Detected_Features matched
			with Map_Features, within
			Distance_Error.
			F: One or more features matched.

Pos_XY	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
			machine in Map frame.
			[mm]

X_Std_Dev	U32	1-4294967295	The standard deviation of the X
			value.
			[mm]

Y_Std_Dev	U32	1-4294967295	The standard deviation of the Y
			value.
			[mm]

Updated	BOOL	T/F	T: Indicates new and valid data,
			without any status/fault errors.
			F: Not valid data, no updated
			detected features, no matches.

Yaw	BUS	——	This bus provides machine yaw and
			its standard deviation in the map
			frame.

Yaw	S32	-72000-72000	The angle used to describe the
			machine's heading using the ENU
			reference frame.
			[0.01 degree]

Yaw_Std_Dev	U32	1-4294967295	The standard deviation of the Yaw
			value.
			[0.01 degree]

Updated	BOOL	T/F	T: Indicates new and valid data,
			without any status/fault errors.
			F: Not valid data.

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.

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]

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	(Array[	0-4294967295	Reports the number of points in each zone from the LiDar scan data.
	15]U32		[Count]
	)

Total_Valid_Points	U32	0-4294967295	Number of valid LiDAR scan points used in the analysis.
			[Count]

Zone_Angles	(Array[	-32768-32767	Array of the calculated orientation of each zone.
	15]S16)		[Count]

Best_Score	U32	0-4294967295	Least points count in a single zone out of all the 15 zones.
			[Count]

Best_Angle	S16	-32768-32767	Angle of the zone with the best score.
			[0.01 deg]

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.

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.

LiDAR

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.

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	S8	-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 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]

AQ295075513101en-000101

+ 95 hidden pages