UAVPathManagerBus issue in the 3D Obstacle Avoidance UAV Package delivery

Hi,

I'm trying to modify the UAV Package Delivery project such as it allows multiple waypoints while avoiding obstacles, using QGroundConrol. Until now I have done these:

-modified replaced FollowWaypoints with the guidance from FullGuidanceLogic

However, MissionData input must be bus signal "uavPathManagerBus"

Currently, the MissionData comes from the function that contains the OA algorithm:

function [y, DTD, needHover] = computeLPWaypoints(mission, Steer,UAVState)
%#codegen

Wp1 = [mission(1).position' mission(1).params(4)];
Wp2x = mission(2).position(1);
Wp2y = mission(2).position(2);
needHover = false;

% Follow obstacle avoidance steer direction when UAV is above 5 meters
% above ground
if (~any(isnan(Steer)) && (UAVState.z < -5))
    if (Steer(5) == 0)
        % Compute look ahead point 6 meters along the steering direction
        LAP_local = 6*Steer(2:4)';
        % Follow yaw command from obstacle avoidance
        yaw = Steer(1);
        LAP = [LAP_local yaw] + [UAVState.x, UAVState.y, UAVState.z, 0];
        if(isnan(LAP(4)))
            LAP(4) = 0;
        end

        y = [ Wp1(1,:) ; LAP ; Wp2x , Wp2y , 0 , LAP(4) ];
        DTD = norm([Wp2x Wp2y] - LAP(1:2));
    else
        % Rotate in place
        y = [ Wp1(1,:) ; UAVState.x, UAVState.y, UAVState.z Steer(1); Wp2x , Wp2y , 0 , Steer(1) ];
        DTD = norm([Wp2x Wp2y] - [UAVState.x, UAVState.y]);
        needHover = true;
    end
    
else
    % If there is no good steer direction or UAV is too low, fly up
    y = [ Wp1(1,:) ; UAVState.x , UAVState.y , UAVState.z-2 , 0 ; Wp2x , Wp2y , 0 , 0 ];
    DTD = norm([Wp2x Wp2y] - [UAVState.x, UAVState.y]);
end

if(DTD<5)
    % When close to target point, switch to landing
    y = [ Wp1(1,:) ; Wp2x , Wp2y, -11 , 0 ; Wp2x , Wp2y , -1 , 0 ];
end


end

I get this obvious error when I try to run the simulation:

I have tried sending the sigal from the GCS dirrectly into the MissionData (as expected), but then the quadcopter just flies back and forth around the first waypoint of the mission:

I would very much appreciate any advice on how to move further with this.

Regards,

Tudor

Answer 1

[Jianxin Sun]

Hi Tudor,

I gave it a try to combine uavPackageDelivery/Multirotor (MultirotorModel)/Guidance Logic/Full Guidance Logic and the uavPackageDelivery/Multirotor (MultirotorModel)/Guidance Logic/Obstacle Avoidance3D so that the guidance logic and takes in the complete mission and uses Lidar for guidance when the UAV is in waypoint following mode. The goal is to create OBCCommands from path manager consists of two waypoints and pass it outside to the Onboard Computer module for obstacle avoidance. Also takes in OBCMsgs from Onboard Computer and generate position command when UAV is in waypoint following mode.

Regards,

Jianxin

[Tudor-Stefan Tonca]

Hi @Jianxin Sun,

I am extremely grateful for your assistance with this matter.

I have modified the project as per your advice. I have understood how it works and what was I missing, however I have the following errors when I try to run the simulation (project configuration: 3D Obstacle Avoidance):

From the images below, it appears that the UAVState and OBCMsgs signals are the issue:

These are the parameters set for them (I'm thinking that the problem might come from the settings):

Thank you once again for your help, it's much appreciated.

Regards,

Tudor

UPDATE

I've changed the connection from the UAV state to the ComputeLPWaypoints and Compute InnerLoop Cmds to be taken after the bus selector and multiplexer, as follows (so it can get x, y and z). However, I still have an error regarding the UAVState bus signal that's entering Landing in the Compute InnerLoop Cmds:

The signal UAVState1 is set to enter as BUS, however I feel that somewhere its parameters are not correctly set.

[Jianxin Sun]

Hi,

If you used the "3D Obstacle Avoidance" short cut in the project menu and updated "uavPackageDelivery/Multirotor (MultirotorModel)/Guidance Logic/Obstacle Avoidance3D" subsystem to follow my previous screen shot, the UAVState should be coming from the plant model as below and the OBCMsgs should be a 5x1 vector come the "uavPackageDelivery/On Board Computer/DataProcessing/Process3DSensorData" subsystem. "Process3DSensorData" should be the active variant in "uavPackageDelivery/On Board Computer/DataProcessing" if you used "3D Obstacle Avoidance" short cut.

You will need to use "useQGC = 1;" in the command window to enable QGC communication.

[Tudor-Stefan Tonca]

@Jianxin Sun,

I have made sure that "Process3DSensorData" is active and "useQGC" is set to 1. I fixed the issue with the UAVState signal (a bus selector was missing). However, the quadcopter does not avoid any obstacles anymore and it just stops at the first waypoint (if I add more) and flies up and down until the simulation stops.

I have also tried working on version 2022b and the original project example, but same result. How have you managed to make it avoid obstacles while flying through multiple waypoints? I ensured that nothing is missing from your screenshots and the model is exactly the same. Is there anything else that needs to be done in order for this to work?

Thank you.

Regards,

Tudor

[Tudor-Stefan Tonca]

@Jianxin Sun

I have found a possible cause of the issue: I observed that while "WP" state is active, the Obstacle Avoidance algorithm is activated as expected. In my previous screenshots, the Obstacle avoidance activated the other way around (when switched from WP to any other state).

The mode is changed right after takeoff from "take-off" to "WP". After carefully analysing the parameters, I have found out that the InnerLoopCommands are sent from the obstacle avoidance while WP is the active state, but the mission parameters entering the Matlab function "computeLPWaypoints" are not the next waypoint parameters, but rather the current one.

This being said, the quadcopter takes-off and does not proceed to the next desired waypoint, but has an up-and-down path, maintaining its current waypoint. I think that by changing the function so that the algorithm computes the path to the desired waypoint, the issue might be solved. However, I am unable to determine how to get the next waypoint from the "OBCCommands" so I can edit the function.

In the code below, do I need to change Wp1, Wp2x and Wp2y in order to take the correct (next) waypoint? If yes, how?

Please accept my sincere apologies for being so insistent with this problem, but I have spent countless hours trying to solve it, without any success.

Thank you once again.

Tudor

function [y, DTD, needHover] = computeLPWaypoints(mission, Steer,UAVState)
%#codegen

Wp1 = [mission(1).position' mission(1).params(4)];
Wp2x = mission(2).position(1); 
Wp2y = mission(2).position(2);
needHover = false;

% Follow obstacle avoidance steer direction when UAV is above 5 meters
% above ground
if (~any(isnan(Steer)) && (UAVState.z < -5))
    if (Steer(5) == 0)
        % Compute look ahead point 6 meters along the steering direction
        LAP_local = 6*Steer(2:4)';
        % Follow yaw command from obstacle avoidance
        yaw = Steer(1);
        LAP = [LAP_local yaw] + [UAVState.x, UAVState.y, UAVState.z, 0];
        if(isnan(LAP(4)))
            LAP(4) = 0;
        end

        y = [ Wp1(1,:) ; LAP ; Wp2x , Wp2y , 0 , LAP(4) ];
        DTD = norm([Wp2x Wp2y] - LAP(1:2));
    else
        % Rotate in place
        y = [ Wp1(1,:) ; UAVState.x, UAVState.y, UAVState.z Steer(1); Wp2x , Wp2y , 0 , Steer(1) ];
        DTD = norm([Wp2x Wp2y] - [UAVState.x, UAVState.y]);
        needHover = true;
    end
    
else
    % If there is no good steer direction or UAV is too low, fly up
    y = [ Wp1(1,:) ; UAVState.x , UAVState.y , UAVState.z-2 , 0 ; Wp2x , Wp2y , 0 , 0 ];
    DTD = norm([Wp2x Wp2y] - [UAVState.x, UAVState.y]);
end

if(DTD<5)
    % When close to target point, switch to landing
    y = [ Wp1(1,:) ; Wp2x , Wp2y, -11 , 0 ; Wp2x , Wp2y , -1 , 0 ];
end


end

[Jianxin Sun]

Yes, I did need to switch the waypoints like below. I also did some minor adjustments to the look ahead point computation. The idea is to only use this logic when the UAV is going from waypoints to waypoints. My snapshot set it up so that the Obstacle Avoidance will start after UAV arrive at waypoint 1 assuming your mission is setup as: takeoff -> waypoint 1 -> waypoint 2 -> waypoint 3 -> etc.

function [y, DTD, needHover] = computeLPWaypoints(mission, Steer,UAVState)
%#codegen

Wp1 = [mission(2).position' mission(2).params(4)];
Wp2x = mission(1).position(1);
Wp2y = mission(1).position(2);
needHover = false;

% Follow obstacle avoidance steer direction when UAV is above 5 meters
% above ground
if (~any(isnan(Steer)) && (UAVState.z < -5))
    if (Steer(5) == 0)
        % Compute look ahead point 6 meters along the steering direction
        LAP_local = 6*Steer(2:4)';
        % Follow yaw command from obstacle avoidance
        yaw = Steer(1);
        LAP = [LAP_local yaw] + [UAVState.x, UAVState.y, UAVState.z, 0];
        if(isnan(LAP(4)))
            LAP(4) = 0;
        end

        y = [ Wp1(1,:) ; LAP ; Wp2x , Wp2y , 0 , LAP(4) ];
        DTD = norm([Wp2x Wp2y] - LAP(1:2));
    else
        % Rotate in place
        y = [ Wp1(1,:) ; UAVState.x, UAVState.y, UAVState.z Steer(1); Wp2x , Wp2y , 0 , Steer(1) ];
        DTD = norm([Wp2x Wp2y] - [UAVState.x, UAVState.y]);
        needHover = true;
    end
    
else
    % If there is no good steer direction or UAV is too low, fly up
    y = [ Wp1(1,:) ; UAVState.x , UAVState.y , UAVState.z-2 , 0 ; Wp2x , Wp2y , 0 , 0 ];
    DTD = norm([Wp2x Wp2y] - [UAVState.x, UAVState.y]);
end

if(DTD<5)
    % When close to target point, switch to landing
    y = [ Wp1(1,:) ; Wp2x , Wp2y, -11 , 0 ; Wp2x , Wp2y , -1 , 0 ];
end


end

[Tudor-Stefan Tonca]

@Jianxin Sun, after changing the algorithm within the function, the issue still persist. The quadcopter just stucks at the waypoint and flies up and down (Mission is Take-off ->Waypoint 1 -> Waypoint 2 -> Waypoint 3 -> Landing). Any ideas regarding why is this happening?

In the picture below, Gndc Mode signal output is 2, so it takes the InnerLoopCommands from the Obstacle Avoidance logic. Until close to the first waypoint, the guidance is taken from the stateflow.

[Jianxin Sun]

At this point I suspect the issue is in how obstacle avoidance algorithm is tuned for your environment and sensor setup. But the model seems to be taking in the multiple waypoints from QGC.

I would suggest you take a look at the following examples and adjust your OA algorithm:

https://www.mathworks.com/help/uav/ug/uav-obstacle-avoidance-in-simulink.html

https://www.mathworks.com/help/uav/ug/tune-3d-vector-field-histogram-controller-for-obstacle-avoidance-in-3d-scene.html

[Tudor-Stefan Tonca]

Dear @Jianxin Sun, I have managed to solve the issue! There is a selector on the OBCCommands Signal from On-Board Computer -> Process3DSensorData. I switched it from 2 (which was actually the "From" or actual waypoint) to 1 ("To" waypoint) and it the quadcopter performs as desired!! I am extremely grateful for your help with this matter. Thank you!