Module/guidance vector field - Revision history

Noether at 11:19, 3 January 2021

2021-01-03T11:19:42Z

← Older revision		Revision as of 05:19, 3 January 2021
Line 252:		Line 252:
	=== How does the ''GVF Parametric'' works?===		=== How does the ''GVF Parametric'' works?===

	The preliminaries of the ''GVF Parametric'' algorithm has been presented in the CDC 2020 (Conference on Decision and Control) [https://arxiv.org/abs/2003.10012 Mobile Robot Path Following Control in 2D Using a 3D Guiding Vector Field: Singularity Elimination and Global Convergence].		The preliminaries of the ''GVF Parametric'' algorithm has been presented in the CDC 2020 (Conference on Decision and Control) [https://arxiv.org/abs/2003.10012 Mobile Robot Path Following Control in 2D Using a 3D Guiding Vector Field: Singularity Elimination and Global Convergence]. A more detailed discussion can be found in the IEEE Transactions on Robotics article [https://ieeexplore.ieee.org/document/9312173 Singularity-Free Guiding Vector Field for Robot Navigation]

	=== Using the ''GVF Parametric'' in the flight plan ===		=== Using the ''GVF Parametric'' in the flight plan ===

Noether at 11:18, 3 January 2021

2021-01-03T11:18:01Z

← Older revision		Revision as of 05:18, 3 January 2021
Line 15:		Line 15:
	The ''GVF Implicit'' algorithm is based on the paper [https://arxiv.org/abs/1610.02797 Guidance algorithm for smooth trajectory tracking of a fixed wing UAV flying in wind flows], presented at ICRA 2017. A more rigorous and detailed analysis can be found in the paper [https://www.researchgate.net/publication/309191959_A_guiding_vector_field_algorithm_for_path_following_control_of_nonholonomic_mobile_robots A guiding vector field algorithm for path following control of nonholonomic mobile robots].		The ''GVF Implicit'' algorithm is based on the paper [https://arxiv.org/abs/1610.02797 Guidance algorithm for smooth trajectory tracking of a fixed wing UAV flying in wind flows], presented at ICRA 2017. A more rigorous and detailed analysis can be found in the paper [https://www.researchgate.net/publication/309191959_A_guiding_vector_field_algorithm_for_path_following_control_of_nonholonomic_mobile_robots A guiding vector field algorithm for path following control of nonholonomic mobile robots].

	The preliminaries of the ''GVF Parametric'' algorithm has been presented in the CDC 2020 (Conference on Decision and Control) [https://arxiv.org/abs/2003.10012 Mobile Robot Path Following Control in 2D Using a 3D Guiding Vector Field: Singularity Elimination and Global Convergence]. A detailed discussion of the algorithm can be found in the [https://ieeexplore.ieee.org/document/9312173 ~~IEEE Transactions on Robotics article~~].		The preliminaries of the ''GVF Parametric'' algorithm has been presented in the CDC 2020 (Conference on Decision and Control) [https://arxiv.org/abs/2003.10012 Mobile Robot Path Following Control in 2D Using a 3D Guiding Vector Field: Singularity Elimination and Global Convergence]. A detailed discussion of the algorithm can be found in the IEEE Transactions on Robotics article [https://ieeexplore.ieee.org/document/9312173 Singularity-Free Guiding Vector Field for Robot Navigation].

	The ''GVF Implicit'' has been exploited to control formations of several aircraft flying at a constant speed. Check the subsection python Apps. A detailed analysis can be found in the paper [https://www.researchgate.net/publication/315514301_Circular_formation_control_of_fixed-wing_UAVs_with_constant_speeds Circular formation control of fixed-wing UAVs with constant speeds] (submitted to IROS 2017). A video about how the circular formation algorithm works https://www.youtube.com/watch?v=q4b8JRU1Gbw .		The ''GVF Implicit'' has been exploited to control formations of several aircraft flying at a constant speed. Check the subsection python Apps. A detailed analysis can be found in the paper [https://www.researchgate.net/publication/315514301_Circular_formation_control_of_fixed-wing_UAVs_with_constant_speeds Circular formation control of fixed-wing UAVs with constant speeds] (submitted to IROS 2017). A video about how the circular formation algorithm works https://www.youtube.com/watch?v=q4b8JRU1Gbw .

Noether at 11:16, 3 January 2021

2021-01-03T11:16:25Z

← Older revision		Revision as of 05:16, 3 January 2021
Line 15:		Line 15:
	The ''GVF Implicit'' algorithm is based on the paper [https://arxiv.org/abs/1610.02797 Guidance algorithm for smooth trajectory tracking of a fixed wing UAV flying in wind flows], presented at ICRA 2017. A more rigorous and detailed analysis can be found in the paper [https://www.researchgate.net/publication/309191959_A_guiding_vector_field_algorithm_for_path_following_control_of_nonholonomic_mobile_robots A guiding vector field algorithm for path following control of nonholonomic mobile robots].		The ''GVF Implicit'' algorithm is based on the paper [https://arxiv.org/abs/1610.02797 Guidance algorithm for smooth trajectory tracking of a fixed wing UAV flying in wind flows], presented at ICRA 2017. A more rigorous and detailed analysis can be found in the paper [https://www.researchgate.net/publication/309191959_A_guiding_vector_field_algorithm_for_path_following_control_of_nonholonomic_mobile_robots A guiding vector field algorithm for path following control of nonholonomic mobile robots].

	The preliminaries of the ''GVF Parametric'' algorithm has been presented in the CDC 2020 (Conference on Decision and Control) [https://arxiv.org/abs/2003.10012 Mobile Robot Path Following Control in 2D Using a 3D Guiding Vector Field: Singularity Elimination and Global Convergence]		The preliminaries of the ''GVF Parametric'' algorithm has been presented in the CDC 2020 (Conference on Decision and Control) [https://arxiv.org/abs/2003.10012 Mobile Robot Path Following Control in 2D Using a 3D Guiding Vector Field: Singularity Elimination and Global Convergence]. A detailed discussion of the algorithm can be found in the [https://ieeexplore.ieee.org/document/9312173 IEEE Transactions on Robotics article].

	The ''GVF Implicit'' has been exploited to control formations of several aircraft flying at a constant speed. Check the subsection python Apps. A detailed analysis can be found in the paper [https://www.researchgate.net/publication/315514301_Circular_formation_control_of_fixed-wing_UAVs_with_constant_speeds Circular formation control of fixed-wing UAVs with constant speeds] (submitted to IROS 2017). A video about how the circular formation algorithm works https://www.youtube.com/watch?v=q4b8JRU1Gbw .		The ''GVF Implicit'' has been exploited to control formations of several aircraft flying at a constant speed. Check the subsection python Apps. A detailed analysis can be found in the paper [https://www.researchgate.net/publication/315514301_Circular_formation_control_of_fixed-wing_UAVs_with_constant_speeds Circular formation control of fixed-wing UAVs with constant speeds] (submitted to IROS 2017). A video about how the circular formation algorithm works https://www.youtube.com/watch?v=q4b8JRU1Gbw .

Noether at 15:16, 22 September 2020

2020-09-22T15:16:21Z

← Older revision		Revision as of 09:16, 22 September 2020
Line 246:		Line 246:

	== ''GVF Parametric'' ==		== ''GVF Parametric'' ==

			=== You need Eigen to make it work! ===
			First of all, we need the Eigen lib for compiling the code. You need to execute the following command in the Paparazzi dir: git submodule update --init --recursive sw/ext/eigen

	=== How does the ''GVF Parametric'' works?===		=== How does the ''GVF Parametric'' works?===

Noether at 17:44, 8 August 2020

2020-08-08T17:44:40Z

@@ Line 394: / Line 394: @@
 }}
-- '''Forth step''' . Finally, we need to code the higher-level function that will call the algorithm to track the trajectory. This function will be called as well from your flightplan.
+- '''Forth step''' . Finally, we need to code the higher-level function that will call the algorithm to track the trajectory. This function will be called as well from your flightplan (check the flightplan gvf_demo.xml). We first define the parameters of the
 === Demo ===

Noether: /* Defining your own trajectory */

2020-08-06T05:43:08Z

Defining your own trajectory

← Older revision		Revision as of 23:43, 5 August 2020
Line 343:		Line 343:
	'f1 = cosf(alpha_rad)nrf1 - sinf(alpha_rad)*nrf2 + xo' .		'f1 = cosf(alpha_rad)nrf1 - sinf(alpha_rad)*nrf2 + xo' .

	These four parameters, together with c_x, \omega_x, \delta_x, etc, are defined in 'gvf_parametric_trajectory.p_parametric[];' (we will define them in the next step in a higher-level function).		These four parameters, together with c_x, \omega_x, \delta_x, etc, are defined in 'gvf_parametric_trajectory.p_parametric[];' (we will define them in the next step in a higher-level function). Of course, do not forget to declare the function in the corresponding gvf_parametric_3d_lissajous.h .

	{{Box Code\|\|sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c		{{Box Code\|\|sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c
Line 394:		Line 394:
	}}		}}

	~~Note that we have added~~ 'xo' ~~and~~ 'yo'		- '''Forth step''' . Finally, we need to code the higher-level function that will call the algorithm to track the trajectory. This function will be called as well from your flightplan.

	=== Demo ===		=== Demo ===

Noether at 05:39, 6 August 2020

2020-08-06T05:39:53Z

← Older revision		Revision as of 23:39, 5 August 2020
Line 328:		Line 328:
	Here there is an example (the 3D Lissajous) about how to code your own trajectory.		Here there is an example (the 3D Lissajous) about how to code your own trajectory.

	- First step, we need to write down the parametric equations. Let us choose <math>w</math> as parameter:		- '''First step''', we need to write down the parametric equations. Let us choose <math>w</math> as parameter:

	x = c_x cos(w \omega_x + delta_x), y = c_y cos(w \omega_y + delta_y), z = c_z cos(w \omega_z + delta_z) (for some reason math environment does not work well with these equations here u_U).		x = c_x cos(w \omega_x + delta_x), y = c_y cos(w \omega_y + delta_y), z = c_z cos(w \omega_z + delta_z) (for some reason math environment does not work well with these equations here u_U).

	- Second step, we need the first and second partial derivatives w.r.t. the parameter <math>w</math>:		- '''Second step''', we need the first and second partial derivatives w.r.t. the parameter <math>w</math>:

	dx/dw = -c_x \omega_x sin(w \omega_x + delta_x), dy/dw = -c_y \omega_x sin(w \omega_y + delta_y), dz/dw = -c_z \omega_z sin(w \omega_z + delta_z)		dx/dw = -c_x \omega_x sin(w \omega_x + delta_x), dy/dw = -c_y \omega_x sin(w \omega_y + delta_y), dz/dw = -c_z \omega_z sin(w \omega_z + delta_z)
Line 338:		Line 338:
	dx²/dw² = -c_x \omega_x² cos(w \omega_x + delta_x), dy²/dw² = -c_y \omega_x² cos(w \omega_y + delta_y), dz²/dw² = -c_z \omega_z² cos(w \omega_z + delta_z)		dx²/dw² = -c_x \omega_x² cos(w \omega_x + delta_x), dy²/dw² = -c_y \omega_x² cos(w \omega_y + delta_y), dz²/dw² = -c_z \omega_z² cos(w \omega_z + delta_z)

	- Third step, code (below) the computations from the previous steps. The controller will need the calculations from pointers f1,f2,f3,...f3dd.		- '''Third step''', code (below) the computations from the previous steps. The controller will need the calculations from pointers f1,f2,f3,...f3dd.

	We first note that to place the trajectory in the maps, it needs to be displaced by (xo,yo,zo), and also rotated by \alpha. That is why you will in the code that we rotate and translate the previous calculations in XY, e.g.,		We first note that to place the trajectory in the maps, it needs to be displaced by (xo,yo,zo), and also rotated by \alpha. That is why you will in the code that we rotate and translate the previous calculations in XY, e.g.,

Noether at 05:35, 6 August 2020

2020-08-06T05:35:43Z

← Older revision		Revision as of 23:35, 5 August 2020
Line 337:		Line 337:

	dx²/dw² = -c_x \omega_x² cos(w \omega_x + delta_x), dy²/dw² = -c_y \omega_x² cos(w \omega_y + delta_y), dz²/dw² = -c_z \omega_z² cos(w \omega_z + delta_z)		dx²/dw² = -c_x \omega_x² cos(w \omega_x + delta_x), dy²/dw² = -c_y \omega_x² cos(w \omega_y + delta_y), dz²/dw² = -c_z \omega_z² cos(w \omega_z + delta_z)

			- Third step, code (below) the computations from the previous steps. The controller will need the calculations from pointers f1,f2,f3,...f3dd.

			We first note that to place the trajectory in the maps, it needs to be displaced by (xo,yo,zo), and also rotated by \alpha. That is why you will in the code that we rotate and translate the previous calculations in XY, e.g.,
			'f1 = cosf(alpha_rad)nrf1 - sinf(alpha_rad)*nrf2 + xo' .

			These four parameters, together with c_x, \omega_x, \delta_x, etc, are defined in 'gvf_parametric_trajectory.p_parametric[];' (we will define them in the next step in a higher-level function).

			{{Box Code\|\|sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c
			<source lang="c">
			void gvf_parametric_3d_lissajous_info(float f1, float f2, float f3, float f1d, float f2d, float f3d,
			float f1dd, float f2dd, float *f3dd)
			{
			float xo = gvf_parametric_trajectory.p_parametric[0];
			float yo = gvf_parametric_trajectory.p_parametric[1];
			float zo = gvf_parametric_trajectory.p_parametric[2];
			float cx = gvf_parametric_trajectory.p_parametric[3];
			float cy = gvf_parametric_trajectory.p_parametric[4];
			float cz = gvf_parametric_trajectory.p_parametric[5];
			float wx = gvf_parametric_trajectory.p_parametric[6];
			float wy = gvf_parametric_trajectory.p_parametric[7];
			float wz = gvf_parametric_trajectory.p_parametric[8];
			float deltax_rad = gvf_parametric_trajectory.p_parametric[9]*M_PI/180;
			float deltay_rad = gvf_parametric_trajectory.p_parametric[10]*M_PI/180;
			float deltaz_rad = gvf_parametric_trajectory.p_parametric[11]*M_PI/180;
			float alpha_rad = gvf_parametric_trajectory.p_parametric[12]*M_PI/180;

			float w = gvf_parametric_control.w;
			float wb = w * gvf_parametric_control.beta * gvf_parametric_control.s;

			// Parametric equations of the trajectory and the partial derivatives w.r.t. 'w'

			float nrf1 = cxcosf(wxwb + deltax_rad);
			float nrf2 = cycosf(wywb + deltay_rad);

			f1 = cosf(alpha_rad)nrf1 - sinf(alpha_rad)*nrf2 + xo;
			f2 = sinf(alpha_rad)nrf1 + cosf(alpha_rad)*nrf2 + yo;
			f3 = czcosf(wz*wb + deltaz_rad) + zo;

			float nrf1d = -wxcxsinf(wx*wb + deltax_rad);
			float nrf2d = -wycysinf(wy*wb + deltay_rad);

			f1d = cosf(alpha_rad)nrf1d - sinf(alpha_rad)*nrf2d;
			f2d = sinf(alpha_rad)nrf1d + cosf(alpha_rad)*nrf2d;
			f3d = -wzczsinf(wzwb + deltaz_rad);

			float nrf1dd = -wxwxcxcosf(wxwb + deltax_rad);
			float nrf2dd = -wywycycosf(wywb + deltay_rad);

			f1dd = cosf(alpha_rad)nrf1dd - sinf(alpha_rad)*nrf2dd;
			f2dd = sinf(alpha_rad)nrf1dd + cosf(alpha_rad)*nrf2dd;
			f3dd = -wzwzczcosf(wz*wb + deltaz_rad);
			}

			</source>
			}}

			Note that we have added 'xo' and 'yo'

	=== Demo ===		=== Demo ===

Noether at 05:22, 6 August 2020

2020-08-06T05:22:26Z

← Older revision		Revision as of 23:22, 5 August 2020
Line 164:		Line 164:
	The idea is that while you can define a straight line in many different ways, you make all the transformations (for example, we take the X and Y coordinates from a waypoint and convert the heading from degrees to radians in this example)		The idea is that while you can define a straight line in many different ways, you make all the transformations (for example, we take the X and Y coordinates from a waypoint and convert the heading from degrees to radians in this example)
	in the function called by the user such that the algorithm is always evaluating the trajectory in the same way. It helps for maintaining the code and for tuning		in the function called by the user such that the algorithm is always evaluating the trajectory in the same way. It helps for maintaining the code and for tuning
	the gains (you do not want to have different set of gains depending on how you call your trajectory).		the gains (you do not want to have a different set of gains depending on how you call your trajectory).

	We finally call the function gvf_line_XY_heading that will invoke the algorithm.		We finally call the function gvf_line_XY_heading that will invoke the algorithm.
Line 200:		Line 200:
	Then gvf_traj_type stands for the kind of trajectory. Right now we have 0 for lines, 1 for ellipses and 2 for sinusoidals. Feel free to choose an index that has not been taken. The rest gvf_param[x]'s are the parameters of the trajectory. You have up to seven for describing a trajectory.		Then gvf_traj_type stands for the kind of trajectory. Right now we have 0 for lines, 1 for ellipses and 2 for sinusoidals. Feel free to choose an index that has not been taken. The rest gvf_param[x]'s are the parameters of the trajectory. You have up to seven for describing a trajectory.

	The function gvf_control_2D executes the algorithm for calculating the desired turn for the vehicle. It is a good practice that all the trajectories (not only straight lines) have the same order of magnitude (for example between 0.00 and 5.00) for <math>k_e</math>. So if you have to add a scaling factor, here is ~~were~~ you have to do it.		The function gvf_control_2D executes the algorithm for calculating the desired turn for the vehicle. It is a good practice that all the trajectories (not only straight lines) have the same order of magnitude (for example between 0.00 and 5.00) for <math>k_e</math>. So if you have to add a scaling factor, here is where you have to do it.

	The variables gvf_xx are sent to the ground station as telemetry, so you can draw the vector field, the trajectory, etc.		The variables gvf_xx are sent to the ground station as telemetry, so you can draw the vector field, the trajectory, etc.

Noether at 05:21, 6 August 2020

2020-08-06T05:21:00Z

← Older revision		Revision as of 23:21, 5 August 2020
Line 335:		Line 335:

	dx/dw = -c_x \omega_x sin(w \omega_x + delta_x), dy/dw = -c_y \omega_x sin(w \omega_y + delta_y), dz/dw = -c_z \omega_z sin(w \omega_z + delta_z)		dx/dw = -c_x \omega_x sin(w \omega_x + delta_x), dy/dw = -c_y \omega_x sin(w \omega_y + delta_y), dz/dw = -c_z \omega_z sin(w \omega_z + delta_z)

	dx²/dw² = -c_x \omega_x² cos(w \omega_x + delta_x), dy²/dw² = -c_y \omega_x² cos(w \omega_y + delta_y), dz²/dw² = -c_z \omega_z² cos(w \omega_z + delta_z)		dx²/dw² = -c_x \omega_x² cos(w \omega_x + delta_x), dy²/dw² = -c_y \omega_x² cos(w \omega_y + delta_y), dz²/dw² = -c_z \omega_z² cos(w \omega_z + delta_z)