UAVPathManagerBus issue in the 3D Obstacle Avoidance UAV Package delivery

Question

Tudor-Stefan Tonca 2023-2-2

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1905201-uavpathmanagerbus-issue-in-the-3d-obstacle-avoidance-uav-package-delivery

评论： Tudor-Stefan Tonca 2023-2-6

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

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Jianxin Sun 2023-2-2

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1905201-uavpathmanagerbus-issue-in-the-3d-obstacle-avoidance-uav-package-delivery#answer_1162586

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

8 个评论
显示 6更早的评论隐藏 6更早的评论

Tudor-Stefan Tonca 2023-2-3

编辑：Tudor-Stefan Tonca 2023-2-3

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.

Tudor-Stefan Tonca 2023-2-5

在 MATLAB Online 中打开

@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 2023-2-6

在 MATLAB Online 中打开

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

Jianxin Sun 2023-2-6

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 2023-2-6

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!

请先登录，再进行评论。

UAVPathManagerBus issue in the 3D Obstacle Avoidance UAV Package delivery

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

采纳的回答

8 个评论
显示 6更早的评论隐藏 6更早的评论

更多回答（0 个）

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

UAVPathManagerBus issue in the 3D Obstacle Avoidance UAV Package delivery

0 个评论 显示 -2更早的评论隐藏 -2更早的评论

采纳的回答

8 个评论 显示 6更早的评论隐藏 6更早的评论

更多回答（0 个）

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

8 个评论
显示 6更早的评论隐藏 6更早的评论